robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

refactor(rbs_skill_servers): update action configuration, dependencies, and skills

Browse source 
- Removed `assembly_config_service.py` node from launch configuration.
- Added default `goal.duration` setting to `MoveToPose` and `MoveToPoseArray`.
- Replaced `timeout_seconds` with `duration` in action definitions for `MoveitSendJointStates` and `MoveitSendPose`.
- Removed dependencies on TinyXML2 and Gazebo/SDFormat, adding `controller_manager_msgs` and `control_msgs` to CMake configuration.
- Added new action servers `cartesian_move_to_pose` and `move_to_joint_states`, registering them in CMakeLists file.
- Introduced `SkillBase`, a template class for managing action-based skills, providing essential ROS 2 action server support and functionalities for handling goals, cancels, accepted actions, and controller management.
- Implemented methods to load, configure, and switch required controllers with conflict detection for active controllers, along with parameter checking and asynchronous handling for required parameters.
- Enhanced error handling for missing controllers, parameters, and resource conflicts.
- Updated `skills.launch.py` to utilize `ComposableNodeContainer` for skill nodes, incorporating `MoveToJointStateActionServer` and `CartesianMoveToPose` as composable nodes.
- Changed the executable name in `cartesian_move_to_pose_action_server` node configuration.
- Added `cartesian_move_to_pose.cpp`, implementing the `CartesianMoveToPose` action server, including trajectory interpolation, pose adjustment, and controller management.
- Updated `package.xml` to include `rclcpp_components` dependency.
- Refactored `MoveToJointStateActionServer` to extend `SkillBase`, leveraging `FollowJointTrajectory` for joint trajectory execution, while removing redundant code and dependencies.
- Implemented trajectory generation based on initial and target joint positions with parameterized interpolation for smoother execution, enhancing joint state handling to dynamically align current and target joint values.
This commit is contained in:
Ilya Uraev 2024-10-30 17:49:03 +03:00
parent 4dccc2b74c
commit a7b7225dd1
24 changed files with 1864 additions and 1204 deletions
Download patch file Download diff file Expand all files Collapse all files

14
env_manager/rbs_runtime/config/default-scene-config.yaml
View file

@ -94,7 +94,7 @@ objects:
relative_to: world
static: false
texture: []
type: 'model'
type: "model"
physics_rollouts_num: 0
plugins:
fts_broadcaster: false
@ -104,12 +104,12 @@ plugins:
robot:
gripper_joint_positions: 0.0
joint_positions:
- 1.57
- 0.5
- 3.14159
- 1.5
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 1.4
- 0.0
name: rbs_arm
randomizer:
@ -132,7 +132,7 @@ robot:
- 0.0
- 0.0
- 1.0
urdf_string: ''
urdf_string: ""
with_gripper: true
terrain:
model_rollouts_num: 1

85
env_manager/rbs_runtime/launch/runtime.launch.py
View file

@ -20,23 +20,14 @@ def launch_setup(context, *args, **kwargs):
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
use_moveit = LaunchConfiguration("use_moveit")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
initial_joint_controllers_file_path = os.path.join(
@ -62,7 +53,7 @@ def launch_setup(context, *args, **kwargs):
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
robot_description = {"robot_description": robot_description_content}
single_robot_setup = IncludeLaunchDescription(
rbs_robot_setup = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
@ -71,29 +62,19 @@ def launch_setup(context, *args, **kwargs):
]
),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": initial_joint_controllers_file_path,
"robot_type": robot_type,
# "controllers_file": controller_paramfile,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_type,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"use_moveit": use_moveit,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": "true",
"gazebo_gui": gazebo_gui,
"use_controllers": "true",
"robot_description": robot_description_content,
}.items(),
)
@ -111,7 +92,7 @@ def launch_setup(context, *args, **kwargs):
output="screen",
)
delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup])
delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup])
nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack]
return nodes_to_start
@ -157,20 +138,6 @@ def generate_launch_description():
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
@ -209,25 +176,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"sim_gazebo", default_value="true", description="Gazebo Simulation"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"env_manager", default_value="true", description="Launch env_manager?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_moveit", default_value="false", description="Launch moveit?"
"use_moveit", default_value="false", description="Launch moveit?"
)
)
declared_arguments.append(
@ -235,21 +184,6 @@ def generate_launch_description():
"launch_perception", default_value="false", description="Launch perception?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_task_planner",
default_value="false",
description="Launch task_planner?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"hardware",
@ -260,16 +194,11 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_controllers",
"use_controllers",
default_value="true",
description="Launch controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gazebo_gui", default_value="true", description="Launch gazebo with gui?"
)
)
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]

268
rbs_bringup/launch/rbs_robot.launch.py
View file

