Added servo node

ur_moveit_config/config/ur_servo.yaml

@@ -4,11 +4,11 @@
 use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
-command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+command_in_type: "speed_units"  # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear:  0.6  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  linear: 0.6  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.3  # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
   joint: 0.01
 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
@@ -32,51 +32,55 @@ publish_joint_accelerations: false
 ## Plugins for smoothing outgoing commands
 smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
 
-# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
-# which other nodes can use as a source for information about the planning environment.
-# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
-# then is_primary_planning_scene_monitor needs to be set to false.
+# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises
+# the /get_planning_scene service, which other nodes can use as a source for information about the
+# planning environment. NOTE: If a different node in your system is responsible for the "primary"
+# planning scene instance (e.g. the MoveGroup node), then is_primary_planning_scene_monitor needs
+# to be set to false.
 is_primary_planning_scene_monitor: false
 
 ## Incoming Joint State properties
 low_pass_filter_coeff: 10.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
-move_group_name:  ur_manipulator  # Often 'manipulator' or 'arm'
+move_group_name: ur_manipulator  # Often 'manipulator' or 'arm'
 planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
 ee_frame: tool0  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame:  tool0  # commands must be given in the frame of a robot link. Usually either the base or end effector
+robot_link_command_frame: tool0  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
-incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
 # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
 # Important because ROS may drop some messages and we need the robot to halt reliably.
 num_outgoing_halt_msgs_to_publish: 4
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
-joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+lower_singularity_threshold: 100.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 200.0  # Stop when the condition number hits this
+
+# added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
 
 ## Topic names
 cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
-joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_command_in_topic: ~/delta_joint_cmds  # Topic for incoming joint angle commands
 joint_topic: /joint_states
-status_topic: ~/status # Publish status to this topic
-command_out_topic: /forward_position_controller/commands # Publish outgoing commands here
+status_topic: ~/status  # Publish status to this topic
+command_out_topic: /forward_position_controller/commands  # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
-collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
-# Two collision check algorithms are available:
-# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
-# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+check_collisions: true  # Check collisions?
+collision_check_rate: 5.0  # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available: "threshold_distance" begins slowing down when
+# nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the
+# distance is decreasing. Requires joint acceleration limits
 collision_check_type: threshold_distance
 # Parameters for "threshold_distance"-type collision checking
-self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
-scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
+self_collision_proximity_threshold: 0.01  # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02  # Start decelerating when a scene collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
-collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
-min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
+collision_distance_safety_factor: 1000.0  # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.01  # Stop if a collision is closer than this [m]

ur_moveit_config/launch/ur_moveit.launch.py

@@ -29,6 +29,7 @@
 #
 # Author: Felix Exner
 import os
+import yaml
 
 from pathlib import Path
 
@@ -46,6 +47,19 @@
 
 from moveit_configs_utils import MoveItConfigsBuilder
 
+from ament_index_python.packages import get_package_share_directory
+
+
+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) as file:
+            return yaml.safe_load(file)
+    except OSError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
 
 def declare_arguments():
     return LaunchDescription(
@@ -71,6 +85,9 @@ def declare_arguments():
                 default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
                 description="Path where the warehouse database should be stored",
             ),
+            DeclareLaunchArgument(
+                "launch_servo", default_value="true", description="Launch Servo?"
+            ),
         ]
     )
 
@@ -79,6 +96,7 @@ def generate_launch_description():
     launch_rviz = LaunchConfiguration("launch_rviz")
     ur_type = LaunchConfiguration("ur_type")
     warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
+    launch_servo = LaunchConfiguration("launch_servo")
 
     moveit_config = (
         MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config")
@@ -111,6 +129,19 @@ def generate_launch_description():
         ],
     )
 
+    servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml")
+    servo_params = {"moveit_servo": servo_yaml}
+    servo_node = Node(
+        package="moveit_servo",
+        condition=IfCondition(launch_servo),
+        executable="servo_node",
+        parameters=[
+            moveit_config.to_dict(),
+            servo_params,
+        ],
+        output="screen",
+    )
+
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("ur_moveit_config"), "config", "moveit.rviz"]
     )
@@ -134,7 +165,8 @@ def generate_launch_description():
     ld.add_action(
         RegisterEventHandler(
             OnProcessExit(
-                target_action=wait_robot_description, on_exit=[move_group_node, rviz_node]
+                target_action=wait_robot_description,
+                on_exit=[move_group_node, rviz_node, servo_node],
             )
         ),
     )