Skip to content

Commit e9eaaba

Browse files
mhubiimhubii
committed
added moveit servo support (#50, #211)
1 parent d5cef2e commit e9eaaba

File tree

5 files changed

+197
-8
lines changed

5 files changed

+197
-8
lines changed

‎lbr_bringup/config/moveit_servo.yaml

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
# Please do e.g. refer to
2+
# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml
3+
# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml
4+
/**/servo_node:
5+
ros__parameters:
6+
moveit_servo:
7+
## Properties of incoming commands
8+
command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
9+
scale:
10+
# Scale parameters are only used if command_in_type=="unitless"
11+
linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
12+
rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
13+
# Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
14+
joint: 0.5
15+
16+
# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
17+
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
18+
19+
## Properties of outgoing commands
20+
publish_period: 0.034 # 1/Nominal publish rate [seconds]
21+
low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
22+
23+
# What type of topic does your robot driver expect?
24+
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
25+
command_out_type: trajectory_msgs/JointTrajectory
26+
27+
# What to publish? Can save some bandwidth as most robots only require positions or velocities
28+
publish_joint_positions: true
29+
publish_joint_velocities: true
30+
publish_joint_accelerations: false
31+
32+
## Plugins for smoothing outgoing commands
33+
smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
34+
35+
# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
36+
# which other nodes can use as a source for information about the planning environment.
37+
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
38+
# then is_primary_planning_scene_monitor needs to be set to false.
39+
is_primary_planning_scene_monitor: true
40+
41+
## MoveIt properties
42+
move_group_name: arm # Often 'manipulator' or 'arm'
43+
planning_frame: link_0 # The MoveIt planning frame. Often 'base_link' or 'world'
44+
45+
## Other frames
46+
ee_frame_name: link_ee # The name of the end effector link, used to return the EE pose
47+
robot_link_command_frame: link_0 # commands must be given in the frame of a robot link. Usually either the base or end effector
48+
49+
## Stopping behaviour
50+
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
51+
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
52+
# Important because ROS may drop some messages and we need the robot to halt reliably.
53+
num_outgoing_halt_msgs_to_publish: 4
54+
55+
## Configure handling of singularities and joint limits
56+
lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
57+
hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
58+
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
59+
leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
60+
61+
## Topic names
62+
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
63+
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
64+
joint_topic: /lbr/joint_states
65+
status_topic: ~/status # Publish status to this topic
66+
command_out_topic: /lbr/joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
67+
68+
## Collision checking for the entire robot body
69+
check_collisions: true # Check collisions?
70+
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
71+
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
72+
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

‎lbr_bringup/launch/move_group.launch.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
from launch.conditions import IfCondition
66
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
77
from lbr_bringup.description import LBRDescriptionMixin
8-
from lbr_bringup.move_group import LBRMoveGroupMixin
8+
from lbr_bringup.moveit import LBRMoveGroupMixin
99
from lbr_bringup.rviz import RVizMixin
1010

1111

‎lbr_bringup/launch/moveit_servo.launch.py

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
from typing import List
2+
3+
from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
4+
from launch.actions import OpaqueFunction, RegisterEventHandler
5+
from launch.conditions import IfCondition
6+
from launch.event_handlers import OnProcessStart
7+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
8+
from launch_ros.substitutions import FindPackageShare
9+
from lbr_bringup.description import LBRDescriptionMixin
10+
from lbr_bringup.moveit import LBRMoveGroupMixin, LBRMoveItServoMixin
11+
12+
13+
def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
14+
ld = LaunchDescription()
15+
16+
moveit_servo_config = PathJoinSubstitution(
17+
[FindPackageShare("lbr_bringup"), "config/moveit_servo.yaml"]
18+
)
19+
model = LaunchConfiguration("model").perform(context)
20+
moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
21+
robot_name=model,
22+
package_name=f"{model}_moveit_config",
23+
).to_moveit_configs()
24+
25+
mode = LaunchConfiguration("mode").perform(context)
26+
use_sim_time = False
27+
if mode == "gazebo":
28+
use_sim_time = True
29+
30+
# moveit servo node
31+
servo_node = LBRMoveItServoMixin.node_moveit_servo(
32+
parameters=[
33+
moveit_configs.robot_description_kinematics,
34+
moveit_configs.robot_description_semantic,
35+
{"use_sim_time": use_sim_time},
36+
moveit_servo_config,
37+
{
38+
"moveit_servo.use_gazebo": mode
39+
== "gazebo", # we configure this parameter dynamically
40+
},
41+
],
42+
)
43+
ld.add_action(servo_node)
44+
45+
# call start servo after servo node start
46+
ld.add_action(
47+
RegisterEventHandler(
48+
OnProcessStart(
49+
target_action=servo_node,
50+
on_start=[
51+
LBRMoveItServoMixin.call_start_servo_service(
52+
condition=IfCondition(
53+
LaunchConfiguration("default_enable_servo")
54+
)
55+
)
56+
],
57+
),
58+
)
59+
)
60+
61+
return ld.entities
62+
63+
64+
def generate_launch_description() -> LaunchDescription:
65+
ld = LaunchDescription()
66+
67+
ld.add_action(LBRDescriptionMixin.arg_mode())
68+
ld.add_action(LBRDescriptionMixin.arg_model())
69+
ld.add_action(LBRMoveItServoMixin.arg_default_enable_servo())
70+
71+
ld.add_action(OpaqueFunction(function=hidden_setup))
72+
return ld