@ -21,26 +21,18 @@ from launch_ros.substitutions import FindPackageShare
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
launch_rviz = LaunchConfiguration("launch_rviz")
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
use_moveit = LaunchConfiguration("use_moveit")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
hardware = LaunchConfiguration("hardware")
launch_controllers = LaunchConfiguration("launch_controllers")
use_controllers = LaunchConfiguration("use_controllers")
gripper_name = LaunchConfiguration("gripper_name")
x_pos = LaunchConfiguration("x")
y_pos = LaunchConfiguration("y")
@ -58,6 +50,9 @@ def launch_setup(context, *args, **kwargs):
controllers_file = controllers_file.perform(context)
multi_robot = multi_robot.perform(context)
robot_description = LaunchConfiguration("robot_description")
robot_description_semantic = LaunchConfiguration("robot_description_semantic")
control_space = LaunchConfiguration("control_space")
control_strategy = LaunchConfiguration("control_strategy")
remappings = []
if multi_robot == "true":
@ -93,6 +88,7 @@ def launch_setup(context, *args, **kwargs):
robot_description = {"robot_description": robot_description_content}
if not robot_description_semantic.perform(context):
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
@ -116,22 +112,21 @@ def launch_setup(context, *args, **kwargs):
" ",
]
)
else:
robot_description_semantic_content = robot_description_semantic
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
# kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": robot_description_kinematics
}
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
@ -141,26 +136,6 @@ def launch_setup(context, *args, **kwargs):
parameters=[{"use_sim_time": use_sim_time}, robot_description],
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
namespace=namespace,
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(launch_rviz),
)
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
@ -174,12 +149,12 @@ def launch_setup(context, *args, **kwargs):
]
),
launch_arguments={
"control_space": "task",
"control_strategy": "position",
"has_gripper": "true",
"control_space": control_space,
"control_strategy": control_strategy,
"has_gripper": with_gripper_condition,
"namespace": namespace,
}.items(),
condition=IfCondition(launch_controllers),
condition=IfCondition(use_controllers),
)
moveit = IncludeLaunchDescription(
@ -204,7 +179,7 @@ def launch_setup(context, *args, **kwargs):
"robot_description_semantic": robot_description_semantic_content,
"namespace": namespace,
}.items(),
condition=IfCondition(launch_moveit),
condition=IfCondition(use_moveit),
)
skills = IncludeLaunchDescription(
@ -226,141 +201,77 @@ def launch_setup(context, *args, **kwargs):
"use_sim_time": use_sim_time,
"with_gripper_condition": with_gripper_condition,
"namespace": namespace,
"use_moveit": use_moveit,
}.items(),
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_task_planner"),
"launch",
"task_planner.launch.py",
]
)
]
),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner),
)
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_perception"),
"launch",
"perception.launch.py",
]
)
]
),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception),
)
asm_config = Node(
package="rbs_utils",
namespace=namespace,
executable="assembly_config_service.py",
)
nodes_to_start = [
robot_state_publisher,
control,
moveit,
skills,
asm_config,
# task_planner,
# perception,
rviz,
]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
description="Type of robot to launch, specified by name.",
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm0_controllers.yaml",
description="YAML file with the controllers configuration.",
description="YAML file containing configuration settings for the controllers.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="rbs_arm",
description="Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
description="Package containing the robot's URDF/XACRO description files. Can be set to use a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="rbs_arm_modular.xacro",
description="URDF/XACRO description file with the robot.",
description="URDF/XACRO file describing the robot's model.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_name",
default_value="arm0",
description="tf_prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="true",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="rbs_arm",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="rbs_arm.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
description="MoveIt SRDF/XACRO file for robot configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Make MoveIt to use simulation time.\
This is needed for the trajectory planing in simulation.",
description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.",
)
)
declared_arguments.append(
@ -368,55 +279,21 @@ def generate_launch_description():
"gripper_name",
default_value="",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
description="Specify gripper name (leave empty if none).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"with_gripper", default_value="true", description="With gripper or not?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_rviz", default_value="false", description="Launch RViz?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"sim_gazebo", default_value="true", description="Gazebo Simulation"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_sim",
"with_gripper",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg",
description="Specify if the robot includes a gripper.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_moveit", default_value="true", description="Launch moveit?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_perception", default_value="false", description="Launch perception?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_task_planner",
default_value="false",
description="Launch task_planner?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"cartesian_controllers",
default_value="false",
description="Load cartesian\
controllers?",
"use_moveit",
default_value="true",
description="Specify if MoveIt should be launched.",
)
)
declared_arguments.append(
@ -424,85 +301,100 @@ def generate_launch_description():
"hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface",
description="Specify the hardware interface to use (e.g., Gazebo or mock).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_controllers",
"use_controllers",
default_value="true",
description="Launch controllers?",
description="Specify if controllers should be launched.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gazebo_gui", default_value="false", description="Launch gazebo with gui?"
"namespace",
default_value="",
description="ROS 2 namespace for the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"namespace", default_value="", description="The ROS2 namespace of a robot"
"x",
default_value="0.0",
description="X-coordinate for the robot's position in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"x", default_value="0.0", description="Position of robot in world by X"
"y",
default_value="0.0",
description="Y-coordinate for the robot's position in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"y", default_value="0.0", description="Position of robot in world by Y"
"z",
default_value="0.0",
description="Z-coordinate for the robot's position in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"z", default_value="0.0", description="Position of robot in world by Z"
"roll",
default_value="0.0",
description="Roll orientation of the robot in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"roll", default_value="0.0", description="Position of robot in world by Z"
"pitch",
default_value="0.0",
description="Pitch orientation of the robot in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"pitch", default_value="0.0", description="Position of robot in world by Z"
"yaw",
default_value="0.0",
description="Yaw orientation of the robot in the world.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"yaw", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"roll", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"pitch", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"yaw", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"multi_robot",
default_value="false",
description="Flag if you use multi robot setup",
description="Specify if the setup is for multiple robots.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_description",
default_value="",
description="Robot description that override internal robot desctioption",
description="Custom robot description that overrides the internal default.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"control_space",
default_value="task",
choices=["task", "joint"],
description="Specify the control space for the robot (e.g., task space).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"control_strategy",
default_value="position",
choices=["position", "velocity", "effort"],
description="Specify the control strategy (e.g., position control).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_description_semantic",
default_value="",
description="Custom semantic robot description (SRDF) to override the default.",
)
)

1
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -22,6 +22,7 @@ public:
bool setGoal(RosActionNode::Goal& goal) override {
getInput("robot_name", goal.robot_name);
getInput("pose", goal.target_pose);
goal.duration = 2.0;
return true;
}

1
rbs_bt_executor/src/MoveToPoseArray.cpp
View file

@ -34,6 +34,7 @@ public:
goal.target_pose = m_pose_vec->poses[0];
goal.end_effector_velocity = 0.3;
goal.end_effector_acceleration = 0.3;
goal.duration = 2.0;
m_pose_vec->poses.erase(m_pose_vec->poses.begin());
setOutput("pose_vec_out", m_pose_vec);

2
rbs_skill_interfaces/action/MoveitSendJointStates.action
View file

