From a7b7225dd1c5d9c82a6ddf98cff6bf4bc7d5bdc2 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 30 Oct 2024 17:49:03 +0300 Subject: [PATCH 01/10] refactor(rbs_skill_servers): update action configuration, dependencies, and skills - 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. --- .../config/default-scene-config.yaml | 222 ++++----- .../rbs_runtime/launch/runtime.launch.py | 85 +--- rbs_bringup/launch/rbs_robot.launch.py | 314 ++++-------- rbs_bt_executor/src/MoveToPose.cpp | 1 + rbs_bt_executor/src/MoveToPoseArray.cpp | 1 + .../action/MoveitSendJointStates.action | 2 +- .../action/MoveitSendPose.action | 2 +- rbs_skill_servers/CMakeLists.txt | 165 +++---- .../config/gripperPositions.yaml | 73 --- rbs_skill_servers/config/skills.yaml | 0 .../include/rbs_skill_servers/base_skill.hpp | 456 ++++++++++++++++++ rbs_skill_servers/launch/skills.launch.py | 145 +++--- rbs_skill_servers/package.xml | 1 + ..._action_server.cpp => gripper_command.cpp} | 0 .../src/move_cartesian_path_action_server.cpp | 221 --------- .../move_to_joint_states_action_server.cpp | 164 ------- .../src/move_topose_action_server.cpp | 176 ------- rbs_skill_servers/src/mtjs_jtc.cpp | 155 ++++++ rbs_skill_servers/src/mtp_cart.cpp | 183 +++++++ rbs_skill_servers/src/mtp_jtc.cpp | 224 +++++++++ rbs_skill_servers/src/mtp_jtc_cart.cpp | 314 ++++++++++++ rbs_skill_servers/src/mtp_moveit.cpp | 68 +++ rbs_skill_servers/src/mtp_moveit_cart.cpp | 95 ++++ ...t_servo_skill_server.cpp => mtp_servo.cpp} | 1 + 24 files changed, 1864 insertions(+), 1204 deletions(-) delete mode 100644 rbs_skill_servers/config/gripperPositions.yaml create mode 100644 rbs_skill_servers/config/skills.yaml create mode 100644 rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp rename rbs_skill_servers/src/{gripper_control_action_server.cpp => gripper_command.cpp} (100%) delete mode 100644 rbs_skill_servers/src/move_cartesian_path_action_server.cpp delete mode 100644 rbs_skill_servers/src/move_to_joint_states_action_server.cpp delete mode 100644 rbs_skill_servers/src/move_topose_action_server.cpp create mode 100644 rbs_skill_servers/src/mtjs_jtc.cpp create mode 100644 rbs_skill_servers/src/mtp_cart.cpp create mode 100644 rbs_skill_servers/src/mtp_jtc.cpp create mode 100644 rbs_skill_servers/src/mtp_jtc_cart.cpp create mode 100644 rbs_skill_servers/src/mtp_moveit.cpp create mode 100644 rbs_skill_servers/src/mtp_moveit_cart.cpp rename rbs_skill_servers/src/{moveit_servo_skill_server.cpp => mtp_servo.cpp} (99%) diff --git a/env_manager/rbs_runtime/config/default-scene-config.yaml b/env_manager/rbs_runtime/config/default-scene-config.yaml index e81ed5b..e0b7f57 100644 --- a/env_manager/rbs_runtime/config/default-scene-config.yaml +++ b/env_manager/rbs_runtime/config/default-scene-config.yaml @@ -1,100 +1,100 @@ camera: -- clip_color: !tuple - - 0.01 - - 1000.0 - clip_depth: !tuple - - 0.05 - - 10.0 - enable: true - height: 128 - horizontal_fov: 1.0471975511965976 - image_format: R8G8B8 - name: robot_camera - noise_mean: null - noise_stddev: null - publish_color: true - publish_depth: true - publish_points: true - random_pose_focal_point_z_offset: 0.0 - random_pose_mode: orbit - random_pose_orbit_distance: 1.0 - random_pose_orbit_height_range: !tuple - - 0.1 - - 0.7 - random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414 - random_pose_rollout_counter: 0.0 - random_pose_rollouts_num: 1 - random_pose_select_position_options: [] - relative_to: base_link - spawn_position: !tuple - - 0.0 - - 0.0 - - 1.0 - spawn_quat_xyzw: !tuple - - 0.0 - - 0.70710678118 - - 0.0 - - 0.70710678118 - type: rgbd_camera - update_rate: 10 - vertical_fov: 1.0471975511965976 - width: 128 + - clip_color: !tuple + - 0.01 + - 1000.0 + clip_depth: !tuple + - 0.05 + - 10.0 + enable: true + height: 128 + horizontal_fov: 1.0471975511965976 + image_format: R8G8B8 + name: robot_camera + noise_mean: null + noise_stddev: null + publish_color: true + publish_depth: true + publish_points: true + random_pose_focal_point_z_offset: 0.0 + random_pose_mode: orbit + random_pose_orbit_distance: 1.0 + random_pose_orbit_height_range: !tuple + - 0.1 + - 0.7 + random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414 + random_pose_rollout_counter: 0.0 + random_pose_rollouts_num: 1 + random_pose_select_position_options: [] + relative_to: base_link + spawn_position: !tuple + - 0.0 + - 0.0 + - 1.0 + spawn_quat_xyzw: !tuple + - 0.0 + - 0.70710678118 + - 0.0 + - 0.70710678118 + type: rgbd_camera + update_rate: 10 + vertical_fov: 1.0471975511965976 + width: 128 gravity: !tuple -- 0.0 -- 0.0 -- -9.80665 + - 0.0 + - 0.0 + - -9.80665 gravity_std: !tuple -- 0.0 -- 0.0 -- 0.0232 + - 0.0 + - 0.0 + - 0.0232 light: color: !tuple - - 1.0 - - 1.0 - - 1.0 - - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 direction: !tuple - - 0.5 - - -0.25 - - -0.75 + - 0.5 + - -0.25 + - -0.75 distance: 1000.0 model_rollouts_num: 1 radius: 25.0 random_minmax_elevation: !tuple - - -0.15 - - -0.65 + - -0.15 + - -0.65 type: sun visual: true objects: -- color: null - name: bishop - orientation: !tuple - - 1.0 - - 0.0 - - 0.0 - - 0.0 - position: !tuple - - 0.0 - - 1.0 - - 0.3 - randomize: - count: 0 - models_rollouts_num: 0 - random_color: false - random_model: false - random_orientation: false - random_pose: false - random_position: false - random_spawn_position_segments: [] - random_spawn_position_update_workspace_centre: false - random_spawn_volume: !tuple - - 0.5 - - 0.5 - - 0.5 - relative_to: world - static: false - texture: [] - type: 'model' + - color: null + name: bishop + orientation: !tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + position: !tuple + - 0.0 + - 1.0 + - 0.3 + randomize: + count: 0 + models_rollouts_num: 0 + random_color: false + random_model: false + random_orientation: false + random_pose: false + random_position: false + random_spawn_position_segments: [] + random_spawn_position_update_workspace_centre: false + random_spawn_volume: !tuple + - 0.5 + - 0.5 + - 0.5 + relative_to: world + static: false + texture: [] + type: "model" physics_rollouts_num: 0 plugins: fts_broadcaster: false @@ -104,13 +104,13 @@ plugins: robot: gripper_joint_positions: 0.0 joint_positions: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 + - 1.57 + - 0.5 + - 3.14159 + - 1.5 + - 0.0 + - 1.4 + - 0.0 name: rbs_arm randomizer: joint_positions: false @@ -120,33 +120,33 @@ robot: joint_positions_std: 0.1 pose: false spawn_volume: !tuple - - 1.0 - - 1.0 - - 0.0 + - 1.0 + - 1.0 + - 0.0 spawn_position: !tuple - - 0.0 - - 0.0 - - 0.0 + - 0.0 + - 0.0 + - 0.0 spawn_quat_xyzw: !tuple - - 0.0 - - 0.0 - - 0.0 - - 1.0 - urdf_string: '' + - 0.0 + - 0.0 + - 0.0 + - 1.0 + urdf_string: "" with_gripper: true terrain: model_rollouts_num: 1 name: ground size: !tuple - - 1.5 - - 1.5 + - 1.5 + - 1.5 spawn_position: !tuple - - 0 - - 0 - - 0 + - 0 + - 0 + - 0 spawn_quat_xyzw: !tuple - - 0 - - 0 - - 0 - - 1 + - 0 + - 0 + - 0 + - 1 type: flat diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index de9875c..f479d6c 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -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)] diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 1ffae1a..8735f46 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -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,45 +88,45 @@ def launch_setup(context, *args, **kwargs): robot_description = {"robot_description": robot_description_content} - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare(moveit_config_package), - "config/moveit", - "rbs_arm.srdf.xacro", - ] - ), - " ", - "name:=", - robot_type, - " ", - "with_gripper:=", - with_gripper_condition, - " ", - "gripper_name:=", - gripper_name, - " ", - ] - ) + if not robot_description_semantic.perform(context): + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(moveit_config_package), + "config/moveit", + "rbs_arm.srdf.xacro", + ] + ), + " ", + "name:=", + robot_type, + " ", + "with_gripper:=", + with_gripper_condition, + " ", + "gripper_name:=", + gripper_name, + " ", + ] + ) + 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.", ) ) diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp index 066c4d7..624862f 100644 --- a/rbs_bt_executor/src/MoveToPose.cpp +++ b/rbs_bt_executor/src/MoveToPose.cpp @@ -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; } diff --git a/rbs_bt_executor/src/MoveToPoseArray.cpp b/rbs_bt_executor/src/MoveToPoseArray.cpp index dd37a67..95f3a2f 100644 --- a/rbs_bt_executor/src/MoveToPoseArray.cpp +++ b/rbs_bt_executor/src/MoveToPoseArray.cpp @@ -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); diff --git a/rbs_skill_interfaces/action/MoveitSendJointStates.action b/rbs_skill_interfaces/action/MoveitSendJointStates.action index e0a74f8..bd1b6e7 100644 --- a/rbs_skill_interfaces/action/MoveitSendJointStates.action +++ b/rbs_skill_interfaces/action/MoveitSendJointStates.action @@ -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 diff --git a/rbs_skill_interfaces/action/MoveitSendPose.action b/rbs_skill_interfaces/action/MoveitSendPose.action index 1de1d7d..8c76779 100644 --- a/rbs_skill_interfaces/action/MoveitSendPose.action +++ b/rbs_skill_interfaces/action/MoveitSendPose.action @@ -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 diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 03c375b..d89010e 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -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 - PRIVATE $ - $) -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) - -# -- PickPlacePoseLoader -- -# add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp) -# target_include_directories( -# pick_place_pose_loader -# PRIVATE $ -# $) -# 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) - -# -- 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_executable(move_topose_action_server src/move_topose_action_server.cpp) -ament_target_dependencies(move_topose_action_server ${deps}) - -add_executable(move_cartesian_path_action_server - src/move_cartesian_path_action_server.cpp) -ament_target_dependencies(move_cartesian_path_action_server ${deps}) +) -# add_executable(servo_action_server -# src/moveit_servo_skill_server.cpp) -# ament_target_dependencies(servo_action_server ${deps}) -# +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 $ + $) + 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() + +# Add components +add_ros2_component(gripper_action_server + src/gripper_command.cpp + "rbs_skill_actions::GripperControlActionServer" + gripper_command) + +add_ros2_component(cartesian_move_to_pose + src/mtp_cart.cpp + "rbs_skill_actions::CartesianMoveToPose" + mtp_cart) + +add_ros2_component(move_to_joint_states + src/mtjs_jtc.cpp + "rbs_skill_actions::MoveToJointStateActionServer" + mtjs_jtc) + +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_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}) diff --git a/rbs_skill_servers/config/gripperPositions.yaml b/rbs_skill_servers/config/gripperPositions.yaml deleted file mode 100644 index e1aeb26..0000000 --- a/rbs_skill_servers/config/gripperPositions.yaml +++ /dev/null @@ -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 \ No newline at end of file diff --git a/rbs_skill_servers/config/skills.yaml b/rbs_skill_servers/config/skills.yaml new file mode 100644 index 0000000..e69de29 diff --git a/rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp b/rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp new file mode 100644 index 0000000..b6842ee --- /dev/null +++ b/rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp @@ -0,0 +1,456 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_skill_actions { + +template 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( + 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/load_controller"); + m_switch_controller_client = + create_client( + "/controller_manager/switch_controller"); + m_list_controllers_client = + create_client( + "/controller_manager/list_controllers"); + m_configure_controller_client = + create_client( + "/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 m_current_goal; + std::shared_ptr m_current_result; + std::shared_ptr> + m_current_goal_handle; + + bool m_interfaces_checked{false}; + + virtual rclcpp_action::GoalResponse + handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr 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(); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + virtual rclcpp_action::CancelResponse handle_canceled( + const std::shared_ptr> + 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> + goal_handle) { + auto execution_thread = [this, goal_handle]() { + this->execute(goal_handle); + }; + std::thread{execution_thread}.detach(); + } + + void execute(const std::shared_ptr> + 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 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 &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:: + 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:: + 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 + &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:: + 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 ¶meter = 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 ¶meter) { + 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 requiredStateControllers() { return {""}; } + + virtual std::vector 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 ¶m : requiredParameters()) { + RCLCPP_INFO(this->get_logger(), "\t- %s", param.c_str()); + } + + auto parameter_callback = + [this, controller_name]( + const std::shared_future> &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 ¶m : 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(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 &)> 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 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::SharedPtr + m_load_controller_client; + rclcpp::Client::SharedPtr + m_switch_controller_client; + rclcpp::Client::SharedPtr + m_list_controllers_client; + rclcpp::Client::SharedPtr + m_configure_controller_client; + rclcpp::AsyncParametersClient::SharedPtr m_parameter_client; + typename rclcpp_action::Server::SharedPtr m_action_server; + std::vector m_required_controllers{}; + std::vector m_missing_controllers{}; + std::unordered_map m_controllers_map; +}; + +} // namespace rbs_skill_actions diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index 32c6a18..f9e5692 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -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, - parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - {"use_sim_time": use_sim_time}, - ], - ) - - gripper_control_node = Node( - package="rbs_skill_servers", - executable="gripper_control_action_server", - namespace=namespace, - 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}, + 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}, + ], + condition=IfCondition(use_moveit), + ), + ComposableNode( + package="rbs_skill_servers", + 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(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)] ) diff --git a/rbs_skill_servers/package.xml b/rbs_skill_servers/package.xml index 1d7712b..f4d0036 100644 --- a/rbs_skill_servers/package.xml +++ b/rbs_skill_servers/package.xml @@ -23,6 +23,7 @@ rbs_skill_interfaces tf2_eigen rbs_utils + rclcpp_components ament_lint_auto ament_lint_common diff --git a/rbs_skill_servers/src/gripper_control_action_server.cpp b/rbs_skill_servers/src/gripper_command.cpp similarity index 100% rename from rbs_skill_servers/src/gripper_control_action_server.cpp rename to rbs_skill_servers/src/gripper_command.cpp diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp deleted file mode 100644 index 989c8f9..0000000 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include -#include -#include -#include -#include - -#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 -#include -#include -*/ -// For Visualization -// #include -#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( - // 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( - 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::SharedPtr action_server_; - - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - - rclcpp_action::GoalResponse - goal_callback(const rclcpp_action::GoalUUID &uuid, - std::shared_ptr 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 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 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 goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - 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 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(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; -} diff --git a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp deleted file mode 100644 index 4487eba..0000000 --- a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include -#include -#include -#include - -#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 -#include -#include -*/ -// For Visualization -// #include -#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( - // 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( - 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::SharedPtr action_server_; - - using ServerGoalHandle = - rclcpp_action::ServerGoalHandle; - - rclcpp_action::GoalResponse - goal_callback(const rclcpp_action::GoalUUID &uuid, - std::shared_ptr 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 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 goal_handle) { - using namespace std::placeholders; - std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), - goal_handle) - .detach(); - } - - void execute(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface( - node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - std::vector 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; -} diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp deleted file mode 100644 index 553bb53..0000000 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include - -#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 -#include -#include -*/ -// For Visualization -// #include -#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 - -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( - // 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( - 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::SharedPtr action_server_; - - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - - rclcpp_action::GoalResponse - goal_callback(const rclcpp_action::GoalUUID &uuid, - std::shared_ptr 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 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 goal_handle) { - using namespace std::placeholders; - std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), - goal_handle) - .detach(); - } - - void execute(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - 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; -} diff --git a/rbs_skill_servers/src/mtjs_jtc.cpp b/rbs_skill_servers/src/mtjs_jtc.cpp new file mode 100644 index 0000000..6fba882 --- /dev/null +++ b/rbs_skill_servers/src/mtjs_jtc.cpp @@ -0,0 +1,155 @@ +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "rbs_skill_servers/base_skill.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_skill_actions { + +using MoveitSendJointStates = + rbs_skill_interfaces::action::MoveitSendJointStates; +using GoalHandleMoveitSendJointStates = + rclcpp_action::ServerGoalHandle; +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; +using FollowJointTrajectoryGoalHandle = + rclcpp_action::ClientGoalHandle; + +class MoveToJointStateActionServer : public SkillBase { +public: + explicit MoveToJointStateActionServer( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : SkillBase("mtjs_jtc", options) { + + m_joint_trajectory_client = + rclcpp_action::create_client( + 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( + "/joint_states", 10, + std::bind(&MoveToJointStateActionServer::jointStateCallback, this, + std::placeholders::_1), + s_options); + } + +protected: + std::string requiredActionController() override { + return "joint_trajectory_controller"; + } + + std::vector 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::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 + generate_trajectory(const std::vector &start_joint_values, + const std::vector &target_joint_values, + const double duration) { + + const int num_points = 100; + std::vector points; + for (int i = 0; i <= num_points; ++i) { + trajectory_msgs::msg::JointTrajectoryPoint point; + double t = static_cast(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::SharedPtr + m_joint_trajectory_client; + rclcpp::Subscription::SharedPtr + m_joint_state_subscriber; + + std::vector m_current_joint_positions; + std::unordered_map m_joint_mame_to_index; + std::vector m_joint_names; +}; + +} // namespace rbs_skill_actions + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + rbs_skill_actions::MoveToJointStateActionServer); diff --git a/rbs_skill_servers/src/mtp_cart.cpp b/rbs_skill_servers/src/mtp_cart.cpp new file mode 100644 index 0000000..c5010eb --- /dev/null +++ b/rbs_skill_servers/src/mtp_cart.cpp @@ -0,0 +1,183 @@ +#include "Eigen/Dense" +#include +#include + +// 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; + +class CartesianMoveToPose : public SkillBase { +public: + explicit CartesianMoveToPose( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : SkillBase("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( + "cartesian_motion_controller/target_frame", 10, p_options); + + m_current_pose = std::make_shared(); + + rclcpp::SubscriptionOptions s_options; + s_options.callback_group = cbg; + + m_current_pose_subscriber = + this->create_subscription( + "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 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(m_trajectory.size()) * 1000.0; + + m_trajectory_timer = this->create_wall_timer( + std::chrono::milliseconds(static_cast(m_step_time)), + [this]() { adjust_and_publish_pose(); }); + } + +private: + rclcpp::Publisher::SharedPtr m_publisher; + rclcpp::Subscription::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 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 + interpolate(const geometry_msgs::msg::Pose &start_pose, + const geometry_msgs::msg::Pose &end_pose) { + std::vector 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(distance / 0.01); + + for (int i = 0; i <= num_steps; ++i) { + double t = static_cast(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); diff --git a/rbs_skill_servers/src/mtp_jtc.cpp b/rbs_skill_servers/src/mtp_jtc.cpp new file mode 100644 index 0000000..c88f637 --- /dev/null +++ b/rbs_skill_servers/src/mtp_jtc.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_skill_actions { + +using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; +using GoalHandleMoveitSendJointStates = + rclcpp_action::ServerGoalHandle; +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; +using FollowJointTrajectoryGoalHandle = + rclcpp_action::ClientGoalHandle; + +class MoveToPose : public SkillBase { +public: + explicit MoveToPose( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : SkillBase("mtp_jtc", options) { + + m_joint_trajectory_client = + rclcpp_action::create_client( + 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( + "/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 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 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::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 + generate_trajectory(const std::vector &start_joint_values, + const std::vector &target_joint_values, + const double duration) { + + const int num_points = 100; + std::vector points; + for (int i = 0; i <= num_points; ++i) { + trajectory_msgs::msg::JointTrajectoryPoint point; + double t = static_cast(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 &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_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::SharedPtr + m_joint_trajectory_client; + rclcpp::Subscription::SharedPtr + m_joint_state_subscriber; + + std::vector m_current_joint_positions; + std::unordered_map m_joint_mame_to_index; + std::vector m_joint_names; +}; + +} // namespace rbs_skill_actions + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPose); diff --git a/rbs_skill_servers/src/mtp_jtc_cart.cpp b/rbs_skill_servers/src/mtp_jtc_cart.cpp new file mode 100644 index 0000000..363180b --- /dev/null +++ b/rbs_skill_servers/src/mtp_jtc_cart.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_skill_actions { + +using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; +using GoalHandleMoveitSendJointStates = + rclcpp_action::ServerGoalHandle; +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; +using FollowJointTrajectoryGoalHandle = + rclcpp_action::ClientGoalHandle; + +class MoveToPoseJtcCartesian : public SkillBase { +public: + explicit MoveToPoseJtcCartesian( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : SkillBase("mtp_jtc_cart", options) { + + m_joint_trajectory_client = + rclcpp_action::create_client( + 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( + "/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 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 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::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 + generate_trajectory(const std::vector &start_joint_values, + const std::vector &target_joint_values, + const double duration) { + const int num_points = 100; + std::vector 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(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 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 &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 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 &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(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::SharedPtr + m_joint_trajectory_client; + rclcpp::Subscription::SharedPtr + m_joint_state_subscriber; + + std::vector m_current_joint_positions; + std::unordered_map m_joint_name_to_index; + std::vector 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); diff --git a/rbs_skill_servers/src/mtp_moveit.cpp b/rbs_skill_servers/src/mtp_moveit.cpp new file mode 100644 index 0000000..7b07c9e --- /dev/null +++ b/rbs_skill_servers/src/mtp_moveit.cpp @@ -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 { +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); diff --git a/rbs_skill_servers/src/mtp_moveit_cart.cpp b/rbs_skill_servers/src/mtp_moveit_cart.cpp new file mode 100644 index 0000000..535bdc3 --- /dev/null +++ b/rbs_skill_servers/src/mtp_moveit_cart.cpp @@ -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 +#include +#include + +namespace rbs_skill_actions { + +using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + +class MoveitMtpCart : public SkillBase { +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 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); diff --git a/rbs_skill_servers/src/moveit_servo_skill_server.cpp b/rbs_skill_servers/src/mtp_servo.cpp similarity index 99% rename from rbs_skill_servers/src/moveit_servo_skill_server.cpp rename to rbs_skill_servers/src/mtp_servo.cpp index 7815920..07a2011 100644 --- a/rbs_skill_servers/src/moveit_servo_skill_server.cpp +++ b/rbs_skill_servers/src/mtp_servo.cpp @@ -1,3 +1,4 @@ +// FIXME: This code currently not used #include #include #include From ea4ae0ed6924fa2b0adc4c339e2bbde4e9a7c3c1 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 12 Nov 2024 22:23:12 +0300 Subject: [PATCH 02/10] feat(rbs): refactor controller paths and launch configuration - Rename default controllers file `rbs_arm0_controllers.yaml` to `controllers.yaml` across all launch files - Enhance `runtime.launch.py` by adding imports (`URDF_parser`, `ControllerManager`, `yaml`) and new arguments (`scene_config_file`, `ee_link_name`, `base_link_name`) - Implement xacro argument loading via YAML - Configure `robot_builder` to save controllers to `controllers.yaml` based on URDF parsing - Add `control.launch.py` in `rbs_bringup` for dynamic controller spawning based on strategies/configurations - Add `rbs_bringup.launch.py` as a unified entry point for single robot setup, initializing core settings like gripper, robot type, and controllers - Remove `launch_env.launch.py` - Remove `multi_robot.launch.py` - Enhance `rbs_robot.launch.py` with `ee_link_name` and `base_link_name` arguments for end-effector and base link customization - Add `ee_link_name` and `base_link_name` to `skills.launch.py` for flexible robot setup in skills - Update `mtp_jtc_cart.cpp` with `base_link` and `robot_ee_link` parameters for KDL chain retrieval - Remove `single_robot.launch.py` for simplified single robot deployment --- env_manager/rbs_gym/launch/evaluate.launch.py | 4 +- env_manager/rbs_gym/launch/optimize.launch.py | 4 +- env_manager/rbs_gym/launch/test_env.launch.py | 4 +- env_manager/rbs_gym/launch/train.launch.py | 4 +- .../rbs_runtime/launch/runtime.launch.py | 110 +++++-- rbs_bringup/launch/control.launch.py | 87 +++++ rbs_bringup/launch/launch_env.launch.py | 254 --------------- rbs_bringup/launch/multi_robot.launch.py | 292 ----------------- rbs_bringup/launch/rbs_bringup.launch.py | 52 +++ rbs_bringup/launch/rbs_robot.launch.py | 25 +- rbs_bringup/launch/single_robot.launch.py | 301 ------------------ rbs_skill_servers/launch/skills.launch.py | 10 + rbs_skill_servers/src/mtp_jtc_cart.cpp | 12 +- 13 files changed, 279 insertions(+), 880 deletions(-) create mode 100644 rbs_bringup/launch/control.launch.py delete mode 100644 rbs_bringup/launch/launch_env.launch.py delete mode 100644 rbs_bringup/launch/multi_robot.launch.py create mode 100644 rbs_bringup/launch/rbs_bringup.launch.py delete mode 100644 rbs_bringup/launch/single_robot.launch.py diff --git a/env_manager/rbs_gym/launch/evaluate.launch.py b/env_manager/rbs_gym/launch/evaluate.launch.py index df4c4a5..4e69f23 100644 --- a/env_manager/rbs_gym/launch/evaluate.launch.py +++ b/env_manager/rbs_gym/launch/evaluate.launch.py @@ -65,7 +65,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -176,7 +176,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/optimize.launch.py b/env_manager/rbs_gym/launch/optimize.launch.py index 40008d2..7e06a9c 100644 --- a/env_manager/rbs_gym/launch/optimize.launch.py +++ b/env_manager/rbs_gym/launch/optimize.launch.py @@ -70,7 +70,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -195,7 +195,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/test_env.launch.py b/env_manager/rbs_gym/launch/test_env.launch.py index b9ac3a7..c2667d6 100644 --- a/env_manager/rbs_gym/launch/test_env.launch.py +++ b/env_manager/rbs_gym/launch/test_env.launch.py @@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" + get_package_share_directory("rbs_arm"), "config", "controllers.yaml" ) xacro_file = os.path.join( @@ -158,7 +158,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_gym/launch/train.launch.py b/env_manager/rbs_gym/launch/train.launch.py index 60db4db..cda85d6 100644 --- a/env_manager/rbs_gym/launch/train.launch.py +++ b/env_manager/rbs_gym/launch/train.launch.py @@ -69,7 +69,7 @@ def launch_setup(context, *args, **kwargs): launch_simulation = LaunchConfiguration("launch_sim") initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' + get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml' ) single_robot_setup = IncludeLaunchDescription( @@ -196,7 +196,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm_controllers_gazebosim.yaml", + default_value="controllers_gazebosim.yaml", description="YAML file with the controllers configuration.", ) ) diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index f479d6c..f142865 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -14,6 +14,10 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from robot_builder.parser.urdf import URDF_parser +from robot_builder.external.ros2_control import ControllerManager +import yaml + def launch_setup(context, *args, **kwargs): # Initialize Arguments @@ -29,29 +33,70 @@ def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration("use_sim_time") hardware = LaunchConfiguration("hardware") gripper_name = LaunchConfiguration("gripper_name") + scene_config_file = LaunchConfiguration("scene_config_file").perform(context) - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) + + if not scene_config_file == "": + config_file = {"config_file": scene_config_file} + else: + config_file = {} + + description_package_abs_path = get_package_share_directory(description_package.perform(context)) + + simulation_controllers = os.path.join( + description_package_abs_path, "config", "controllers.yaml" ) xacro_file = os.path.join( - get_package_share_directory(description_package.perform(context)), + description_package_abs_path, "urdf", description_file.perform(context), ) - robot_description_doc = xacro.process_file( - xacro_file, - mappings={ - "gripper_name": gripper_name.perform(context), - "hardware": hardware.perform(context), - "simulation_controllers": initial_joint_controllers_file_path, - "namespace": "", - }, - ) + xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml" + + # TODO: hide this to another place + # Load xacro_args + def param_constructor(loader, node, local_vars): + value = loader.construct_scalar(node) + return LaunchConfiguration(value).perform( + local_vars.get("context", "Launch context if not defined") + ) + + def variable_constructor(loader, node, local_vars): + value = loader.construct_scalar(node) + return local_vars.get(value, f"Variable '{value}' not found") + + def load_xacro_args(yaml_file, local_vars): + # Get valut from ros2 argument + yaml.add_constructor( + "!param", lambda loader, node: param_constructor(loader, node, local_vars) + ) + + # Get value from local variable in this code + # The local variable should be initialized before the loader was called + yaml.add_constructor( + "!variable", + lambda loader, node: variable_constructor(loader, node, local_vars), + ) + + with open(yaml_file, "r") as file: + return yaml.load(file, Loader=yaml.FullLoader) + + mappings_data = load_xacro_args(xacro_config_file, locals()) + + robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data) robot_description_content = robot_description_doc.toprettyxml(indent=" ") robot_description = {"robot_description": robot_description_content} + + # Parse robot and configure controller's file for ControllerManager + robot = URDF_parser.load_string(robot_description_content, ee_link_name="gripper_grasp_point") + ControllerManager.save_to_yaml( + robot, description_package_abs_path, "controllers.yaml" + ) rbs_robot_setup = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -64,7 +109,7 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "with_gripper": with_gripper_condition, "gripper_name": gripper_name, - "controllers_file": initial_joint_controllers_file_path, + "controllers_file": simulation_controllers, "robot_type": robot_type, "description_package": description_package, "description_file": description_file, @@ -76,13 +121,15 @@ def launch_setup(context, *args, **kwargs): "hardware": hardware, "use_controllers": "true", "robot_description": robot_description_content, + "base_link_name": base_link_name, + "ee_link_name": ee_link_name }.items(), ) rbs_runtime = Node( package="rbs_runtime", executable="runtime", - parameters=[robot_description, {"use_sim_time": True}], + parameters=[robot_description, config_file, {"use_sim_time": True}], ) clock_bridge = Node( @@ -109,13 +156,13 @@ def generate_launch_description(): ) ) # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rbs_arm0_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) + # declared_arguments.append( + # DeclareLaunchArgument( + # "controllers_file", + # default_value="controllers.yaml", + # description="YAML file with the controllers configuration.", + # ) + # ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -199,6 +246,27 @@ def generate_launch_description(): description="Launch controllers?", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "scene_config_file", + default_value="", + description="Path to a scene configuration file", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ee_link_name", + default_value="", + description="End effector name of robot arm", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "base_link_name", + default_value="", + description="Base link name if robot arm", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/control.launch.py b/rbs_bringup/launch/control.launch.py new file mode 100644 index 0000000..91ab0b9 --- /dev/null +++ b/rbs_bringup/launch/control.launch.py @@ -0,0 +1,87 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction + +# from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + + +def create_spawner(controller_name, namespace, condition=None, inactive=False): + args = [controller_name, "-c", f"{namespace}/controller_manager"] + if inactive: + args.append("--inactive") + return Node( + package="controller_manager", + executable="spawner", + namespace=namespace, + arguments=args, + condition=condition, + ) + + +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + control_strategy = LaunchConfiguration("control_strategy").perform(context) + control_space = LaunchConfiguration("control_space").perform(context) + has_gripper = LaunchConfiguration("has_gripper").perform(context) + interactive_control = LaunchConfiguration("interactive_control").perform(context) + + controllers_config = { + "joint_state_broadcaster": True, + "gripper_controller": has_gripper == "true", + "joint_trajectory_controller": control_strategy == "position" + and control_space == "joint", + "cartesian_motion_controller": control_strategy == "position" + and control_space == "task", + "cartesian_force_controller": control_strategy == "effort" + and control_space == "task", + "joint_effort_controller": control_strategy == "effort" + and control_space == "joint", + "force_torque_sensor_broadcaster": control_strategy == "effort" + and control_space == "task", + "motion_control_handle": interactive_control == "true" + and control_space == "task" + and control_strategy == "position", + } + + nodes_to_start = [] + for controller_name, should_start in controllers_config.items(): + if should_start: + nodes_to_start.append(create_spawner(controller_name, namespace)) + + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "namespace", default_value="", description="A robot's namespace" + ), + DeclareLaunchArgument( + "control_space", + default_value="task", + choices=["task", "joint"], + description="Control type: 'task' for Cartesian, 'joint' for joint space control.", + ), + DeclareLaunchArgument( + "control_strategy", + default_value="position", + choices=["position", "velocity", "effort"], + description="Control strategy: 'position', 'velocity', or 'effort'.", + ), + DeclareLaunchArgument( + "has_gripper", + default_value="true", + description="Whether to activate the gripper_controller.", + ), + + DeclareLaunchArgument( + "interactive_control", + default_value="true", + description="Whether to activate the gripper_controller.", + ), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/rbs_bringup/launch/launch_env.launch.py b/rbs_bringup/launch/launch_env.launch.py deleted file mode 100644 index 20bcf42..0000000 --- a/rbs_bringup/launch/launch_env.launch.py +++ /dev/null @@ -1,254 +0,0 @@ -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import Node -import os -from ament_index_python.packages import get_package_share_directory -from nav2_common.launch import RewrittenYaml - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - 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_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - - - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - - configured_params = RewrittenYaml( - source_file=os.path.join( - get_package_share_directory( - description_package.perform(context)), - "config", - controllers_file.perform(context)), - root_key=robot_name.perform(context), - param_rewrites={}, - convert_types=True, - ) - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' - ) - controller_paramfile = configured_params.perform(context) - namespace = "/" + robot_name.perform(context) - - # spawner = Node( - # package="gz_enviroment_python", - # executable="spawner.py", - # namespace=namespace - # ) - - single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), - launch_arguments={ - "env_manager": env_manager, - "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": controllers_file, - "robot_type": robot_type, - "controllers_file": initial_joint_controllers_file_path, - "cartesian_controllers": cartesian_controllers, - "description_package": description_package, - "description_file": description_file, - "robot_name": robot_name, - "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, - "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, - }.items() - ) - - nodes_to_start = [ - # spawner, - single_robot_setup - ] - return nodes_to_start - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot 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_arm_controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - 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", - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "moveit_config_file", - default_value="rbs_arm.srdf.xacro", - description="MoveIt SRDF/XACRO description file with the robot.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") - ) - declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") - ) - declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - 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?") - ) - 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="true", - description="Load cartesian\ - controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_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)]) diff --git a/rbs_bringup/launch/multi_robot.launch.py b/rbs_bringup/launch/multi_robot.launch.py deleted file mode 100644 index e3b19e4..0000000 --- a/rbs_bringup/launch/multi_robot.launch.py +++ /dev/null @@ -1,292 +0,0 @@ -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml -import os -from ament_index_python import get_package_share_directory -from rbs_launch_utils.merged_yaml import MergedYaml -from rbs_launch_utils.launch_common import load_yaml -import yaml - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - 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_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - robots_config_file = LaunchConfiguration("robots_config_file") - gazebo_world_filename = LaunchConfiguration("gazebo_world_filename") - - ld = [] - ld.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_simulation'), - 'launch', - 'simulation_gazebo.launch.py' - ]) - ]), - launch_arguments={ - 'sim_gazebo': sim_gazebo, - 'debugger': "false", - 'launch_env_manager': "false", - "gazebo_world_filename": gazebo_world_filename - }.items(), - condition=IfCondition(launch_simulation) - )) - scene_file = robots_config_file.perform(context) - robots = load_yaml("rbs_bringup", "config/" + scene_file) - - description_package = description_package.perform(context) - controllers_file = controllers_file.perform(context) - config = MergedYaml(context, - os.path.join(get_package_share_directory(description_package), "config", controllers_file), - root_keys=[i["name"] for i in robots["scene_config"]], - param_rewrites={}, - convert_types=False, - ).merge_yamls() - for robot in robots["scene_config"]: - namespace: str = "/" + robot["name"] - ld.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('rbs_bringup'), - "launch", - "rbs_robot.launch.py" - ]) - ]), - launch_arguments={ - "env_manager": env_manager, - "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, - "controllers_file": controllers_file, - "robot_type": robot["type"], - "controllers_file": config, - "cartesian_controllers": cartesian_controllers, - "description_package": description_package, - "description_file": description_file, - "robot_name": robot["name"], - "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, - "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": launch_controllers, - "gazebo_gui": gazebo_gui, - "namespace": namespace, - "x": str(robot["pose"]["position"]["x"]), - "y": str(robot["pose"]["position"]["y"]), - "z": str(robot["pose"]["position"]["z"]), - "roll": str(robot["pose"]["orientation"]["x"]), - "pitch": str(robot["pose"]["orientation"]["y"]), - "yaw": str(robot["pose"]["orientation"]["z"]), - }.items() - )) - - gz_spawner = Node( - package='ros_gz_sim', - executable='create', - arguments=[ - '-name', robot_name, - # '-x', str(robot["pose"]["position"]["x"]), - # '-y', str(robot["pose"]["position"]["y"]), - # '-z', str(robot["pose"]["position"]["z"]), - # '-R', str(robot["pose"]["orientation"]["x"]), - # '-P', str(robot["pose"]["orientation"]["y"]), - # '-Y', str(robot["pose"]["orientation"]["z"]), - '-topic', namespace + '/robot_description'], - output='screen', - condition=IfCondition(sim_gazebo)) - ld.append(gz_spawner) - - return ld - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot 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_arm_controllers_gazebosim.yaml", - description="YAML file with the controllers configuration.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - 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", - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "moveit_config_file", - default_value="rbs_arm.srdf.xacro", - description="MoveIt SRDF/XACRO description file with the robot.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument("with_gripper", - default_value="true", - description="With gripper or not?") - ) - declared_arguments.append( - DeclareLaunchArgument("sim_gazebo", - default_value="true", - description="Gazebo Simulation") - ) - declared_arguments.append( - DeclareLaunchArgument("env_manager", - default_value="false", - 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?") - ) - 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="true", - description="Load cartesian\ - controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface") - ) - declared_arguments.append( - DeclareLaunchArgument("launch_controllers", - default_value="true", - description="Launch controllers?") - ) - declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", - default_value="true", - description="Launch gazebo with gui?") - ) - declared_arguments.append( - DeclareLaunchArgument("gazebo_world_filename", - default_value="asm2.sdf", - description="Filename of Gazebo world file to launch") - ) - declared_arguments.append( - DeclareLaunchArgument("robots_config_file", - default_value="roboclone.yaml", - description="Filename for config file with robots in scene") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py new file mode 100644 index 0000000..63253b4 --- /dev/null +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + main_script = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"] + ) + ] + ), + launch_arguments={ + "with_gripper": "true", + "gripper_name": "rbs_gripper", + "robot_type": "rbs_arm", + "description_package": "rbs_arm", + "description_file": "rbs_arm_modular.xacro", + "robot_name": "rbs_arm", + "use_moveit": "false", + "moveit_config_package": "rbs_arm", + "moveit_config_file": "rbs_arm.srdf.xacro", + "use_sim_time": "true", + "hardware": "gazebo", + "use_controllers": "true", + "scene_config_file": "", + "base_link_name": "base_link", + "ee_link_name": "gripper_grasp_point", + }.items(), + ) + + nodes_to_start = [ + main_script, + ] + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 8735f46..2d42d35 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -53,6 +53,8 @@ def launch_setup(context, *args, **kwargs): robot_description_semantic = LaunchConfiguration("robot_description_semantic") control_space = LaunchConfiguration("control_space") control_strategy = LaunchConfiguration("control_strategy") + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) remappings = [] if multi_robot == "true": @@ -141,7 +143,7 @@ def launch_setup(context, *args, **kwargs): [ PathJoinSubstitution( [ - FindPackageShare(description_package), + FindPackageShare("rbs_bringup"), "launch", "control.launch.py", ] @@ -202,6 +204,8 @@ def launch_setup(context, *args, **kwargs): "with_gripper_condition": with_gripper_condition, "namespace": namespace, "use_moveit": use_moveit, + "ee_link_name": ee_link_name, + "base_link_name": base_link_name, }.items(), ) @@ -221,14 +225,14 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot to launch, specified by name.", - choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + choices=["rbs_arm", "ar4", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], default_value="rbs_arm", ) ) declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="rbs_arm0_controllers.yaml", + default_value="controllers.yaml", description="YAML file containing configuration settings for the controllers.", ) ) @@ -397,6 +401,21 @@ def generate_launch_description(): description="Custom semantic robot description (SRDF) to override the default.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "ee_link_name", + default_value="", + description="End effector name of robot arm", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "base_link_name", + default_value="", + description="Base link name if robot arm", + ) + ) + return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/single_robot.launch.py b/rbs_bringup/launch/single_robot.launch.py deleted file mode 100644 index 5397885..0000000 --- a/rbs_bringup/launch/single_robot.launch.py +++ /dev/null @@ -1,301 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def launch_setup(context, *args, **kwargs): - # Initialize Arguments - robot_type = LaunchConfiguration("robot_type") - # General arguments - 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_simulation = LaunchConfiguration("launch_sim") - launch_moveit = LaunchConfiguration("launch_moveit") - launch_task_planner = LaunchConfiguration("launch_task_planner") - launch_perception = LaunchConfiguration("launch_perception") - 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") - launch_controllers = LaunchConfiguration("launch_controllers") - gazebo_gui = LaunchConfiguration("gazebo_gui") - gripper_name = LaunchConfiguration("gripper_name") - - sim_gazebo = LaunchConfiguration("sim_gazebo") - launch_simulation = LaunchConfiguration("launch_sim") - simulation = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("rbs_simulation"), - "launch", - "simulation_gazebo.launch.py", - ] - ) - ] - ), - launch_arguments={ - "sim_gazebo": sim_gazebo, - "debugger": "false", - "launch_env_manager": env_manager, - "gazebo_world_filename": "asm2.sdf", - }.items(), - condition=IfCondition(launch_simulation), - ) - - # FIXME: namespaces - # configured_params = RewrittenYaml( - # source_file=os.path.join( - # get_package_share_directory( - # description_package.perform(context)), - # "config", - # controllers_file.perform(context)), - # root_key=robot_name.perform(context), - # param_rewrites={}, - # convert_types=True, - # ) - - initial_joint_controllers_file_path = os.path.join( - get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml" - ) - - # controller_paramfile = configured_params.perform(context) - # controller_paramfile = PathJoinSubstitution([ - # FindPackageShare(robot_type), "config", "rbs_arm0_controllers.yaml" - # ]) - # namespace = "/" + robot_name.perform(context) - namespace = "" - - gz_spawner = Node( - package="ros_gz_sim", - executable="create", - # prefix=['gdbserver localhost:1234'], - arguments=[ - "-name", - robot_name.perform(context), - "-topic", - namespace + "/robot_description", - ], - output="screen", - condition=IfCondition(sim_gazebo), - ) - - single_robot_setup = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"] - ) - ] - ), - 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, - "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, - # "x": "0.5", - # "y": "0.5", - # "z": "0.5" - }.items(), - ) - - nodes_to_start = [simulation, gz_spawner, single_robot_setup] - return nodes_to_start - - -def generate_launch_description(): - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "robot_type", - description="Type of robot 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.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rbs_arm_modular.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_name", - default_value="arm0", - 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", - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "moveit_config_file", - default_value="rbs_arm.srdf.xacro", - description="MoveIt SRDF/XACRO description file with the robot.", - ) - ) - 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.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "with_gripper", default_value="true", description="With gripper or not?" - ) - ) - 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?" - ) - ) - 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="true", - description="Load cartesian\ - controllers?", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "launch_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)] - ) diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index f9e5692..8e5a34c 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -13,6 +13,8 @@ def launch_setup(context, *args, **kwargs): robot_description_kinematics = LaunchConfiguration("robot_description_kinematics") use_sim_time = LaunchConfiguration("use_sim_time") use_moveit = LaunchConfiguration("use_moveit") + ee_link_name = LaunchConfiguration("ee_link_name").perform(context) + base_link_name = LaunchConfiguration("base_link_name").perform(context) # with_gripper_condition = LaunchConfiguration("with_gripper_condition") robot_description = {"robot_description": robot_description_decl} @@ -64,6 +66,8 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"use_sim_time": use_sim_time}, {"robot_name": namespace}, + {"base_link": base_link_name}, + {"robot_ee_link": ee_link_name}, robot_description, ], ), @@ -136,6 +140,12 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument("namespace", default_value="") ) + declared_arguments.append( + DeclareLaunchArgument("ee_link_name", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("base_link_name", default_value="") + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] ) diff --git a/rbs_skill_servers/src/mtp_jtc_cart.cpp b/rbs_skill_servers/src/mtp_jtc_cart.cpp index 363180b..4fdcc98 100644 --- a/rbs_skill_servers/src/mtp_jtc_cart.cpp +++ b/rbs_skill_servers/src/mtp_jtc_cart.cpp @@ -59,6 +59,8 @@ public: std::placeholders::_1), s_options); this->declare_parameter("robot_description", ""); + this->declare_parameter("base_link", ""); + this->declare_parameter("robot_ee_link", ""); auto robot_description = this->get_parameter("robot_description").as_string(); @@ -70,7 +72,15 @@ public: throw std::runtime_error("KDL Tree initialization failed"); } - if (!kdl_tree.getChain("base_link", "gripper_grasp_point", m_kdl_chain)) { + auto base_link = this->get_parameter("base_link").as_string(); + auto robot_ee_link = this->get_parameter("robot_ee_link").as_string(); + if (base_link.empty() or robot_ee_link.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Describe robot end-effector link and base link to continue"); + throw std::runtime_error("Describe robot end-effector link and base link to continue"); + } + + if (!kdl_tree.getChain(base_link, robot_ee_link, 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"); From 85eec1b7d7baa0ad09ade35bd3c89e2fa1524190 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 15 Nov 2024 23:25:02 +0300 Subject: [PATCH 03/10] added doc on how to use framework with your robot or how to add new robot --- doc/en/add_new_robot.md | 106 +++++++++++++++++ doc/en/index.md | 18 +++ doc/index.md | 20 ---- doc/ru/add_new_robot.md | 108 ++++++++++++++++++ doc/ru/index.md | 18 +++ .../rbs_runtime/launch/runtime.launch.py | 41 +++++-- rbs_bringup/launch/rbs_bringup.launch.py | 2 + rbs_bringup/launch/rbs_robot.launch.py | 13 ++- 8 files changed, 292 insertions(+), 34 deletions(-) create mode 100644 doc/en/add_new_robot.md create mode 100644 doc/en/index.md delete mode 100644 doc/index.md create mode 100644 doc/ru/add_new_robot.md create mode 100644 doc/ru/index.md diff --git a/doc/en/add_new_robot.md b/doc/en/add_new_robot.md new file mode 100644 index 0000000..8b069ce --- /dev/null +++ b/doc/en/add_new_robot.md @@ -0,0 +1,106 @@ +# Instructions for Adding a New Robot to the Robossembler ROS 2 Framework + +First, you need to download the robot package containing `xacro` or `urdf` files, as well as geometry files in formats such as `.stl`, `.dae`, `.obj`, etc. + +Before starting, it is important to understand the basics of the [xacro](https://github.com/ros/xacro/wiki) format. This format allows you to reuse existing fragments of a robot's URDF description, making it easier to create and modify descriptions. + +### Steps for Adding a New Robot: + +1. **Install the Robot Package** + After installing the robot package, create a file named `xacro_args.yaml` in the `{description_package}/config/` directory. This file should specify the arguments needed to convert the `xacro` file into a `urdf`. + +2. **Configure the Launch File** + Edit the file [`rbs_bringup.launch.py`](../../rbs_bringup/launch/rbs_bringup.launch.py) to define the parameters required to launch the robot. + + Example of a standard implementation: + ```python + main_script = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"] + ) + ] + ), + launch_arguments={ + "with_gripper": "true", + "gripper_name": "rbs_gripper", + "robot_type": "rbs_arm", + "description_package": "rbs_arm", + "description_file": "rbs_arm_modular.xacro", + "robot_name": "rbs_arm", + "use_moveit": "false", + "moveit_config_package": "rbs_arm", + "moveit_config_file": "rbs_arm.srdf.xacro", + "use_sim_time": "true", + "hardware": "gazebo", + "use_controllers": "true", + "scene_config_file": "", + "base_link_name": "base_link", + "ee_link_name": "gripper_grasp_point", + }.items(), + ) + ``` + This configuration launches another file with specific arguments. Below is a description of each argument to help you decide whether it is needed. + +### Parameter Descriptions: + +- **`with_gripper`** + Indicates whether the robot has a gripper. If set to `true`, the `gripper_controller` will be configured and launched. + +- **`gripper_name`** + Used as a keyword to identify links and joints related to the gripper. It is also applied in `xacro` arguments. + +- **`robot_type`** + Specifies a group of robots of the same type, allowing you to semantically group robots with different names but similar designs. + +- **`description_package`** + The package containing the robot's URDF description. This parameter is mandatory and is used to locate the URDF file and controller configuration files. + +- **`description_file`** + The name of the description file, which should be located in `{description_package}/urdf/`. + +- **`robot_name`** + A unique name for the robot to distinguish it from others in the scene. + +- **`use_moveit`** + Indicates whether [MoveIt 2](https://moveit.picknik.ai/humble/index.html) should be used. While it is not required for basic movements, it is recommended when working with obstacles. + +- **`moveit_config_package`** + The name of the MoveIt 2 configuration package generated based on the `{description_package}`. This parameter is required if you plan to use MoveIt 2. + +- **`moveit_config_file`** + The MoveIt 2 launch file located in `{moveit_config_package}/launch/`. + +- **`use_sim_time`** + A mandatory parameter for simulation. It ensures that time is synchronized with the simulator. + +- **`hardware`** + Specifies the interface to be used for controlling the robot. For example, `gazebo`. This parameter is primarily used in `xacro` files. + +- **`use_controllers`** + Indicates whether to use standard controllers. If set to `false`, the robot will not be able to move. This parameter controls the execution of the [control.launch.py](../../rbs_bringup/launch/control.launch.py) file. You can write a custom implementation instead of using this flag. + +- **`scene_config_file`** + A YAML file that defines the scene configuration. A default example is available [here](../../env_manager/rbs_runtime/config/default-scene-config.yaml). Ensure that the degrees of freedom of your robot match the configuration file. + +- **`base_link_name`** + The name of the robot's base link, which defines where the robot begins. This parameter is important for skill configuration. Review your robot's URDF to set this correctly. + +- **`ee_link_name`** + The name of the end-effector link, which defines where the robot ends. This parameter is also important for skill configuration and should be set based on the URDF. + +- **`control_space`** + Specifies the space in which the robot will be controlled. + Possible values: + - `task` — control in task space (e.g., using positions and orientations in the workspace). + - `joint` — control in joint space (e.g., using angular values for each robot joint). + Default value: `task`. + +- **`control_strategy`** + Specifies the control strategy for the robot. + Possible values: + - `position` — position control, where desired positions are set for joints or the workspace target point. + - `velocity` — velocity control, where desired movement speeds are set. + - `effort` — effort control, where torques or forces applied to the joints are specified. + Default value: `position`. diff --git a/doc/en/index.md b/doc/en/index.md new file mode 100644 index 0000000..597fe05 --- /dev/null +++ b/doc/en/index.md @@ -0,0 +1,18 @@ +Robossembler ROS 2 Packages + +- **`env_manager`** - virtual environment manager: + - **`env_manager`** - manages objects in Gazebo simulation scenes. + - **`env_manager_interfaces`** - ROS 2 interfaces for configuring, loading, activating, and unloading environments. + - **`rbs_gym`** - reinforcement learning module: training management, simulation environment creation, action and observation space handling, utilities. + - **`rbs_runtime`** - runs the main runtime using `env_manager`. + +- **`rbs_bringup`** - launch scenarios: simulation, real robot, multi-robot configurations. +- **`rbs_bt_executor`** - executes behavior trees with Behavior Tree CPP v4. +- **`rbs_interface`** - interface linking behavior trees with skill servers (recommended to merge with `rbs_bt_executor`). +- **`rbs_perception`** - machine vision module with multiple implementations. +- **`rbs_simulation`** - simulation models (recommended to merge with `env_manager` or `rbs_gym`). +- **`rbs_skill_interfaces`** - common interfaces for interacting with skill servers and behavior trees. +- **`rbs_skill_servers`** - packages for skill servers (recommended to replace with individual packages for each server). +- **`rbs_task_planner`** - task planner based on PDDL. +- **`rbs_utils`** - utilities for working with configurations containing grasp positions. +- **`rbss_objectdetection`** - skill server for object detection using YOLOv8. diff --git a/doc/index.md b/doc/index.md deleted file mode 100644 index b8282f3..0000000 --- a/doc/index.md +++ /dev/null @@ -1,20 +0,0 @@ -Robossembler ROS 2 Packages - -- `env_manager` - менеджер переключения виртуальных сред - - `env_interface` - базовый класс для создания конкретной среды на базе ROS 2 LifeCycle Node - - `env_manager` - основной пакет менеджера переключения виртуальных сред - - `env_manager_interfaces` - интерфейсы ROS 2 для env_manager, описывают сообщения о состоянии среды, сервисы по конфигурации/загрузке/включению/выгрузке среды - - `gz_environment` - конкретный экземпляр `env_interface` для симулятора Gazebo - - `planning_scene_manager` - управление сценой планирования для MoveIt (среда для MoveIt) - - `rbs_gym` - модуль обучения с подреплением: управление процесом обучения, формирование сред симуляции, управление пространствами действий(actions) и восприятия(observation), управление задачами, утилиты - -- `rbs_bringup` - пакет для запуска разных launch сценариев: симуляция, реальный робот, разные конфигурации оборудования (multi-robot) -- `rbs_bt_executor` - модуль запуска деревьев поведения на Behavior Tree CPP v4 -- `rbs_interface` - пакет для связывания деревьев со скилл-серверами (объединить с - `rbs_bt_executor`) -- `rbs_perception` - модуль машинного восприятия, где реализованы разные версии -- `rbs_simulation` - модели для симуляции (к удалить или объединить с env_manager/rbs_gym) -- `rbs_skill_interfaces` - общеиспользуемые (common) интерфейсы для взаимодействия с Серверами Навыков и Деревом поведения (специфичные интерфейсы размещаются в пакетах Серверов Навыков) -- `rbs_skill_servers` - пакеты Серверов Навыков (упразднить, для каждого Сервера Навыков свой пакет) -- `rbs_task_planner` - планировщик задач на базе PDDL -- `rbs_utils` - работа с конфигом, содержащим позиции захвата для деталей -- `rbss_objectdetection` - Сервер Навыка обнаружения объектов с помощью YOLOv8 \ No newline at end of file diff --git a/doc/ru/add_new_robot.md b/doc/ru/add_new_robot.md new file mode 100644 index 0000000..f9cebd2 --- /dev/null +++ b/doc/ru/add_new_robot.md @@ -0,0 +1,108 @@ +# Инструкция по добавлению нового робота в фреймворк Robossembler ROS 2 + +Прежде всего необходимо скачать пакет робота, содержащий файлы `xacro` или `urdf`, а также файлы геометрии робота в формате `.stl`, `.dae`, `.obj` и других. + +Перед началом работы важно ознакомиться с основными аспектами формата [xacro](https://github.com/ros/xacro/wiki). Этот формат позволяет переиспользовать существующие фрагменты URDF-описания робота, что упрощает создание и модификацию описания. + +### Шаги по добавлению нового робота: + +1. **Установка пакета робота** + После установки пакета робота создайте файл `xacro_args.yaml` в директории `{description_package}/config/`. В этом файле необходимо указать аргументы для преобразования xacro-файла в URDF. + +2. **Настройка запуска** + Отредактируйте файл [`rbs_bringup.launch.py`](../../rbs_bringup/launch/rbs_bringup.launch.py), указав параметры для запуска робота. + + Пример стандартной реализации: + ```python + main_script = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"] + ) + ] + ), + launch_arguments={ + "with_gripper": "true", + "gripper_name": "rbs_gripper", + "robot_type": "rbs_arm", + "description_package": "rbs_arm", + "description_file": "rbs_arm_modular.xacro", + "robot_name": "rbs_arm", + "use_moveit": "false", + "moveit_config_package": "rbs_arm", + "moveit_config_file": "rbs_arm.srdf.xacro", + "use_sim_time": "true", + "hardware": "gazebo", + "use_controllers": "true", + "scene_config_file": "", + "base_link_name": "base_link", + "ee_link_name": "gripper_grasp_point", + "control_space": "task", + "control_strategy": "position", + }.items(), + ) + ``` + Здесь выполняется запуск другого launch-файла с указанными аргументами. Ниже приводится описание каждого аргумента. + +### Описание параметров: + +- **`with_gripper`** + Указывает, есть ли на роботе захватное устройство (гриппер). Если значение `true`, будет настроен и запущен `gripper_controller`. + +- **`gripper_name`** + Используется как ключевое слово для указания линков и джоинтов, относящихся к грипперу. Также применяется в xacro-аргументах. + +- **`robot_type`** + Обозначает группу роботов одного типа. Например, все роботы с разными именами, но одинаковой конструкцией. + +- **`description_package`** + Пакет, содержащий описание URDF робота. Обязательный параметр. Используется для определения пути к файлу описания и конфигурации контроллеров. + +- **`description_file`** + Имя файла описания, который должен находиться в `{description_package}/urdf/`. + +- **`robot_name`** + Уникальное имя робота, которое позволяет отличить его от других в сцене. + +- **`use_moveit`** + Указывает, нужно ли использовать [MoveIt 2](https://moveit.picknik.ai/humble/index.html). Для базовых перемещений MoveIt 2 не обязателен, но для работы с препятствиями рекомендуется его включить. + +- **`moveit_config_package`** + Имя пакета конфигурации MoveIt 2, который генерируется на основе `{description_package}`. Обязательный параметр, если используется MoveIt 2. + +- **`moveit_config_file`** + Файл запуска MoveIt 2, находящийся в `{moveit_config_package}/launch/`. + +- **`use_sim_time`** + Обязательный параметр при работе в симуляции. Обеспечивает синхронизацию времени с симулятором. + +- **`hardware`** + Указывает интерфейс для управления роботом. Например, `gazebo`. Используется в основном в xacro-файлах. + +- **`use_controllers`** + Указывает, нужно ли использовать стандартные контроллеры. Если значение `false`, робот не сможет двигаться. Влияет на запуск файла [control.launch.py](../../rbs_bringup/launch/control.launch.py). + +- **`scene_config_file`** + Файл конфигурации сцены в формате YAML. Пример можно найти [здесь](../../env_manager/rbs_runtime/config/default-scene-config.yaml). Обратите внимание на соответствие количества степеней свободы робота. + +- **`base_link_name`** + Имя базового линка, от которого начинается робот. Важно для настройки навыков. Рекомендуется свериться с URDF. + +- **`ee_link_name`** + Имя конечного линка (энд-эффектора), где заканчивается робот. Этот параметр также настраивается на основе URDF. + +- **`control_space`** + Указывает, в каком пространстве робот будет управляться. + Возможные значения: + - `task` — управление в пространстве задач (например, с использованием позиций и ориентации в рабочем пространстве). + - `joint` — управление в пространстве суставов (например, с использованием угловых значений для каждого сустава робота). + Значение по умолчанию: `task`. + +- **`control_strategy`** + Указывает стратегию управления роботом. + Возможные значения: + - `position` — управление положением, когда задаются желаемые позиции для суставов или рабочей точки. + - `velocity` — управление скоростью, когда задаются желаемые скорости движения. + - `effort` — управление усилием, когда задаются моменты или силы, прикладываемые к суставам. + Значение по умолчанию: `position`. diff --git a/doc/ru/index.md b/doc/ru/index.md new file mode 100644 index 0000000..7e73a5e --- /dev/null +++ b/doc/ru/index.md @@ -0,0 +1,18 @@ +# Robossembler ROS 2 Packages + +- **`env_manager`** - менеджер виртуальных сред: + - **`env_manager`** - управление объектами в сцене симуляции Gazebo. + - **`env_manager_interfaces`** - ROS 2 интерфейсы для конфигурации, загрузки, активации и выгрузки сред. + - **`rbs_gym`** - модуль обучения с подкреплением: управление обучением, создание симуляционных сред, управление пространствами действий и наблюдений, утилиты. + - **`rbs_runtime`** - запуск основного рантайма с использованием `env_manager`. + +- **`rbs_bringup`** - запуск сценариев: симуляция, реальный робот, многороботные конфигурации. +- **`rbs_bt_executor`** - выполнение деревьев поведения с Behavior Tree CPP v4. +- **`rbs_interface`** - интерфейс для связи деревьев поведения со скилл-серверами (рекомендуется объединить с `rbs_bt_executor`). +- **`rbs_perception`** - модуль машинного зрения с различными версиями. +- **`rbs_simulation`** - модели для симуляции (рекомендуется объединить с `env_manager` или `rbs_gym`). +- **`rbs_skill_interfaces`** - общие интерфейсы для взаимодействия с скилл-серверами и деревьями поведения. +- **`rbs_skill_servers`** - пакеты для скилл-серверов (рекомендуется заменить на индивидуальные пакеты для каждого сервера). +- **`rbs_task_planner`** - планировщик задач на основе PDDL. +- **`rbs_utils`** - утилиты для работы с конфигурациями, содержащими позиции захвата. +- **`rbss_objectdetection`** - скилл-сервер для обнаружения объектов с YOLOv8. diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index f142865..0ff5ed9 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -38,12 +38,17 @@ def launch_setup(context, *args, **kwargs): ee_link_name = LaunchConfiguration("ee_link_name").perform(context) base_link_name = LaunchConfiguration("base_link_name").perform(context) + control_space = LaunchConfiguration("control_space").perform(context) + control_strategy = LaunchConfiguration("control_strategy").perform(context) + if not scene_config_file == "": config_file = {"config_file": scene_config_file} else: config_file = {} - description_package_abs_path = get_package_share_directory(description_package.perform(context)) + description_package_abs_path = get_package_share_directory( + description_package.perform(context) + ) simulation_controllers = os.path.join( description_package_abs_path, "config", "controllers.yaml" @@ -91,9 +96,11 @@ def launch_setup(context, *args, **kwargs): robot_description_content = robot_description_doc.toprettyxml(indent=" ") robot_description = {"robot_description": robot_description_content} - + # Parse robot and configure controller's file for ControllerManager - robot = URDF_parser.load_string(robot_description_content, ee_link_name="gripper_grasp_point") + robot = URDF_parser.load_string( + robot_description_content, ee_link_name="gripper_grasp_point" + ) ControllerManager.save_to_yaml( robot, description_package_abs_path, "controllers.yaml" ) @@ -122,7 +129,9 @@ def launch_setup(context, *args, **kwargs): "use_controllers": "true", "robot_description": robot_description_content, "base_link_name": base_link_name, - "ee_link_name": ee_link_name + "ee_link_name": ee_link_name, + "control_space": control_space, + "control_strategy": control_strategy, }.items(), ) @@ -155,14 +164,6 @@ def generate_launch_description(): default_value="rbs_arm", ) ) - # General arguments - # declared_arguments.append( - # DeclareLaunchArgument( - # "controllers_file", - # default_value="controllers.yaml", - # description="YAML file with the controllers configuration.", - # ) - # ) declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -267,6 +268,22 @@ def generate_launch_description(): description="Base link name if robot arm", ) ) + 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).", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py index 63253b4..06c1047 100644 --- a/rbs_bringup/launch/rbs_bringup.launch.py +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -35,6 +35,8 @@ def launch_setup(context, *args, **kwargs): "scene_config_file": "", "base_link_name": "base_link", "ee_link_name": "gripper_grasp_point", + "control_space": "task", + "control_strategy": "position", }.items(), ) diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 2d42d35..adef203 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -225,7 +225,17 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot to launch, specified by name.", - choices=["rbs_arm", "ar4", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + choices=[ + "rbs_arm", + "ar4", + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + ], default_value="rbs_arm", ) ) @@ -416,7 +426,6 @@ def generate_launch_description(): ) ) - return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] ) From 17401c29a7eea7a089687cce0e44e0cdd2e6c60a Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 18 Nov 2024 23:59:49 +0300 Subject: [PATCH 04/10] add interactive argument to control robot with interactive marker --- doc/en/add_new_robot.md | 4 ++++ doc/ru/add_new_robot.md | 4 ++++ env_manager/rbs_runtime/launch/runtime.launch.py | 10 ++++++++++ rbs_bringup/launch/control.launch.py | 2 +- rbs_bringup/launch/rbs_bringup.launch.py | 1 + 5 files changed, 20 insertions(+), 1 deletion(-) diff --git a/doc/en/add_new_robot.md b/doc/en/add_new_robot.md index 8b069ce..ef70a66 100644 --- a/doc/en/add_new_robot.md +++ b/doc/en/add_new_robot.md @@ -104,3 +104,7 @@ Before starting, it is important to understand the basics of the [xacro](https:/ - `velocity` — velocity control, where desired movement speeds are set. - `effort` — effort control, where torques or forces applied to the joints are specified. Default value: `position`. + +- **`interactive`** + Specifies whether to run the `motion_control_handle` controller. + Default value: `true`. diff --git a/doc/ru/add_new_robot.md b/doc/ru/add_new_robot.md index f9cebd2..6f1bbb8 100644 --- a/doc/ru/add_new_robot.md +++ b/doc/ru/add_new_robot.md @@ -106,3 +106,7 @@ - `velocity` — управление скоростью, когда задаются желаемые скорости движения. - `effort` — управление усилием, когда задаются моменты или силы, прикладываемые к суставам. Значение по умолчанию: `position`. + +- **`interactive`** + Указывает, нужно ли запускать контроллер `motion_control_handle`. + Значение по умолчанию: `true`. diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index 0ff5ed9..45d6af7 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -41,6 +41,8 @@ def launch_setup(context, *args, **kwargs): control_space = LaunchConfiguration("control_space").perform(context) control_strategy = LaunchConfiguration("control_strategy").perform(context) + interactive = LaunchConfiguration("interactive").perform(context) + if not scene_config_file == "": config_file = {"config_file": scene_config_file} else: @@ -132,6 +134,7 @@ def launch_setup(context, *args, **kwargs): "ee_link_name": ee_link_name, "control_space": control_space, "control_strategy": control_strategy, + "interactive_control": interactive, }.items(), ) @@ -284,6 +287,13 @@ def generate_launch_description(): description="Specify the control strategy (e.g., position control).", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "interactive", + default_value="true", + description="Wheter to run the motion_control_handle controller", + ), + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_bringup/launch/control.launch.py b/rbs_bringup/launch/control.launch.py index 91ab0b9..a2d6df6 100644 --- a/rbs_bringup/launch/control.launch.py +++ b/rbs_bringup/launch/control.launch.py @@ -78,7 +78,7 @@ def generate_launch_description(): DeclareLaunchArgument( "interactive_control", default_value="true", - description="Whether to activate the gripper_controller.", + description="Wheter to run the motion_control_handle controller", ), ] diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py index 06c1047..5872087 100644 --- a/rbs_bringup/launch/rbs_bringup.launch.py +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -37,6 +37,7 @@ def launch_setup(context, *args, **kwargs): "ee_link_name": "gripper_grasp_point", "control_space": "task", "control_strategy": "position", + "interactive": "false" }.items(), ) From a8874fa0d761e48e25b47582080526bd54542b7b Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 20 Nov 2024 21:07:19 +0300 Subject: [PATCH 05/10] Update Dockerfile and dependencies, add installation guide - Switched base image in `Dockerfile` to `nvidia/cuda:12.6.2-cudnn-devel-ubuntu22.04`. - Added non-interactive mode for Debian and streamlined package installation. - Removed unused JSON library build steps. - Enhanced ROS2 installation with keyring and repository setup. - Simplified Python package and framework dependencies installation. - Added commands for `git-lfs` installation and cloning `rbs_assets_library`. - Introduced a new Russian installation guide (`doc/ru/installation.md`) with detailed steps for setting up the framework and dependencies. - Fixed import paths in `octree.py` for consistency with project structure. - Added `all-deps.repos` and `requirements.txt` for managing dependencies. --- Dockerfile | 69 ++++++++++++------- doc/ru/installation.md | 32 +++++++++ .../rbs_gym/envs/observation/octree.py | 2 +- repos/all-deps.repos | 37 ++++++++++ repos/requirements.txt | 29 ++++++++ 5 files changed, 143 insertions(+), 26 deletions(-) create mode 100644 doc/ru/installation.md create mode 100644 repos/all-deps.repos create mode 100644 repos/requirements.txt diff --git a/Dockerfile b/Dockerfile index 1ff2dcb..0aa4050 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,46 +1,65 @@ -FROM billf1nger/ros2-humble-cuda:v12.2.2-cudnn8-ubuntu22.04-gz-nvidia - +FROM nvidia/cuda:12.6.2-cudnn-devel-ubuntu22.04 ARG WSDIR=rbs_ws -ARG config_type=sim ENV RBS_ASSEMBLY_DIR=/assembly +ENV DEBIAN_FRONTEND=noninteractive # COPY /home/bill-finger/assembly /assembly ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/ -RUN apt update && apt install -y \ +RUN apt-get update && apt-get upgrade -y && apt-get install -y \ git \ + software-properties-common \ python3-pip \ lsb-release \ - wget \ - gnupg + curl \ + wget -RUN pip install vcstool -WORKDIR /libs -RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\ - tar -xf v3.11.3.tar.gz &&\ - cd json-3.11.3 &&\ - mkdir build &&\ - cd build &&\ - cmake .. &&\ - make &&\ - make install +# WORKDIR /libs +# RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\ +# tar -xf v3.11.3.tar.gz &&\ +# cd json-3.11.3 &&\ +# mkdir build &&\ +# cd build &&\ +# cmake .. &&\ +# make &&\ +# make install -RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\ - cd megapose6d &&\ - pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\ - pip install -e . -RUN git clone https://github.com/thodan/bop_toolkit &&\ - cd bop_toolkit &&\ - pip install -e . +RUN add-apt-repository universe +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + + +RUN apt-get update && apt-get upgrade && apt-get install -y ros-humble-ros-base + +# RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\ +# cd megapose6d &&\ +# pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\ +# pip install -e . +# RUN git clone https://github.com/thodan/bop_toolkit &&\ +# cd bop_toolkit &&\ +# pip install -e . WORKDIR /${WSDIR} COPY . src/robossembler-ros2/ -RUN vcs import src/. < src/robossembler-ros2/rbs.${config_type}.repos -RUN apt update && rosdep update && \ +RUN pip install vcstool uv + +# Install framework and dependencies +RUN vcs import src/. < src/robossembler-ros2/repos/all-deps.repos +RUN uv pip install --system -r src/robossembler-ros2/repos/requirements.txt +RUN apt-get update && rosdep update && \ rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble RUN . /opt/ros/humble/setup.sh && \ colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1 + + +WORKDIR /${RBS_ASSEMBLY_DIR} +RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash +RUN apt-get install git-lfs +RUN git clone https://github.com/solid-sinusoid/rbs_assets_library.git +RUN cd rbs_assets_library && git lfs pull && pip install -e . + +WORKDIR /${WSDIR} diff --git a/doc/ru/installation.md b/doc/ru/installation.md new file mode 100644 index 0000000..55d2446 --- /dev/null +++ b/doc/ru/installation.md @@ -0,0 +1,32 @@ +# Инструкция по установке фреймворка + +Первым делом необходимо установить [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html). Рекомендуется минимальная установка. + +Вторым делом надо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников из этого [форка](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers) стоит отметить, что также существует альтернативная установка с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool) который поставляется с базовыми пакетами ROS2. + +Если устанавливать через `vcstool` тогда необходимые пакеты будут клонированы в тоже рабочее пространство, что и сам фреймворк. Сама команда будет выглядеть так +```sh +vcs import . < robossembler-ros2/repos/all-deps.repos +``` + +Заодно можно выполнить команду для установки всех требуемых библиотек Python +```shell +pip insatll -r robossembler-ros2/repos/requirements.txt +``` + +При этом команду надо выполнять в директории `{robossembler_ws}/src/` + +Более четкая последовательность команд кому лень: +```sh +cd +mkdir -p robossembler-ros2/src && cd robossembler-ros2 +git clone git clone https://seed.robossembler.org/z46gtVRpXaXrGQM7Fxiqu7pLy7kip.git robossembler-ros2 +# Или если вы предпочитаете radicle +rad clone rad:z46gtVRpXaXrGQM7Fxiqu7pLy7kip +cd src +vcs import . < robossembler-ros2/repos/all-deps.repos +pip insatll -r robossembler-ros2/repos/requirements.txt +cd .. +rosdep install --from-paths src -y --ignore-src +colcon build --symlink-install +``` diff --git a/env_manager/rbs_gym/rbs_gym/envs/observation/octree.py b/env_manager/rbs_gym/rbs_gym/envs/observation/octree.py index 4b8f5d0..55caaaf 100644 --- a/env_manager/rbs_gym/rbs_gym/envs/observation/octree.py +++ b/env_manager/rbs_gym/rbs_gym/envs/observation/octree.py @@ -7,7 +7,7 @@ import torch from rclpy.node import Node from sensor_msgs.msg import PointCloud2 -from rbs_gym.envs.utils import Tf2Listener, conversions +from env_manager.utils import Tf2Listener, conversions class OctreeCreator: diff --git a/repos/all-deps.repos b/repos/all-deps.repos new file mode 100644 index 0000000..6dde101 --- /dev/null +++ b/repos/all-deps.repos @@ -0,0 +1,37 @@ +repositories: + rbs_arm: + type: git + url: https://github.com/solid-sinusoid/rbs-arm.git + version: main + robot_builder: + type: git + url: https://github.com/solid-sinusoid/robot-builder.git + version: main + rbs_gripper: + type: git + url: https://github.com/solid-sinusoid/rbs-gripper.git + version: main + behavior_tree: + type: git + url: https://github.com/BehaviorTree/BehaviorTree.ROS2.git + version: humble + dynamic_message_introspection: + type: git + url: https://github.com/osrf/dynamic_message_introspection.git + version: main + robot_builder: + type: git + url: https://github.com/solid-sinusoid/robot-builder.git + version: main + cartesian_controllers: + type: git + url: https://github.com/solid-sinusoid/cartesian_controllers.git + version: gazebo-simulation + ros2_control: + type: git + url: https://github.com/solid-sinusoid/ros2_control.git + version: gz-ros2-cartesian-controllers + gz_ros2_control: + type: git + url: https://github.com/solid-sinusoid/gz_ros2_control.git + version: fts-sensor diff --git a/repos/requirements.txt b/repos/requirements.txt new file mode 100644 index 0000000..1b63770 --- /dev/null +++ b/repos/requirements.txt @@ -0,0 +1,29 @@ +trimesh +pcg-gazebo +loguru +# markupsafe==2.0.1 +# Jinja2==2.6 + + +numpy!=1.24.0,>=1.20 +flask==3.0.3 +Jinja2>=3.1.2 +docutils<0.18,>=0.15 +markdown-it-py<3.0.0,>=1.0.0 +setuptools-scm + +dacite>=1.8.1 +gymnasium>=0.29.1 +numpy>=1.24.0 +open3d>=0.18.0 +scipy>=1.14.1 +tensorflow>=2.17.0 +torch>=2.4.1 +torchvision>=0.19.1 +trimesh>=4.4.9 +wandb>=0.18.6 + +# Packages from devpi custom repository +scenario @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/63e/86b0583c22e52/scenario-1.4.0-cp310-cp310-linux_x86_64.whl#sha256=63e86b0583c22e52c299b9a25e74a26178fc7dbc423f661bf34e4dd26543d11f +gym-gz @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/539/1f448c1391486/gym_gz-1.4.0-py3-none-any.whl#sha256=5391f448c13914860e2a65904f6d0fac7eba71daaa3327d8399d7ae516471a68 +gym-gz-models @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/f42/4784934183e88/gym_gz_models-1.2.0-cp310-cp310-linux_x86_64.whl#sha256=f424784934183e88541c703c789315ba6118661bd221a4bf9135c6a0ee012e96 From 552a3c6a716ca7007f74307a24dbdd3148f97cff Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 20 Nov 2024 23:12:58 +0300 Subject: [PATCH 06/10] fix: install nlohmann-json via rosdep --- rbs_utils/rbs_utils/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rbs_utils/rbs_utils/package.xml b/rbs_utils/rbs_utils/package.xml index 9c19790..4aa08d4 100644 --- a/rbs_utils/rbs_utils/package.xml +++ b/rbs_utils/rbs_utils/package.xml @@ -21,7 +21,7 @@ rbs_utils_interfaces dynmsg sensor_msgs - nlohmann_json + nlohmann-json-dev rosbag2_cpp ament_lint_auto From 5747057baf40da4380518be038691d89e9415a6f Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 21 Nov 2024 00:25:22 +0300 Subject: [PATCH 07/10] fix: deps version resolving --- repos/requirements.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/repos/requirements.txt b/repos/requirements.txt index 1b63770..74e5ae2 100644 --- a/repos/requirements.txt +++ b/repos/requirements.txt @@ -4,16 +4,15 @@ loguru # markupsafe==2.0.1 # Jinja2==2.6 - -numpy!=1.24.0,>=1.20 +numpy==1.24.0 flask==3.0.3 Jinja2>=3.1.2 docutils<0.18,>=0.15 markdown-it-py<3.0.0,>=1.0.0 -setuptools-scm +# setuptools-scm dacite>=1.8.1 -gymnasium>=0.29.1 +gymnasium==0.29.1 numpy>=1.24.0 open3d>=0.18.0 scipy>=1.14.1 @@ -27,3 +26,4 @@ wandb>=0.18.6 scenario @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/63e/86b0583c22e52/scenario-1.4.0-cp310-cp310-linux_x86_64.whl#sha256=63e86b0583c22e52c299b9a25e74a26178fc7dbc423f661bf34e4dd26543d11f gym-gz @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/539/1f448c1391486/gym_gz-1.4.0-py3-none-any.whl#sha256=5391f448c13914860e2a65904f6d0fac7eba71daaa3327d8399d7ae516471a68 gym-gz-models @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/f42/4784934183e88/gym_gz_models-1.2.0-cp310-cp310-linux_x86_64.whl#sha256=f424784934183e88541c703c789315ba6118661bd221a4bf9135c6a0ee012e96 +rbs_asstss_library @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/165/dd2476d9dc455/rbs_assets_library-0.1.0-py2.py3-none-any.whl#sha256=165dd2476d9dc455b425a8c8d3e09a9a9541d54c1f900a3a0e63526c6118de6d From b71a2bdf55774e6be6d6fb2722ef595f2e2214d0 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 22 Nov 2024 02:13:01 +0300 Subject: [PATCH 08/10] add material properties to terrain and ground models - Introduced `ambient`, `specular`, and `diffuse` attributes to `TerrainData` class. - Updated `Ground` class to accept and use material properties for ambient, specular, and diffuse values. - Modified the SDF generation to dynamically apply the provided material properties. - Integrated material property support in the `Scene` class for terrain setup. --- .../env_manager/models/configs/terrain.py | 3 +++ .../env_manager/models/terrains/ground.py | 14 ++++++++++---- env_manager/env_manager/env_manager/scene/scene.py | 3 +++ 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/env_manager/env_manager/env_manager/models/configs/terrain.py b/env_manager/env_manager/env_manager/models/configs/terrain.py index ea1a89e..f240132 100644 --- a/env_manager/env_manager/env_manager/models/configs/terrain.py +++ b/env_manager/env_manager/env_manager/models/configs/terrain.py @@ -27,4 +27,7 @@ class TerrainData: spawn_position: tuple[float, float, float] = field(default=(0, 0, 0)) spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1)) size: tuple[float, float] = field(default=(1.5, 1.5)) + ambient: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0)) + specular: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0)) + diffuse: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0)) model_rollouts_num: int = field(default=1) diff --git a/env_manager/env_manager/env_manager/models/terrains/ground.py b/env_manager/env_manager/env_manager/models/terrains/ground.py index 4beaf41..09044a3 100644 --- a/env_manager/env_manager/env_manager/models/terrains/ground.py +++ b/env_manager/env_manager/env_manager/models/terrains/ground.py @@ -20,6 +20,9 @@ class Ground(model_wrapper.ModelWrapper): size (tuple[float, float], optional): The size of the ground plane. Defaults to (1.5, 1.5). collision_thickness (float, optional): The thickness of the collision surface. Defaults to 0.05. friction (float, optional): The friction coefficient for the ground surface. Defaults to 5.0. + ambient (tuple[float, float, float, float], optional): The ambient color of the material. Defaults to (0.8, 0.8, 0.8, 1.0). + specular (tuple[float, float, float, float], optional): The specular color of the material. Defaults to (0.8, 0.8, 0.8, 1.0). + diffuse (tuple[float, float, float, float], optional): The diffuse color of the material. Defaults to (0.8, 0.8, 0.8, 1.0). **kwargs: Additional keyword arguments for future extensions. Raises: @@ -35,6 +38,9 @@ class Ground(model_wrapper.ModelWrapper): size: tuple[float, float] = (1.5, 1.5), collision_thickness=0.05, friction: float = 5.0, + ambient: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), + specular: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), + diffuse: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), **kwargs, ): # Get a unique model name @@ -43,7 +49,7 @@ class Ground(model_wrapper.ModelWrapper): # Initial pose initial_pose = scenario_core.Pose(position, orientation) - # Create SDF string for the model + # Create SDF string for the model with the provided material properties sdf = f""" true @@ -75,9 +81,9 @@ class Ground(model_wrapper.ModelWrapper): - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + {' '.join(map(str, ambient))} + {' '.join(map(str, diffuse))} + {' '.join(map(str, specular))} diff --git a/env_manager/env_manager/env_manager/scene/scene.py b/env_manager/env_manager/env_manager/scene/scene.py index 8cdac9a..e34a5e5 100644 --- a/env_manager/env_manager/env_manager/scene/scene.py +++ b/env_manager/env_manager/env_manager/scene/scene.py @@ -573,6 +573,9 @@ class Scene: position=self.terrain.spawn_position, orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw), size=self.terrain.size, + ambient=self.terrain.ambient, + diffuse=self.terrain.diffuse, + specular=self.terrain.specular, ) # Enable contact detection From fbddca1b8b4645f81f6c6644f0a9b5b49faaec31 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 22 Nov 2024 02:15:09 +0300 Subject: [PATCH 09/10] refactor and extend runtime configuration handling - **Scene configuration:** - Replaced `bishop` object with `hole` and added a new `peg` object. - Enhanced object properties with new attributes like `randomize`, `relative_to`, `static`, and `texture`. - Updated `robot` joint positions in the default scene configuration. - **Launch file updates:** - Added handling for MoveIt SRDF file processing, including semantic descriptions. - Refactored unused parameters (`gripper_name`, `hardware`) and streamlined arguments. - Extended `robot_type` choices in the `generate_launch_description` method. - **Runtime initialization:** - Introduced `object_factory` to handle dynamic object instantiation based on type (`box`, `cylinder`, `mesh`, or `model`). - Enhanced `scene_config_loader` to process and instantiate objects using the factory function. --- .../config/default-scene-config.yaml | 35 ++++++++++- .../rbs_runtime/launch/runtime.launch.py | 61 +++++++++++-------- .../rbs_runtime/rbs_runtime/__init__.py | 22 +++++++ 3 files changed, 91 insertions(+), 27 deletions(-) diff --git a/env_manager/rbs_runtime/config/default-scene-config.yaml b/env_manager/rbs_runtime/config/default-scene-config.yaml index e0b7f57..474f3b1 100644 --- a/env_manager/rbs_runtime/config/default-scene-config.yaml +++ b/env_manager/rbs_runtime/config/default-scene-config.yaml @@ -67,7 +67,36 @@ light: visual: true objects: - color: null - name: bishop + name: hole + orientation: !tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + position: !tuple + - 0.1 + - 0.3 + - 0.1 + randomize: + count: 0 + models_rollouts_num: 0 + random_color: false + random_model: false + random_orientation: false + random_pose: false + random_position: false + random_spawn_position_segments: [] + random_spawn_position_update_workspace_centre: false + random_spawn_volume: !tuple + - 0.5 + - 0.5 + - 0.5 + relative_to: world + static: false + texture: [] + type: "model" + - color: null + name: peg orientation: !tuple - 1.0 - 0.0 @@ -75,8 +104,8 @@ objects: - 0.0 position: !tuple - 0.0 - - 1.0 - 0.3 + - 0.1 randomize: count: 0 models_rollouts_num: 0 @@ -104,7 +133,7 @@ plugins: robot: gripper_joint_positions: 0.0 joint_positions: - - 1.57 + - -1.57 - 0.5 - 3.14159 - 1.5 diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py index 45d6af7..9656efa 100644 --- a/env_manager/rbs_runtime/launch/runtime.launch.py +++ b/env_manager/rbs_runtime/launch/runtime.launch.py @@ -1,5 +1,6 @@ import os +from launch.launch_introspector import indent import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -26,13 +27,10 @@ def launch_setup(context, *args, **kwargs): with_gripper_condition = LaunchConfiguration("with_gripper") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") - robot_name = LaunchConfiguration("robot_name") 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") - gripper_name = LaunchConfiguration("gripper_name") scene_config_file = LaunchConfiguration("scene_config_file").perform(context) ee_link_name = LaunchConfiguration("ee_link_name").perform(context) @@ -64,6 +62,7 @@ def launch_setup(context, *args, **kwargs): xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml" + # TODO: hide this to another place # Load xacro_args def param_constructor(loader, node, local_vars): @@ -96,12 +95,29 @@ def launch_setup(context, *args, **kwargs): robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data) + robot_description_semantic_content = "" + + if use_moveit.perform(context) == "true": + srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml" + srdf_file = os.path.join( + get_package_share_directory(moveit_config_package.perform(context)), + "srdf", + moveit_config_file.perform(context), + ) + srdf_mappings = load_xacro_args(srdf_config_file, locals()) + robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings) + robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ") + control_space = "joint" + control_strategy = "position" + interactive = "false" + + robot_description_content = robot_description_doc.toprettyxml(indent=" ") robot_description = {"robot_description": robot_description_content} # Parse robot and configure controller's file for ControllerManager robot = URDF_parser.load_string( - robot_description_content, ee_link_name="gripper_grasp_point" + robot_description_content, ee_link_name=ee_link_name ) ControllerManager.save_to_yaml( robot, description_package_abs_path, "controllers.yaml" @@ -117,7 +133,6 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "with_gripper": with_gripper_condition, - "gripper_name": gripper_name, "controllers_file": simulation_controllers, "robot_type": robot_type, "description_package": description_package, @@ -127,9 +142,9 @@ def launch_setup(context, *args, **kwargs): "moveit_config_package": moveit_config_package, "moveit_config_file": moveit_config_file, "use_sim_time": use_sim_time, - "hardware": hardware, "use_controllers": "true", "robot_description": robot_description_content, + "robot_description_semantic": robot_description_semantic_content, "base_link_name": base_link_name, "ee_link_name": ee_link_name, "control_space": control_space, @@ -153,7 +168,11 @@ def launch_setup(context, *args, **kwargs): delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup]) - nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack] + nodes_to_start = [ + rbs_runtime, + clock_bridge, + delay_robot_control_stack + ] return nodes_to_start @@ -163,7 +182,17 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_type", description="Type of robot by name", - choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + choices=[ + "rbs_arm", + "ar4", + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + ], default_value="rbs_arm", ) ) @@ -212,14 +241,6 @@ def generate_launch_description(): This is needed for the trajectory planing in simulation.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "gripper_name", - default_value="rbs_gripper", - choices=["rbs_gripper", ""], - description="choose gripper by name (leave empty if hasn't)", - ) - ) declared_arguments.append( DeclareLaunchArgument( "with_gripper", default_value="true", description="With gripper or not?" @@ -235,14 +256,6 @@ def generate_launch_description(): "launch_perception", default_value="false", description="Launch perception?" ) ) - declared_arguments.append( - DeclareLaunchArgument( - "hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Choose your harware_interface", - ) - ) declared_arguments.append( DeclareLaunchArgument( "use_controllers", diff --git a/env_manager/rbs_runtime/rbs_runtime/__init__.py b/env_manager/rbs_runtime/rbs_runtime/__init__.py index 2659497..3dec5f1 100644 --- a/env_manager/rbs_runtime/rbs_runtime/__init__.py +++ b/env_manager/rbs_runtime/rbs_runtime/__init__.py @@ -4,6 +4,24 @@ import yaml from dacite import from_dict from env_manager.models.configs import SceneData +from typing import Dict, Any +from env_manager.models.configs import ( + BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData +) + +def object_factory(obj_data: Dict[str, Any]) -> ObjectData: + obj_type = obj_data.get('type', '') + + if obj_type == 'box': + return BoxObjectData(**obj_data) + elif obj_type == 'cylinder': + return CylinderObjectData(**obj_data) + elif obj_type == 'mesh': + return MeshData(**obj_data) + elif obj_type == 'model': + return ModelData(**obj_data) + else: + return ObjectData(**obj_data) def scene_config_loader(file: str | Path) -> SceneData: def tuple_constructor(loader, node): @@ -15,4 +33,8 @@ def scene_config_loader(file: str | Path) -> SceneData: scene_data = from_dict(data_class=SceneData, data=scene_cfg) + scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])] + + print(scene_data) + return scene_data From d0788041e91d0767f5c77d15624bc9700470e88d Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 22 Nov 2024 02:16:20 +0300 Subject: [PATCH 10/10] refactor launch files for enhanced MoveIt integration and cleanup - **RBS Bringup Launch File:** - Enabled MoveIt by default (`use_moveit: true`). - Removed unused arguments (`gripper_name`, `hardware`) for cleaner configuration. - Updated `robot_description_kinematics` to use the new kinematics file path handling. - **RBS Robot Launch File:** - Streamlined robot description setup, removing unnecessary parameters like `gripper_name`, `hardware`, and pose arguments (`x`, `y`, `z`, `roll`, etc.). - Updated robot description and semantic description to use launch configurations. - Refactored kinematics file handling for clarity. - **Skills Launch File:** - Updated kinematics YAML loading to use `load_yaml_abs` for absolute path resolution. - Standardized `robot_description`, `robot_description_semantic`, and `kinematics` parameters. - **General Improvements:** - Consolidated redundant logic across launch files. - Improved maintainability by removing hardcoded and unused configurations. --- rbs_bringup/launch/rbs_bringup.launch.py | 4 +- rbs_bringup/launch/rbs_robot.launch.py | 121 +++++----------------- rbs_skill_servers/launch/skills.launch.py | 15 ++- 3 files changed, 33 insertions(+), 107 deletions(-) diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py index 5872087..844a50c 100644 --- a/rbs_bringup/launch/rbs_bringup.launch.py +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -21,16 +21,14 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "with_gripper": "true", - "gripper_name": "rbs_gripper", "robot_type": "rbs_arm", "description_package": "rbs_arm", "description_file": "rbs_arm_modular.xacro", "robot_name": "rbs_arm", - "use_moveit": "false", + "use_moveit": "true", "moveit_config_package": "rbs_arm", "moveit_config_file": "rbs_arm.srdf.xacro", "use_sim_time": "true", - "hardware": "gazebo", "use_controllers": "true", "scene_config_file": "", "base_link_name": "base_link", diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index adef203..5195ebd 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -23,23 +23,13 @@ from launch_ros.substitutions import FindPackageShare def launch_setup(context, *args, **kwargs): robot_type = LaunchConfiguration("robot_type") with_gripper_condition = LaunchConfiguration("with_gripper") - controllers_file = LaunchConfiguration("controllers_file") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") robot_name = LaunchConfiguration("robot_name") 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") use_controllers = LaunchConfiguration("use_controllers") - gripper_name = LaunchConfiguration("gripper_name") - x_pos = LaunchConfiguration("x") - y_pos = LaunchConfiguration("y") - z_pos = LaunchConfiguration("z") - roll = LaunchConfiguration("roll") - pitch = LaunchConfiguration("pitch") - yaw = LaunchConfiguration("yaw") namespace = LaunchConfiguration("namespace") multi_robot = LaunchConfiguration("multi_robot") robot_name = robot_name.perform(context) @@ -47,10 +37,11 @@ def launch_setup(context, *args, **kwargs): robot_type = robot_type.perform(context) description_package = description_package.perform(context) description_file = description_file.perform(context) - controllers_file = controllers_file.perform(context) multi_robot = multi_robot.perform(context) - robot_description = LaunchConfiguration("robot_description") - robot_description_semantic = LaunchConfiguration("robot_description_semantic") + robot_description_content = LaunchConfiguration("robot_description") + robot_description_semantic_content = LaunchConfiguration( + "robot_description_semantic" + ) control_space = LaunchConfiguration("control_space") control_strategy = LaunchConfiguration("control_strategy") ee_link_name = LaunchConfiguration("ee_link_name").perform(context) @@ -60,74 +51,19 @@ def launch_setup(context, *args, **kwargs): if multi_robot == "true": remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")]) - controllers_file = os.path.join( - get_package_share_directory(description_package), "config", controllers_file - ) - - if not robot_description.perform(context): - xacro_file = os.path.join( - get_package_share_directory(description_package), "urdf", description_file - ) - robot_description_doc = xacro.process_file( - xacro_file, - mappings={ - "gripper_name": gripper_name.perform(context), - "hardware": hardware.perform(context), - "simulation_controllers": controllers_file, - "namespace": namespace, - "x": x_pos.perform(context), - "y": y_pos.perform(context), - "z": z_pos.perform(context), - "roll": roll.perform(context), - "pitch": pitch.perform(context), - "yaw": yaw.perform(context), - }, - ) - - robot_description_content = robot_description_doc.toprettyxml(indent=" ") - else: - robot_description_content = robot_description.perform(context) - - robot_description = {"robot_description": robot_description_content} - - if not robot_description_semantic.perform(context): - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare(moveit_config_package), - "config/moveit", - "rbs_arm.srdf.xacro", - ] - ), - " ", - "name:=", - robot_type, - " ", - "with_gripper:=", - with_gripper_condition, - " ", - "gripper_name:=", - gripper_name, - " ", - ] - ) - else: - robot_description_semantic_content = robot_description_semantic - - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_content + robot_description = { + "robot_description": robot_description_content.perform(context) } - robot_description_kinematics = PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] + robot_description_kinematics_filepath = os.path.join( + get_package_share_directory(moveit_config_package.perform(context)), + "config", + "kinematics.yaml", ) - robot_description_kinematics = { - "robot_description_kinematics": robot_description_kinematics - } + # robot_description_kinematics = { + # "robot_description_kinematics": robot_description_kinematics + # } robot_state_publisher = Node( package="robot_state_publisher", @@ -172,13 +108,14 @@ def launch_setup(context, *args, **kwargs): ] ), launch_arguments={ - "robot_description": robot_description_content, "moveit_config_package": moveit_config_package, - "moveit_config_file": moveit_config_file, + # "moveit_config_file": moveit_config_file, "use_sim_time": use_sim_time, "tf_prefix": robot_name, "with_gripper": with_gripper_condition, + "robot_description": robot_description_content, "robot_description_semantic": robot_description_semantic_content, + "robot_description_kinematics": robot_description_kinematics_filepath, "namespace": namespace, }.items(), condition=IfCondition(use_moveit), @@ -199,7 +136,7 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "robot_description": robot_description_content, "robot_description_semantic": robot_description_semantic_content, - "robot_description_kinematics": robot_description_kinematics, + "robot_description_kinematics": robot_description_kinematics_filepath, "use_sim_time": use_sim_time, "with_gripper_condition": with_gripper_condition, "namespace": namespace, @@ -288,14 +225,14 @@ def generate_launch_description(): description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "gripper_name", - default_value="", - choices=["rbs_gripper", ""], - description="Specify gripper name (leave empty if none).", - ) - ) + # declared_arguments.append( + # DeclareLaunchArgument( + # "gripper_name", + # default_value="", + # choices=["rbs_gripper", ""], + # description="Specify gripper name (leave empty if none).", + # ) + # ) declared_arguments.append( DeclareLaunchArgument( "with_gripper", @@ -310,14 +247,6 @@ def generate_launch_description(): description="Specify if MoveIt should be launched.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "hardware", - choices=["gazebo", "mock"], - default_value="gazebo", - description="Specify the hardware interface to use (e.g., Gazebo or mock).", - ) - ) declared_arguments.append( DeclareLaunchArgument( "use_controllers", diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index 8e5a34c..aae19fa 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -4,27 +4,26 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration 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 +from rbs_launch_utils.launch_common import load_yaml, load_yaml_abs def launch_setup(context, *args, **kwargs): - robot_description_decl = LaunchConfiguration("robot_description") - robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic") - robot_description_kinematics = LaunchConfiguration("robot_description_kinematics") + robot_description_content = LaunchConfiguration("robot_description") + robot_description_semantic_content = LaunchConfiguration("robot_description_semantic") + robot_description_kinematics_filepath = LaunchConfiguration("robot_description_kinematics") use_sim_time = LaunchConfiguration("use_sim_time") use_moveit = LaunchConfiguration("use_moveit") ee_link_name = LaunchConfiguration("ee_link_name").perform(context) base_link_name = LaunchConfiguration("base_link_name").perform(context) # with_gripper_condition = LaunchConfiguration("with_gripper_condition") - robot_description = {"robot_description": robot_description_decl} + robot_description = {"robot_description": robot_description_content} robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_decl + "robot_description_semantic": robot_description_semantic_content } namespace = LaunchConfiguration("namespace") - kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml") - + kinematics_yaml = load_yaml_abs(robot_description_kinematics_filepath.perform(context)) robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} skills_container = ComposableNodeContainer(