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