@ -6,7 +6,7 @@ rbs_skill_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (join
float64[] joint_values
float32 joints_velocity_scaling_factor
float32 joints_acceleration_scaling_factor
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
float32 duration #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success

2
rbs_skill_interfaces/action/MoveitSendPose.action
View file

@ -11,7 +11,7 @@ geometry_msgs/Pose target_pose
string robot_name
float32 end_effector_velocity
float32 end_effector_acceleration
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
float32 duration #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success

149
rbs_skill_servers/CMakeLists.txt
View file

@ -1,12 +1,10 @@
cmake_minimum_required(VERSION 3.8)
project(rbs_skill_servers)
# Default to C99
# Set default standards
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
@ -15,7 +13,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS TRUE)
# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
@ -30,126 +30,89 @@ find_package(rbs_skill_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package(Eigen3 3.3 REQUIRED)
find_package(rbs_utils REQUIRED)
# find_package(moveit_servo REQUIRED)
find_package(controller_manager_msgs REQUIRED)
find_package(control_msgs REQUIRED)
# Default to Fortress
set(SDF_VER 12)
# If the user didn't specify a GZ distribution, pick the one matching the ROS
# distribution according to REP 2000
if(NOT DEFINED ENV{GZ_VERSION} AND DEFINED ENV{ROS_DISTRO})
if("$ENV{ROS_DISTRO}" STREQUAL "humble")
set(ENV{GZ_VERSION} "fortress")
endif()
endif()
# Find libsdformat matching the picked GZ distribution
if("$ENV{GZ_VERSION}" STREQUAL "fortress")
find_package(sdformat12 REQUIRED)
set(SDF_VER ${sdformat12_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Fortress (libSDFormat 12)")
elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
find_package(sdformat13 REQUIRED)
set(SDF_VER ${sdformat13_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden (libSDFormat 13)")
# No GZ distribution specified, find any version of libsdformat we can
else()
foreach(major RANGE 13 9)
find_package(sdformat${major} QUIET)
if(sdformat${major}_FOUND)
# Next `find_package` call will be a noop
set(SDF_VER ${major})
message(STATUS "Compiling against libSDFormat ${major}")
break()
endif()
endforeach()
endif()
include_directories(SYSTEM ${TINYXML2_INCLUDE_DIR})
link_directories(${TINYXML2_LIBRARY_DIRS})
set(deps
set(DEPS
rclcpp
rclcpp_action
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
control_msgs
controller_manager_msgs
moveit_msgs
# moveit_servo
geometry_msgs
tf2_ros
rclcpp_components
rbs_skill_interfaces
tf2_eigen
tf2_msgs
tinyxml2_vendor
geometric_shapes
sdformat${SDF_VER})
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
# -- GripperActionServer --
add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
target_include_directories(
gripper_action_server
macro(add_ros2_component target_name source_file plugin_name executable_name)
add_library(${target_name} SHARED ${source_file})
target_include_directories(${target_name}
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(gripper_action_server
PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
ament_target_dependencies(gripper_action_server ${deps})
rclcpp_components_register_node(
gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer"
EXECUTABLE gripper_control_action_server)
target_compile_definitions(${target_name} PRIVATE "${target_name}_BUILDING_DLL")
ament_target_dependencies(${target_name} ${DEPS})
rclcpp_components_register_node(${target_name} PLUGIN ${plugin_name} EXECUTABLE ${executable_name})
endmacro()
# -- PickPlacePoseLoader --
# add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
# target_include_directories(
# pick_place_pose_loader
# PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
# target_compile_definitions(pick_place_pose_loader
# PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
# ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3
# rbs_utils)
# rclcpp_components_register_node(
# pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
# EXECUTABLE pick_place_pose_loader_service_server)
# Add components
add_ros2_component(gripper_action_server
src/gripper_command.cpp
"rbs_skill_actions::GripperControlActionServer"
gripper_command)
# -- MoveitActionServers --
add_executable(move_to_joint_states_action_server
src/move_to_joint_states_action_server.cpp)
ament_target_dependencies(move_to_joint_states_action_server ${deps})
add_ros2_component(cartesian_move_to_pose
src/mtp_cart.cpp
"rbs_skill_actions::CartesianMoveToPose"
mtp_cart)
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
ament_target_dependencies(move_topose_action_server ${deps})
add_ros2_component(move_to_joint_states
src/mtjs_jtc.cpp
"rbs_skill_actions::MoveToJointStateActionServer"
mtjs_jtc)
add_executable(move_cartesian_path_action_server
src/move_cartesian_path_action_server.cpp)
ament_target_dependencies(move_cartesian_path_action_server ${deps})
add_ros2_component(move_to_pose
src/mtp_jtc.cpp
"rbs_skill_actions::MoveToPose"
mtp_jtc)
add_ros2_component(move_to_pose_cartesian
src/mtp_jtc_cart.cpp
"rbs_skill_actions::MoveToPoseJtcCartesian"
mtp_jtc_cart)
# add_executable(servo_action_server
# src/moveit_servo_skill_server.cpp)
# ament_target_dependencies(servo_action_server ${deps})
#
add_ros2_component(move_to_pose_moveit
src/mtp_moveit.cpp
"rbs_skill_actions::MoveitMtp"
mtp_moveit)
add_ros2_component(move_to_pose_moveit_cartesian
src/mtp_moveit_cart.cpp
"rbs_skill_actions::MoveitMtpCart"
mtp_moveit_cart)
# Install directories and targets
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
install(
TARGETS move_topose_action_server
TARGETS
gripper_action_server
move_to_joint_states_action_server
move_cartesian_path_action_server
# servo_action_server
cartesian_move_to_pose
move_to_joint_states
move_to_pose
move_to_pose_cartesian
move_to_pose_moveit_cartesian
move_to_pose_moveit
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME})

73
rbs_skill_servers/config/gripperPositions.yaml
View file

@ -1,73 +0,0 @@
box1:
PreGraspPose: [0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box2:
PreGraspPose: [0.0, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
box3:
PreGraspPose: [-0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box4:
PreGraspPose: [0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box5:
PreGraspPose: [0.0, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box6:
PreGraspPose: [-0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.56, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
pre_place_joint_states:
- 0.11030024152330242
- 0.8306162497371689
- -1.7680363334650195
- -0.6262361658856159
- 1.4713289344704141
- -0.11066274809566562
scene_objects:
- box1
- box2
- box3
- box4
- box5
- box6
- last

0
rbs_skill_servers/config/skills.yaml Normal file
View file

456
rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp Normal file
View file

@ -0,0 +1,456 @@
#include <algorithm>
#include <controller_manager_msgs/srv/detail/configure_controller__struct.hpp>
#include <controller_manager_msgs/srv/detail/list_controllers__struct.hpp>
#include <controller_manager_msgs/srv/detail/load_controller__struct.hpp>
#include <controller_manager_msgs/srv/detail/switch_controller__struct.hpp>
#include <memory>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/node_options.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <rmw/qos_profiles.h>
#include <string>
#include <thread>
#include <vector>
namespace rbs_skill_actions {
template <typename ActionT> class SkillBase : public rclcpp::Node {
public:
explicit SkillBase(const std::string &node_name,
const std::string &action_server_name,
const rclcpp::NodeOptions &options)
: Node(node_name, options) {
auto act_cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
m_action_server = rclcpp_action::create_server<ActionT>(
this, action_server_name,
std::bind(&SkillBase::handle_goal, this, std::placeholders::_1,
std::placeholders::_2),
std::bind(&SkillBase::handle_canceled, this, std::placeholders::_1),
std::bind(&SkillBase::handle_accepted, this, std::placeholders::_1));
m_load_controller_client =
this->create_client<controller_manager_msgs::srv::LoadController>(
"/controller_manager/load_controller");
m_switch_controller_client =
create_client<controller_manager_msgs::srv::SwitchController>(
"/controller_manager/switch_controller");
m_list_controllers_client =
create_client<controller_manager_msgs::srv::ListControllers>(
"/controller_manager/list_controllers");
m_configure_controller_client =
create_client<controller_manager_msgs::srv::ConfigureController>(
"/controller_manager/configure_controller");
}
explicit SkillBase(const std::string &node_name,
const rclcpp::NodeOptions &options)
: SkillBase(node_name, node_name, options) {}
protected:
std::shared_ptr<const typename ActionT::Goal> m_current_goal;
std::shared_ptr<typename ActionT::Result> m_current_result;
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
m_current_goal_handle;
bool m_interfaces_checked{false};
virtual rclcpp_action::GoalResponse
handle_goal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const typename ActionT::Goal> goal) {
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s]",
goal->robot_name.c_str());
(void)uuid;
m_current_goal = goal;
m_current_result = std::make_shared<typename ActionT::Result>();
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
virtual rclcpp_action::CancelResponse handle_canceled(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
goal_handle) {
auto execution_thread = [this, goal_handle]() {
this->execute(goal_handle);
};
std::thread{execution_thread}.detach();
}
void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
goal_handle) {
m_current_goal_handle = goal_handle;
m_required_controllers = requiredStateControllers();
m_required_controllers.push_back(requiredActionController());
m_required_controllers.erase(std::remove_if(
m_required_controllers.begin(), m_required_controllers.end(),
[](const std::string &str) { return str.empty(); }));
if (m_required_controllers.empty()) {
RCLCPP_ERROR(this->get_logger(), "Required controllers is empty");
m_current_goal_handle->abort(m_current_result);
return;
}
updateControllersMap();
if (m_controllers_map.count(requiredActionController()) &&
m_controllers_map[requiredActionController()] == "inactive") {
switchController(requiredActionController());
} else if (m_controllers_map.count(requiredActionController()) == 0) {
loadConfigureSwitchExecuteController({requiredActionController()});
} else if (!checkParameters()) {
getRequirementParametersAndExecute(requiredActionController());
} else {
executeAction();
}
}
void updateControllersMap() {
using namespace std::chrono_literals;
RCLCPP_INFO(this->get_logger(), "Called list_controllers service to check "
"if required controllers are loaded");
auto list_controllers_request = std::make_shared<
controller_manager_msgs::srv::ListControllers::Request>();
auto list_controllers_future =
m_list_controllers_client->async_send_request(list_controllers_request);
if (list_controllers_future.wait_for(3s) != std::future_status::ready) {
RCLCPP_ERROR(this->get_logger(),
"Timeout waiting for list_controllers response.");
return;
}
auto response = list_controllers_future.get();
m_controllers_map.clear();
std::unordered_set<std::string> loaded_controllers;
loaded_controllers.reserve(response->controller.size());
for (const auto &controller : response->controller) {
m_controllers_map[controller.name] = controller.state;
}
// if (m_missing_controllers.empty()) {
// RCLCPP_INFO(this->get_logger(), "All required controllers are
// loaded."); m_interfaces_checked = true; return true;
// } else {
// RCLCPP_WARN(this->get_logger(), "Missing required controllers:");
// for (const auto &missing_controller : m_missing_controllers) {
// RCLCPP_WARN(this->get_logger(), "\t- %s",
// missing_controller.c_str());
// }
// m_interfaces_checked = false;
// return false;
// }
}
void loadConfigureSwitchExecuteController(
const std::vector<std::string> &controller_names) {
for (auto &controller_name : controller_names) {
RCLCPP_WARN(this->get_logger(), "Attempting to load controller [%s]",
controller_name.c_str());
auto load_request = std::make_shared<
controller_manager_msgs::srv::LoadController::Request>();
load_request->name = controller_name;
auto load_future = m_load_controller_client->async_send_request(
load_request,
[this, controller_name](
rclcpp::Client<controller_manager_msgs::srv::LoadController>::
SharedFuture future) {
if (future.get()->ok) {
RCLCPP_INFO(this->get_logger(),
"Controller %s successfully loaded",
controller_name.c_str());
configureController(controller_name);
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to load controller %s",
controller_name.c_str());
m_current_goal_handle->abort(m_current_result);
}
});
}
}
void configureController(const std::string &controller_name) {
auto configure_request = std::make_shared<
controller_manager_msgs::srv::ConfigureController::Request>();
configure_request->name = controller_name;
auto configure_future = m_configure_controller_client->async_send_request(
configure_request,
[this, controller_name](
rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::
SharedFuture future) {
if (future.get()->ok) {
RCLCPP_INFO(this->get_logger(),
"Controller %s successfully configured",
controller_name.c_str());
switchController(controller_name);
} else {
RCLCPP_ERROR(this->get_logger(),
"Failed to configure controller %s",
controller_name.c_str());
m_current_goal_handle->abort(m_current_result);
}
});
}
void switchController(const std::string &controller_name) {
detectResourceConflict(controller_name, [this, controller_name](
const std::vector<std::string>
&conflicted_controllers) {
auto switch_request = std::make_shared<
controller_manager_msgs::srv::SwitchController::Request>();
switch_request->activate_controllers = {controller_name};
switch_request->deactivate_controllers = conflicted_controllers;
switch_request->strictness =
controller_manager_msgs::srv::SwitchController::Request::STRICT;
auto switch_future = m_switch_controller_client->async_send_request(
switch_request,
[this, controller_name](
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::
SharedFuture future) {
if (future.get()->ok) {
RCLCPP_INFO(this->get_logger(),
"Controller %s successfully switched",
controller_name.c_str());
if (!checkParameters()) {
getRequirementParametersAndExecute(controller_name);
} else {
if (controller_name == requiredActionController()) {
executeAction();
}
}
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to switch controller %s",
controller_name.c_str());
m_current_goal_handle->abort(m_current_result);
}
});
});
}
bool checkParameters() {
std::ostringstream missing_params_stream;
std::ostringstream empty_params_stream;
for (const auto &name : requiredParameters()) {
if (!this->has_parameter(name)) {
missing_params_stream << "\n\t- " << name;
continue;
}
const auto &parameter = this->get_parameter(name);
if (isEmptyParameter(parameter)) {
empty_params_stream << "\n\t- " << name;
}
}
if (missing_params_stream.tellp() > 0) {
RCLCPP_WARN(this->get_logger(),
"The following required parameters are missing:%s",
missing_params_stream.str().c_str());
}
if (empty_params_stream.tellp() > 0) {
RCLCPP_WARN(this->get_logger(),
"The following required parameters are empty:%s",
empty_params_stream.str().c_str());
}
return missing_params_stream.tellp() == 0 &&
empty_params_stream.tellp() == 0;
}
bool isEmptyParameter(const rclcpp::Parameter &parameter) {
switch (parameter.get_type()) {
case rclcpp::ParameterType::PARAMETER_STRING:
return parameter.as_string().empty();
case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
return parameter.as_string_array().empty();
case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
return parameter.as_integer_array().empty();
case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
return parameter.as_double_array().empty();
default:
return false;
}
}
virtual std::string requiredActionController() = 0;
virtual std::vector<std::string> requiredStateControllers() { return {""}; }
virtual std::vector<std::string> requiredParameters() { return {}; }
virtual void executeAction() = 0;
void getRequirementParametersAndExecute(const std::string &controller_name) {
RCLCPP_INFO(this->get_logger(),
"Connected to service %s, asking for parameters:",
controller_name.c_str());
for (const auto &param : requiredParameters()) {
RCLCPP_INFO(this->get_logger(), "\t- %s", param.c_str());
}
auto parameter_callback =
[this, controller_name](
const std::shared_future<std::vector<rclcpp::Parameter>> &future) {
try {
auto parameters = future.get();
if (parameters.empty()) {
RCLCPP_ERROR(this->get_logger(),
"No parameter found or empty array received");
m_current_goal_handle->abort(m_current_result);
} else {
for (const auto &param : parameters) {
switch (param.get_type()) {
case rclcpp::ParameterType::PARAMETER_BOOL:
this->declare_parameter(param.get_name(), param.as_bool());
break;
case rclcpp::ParameterType::PARAMETER_INTEGER:
this->declare_parameter(param.get_name(), param.as_int());
break;
case rclcpp::ParameterType::PARAMETER_DOUBLE:
this->declare_parameter(param.get_name(), param.as_double());
break;
case rclcpp::ParameterType::PARAMETER_STRING:
this->declare_parameter(param.get_name(), param.as_string());
break;
case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
this->declare_parameter(param.get_name(),
param.as_string_array());
break;
case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
this->declare_parameter(param.get_name(),
param.as_integer_array());
break;
case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
this->declare_parameter(param.get_name(),
param.as_double_array());
break;
default:
RCLCPP_WARN(this->get_logger(),
"Unsupported parameter type for parameter '%s'",
param.get_name().c_str());
break;
}
}
if (controller_name == requiredActionController()) {
executeAction();
}
}
} catch (const std::exception &e) {
RCLCPP_ERROR(this->get_logger(), "Failed to get parameter: %s",
e.what());
}
};
m_parameter_client =
std::make_shared<rclcpp::AsyncParametersClient>(this, controller_name);
auto future = m_parameter_client->get_parameters(
requiredParameters(), std::move(parameter_callback));
RCLCPP_INFO(this->get_logger(),
"Asynchronous request sent for required parameters");
}
void detectResourceConflict(
const std::string &new_controller,
std::function<void(const std::vector<std::string> &)> callback) {
auto list_controllers_request = std::make_shared<
controller_manager_msgs::srv::ListControllers::Request>();
m_list_controllers_client->async_send_request(
list_controllers_request,
[this, new_controller,
callback](const rclcpp::Client<
controller_manager_msgs::srv::ListControllers>::SharedFuture
future) {
std::vector<std::string> conflicted_controller;
auto it_new_controller = std::find_if(
future.get()->controller.begin(), future.get()->controller.end(),
[&new_controller](
const controller_manager_msgs::msg::ControllerState
controller) {
return controller.name == new_controller;
});
if (it_new_controller != future.get()->controller.end()) {
auto new_controller_interface =
it_new_controller->required_command_interfaces;
RCLCPP_INFO(this->get_logger(), "Received list of controllers");
for (const auto &controller : future.get()->controller) {
if (controller.state == "active") {
RCLCPP_INFO(this->get_logger(), "Checking controller: %s",
controller.name.c_str());
bool has_conflict = std::any_of(
controller.required_command_interfaces.begin(),
controller.required_command_interfaces.end(),
[&new_controller_interface](const std::string &interface) {
return std::find(new_controller_interface.begin(),
new_controller_interface.end(),
interface) !=
new_controller_interface.end();
});
if (has_conflict) {
conflicted_controller.push_back(controller.name);
RCLCPP_WARN(this->get_logger(), "Conflicted controller: %s",
controller.name.c_str());
}
}
}
} else {
RCLCPP_WARN(this->get_logger(),
"New controller not found in loaded controllers");
}
callback(conflicted_controller);
});
}
private:
rclcpp::Client<controller_manager_msgs::srv::LoadController>::SharedPtr
m_load_controller_client;
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr
m_switch_controller_client;
rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr
m_list_controllers_client;
rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr
m_configure_controller_client;
rclcpp::AsyncParametersClient::SharedPtr m_parameter_client;
typename rclcpp_action::Server<ActionT>::SharedPtr m_action_server;
std::vector<std::string> m_required_controllers{};
std::vector<std::string> m_missing_controllers{};
std::unordered_map<std::string, std::string> m_controllers_map;
};
} // namespace rbs_skill_actions

119
rbs_skill_servers/launch/skills.launch.py
View file

@ -2,7 +2,8 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.actions.composable_node_container import ComposableNode
from rbs_launch_utils.launch_common import load_yaml
@ -11,73 +12,85 @@ def launch_setup(context, *args, **kwargs):
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
use_moveit = LaunchConfiguration("use_moveit")
# with_gripper_condition = LaunchConfiguration("with_gripper_condition")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_decl
}
namespace = LaunchConfiguration("namespace")
points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml")
kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
skills_container = ComposableNodeContainer(
name="skills",
namespace=namespace,
package="rclcpp_components",
# prefix=['gdbserver localhost:1234'],
executable="component_container_mt",
composable_node_descriptions=[
ComposableNode(
package="rbs_skill_servers",
plugin="rbs_skill_actions::MoveToJointStateActionServer",
name="mtjs_jtc",
parameters=[{"use_sim_time": use_sim_time}],
),
ComposableNode(
package="rbs_skill_servers",
plugin="rbs_skill_actions::CartesianMoveToPose",
name="mtp_cart",
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
],
),
ComposableNode(
package="rbs_skill_servers",
plugin="rbs_skill_actions::MoveToPose",
name="mtp_jtc",
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
robot_description,
],
),
ComposableNode(
package="rbs_skill_servers",
plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
name="mtp_jtc_cart",
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
robot_description,
],
),
ComposableNode(
package="rbs_skill_servers",
plugin="rbs_skill_actions::MoveitMtp",
name="mtp_moveit",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
)
gripper_control_node = Node(
condition=IfCondition(use_moveit),
),
ComposableNode(
package="rbs_skill_servers",
executable="gripper_control_action_server",
namespace=namespace,
plugin="rbs_skill_actions::MoveitMtpCart",
name="mtp_moveit_cart",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition),
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
)
cartesian_move_to_pose_action_server = Node(
package="rbs_skill_servers",
executable="move_to_pose.py",
namespace=namespace,
parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
condition=IfCondition(use_moveit),
),
],
)
@ -90,12 +103,7 @@ def launch_setup(context, *args, **kwargs):
nodes_to_start = [
assembly_config,
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
cartesian_move_to_pose_action_server,
# grasp_pose_loader
skills_container,
]
return nodes_to_start
@ -116,15 +124,18 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument("robot_description_kinematics", default_value="")
)
declared_arguments.append(DeclareLaunchArgument("use_sim_time", default_value=""))
declared_arguments.append(
DeclareLaunchArgument("use_sim_time", default_value="false")
)
declared_arguments.append(
DeclareLaunchArgument("use_moveit", default_value="false")
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper_condition", default_value="")
)
declared_arguments.append(
DeclareLaunchArgument("points_params_filepath", default_value="")
DeclareLaunchArgument("namespace", default_value="")
)
declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]
)

1
rbs_skill_servers/package.xml
View file

@ -23,6 +23,7 @@
<depend>rbs_skill_interfaces</depend>
<depend>tf2_eigen</depend>
<depend>rbs_utils</depend>
<depend>rclcpp_components</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

0
rbs_skill_servers/src/gripper_control_action_server.cpp → rbs_skill_servers/src/gripper_command.cpp
View file

221
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -1,221 +0,0 @@
#include <functional>
#include <memory>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
#include "moveit/trajectory_processing/time_optimal_trajectory_generation.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
namespace rbs_skill_actions {
class MoveCartesianActionServer : public rclcpp::Node {
public:
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
// explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_cartesian_action_server"), node_(node) {
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
}
void init() {
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(), node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "move_cartesian",
std::bind(&MoveCartesianActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveCartesianActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveCartesianActionServer::accepted_callback, this,
std::placeholders::_1));
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal) {
RCLCPP_INFO(
this->get_logger(),
"Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
"%f, %f]",
goal->robot_name.c_str(), goal->target_pose.position.x,
goal->target_pose.position.y, goal->target_pose.position.z,
goal->target_pose.orientation.x, goal->target_pose.orientation.y,
goal->target_pose.orientation.z, goal->target_pose.orientation.w);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1),
goal_handle)
.detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(
*move_group_interface.getCurrentState());
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
// waypoints.push_back(current_pose.pose);
// geometry_msgs::msg::Pose start_pose = current_pose.pose;
geometry_msgs::msg::Pose target_pose = goal->target_pose;
// target_pose.position = goal->target_pose.position;
// int num_waypoints = 100;
// for (int i = 1; i <= num_waypoints; ++i) {
// geometry_msgs::msg::Pose intermediate_pose;
// double fraction = static_cast<double>(i) / (num_waypoints + 1);
//
// intermediate_pose.position.x =
// start_pose.position.x +
// fraction * (target_pose.position.x - start_pose.position.x);
// intermediate_pose.position.y =
// start_pose.position.y +
// fraction * (target_pose.position.y - start_pose.position.y);
// intermediate_pose.position.z =
// start_pose.position.z +
// fraction * (target_pose.position.z - start_pose.position.z);
//
// intermediate_pose.orientation = start_pose.orientation;
//
// waypoints.push_back(intermediate_pose);
// }
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
target_pose.position.x, target_pose.position.y,
target_pose.position.z);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(
waypoints, eef_step, jump_threshold, trajectory);
robot_trajectory::RobotTrajectory rt(
move_group_interface.getCurrentState()->getRobotModel(),
goal->robot_name);
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
bool su = tp.computeTimeStamps(rt);
rt.getRobotTrajectoryMsg(trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory;
if (fraction > 0) {
RCLCPP_INFO(this->get_logger(), "Planning success");
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
goal_handle->abort(result);
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
}
}; // class MoveCartesianActionServer
} // namespace rbs_skill_actions
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
rbs_skill_actions::MoveCartesianActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

