aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: add controller support

Browse Source
This commit is contained in:
David
2025-09-02 02:50:47 -05:00
parent 9579b64cb0
commit 9471992d3b
8 changed files with 693 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/astra_arm_moveit_config/config/astra_arm_simulated_config.yaml Normal file
View File

@@ -0,0 +1,71 @@
###############################################
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
## Properties of incoming commands
command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
# Scale parameters are only used if command_in_type=="unitless"
linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.4 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
# Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
joint: 0.5
# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
## Properties of outgoing commands
publish_period: 0.034 # 1/Nominal publish rate [seconds]
low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: trajectory_msgs/JointTrajectory
# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false
## Plugins for smoothing outgoing commands
smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: true
## MoveIt properties
move_group_name: astra_arm # Often 'manipulator' or 'arm'
planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
## Other frames
ee_frame_name: End_Effector # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
## Stopping behaviour
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4
## Configure handling of singularities and joint limits
lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: ~/status # Publish status to this topic
command_out_topic: /astra_arm_controller/joint_trajectory # Publish outgoing commands here
## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

4
src/astra_arm_moveit_config/config/initial_positions.yaml
View File

@@ -3,8 +3,8 @@
initial_positions: initial_positions:
Axis_0_Joint: 0 Axis_0_Joint: 0
Axis_1_Joint: 0 Axis_1_Joint: 0
Axis_2_Joint: 0 Axis_2_Joint: -1.6
Axis_3_Joint: 0 Axis_3_Joint: -1.6
Gripper_Slider_Left: 0 Gripper_Slider_Left: 0
Wrist-EF_Roll_Joint: 0 Wrist-EF_Roll_Joint: 0
Wrist_Differential_Joint: 0 Wrist_Differential_Joint: 0

2
src/astra_arm_moveit_config/config/ros2_controllers.yaml
View File

@@ -29,7 +29,7 @@ astra_arm_controller:
state_interfaces: state_interfaces:
- position - position
- velocity - velocity
allow_nonzero_velocity_at_trajectory_end: true allow_nonzero_velocity_at_trajectory_end: false
hand_controller: hand_controller:
ros__parameters: ros__parameters:
joint: Gripper_Slider_Left joint: Gripper_Slider_Left

67
src/astra_arm_moveit_config/launch/demo.launch.py
View File

