From 5fe4345915e7e044ea4aaab634597741912da9a7 Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Tue, 3 Mar 2026 16:25:09 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0servo=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/moveit_servo.yaml | 49 ++++++++++ .../config/moveit_servo_left.yaml | 19 ++++ .../config/moveit_servo_right.yaml | 17 ++++ .../launch/demo.launch.py | 89 +++++++++++++++++++ 4 files changed, 174 insertions(+) create mode 100644 openarm_bimanual_moveit_config/config/moveit_servo.yaml create mode 100644 openarm_bimanual_moveit_config/config/moveit_servo_left.yaml create mode 100644 openarm_bimanual_moveit_config/config/moveit_servo_right.yaml diff --git a/openarm_bimanual_moveit_config/config/moveit_servo.yaml b/openarm_bimanual_moveit_config/config/moveit_servo.yaml new file mode 100644 index 0000000..b7f372d --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit_servo.yaml @@ -0,0 +1,49 @@ +left_servo_node: + ros__parameters: + move_group_name: left_arm + group_name: left_arm + command_in_type: twist + enable_servo: true + + scale: + linear: 0.3 + angular: 0.3 + + publish_period: 0.01 + + command_out_type: trajectory + trajectory_out_topic: left_joint_trajectory_controller/joint_trajectory + + planning_frame: openarm_body_link0 + ee_frame_name: openarm_left_link7 + robot_link_command_frame: openarm_left_link7 + + incoming_command_timeout: 0.1 + num_outgoing_halt_msgs_to_publish: 4 + + use_gazebo: false + +right_servo_node: + ros__parameters: + move_group_name: right_arm + group_name: right_arm + command_in_type: twist + enable_servo: true + + scale: + linear: 0.3 + angular: 0.3 + + publish_period: 0.01 + + command_out_type: trajectory + trajectory_out_topic: right_joint_trajectory_controller/joint_trajectory + + planning_frame: openarm_body_link0 + ee_frame_name: openarm_right_link7 + robot_link_command_frame: openarm_right_link7 + + incoming_command_timeout: 0.1 + num_outgoing_halt_msgs_to_publish: 4 + + use_gazebo: false diff --git a/openarm_bimanual_moveit_config/config/moveit_servo_left.yaml b/openarm_bimanual_moveit_config/config/moveit_servo_left.yaml new file mode 100644 index 0000000..4bba2ed --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit_servo_left.yaml @@ -0,0 +1,19 @@ +move_group_name: left_arm +group_name: left_arm +command_in_type: unitless +enable_servo: true +scale: + linear: 1.0 + angular: 1.0 +publish_period: 0.01 +command_out_type: trajectory_msgs/JointTrajectory +# Ensure this matches the controller name loaded by the controller_manager +# The controllers file in openarm_bringup defines "left_joint_trajectory_controller" +trajectory_out_topic: /left_joint_trajectory_controller/joint_trajectory +planning_frame: openarm_body_link0 +ee_frame_name: openarm_left_link7 +robot_link_command_frame: openarm_left_link7 +command_out_topic: /left_joint_trajectory_controller/joint_trajectory +incoming_command_timeout: 1.0 +num_outgoing_halt_msgs_to_publish: 4 +use_gazebo: false \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/moveit_servo_right.yaml b/openarm_bimanual_moveit_config/config/moveit_servo_right.yaml new file mode 100644 index 0000000..fe981e1 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit_servo_right.yaml @@ -0,0 +1,17 @@ +move_group_name: right_arm +group_name: right_arm +command_in_type: unitless +enable_servo: true +scale: + linear: 0.3 + angular: 0.3 +publish_period: 0.01 +command_out_type: trajectory_msgs/JointTrajectory +# Use the namespaced trajectory topic matching the controllers loaded by the bringup +trajectory_out_topic: right_joint_trajectory_controller/joint_trajectory +planning_frame: openarm_body_link0 +ee_frame_name: openarm_right_link7 +robot_link_command_frame: openarm_right_link7 +incoming_command_timeout: 0.1 +num_outgoing_halt_msgs_to_publish: 4 +use_gazebo: false diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py index 0b2ebfe..0725904 100644 --- a/openarm_bimanual_moveit_config/launch/demo.launch.py +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -23,6 +23,7 @@ from launch.actions import ( TimerAction, OpaqueFunction, ) +from launch.conditions import IfCondition from launch.substitutions import ( LaunchConfiguration, PathJoinSubstitution, @@ -30,6 +31,7 @@ from launch.substitutions import ( from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder +import yaml def generate_robot_description( @@ -138,6 +140,71 @@ def controller_spawner(context: LaunchContext, robot_controller): ] +def servo_nodes_spawner( + context: LaunchContext, + description_package, + description_file, + arm_type, + use_fake_hardware, + right_can_interface, + left_can_interface, + arm_prefix, + use_servo, +): + robot_description = generate_robot_description( + context, + description_package, + description_file, + arm_type, + use_fake_hardware, + right_can_interface, + left_can_interface, + arm_prefix, + ) + print("description_file: ", context.perform_substitution(description_file)) + robot_description_param = {"robot_description": robot_description} + + robot_description_semantic_path = os.path.join( + get_package_share_directory("openarm_bimanual_moveit_config"), + "config", + "openarm_bimanual.srdf", + ) + robot_description_semantic = { + "robot_description_semantic": open(robot_description_semantic_path).read() + } + + left_servo_yaml = load_yaml( + "openarm_bimanual_moveit_config", "config/moveit_servo_left.yaml" + ) + servo_params = {"moveit_servo": left_servo_yaml} + + left_servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + name="left_servo_node", + output={"stdout": "screen", + "stderr": "screen", + }, + parameters=[ + servo_params, + robot_description_param, + robot_description_semantic, + ], + condition=IfCondition(use_servo), + ) + + return [left_servo_node] + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + def generate_launch_description(): declared_arguments = [ DeclareLaunchArgument( @@ -166,6 +233,11 @@ def generate_launch_description(): "controllers_file", default_value="openarm_v10_bimanual_controllers.yaml", ), + DeclareLaunchArgument( + "use_servo", + default_value="true", + choices=["true", "false"], + ), ] description_package = LaunchConfiguration("description_package") @@ -178,6 +250,7 @@ def generate_launch_description(): right_can_interface = LaunchConfiguration("right_can_interface") left_can_interface = LaunchConfiguration("left_can_interface") arm_prefix = LaunchConfiguration("arm_prefix") + use_servo = LaunchConfiguration("use_servo") controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", @@ -247,6 +320,21 @@ def generate_launch_description(): parameters=[moveit_params], ) + servo_nodes_spawner_func = OpaqueFunction( + function=servo_nodes_spawner, + args=[ + description_package, + description_file, + arm_type, + use_fake_hardware, + right_can_interface, + left_can_interface, + arm_prefix, + use_servo, + ], + ) + delayed_servo = TimerAction(period=4.0, actions=[servo_nodes_spawner_func]) + return LaunchDescription( declared_arguments + [ @@ -256,5 +344,6 @@ def generate_launch_description(): delayed_gripper, run_move_group_node, rviz_node, + delayed_servo, ] )