164
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -1,164 +0,0 @@
#include <cinttypes>
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
namespace rbs_skill_actions {
class MoveToJointStateActionServer : public rclcpp::Node {
public:
using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
// explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_to_joint_states_action_server"), node_(node) {
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
void init() {
action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
node_->get_node_base_interface(), node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "move_to_joint_states",
std::bind(&MoveToJointStateActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToJointStateActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveToJointStateActionServer::accepted_callback, this,
std::placeholders::_1));
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal) {
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]",
goal->robot_name.c_str());
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1),
goal_handle)
.detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
std::vector<double> joint_states = goal->joint_values;
for (auto &joint : joint_states) {
RCLCPP_INFO(this->get_logger(), "Joint value %f", joint);
}
move_group_interface.setJointValueTarget(goal->joint_values);
move_group_interface.setMaxVelocityScalingFactor(
goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(
goal->joints_acceleration_scaling_factor);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) ==
moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToJointStateActionServer
} // namespace rbs_skill_actions
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node =
rclcpp::Node::make_shared("move_to_joint_states", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

176
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -1,176 +0,0 @@
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include <moveit_msgs/msg/display_robot_state.hpp>
namespace rbs_skill_actions {
class MoveToPoseActionServer : public rclcpp::Node {
public:
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
// explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_topose_action_server"), node_(node) {
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
void init() {
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(), node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "move_topose",
std::bind(&MoveToPoseActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToPoseActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveToPoseActionServer::accepted_callback, this,
std::placeholders::_1));
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal) {
RCLCPP_INFO(
this->get_logger(),
"Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
"%f, %f]",
goal->robot_name.c_str(), goal->target_pose.position.x,
goal->target_pose.position.y, goal->target_pose.position.z,
goal->target_pose.orientation.x, goal->target_pose.orientation.y,
goal->target_pose.orientation.z, goal->target_pose.orientation.w);
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1),
goal_handle)
.detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(
goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(
goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::core::MoveItErrorCode plan_err_code =
move_group_interface.plan(plan);
if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
move_group_interface.plan(plan);
}
if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "Planning success");
// move_group_interface.execute(plan);
moveit::core::MoveItErrorCode move_err_code =
move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
} else {
RCLCPP_WARN_STREAM(this->get_logger(),
"Failed to generate plan because "
<< error_code_to_string(plan_err_code));
goal_handle->abort(result);
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
}
}; // class MoveToPoseActionServer
} // namespace rbs_skill_actions
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
rbs_skill_actions::MoveToPoseActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