@@ -22,6 +22,11 @@ from moveit_configs_utils.launch_utils import (
DeclareBooleanLaunchArg, DeclareBooleanLaunchArg,
) )
from launch_param_builder import ParameterBuilder
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
def generate_launch_description(): def generate_launch_description():
moveit_config = MoveItConfigsBuilder("ASTRA_Arm", package_name="astra_arm_moveit_config").to_moveit_configs() moveit_config = MoveItConfigsBuilder("ASTRA_Arm", package_name="astra_arm_moveit_config").to_moveit_configs()
# return generate_demo_launch(moveit_config) # return generate_demo_launch(moveit_config)
@@ -106,6 +111,68 @@ def generate_launch_description():
) )
) )
## SERVO
# This sets the update rate and planning group name for the acceleration limiting filter.
acceleration_filter_update_period = {"update_period": 0.01}
planning_group_name = {"planning_group_name": "astra_arm"}
# Get parameters for the Servo node
servo_params = {
"moveit_servo": ParameterBuilder("astra_arm_moveit_config")
.yaml("config/astra_arm_simulated_config.yaml")
.to_dict()
}
ld.add_action(
ComposableNodeContainer(
name="moveit_servo_demo_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
# Example of launching Servo as a node component
# Assuming ROS2 intraprocess communications works well, this is a more efficient way.
# ComposableNode(
# package="moveit_servo",
# plugin="moveit_servo::ServoServer",
# name="servo_server",
# parameters=[
# servo_params,
# moveit_config.robot_description,
# moveit_config.robot_description_semantic,
# ],
# ),
ComposableNode(
package="servo_arm_twist_pkg",
plugin="servo_arm_twist_pkg::JoyToServoPub",
name="controller_to_servo_twist_node",
),
ComposableNode(
package="joy",
plugin="joy::Joy",
name="joy_node",
),
],
output="screen",
)
)
# Launch a standalone Servo node.
# As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
ld.add_action(
Node(
package="moveit_servo",
executable="servo_node_main",
parameters=[
servo_params,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
],
output="screen",
)
)
ld.add_action( ld.add_action(
IncludeLaunchDescription( IncludeLaunchDescription(
PythonLaunchDescriptionSource( PythonLaunchDescriptionSource(

219
src/servo_arm_twist_pkg/CMakeLists.txt Normal file
View File

@@ -0,0 +1,219 @@
cmake_minimum_required(VERSION 3.22)
project(servo_arm_twist_pkg)
# C++ Libraries #################################################
# Core C++ library for calculations and collision checking.
# Provides interface used by the component node.
set(SERVO_LIB_NAME servo_arm_twist_lib)
# C++ library containing the parameters initialization
# - This is intended to use with the component node when you
# - need to read the parameters loaded into servo in a separate
# - node.
# set(SERVO_PARAM_LIB_NAME ${SERVO_LIB_NAME}_parameters)
# Pose Tracking
set(POSE_TRACKING pose_tracking)
# Component Nodes (Shared libraries) ############################
set(SERVO_COMPONENT_NODE servo_node)
set(SERVO_CONTROLLER_INPUT servo_controller_input)
# Executable Nodes ##############################################
set(SERVO_NODE_MAIN_NAME servo_node_main)
set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
#################################################################
# Common cmake code applied to all moveit packages
find_package(moveit_common REQUIRED)
moveit_package()
set(THIS_PACKAGE_INCLUDE_DEPENDS
control_msgs
control_toolbox
geometry_msgs
moveit_core
moveit_msgs
moveit_ros_planning
pluginlib
rclcpp
rclcpp_components
sensor_msgs
std_msgs
std_srvs
tf2_eigen
trajectory_msgs
)
find_package(ament_cmake REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
include_directories(
include
)
###################
## C++ Libraries ##
###################
# # This library provides a way of loading parameters for servo
# add_library(${SERVO_PARAM_LIB_NAME} SHARED
# src/servo_parameters.cpp
# src/parameter_descriptor_builder.cpp
# )
# set_target_properties(${SERVO_PARAM_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
# ament_target_dependencies(${SERVO_PARAM_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# # This library provides a C++ interface for sending realtime twist or joint commands to a robot
# add_library(${SERVO_LIB_NAME} SHARED
# # These files are used to produce differential motion
# src/collision_check.cpp
# src/enforce_limits.cpp
# src/servo.cpp
# src/servo_calcs.cpp
# src/utilities.cpp
# )
# set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
# ament_target_dependencies(${SERVO_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# target_link_libraries(${SERVO_LIB_NAME} ${SERVO_PARAM_LIB_NAME})
# add_library(${POSE_TRACKING} SHARED src/pose_tracking.cpp)
# ament_target_dependencies(${POSE_TRACKING} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# target_link_libraries(${POSE_TRACKING} ${SERVO_LIB_NAME})
#####################
## Component Nodes ##
#####################
# # Add and export library to run as a ROS node component, and receive commands via topics
# add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_node.cpp)
# ament_target_dependencies(${SERVO_COMPONENT_NODE} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# target_link_libraries(${SERVO_COMPONENT_NODE} ${SERVO_LIB_NAME})
# rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "servo_arm_twist_pkg::ServoNode")
# Add executable for using a controller
add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
######################
## Executable Nodes ##
######################
# # An executable node for the servo server
# add_executable(${SERVO_NODE_MAIN_NAME} src/servo_node_main.cpp)
# target_link_libraries(${SERVO_NODE_MAIN_NAME} ${SERVO_COMPONENT_NODE})
# ament_target_dependencies(${SERVO_NODE_MAIN_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# # An example of pose tracking
# add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp)
# target_link_libraries(${POSE_TRACKING_DEMO_NAME} ${POSE_TRACKING})
# ament_target_dependencies(${POSE_TRACKING_DEMO_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# # Add executable to publish fake servo commands for testing/demo purposes
# add_executable(${FAKE_SERVO_CMDS_NAME} test/publish_fake_jog_commands.cpp)
# ament_target_dependencies(${FAKE_SERVO_CMDS_NAME}
# rclcpp
# geometry_msgs
# std_srvs
# )
#############
## Install ##
#############
# Install Libraries
install(
TARGETS
# ${SERVO_LIB_NAME}
# ${SERVO_LIB_NAME}_parameters
# ${POSE_TRACKING}
# ${SERVO_COMPONENT_NODE}
${SERVO_CONTROLLER_INPUT}
EXPORT export_${PROJECT_NAME}
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)
# # Install Binaries
# install(
# TARGETS
# ${SERVO_NODE_MAIN_NAME}
# ${CPP_DEMO_NAME}
# ${POSE_TRACKING_DEMO_NAME}
# ${FAKE_SERVO_CMDS_NAME}
# ARCHIVE DESTINATION lib
# LIBRARY DESTINATION lib
# RUNTIME DESTINATION lib/${PROJECT_NAME}
# )
# Install include, launch, config directories
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
#############
## TESTING ##
#############
# if(BUILD_TESTING)
# find_package(ament_lint_auto REQUIRED)
# find_package(ament_cmake_gtest REQUIRED)
# find_package(ros_testing REQUIRED)
# find_package(Boost REQUIRED COMPONENTS filesystem)
# # These don't pass yet, disable them for now
# set(ament_cmake_copyright_FOUND TRUE)
# set(ament_cmake_cpplint_FOUND TRUE)
# set(ament_cmake_flake8_FOUND TRUE)
# set(ament_cmake_uncrustify_FOUND TRUE)
# # Run all lint tests in package.xml except those listed above
# ament_lint_auto_find_test_dependencies()
# # Servo integration launch test
# ament_add_gtest_executable(test_servo_integration
# test/test_servo_interface.cpp
# test/servo_launch_test_common.hpp
# )
# target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME})
# ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
# add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
# # Servo collision checking integration test
# ament_add_gtest_executable(test_servo_collision
# test/test_servo_collision.cpp
# test/servo_launch_test_common.hpp
# )
# target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME})
# ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
# add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
# # pose_tracking
# ament_add_gtest_executable(test_servo_pose_tracking
# test/pose_tracking_test.cpp
# )
# ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
# target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING})
# add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
# # Unit tests
# ament_add_gtest(servo_calcs_unit_tests
# test/servo_calcs_unit_tests.cpp
# )
# target_link_libraries(servo_calcs_unit_tests ${SERVO_LIB_NAME})
# endif()
ament_package()

3
src/servo_arm_twist_pkg/README.md Normal file
View File

@@ -0,0 +1,3 @@
# Moveit Servo
See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.

59
src/servo_arm_twist_pkg/package.xml Normal file
View File

@@ -0,0 +1,59 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>servo_arm_twist_pkg</name>
<version>2.5.9</version>
<description>Provides real-time manipulator Cartesian and joint servoing.</description>
<maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
<maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
<maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
<maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
<license>BSD 3-Clause</license>
<url type="website">https://ros-planning.github.io/moveit_tutorials</url>
<author>Brian O'Neil</author>
<author email="andyz@utexas.edu">Andy Zelenak</author>
<author>Blake Anderson</author>
<author email="alex@machinekoder.com">Alexander Rössler</author>
<author email="tyler@picknik.ai">Tyler Weaver</author>
<author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>moveit_common</depend>
<depend>control_msgs</depend>
<depend>control_toolbox</depend>
<depend>geometry_msgs</depend>
<depend>moveit_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>pluginlib</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_eigen</depend>
<depend>trajectory_msgs</depend>
<exec_depend>gripper_controllers</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>joy</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>moveit_configs_utils</exec_depend>
<exec_depend>launch_param_builder</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>controller_manager</test_depend>
<!-- <test_depend>moveit_resources_panda_moveit_config</test_depend> -->
<test_depend>ros_testing</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

271
src/servo_arm_twist_pkg/src/joystick_twist.cpp Normal file
View File

@@ -0,0 +1,271 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, PickNik Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PickNik Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Title : joystick_servo_example.cpp
* Project : servo_arm_twist_pkg
* Created : 08/07/2020
* Author : Adam Pettinger
*/
#include <sensor_msgs/msg/joy.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <control_msgs/msg/joint_jog.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <rclcpp/client.hpp>
#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/qos_event.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp/utilities.hpp>
#include <thread>
// We'll just set up parameters here
const std::string JOY_TOPIC = "/joy";
const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
const std::string EEF_FRAME_ID = "End_Effector";
const std::string BASE_FRAME_ID = "base_link";
// Enums for button names -> axis/button array index
// For XBOX 1 controller
enum Axis
{
LEFT_STICK_X = 0,
LEFT_STICK_Y = 1,
LEFT_TRIGGER = 2,
RIGHT_STICK_X = 3,
RIGHT_STICK_Y = 4,
RIGHT_TRIGGER = 5,
D_PAD_X = 6,
D_PAD_Y = 7
};
enum Button
{
A = 0,
B = 1,
X = 2,
Y = 3,
LEFT_BUMPER = 4,
RIGHT_BUMPER = 5,
CHANGE_VIEW = 6,
MENU = 7,
HOME = 8,
LEFT_STICK_CLICK = 9,
RIGHT_STICK_CLICK = 10
};
// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
// This will map the default values for the axes
std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
std::map<Button, double> BUTTON_DEFAULTS;
// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
// functions
/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
* @param axes The vector of continuous controller joystick axes
* @param buttons The vector of discrete controller button values
* @param twist A TwistStamped message to update in prep for publishing
* @param joint A JointJog message to update in prep for publishing
* @return return true if you want to publish a Twist, false if you want to publish a JointJog
*/
bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
std::unique_ptr<control_msgs::msg::JointJog>& joint)
{
// // Give joint jogging priority because it is only buttons
// // If any joint jog command is requested, we are only publishing joint commands
// if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
// {
// // Map the D_PAD to the proximal joints
// joint->joint_names.push_back("panda_joint1");
// joint->velocities.push_back(axes[D_PAD_X]);
// joint->joint_names.push_back("panda_joint2");
// joint->velocities.push_back(axes[D_PAD_Y]);
// // Map the diamond to the distal joints
// joint->joint_names.push_back("panda_joint7");
// joint->velocities.push_back(buttons[B] - buttons[X]);
// joint->joint_names.push_back("panda_joint6");
// joint->velocities.push_back(buttons[Y] - buttons[A]);
// return false;
// }
// The bread and butter: map buttons to twist commands
twist->twist.linear.z = axes[RIGHT_STICK_Y];
twist->twist.linear.y = axes[RIGHT_STICK_X];
double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
twist->twist.linear.x = lin_x_right + lin_x_left;
twist->twist.angular.y = axes[LEFT_STICK_Y];
twist->twist.angular.x = axes[LEFT_STICK_X];
double roll_positive = buttons[RIGHT_BUMPER];
double roll_negative = -1 * (buttons[LEFT_BUMPER]);
twist->twist.angular.z = roll_positive + roll_negative;
return true;
}
/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
* @param frame_name Set the command frame to this
* @param buttons The vector of discrete controller button values
*/
void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
{
if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
frame_name = BASE_FRAME_ID;
else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
frame_name = EEF_FRAME_ID;
}
namespace servo_arm_twist_pkg
{
class JoyToServoPub : public rclcpp::Node
{
public:
JoyToServoPub(const rclcpp::NodeOptions& options)
: Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
{
// Setup pub/sub
joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
// collision_pub_ =
// this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
// Create a service client to start the ServoNode
servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
servo_start_client_->wait_for_service(std::chrono::seconds(1));
servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
// // Load the collision scene asynchronously
// collision_pub_thread_ = std::thread([this]() {
// rclcpp::sleep_for(std::chrono::seconds(3));
// // Create collision object, in the way of servoing
// moveit_msgs::msg::CollisionObject collision_object;
// collision_object.header.frame_id = "panda_link0";
// collision_object.id = "box";
// shape_msgs::msg::SolidPrimitive table_1;
// table_1.type = table_1.BOX;
// table_1.dimensions = { 0.4, 0.6, 0.03 };
// geometry_msgs::msg::Pose table_1_pose;
// table_1_pose.position.x = 0.6;
// table_1_pose.position.y = 0.0;
// table_1_pose.position.z = 0.4;
// shape_msgs::msg::SolidPrimitive table_2;
// table_2.type = table_2.BOX;
// table_2.dimensions = { 0.6, 0.4, 0.03 };
// geometry_msgs::msg::Pose table_2_pose;
// table_2_pose.position.x = 0.0;
// table_2_pose.position.y = 0.5;
// table_2_pose.position.z = 0.25;
// collision_object.primitives.push_back(table_1);
// collision_object.primitive_poses.push_back(table_1_pose);
// collision_object.primitives.push_back(table_2);
// collision_object.primitive_poses.push_back(table_2_pose);
// collision_object.operation = collision_object.ADD;
// moveit_msgs::msg::PlanningSceneWorld psw;
// psw.collision_objects.push_back(collision_object);
// auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
// ps->world = psw;
// ps->is_diff = true;
// collision_pub_->publish(std::move(ps));
// });
}
// ~JoyToServoPub() override
// {
// if (collision_pub_thread_.joinable())
// collision_pub_thread_.join();
// }
void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
{
// Create the messages we might publish
auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
// This call updates the frame for twist commands
updateCmdFrame(frame_to_publish_, msg->buttons);
// Convert the joystick message to Twist or JointJog and publish
if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
{
// publish the TwistStamped
twist_msg->header.frame_id = frame_to_publish_;
twist_msg->header.stamp = this->now();
twist_pub_->publish(std::move(twist_msg));
}
// else
// {
// // publish the JointJog
// joint_msg->header.stamp = this->now();
// joint_msg->header.frame_id = "panda_link3";
// joint_pub_->publish(std::move(joint_msg));
// }
}
private:
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
std::string frame_to_publish_;
// std::thread collision_pub_thread_;
}; // class JoyToServoPub
} // namespace servo_arm_twist_pkg
// Register the component with class_loader
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)