‎lbr_bringup/lbr_bringup/move_group.py renamed to ‎lbr_bringup/lbr_bringup/moveit.py

+50-7
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,17 @@
11
import os
2-
from typing import Any, Dict, List
2+
from typing import Any, Dict, List, Optional, Union
33

44
from ament_index_python import get_package_share_directory
5-
from launch.actions import DeclareLaunchArgument
6-
from launch.substitutions import LaunchConfiguration
5+
from launch.actions import DeclareLaunchArgument, ExecuteProcess
6+
from launch.substitutions import (
7+
FindExecutable,
8+
LaunchConfiguration,
9+
PathJoinSubstitution,
10+
)
711
from launch_ros.actions import Node
812
from launch_ros.parameter_descriptions import ParameterValue
913
from moveit_configs_utils import MoveItConfigs, MoveItConfigsBuilder
1014

11-
# NOTE TO SELF:
12-
# due to individual moveit configs, put mixins into lbr_bringup rather than lbr_moveit_config
13-
# most of the configs are taken from Python package moveit_configs_utils.launches
14-
1515

1616
class LBRMoveGroupMixin:
1717
@staticmethod
@@ -118,3 +118,46 @@ def node_move_group(**kwargs) -> Node:
118118
output="screen",
119119
**kwargs,
120120
)
121+
122+
123+
class LBRMoveItServoMixin:
124+
@staticmethod
125+
def arg_default_enable_servo() -> DeclareLaunchArgument:
126+
return DeclareLaunchArgument(
127+
name="default_enable_servo",
128+
default_value="true",
129+
description="Whether to enable the servo node by default.",
130+
)
131+
132+
@staticmethod
133+
def node_moveit_servo(
134+
robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
135+
"robot_name", default="lbr"
136+
),
137+
**kwargs,
138+
) -> Node:
139+
return Node(
140+
package="moveit_servo",
141+
executable="servo_node_main",
142+
output="screen",
143+
namespace=robot_name,
144+
**kwargs,
145+
)
146+
147+
@staticmethod
148+
def call_start_servo_service(
149+
robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
150+
"robot_name", default="lbr"
151+
),
152+
**kwargs,
153+
) -> ExecuteProcess:
154+
return ExecuteProcess(
155+
cmd=[
156+
FindExecutable(name="ros2"),
157+
"service",
158+
"call",
159+
PathJoinSubstitution([robot_name, "servo_node/start_servo"]),
160+
"std_srvs/srv/Trigger",
161+
],
162+
**kwargs,
163+
)

‎lbr_bringup/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717
<exec_depend>lbr_description</exec_depend>
1818
<exec_depend>lbr_fri_ros2</exec_depend>
1919
<exec_depend>lbr_ros2_control</exec_depend>
20+
<exec_depend>moveit_ros_move_group</exec_depend>
21+
<exec_depend>moveit_servo</exec_depend>
2022
<exec_depend>rclpy</exec_depend>
2123
<exec_depend>robot_state_publisher</exec_depend>
2224
<exec_depend>ros_gz_sim</exec_depend>

0 commit comments

Comments
 (0)