155
rbs_skill_servers/src/mtjs_jtc.cpp Normal file
View file

@ -0,0 +1,155 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_joint_states__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToJointStateActionServer : public SkillBase<MoveitSendJointStates> {
public:
explicit MoveToJointStateActionServer(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendJointStates>("mtjs_jtc", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToJointStateActionServer::jointStateCallback, this,
std::placeholders::_1),
s_options);
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
}
trajectory_goal.trajectory.points = generate_trajectory(
m_current_joint_positions, m_current_goal->joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
// RCLCPP_INFO(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_mame_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
if (index_it != m_joint_mame_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
t * (target_joint_values[j] - start_joint_values[j]);
point.positions.push_back(position);
}
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(
rbs_skill_actions::MoveToJointStateActionServer);

183
rbs_skill_servers/src/mtp_cart.cpp Normal file
View file

@ -0,0 +1,183 @@
#include "Eigen/Dense"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/logging.hpp>
// Action libs
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendPose =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
class CartesianMoveToPose : public SkillBase<MoveitSendPose> {
public:
explicit CartesianMoveToPose(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_cart", options), m_current_step(0) {
// Initialize publisher for target poses
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::PublisherOptions p_options;
p_options.callback_group = cbg;
m_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>(
"cartesian_motion_controller/target_frame", 10, p_options);
m_current_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_current_pose_subscriber =
this->create_subscription<geometry_msgs::msg::PoseStamped>(
"cartesian_motion_controller/current_pose", 10,
std::bind(&CartesianMoveToPose::on_pose_callback, this,
std::placeholders::_1),
s_options);
}
protected:
std::string requiredActionController() override {
return "cartesian_motion_controller";
}
std::vector<std::string> requiredParameters() override {
return {"end_effector_link", "robot_base_link"};
}
void executeAction() override {
m_base_link = this->get_parameter("robot_base_link").as_string();
m_trajectory =
interpolate(m_current_pose->pose, m_current_goal->target_pose);
m_current_step = 0;
m_total_time = m_current_goal->duration;
m_step_time =
m_total_time / static_cast<double>(m_trajectory.size()) * 1000.0;
m_trajectory_timer = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(m_step_time)),
[this]() { adjust_and_publish_pose(); });
}
private:
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_publisher;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
m_current_pose_subscriber;
geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose;
std::string m_base_link;
double m_threshold_distance = 0.02;
std::vector<geometry_msgs::msg::Pose> m_trajectory;
size_t m_current_step;
rclcpp::TimerBase::SharedPtr m_trajectory_timer;
double m_total_time;
double m_step_time;
void adjust_and_publish_pose() {
if (m_current_step < m_trajectory.size()) {
publish_pose(m_trajectory[m_current_step++]);
} else {
double distance =
calculate_distance(m_current_pose->pose, m_current_goal->target_pose);
if (distance <= m_threshold_distance) {
m_current_result->success = true;
m_current_goal_handle->succeed(m_current_result);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
m_trajectory_timer->cancel();
} else {
publish_pose(m_trajectory.back());
}
}
}
double calculate_distance(const geometry_msgs::msg::Pose &p1,
const geometry_msgs::msg::Pose &p2) {
Eigen::Vector3d pos1(p1.position.x, p1.position.y, p1.position.z);
Eigen::Vector3d pos2(p2.position.x, p2.position.y, p2.position.z);
double position_distance = (pos1 - pos2).norm();
Eigen::Quaterniond orient1 = normalize_orientation(p1.orientation);
Eigen::Quaterniond orient2 = normalize_orientation(p2.orientation);
return position_distance + orient1.angularDistance(orient2);
}
Eigen::Quaterniond
normalize_orientation(const geometry_msgs::msg::Quaternion &q) {
Eigen::Quaterniond orientation(q.w, q.x, q.y, q.z);
orientation.normalize();
if (orientation.norm() == 0) {
RCLCPP_FATAL(this->get_logger(),
"Quaternion has zero norm; normalization failed.");
throw std::runtime_error("Quaternion normalization failed.");
}
return orientation;
}
std::vector<geometry_msgs::msg::Pose>
interpolate(const geometry_msgs::msg::Pose &start_pose,
const geometry_msgs::msg::Pose &end_pose) {
std::vector<geometry_msgs::msg::Pose> trajectory;
Eigen::Vector3d start_position(start_pose.position.x, start_pose.position.y,
start_pose.position.z);
Eigen::Vector3d end_position(end_pose.position.x, end_pose.position.y,
end_pose.position.z);
Eigen::Quaterniond start_orientation =
normalize_orientation(start_pose.orientation);
Eigen::Quaterniond end_orientation =
normalize_orientation(end_pose.orientation);
double distance = (end_position - start_position).norm();
int num_steps = static_cast<int>(distance / 0.01);
for (int i = 0; i <= num_steps; ++i) {
double t = static_cast<double>(i) / num_steps;
Eigen::Vector3d interpolated_position =
start_position + t * (end_position - start_position);
Eigen::Quaterniond interpolated_orientation =
start_orientation.slerp(t, end_orientation);
geometry_msgs::msg::Pose interpolated_pose;
interpolated_pose.position.x = interpolated_position.x();
interpolated_pose.position.y = interpolated_position.y();
interpolated_pose.position.z = interpolated_position.z();
interpolated_pose.orientation.x = interpolated_orientation.x();
interpolated_pose.orientation.y = interpolated_orientation.y();
interpolated_pose.orientation.z = interpolated_orientation.z();
interpolated_pose.orientation.w = interpolated_orientation.w();
trajectory.push_back(interpolated_pose);
}
return trajectory;
}
void publish_pose(const geometry_msgs::msg::Pose &pose) {
geometry_msgs::msg::PoseStamped tp;
tp.pose = pose;
tp.header.stamp = this->now();
tp.header.frame_id = m_base_link;
m_publisher->publish(tp);
}
void on_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
m_current_pose = msg;
}
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::CartesianMoveToPose);

