增加servo控制

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

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

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

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,
         ]
     )