224
rbs_skill_servers/src/mtp_jtc.cpp Normal file
View file

@ -0,0 +1,224 @@
#include "Eigen/Dense"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "kdl/chainiksolverpos_lma.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <cstddef>
#include <iterator>
#include <kdl/chain.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToPose : public SkillBase<MoveitSendPose> {
public:
explicit MoveToPose(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_jtc", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToPose::jointStateCallback, this,
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
}
std::vector<double> target_joint_values;
solveIK(target_joint_values);
trajectory_goal.trajectory.points =
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
RCLCPP_WARN(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_mame_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
if (index_it != m_joint_mame_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
t * (target_joint_values[j] - start_joint_values[j]);
point.positions.push_back(position);
}
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
void solveIK(std::vector<double> &out) {
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
}
KDL::JntArray q_out(m_joint_names.size());
Eigen::Affine3d target_pose;
Eigen::fromMsg(m_current_goal->target_pose, target_pose);
KDL::Frame target_pose_kdl(
KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
KDL::Vector(target_pose.translation().x(),
target_pose.translation().y(),
target_pose.translation().z()));
const std::string m_base_link = "base_link";
const std::string m_ee_link = "gripper_grasp_point";
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to parse KDL tree from robot description.");
return;
}
KDL::Chain kdl_chain;
if (!kdl_tree.getChain(m_base_link, m_ee_link, kdl_chain)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to get KDL chain from base to end-effector.");
return;
}
auto ik_solver =
std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_chain, 1e-5, 500);
if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
out.resize(q_out.rows());
for (size_t i = 0; i < out.size(); i++) {
out[i] = q_out(i);
}
} else {
RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
}
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPose);

314
rbs_skill_servers/src/mtp_jtc_cart.cpp Normal file
View file

@ -0,0 +1,314 @@
#include "Eigen/Dense"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "kdl/chainfksolverpos_recursive.hpp"
#include "kdl/chainiksolverpos_lma.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <Eigen/src/Geometry/Transform.h>
#include <cstddef>
#include <iterator>
#include <kdl/chain.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToPoseJtcCartesian : public SkillBase<MoveitSendPose> {
public:
explicit MoveToPoseJtcCartesian(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_jtc_cart", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToPoseJtcCartesian::jointStateCallback, this,
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to obtain KDL tree from the robot description.");
throw std::runtime_error("KDL Tree initialization failed");
}
if (!kdl_tree.getChain("base_link", "gripper_grasp_point", m_kdl_chain)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to obtain KDL chain from base to end-effector.");
throw std::runtime_error("KDL Chain initialization failed");
}
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
std::vector<double> target_joint_values;
Eigen::Affine3d target_pose;
Eigen::fromMsg(m_current_goal->target_pose, target_pose);
if (!solveIK(target_pose, target_joint_values)) {
RCLCPP_ERROR(this->get_logger(),
"IK solution not found for goal position");
m_current_goal_handle->abort(m_current_result);
}
trajectory_goal.trajectory.points =
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
if (m_joint_name_to_index.empty()) {
m_joint_name_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_name_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_name_to_index.find(m_joint_names[j]);
if (index_it != m_joint_name_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
KDL::Frame start_pose_kdl, target_pose_kdl;
if (!getEndEffectorPose(start_joint_values, start_pose_kdl) ||
!getEndEffectorPose(target_joint_values, target_pose_kdl)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to get initial or target end-effector pose");
return points;
}
Eigen::Affine3d start_pose = KDLFrameToEigen(start_pose_kdl);
Eigen::Affine3d target_pose = KDLFrameToEigen(target_pose_kdl);
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
Eigen::Vector3d start_translation = start_pose.translation();
Eigen::Vector3d target_translation = target_pose.translation();
Eigen::Vector3d interpolated_translation =
(1.0 - t) * start_translation + t * target_translation;
Eigen::Translation3d interpolated_translation_transform(
interpolated_translation);
Eigen::Quaterniond start_quaternion(start_pose.rotation());
Eigen::Quaterniond target_quaternion(target_pose.rotation());
Eigen::Quaterniond interpolated_quaternion =
start_quaternion.slerp(t, target_quaternion);
Eigen::Affine3d interpolated_pose =
interpolated_translation_transform * interpolated_quaternion;
std::vector<double> ik_joint_values;
if (!solveIK(interpolated_pose, ik_joint_values)) {
RCLCPP_ERROR(this->get_logger(), "IK solution not found for point %d",
i);
return {};
}
point.positions = ik_joint_values;
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
bool getEndEffectorPose(const std::vector<double> &joint_positions,
KDL::Frame &end_effector_pose) {
if (joint_positions.size() != m_joint_names.size()) {
RCLCPP_ERROR(this->get_logger(), "Mismatch between provided joint "
"positions and expected joint names.");
RCLCPP_ERROR(this->get_logger(), "Joint pos size: %zu",
joint_positions.size());
RCLCPP_ERROR(this->get_logger(), "Joint names size: %zu",
m_joint_names.size());
return false;
}
std::unordered_set<std::string> available_joints(m_joint_names.begin(),
m_joint_names.end());
if (available_joints.size() != m_joint_names.size()) {
RCLCPP_ERROR(this->get_logger(),
"Duplicate joints detected in joint names list.");
return false;
}
KDL::JntArray q_in(joint_positions.size());
for (size_t i = 0; i < joint_positions.size(); ++i) {
q_in(i) = joint_positions[i];
}
KDL::ChainFkSolverPos_recursive fk_solver(m_kdl_chain);
if (fk_solver.JntToCart(q_in, end_effector_pose) < 0) {
RCLCPP_ERROR(this->get_logger(),
"Failed to compute FK for the specified joint positions.");
return false;
}
return true;
}
Eigen::Affine3d KDLFrameToEigen(const KDL::Frame &frame) {
Eigen::Affine3d transform;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
transform(i, j) = frame.M(i, j);
}
transform(i, 3) = frame.p[i];
}
transform(3, 3) = 1.0;
return transform;
}
bool solveIK(const Eigen::Affine3d &target_pose, std::vector<double> &out) {
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
}
KDL::JntArray q_out(m_joint_names.size());
KDL::Frame target_pose_kdl(
KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
KDL::Vector(target_pose.translation().x(),
target_pose.translation().y(),
target_pose.translation().z()));
auto ik_solver =
std::make_unique<KDL::ChainIkSolverPos_LMA>(m_kdl_chain, 1e-5, 500);
if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
out.resize(q_out.rows());
for (size_t i = 0; i < out.size(); i++) {
out[i] = q_out(i);
}
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
return false;
}
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_name_to_index;
std::vector<std::string> m_joint_names;
KDL::Chain m_kdl_chain;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPoseJtcCartesian);

68
rbs_skill_servers/src/mtp_moveit.cpp Normal file
View file

@ -0,0 +1,68 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
class MoveitMtp : public SkillBase<MoveitSendPose> {
public:
explicit MoveitMtp(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase("mtp_moveit", options) {}
private:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
moveit::planning_interface::MoveGroupInterface move_group_interface(
this->shared_from_this(), m_current_goal->robot_name);
move_group_interface.startStateMonitor();
move_group_interface.setPoseTarget(m_current_goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(
m_current_goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(
m_current_goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::core::MoveItErrorCode plan_err_code =
move_group_interface.plan(plan);
if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
move_group_interface.plan(plan);
}
if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "Planning success");
// move_group_interface.execute(plan);
moveit::core::MoveItErrorCode move_err_code =
move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
m_current_result->success = true;
m_current_goal_handle->succeed(m_current_result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
} else {
RCLCPP_WARN_STREAM(this->get_logger(),
"Failed to generate plan because "
<< error_code_to_string(plan_err_code));
m_current_goal_handle->abort(m_current_result);
}
if (m_current_goal_handle->is_canceling()) {
m_current_goal_handle->canceled(m_current_result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
}
}; // class MoveToPoseActionServer
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtp);

95
rbs_skill_servers/src/mtp_moveit_cart.cpp Normal file
View file

@ -0,0 +1,95 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <rclcpp/logging.hpp>
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
class MoveitMtpCart : public SkillBase<MoveitSendPose> {
public:
explicit MoveitMtpCart(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase("mtp_moveit_cart", options) {}
private:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
moveit::planning_interface::MoveGroupInterface move_group_interface(
this->shared_from_this(), m_current_goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(
*move_group_interface.getCurrentState());
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
geometry_msgs::msg::Pose target_pose = m_current_goal->target_pose;
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
target_pose.position.x, target_pose.position.y,
target_pose.position.z);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(
waypoints, eef_step, jump_threshold, trajectory);
robot_trajectory::RobotTrajectory rt(
move_group_interface.getCurrentState()->getRobotModel(),
m_current_goal->robot_name);
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
bool su = tp.computeTimeStamps(rt);
if (!su) {
RCLCPP_ERROR(this->get_logger(), "Failed to compute timestamp of trajectory");
m_current_goal_handle->abort(m_current_result);
}
rt.getRobotTrajectoryMsg(trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory;
if (fraction > 0) {
RCLCPP_INFO(this->get_logger(), "Planning success");
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
m_current_result->success = true;
m_current_goal_handle->succeed(m_current_result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
m_current_goal_handle->abort(m_current_result);
}
if (m_current_goal_handle->is_canceling()) {
m_current_goal_handle->canceled(m_current_result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
}
}; // class MoveToPoseActionServer
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtpCart);

1
rbs_skill_servers/src/moveit_servo_skill_server.cpp → rbs_skill_servers/src/mtp_servo.cpp
View file

@ -1,3 +1,4 @@
// FIXME: This code currently not used
#include <functional>
#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>