Skip to content

Commit 3e38458

Browse files
ibrahiminfiniteibrahiminfinite
authored
[Servo] Update servo tutorials for refactor (#734)
1 parent 93675a1 commit 3e38458

9 files changed

+728
-749
lines changed

‎doc/examples/realtime_servo/CMakeLists.txt‎

Lines changed: 4 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,10 @@
1-
add_executable(servo_keyboard_input src/servo_keyboard_input.cpp)
2-
target_include_directories(servo_keyboard_input PUBLIC include)
3-
ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS})
4-
5-
add_executable(servo_cpp_interface_demo src/servo_cpp_interface_demo.cpp)
6-
target_include_directories(servo_cpp_interface_demo PUBLIC include)
7-
ament_target_dependencies(servo_cpp_interface_demo ${THIS_PACKAGE_INCLUDE_DEPENDS})
1+
add_executable(pose_tracking_tutorial src/pose_tracking_tutorial.cpp)
2+
target_include_directories(pose_tracking_tutorial PUBLIC include)
3+
ament_target_dependencies(pose_tracking_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS})
84

95
install(
106
TARGETS
11-
servo_keyboard_input
12-
servo_cpp_interface_demo
7+
pose_tracking_tutorial
138
DESTINATION
149
lib/${PROJECT_NAME}
1510
)

‎doc/examples/realtime_servo/config/demo_rviz_config.rviz‎

Lines changed: 42 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@ Panels:
66
Expanded:
77
- /Global Options1
88
- /Status1
9+
- /TF1/Frames1
10+
- /MarkerArray1
911
Splitter Ratio: 0.5
10-
Tree Height: 628
12+
Tree Height: 549
1113
- Class: rviz_common/Selection
1214
Name: Selection
1315
- Class: rviz_common/Tool Properties
@@ -21,6 +23,11 @@ Panels:
2123
- /Current View1
2224
Name: Views
2325
Splitter Ratio: 0.5
26+
- Class: rviz_common/Time
27+
Experimental: false
28+
Name: Time
29+
SyncMode: 0
30+
SyncSource: ""
2431
Visualization Manager:
2532
Class: ""
2633
Displays:
@@ -46,12 +53,12 @@ Visualization Manager:
4653
Enabled: true
4754
Move Group Namespace: ""
4855
Name: PlanningScene
49-
Planning Scene Topic: /moveit_servo/publish_planning_scene
56+
Planning Scene Topic: /servo_node/publish_planning_scene
5057
Robot Description: robot_description
5158
Scene Geometry:
5259
Scene Alpha: 0.8999999761581421
5360
Scene Color: 50; 230; 50
54-
Scene Display Time: 0.20000000298023224
61+
Scene Display Time: 0.009999999776482582
5562
Show Scene Geometry: true
5663
Voxel Coloring: Z-Axis
5764
Voxel Rendering: Occupied Voxels
@@ -136,7 +143,7 @@ Visualization Manager:
136143
panda_leftfinger:
137144
Value: false
138145
panda_link0:
139-
Value: false
146+
Value: true
140147
panda_link1:
141148
Value: false
142149
panda_link2:
@@ -156,7 +163,7 @@ Visualization Manager:
156163
panda_rightfinger:
157164
Value: false
158165
world:
159-
Value: true
166+
Value: false
160167
Marker Scale: 1
161168
Name: TF
162169
Show Arrows: true
@@ -180,6 +187,18 @@ Visualization Manager:
180187
{}
181188
Update Interval: 0
182189
Value: true
190+
- Class: rviz_default_plugins/MarkerArray
191+
Enabled: true
192+
Name: MarkerArray
193+
Namespaces:
194+
{}
195+
Topic:
196+
Depth: 5
197+
Durability Policy: Volatile
198+
History Policy: Keep Last
199+
Reliability Policy: Reliable
200+
Value: /path_markers
201+
Value: true
183202
Enabled: true
184203
Global Options:
185204
Background Color: 48; 48; 48
@@ -195,6 +214,9 @@ Visualization Manager:
195214
- Class: rviz_default_plugins/Measure
196215
Line color: 128; 128; 0
197216
- Class: rviz_default_plugins/SetInitialPose
217+
Covariance x: 0.25
218+
Covariance y: 0.25
219+
Covariance yaw: 0.06853891909122467
198220
Topic:
199221
Depth: 5
200222
Durability Policy: Volatile
@@ -223,39 +245,41 @@ Visualization Manager:
223245
Views:
224246
Current:
225247
Class: rviz_default_plugins/Orbit
226-
Distance: 2.155569553375244
248+
Distance: 4.086756229400635
227249
Enable Stereo Rendering:
228250
Stereo Eye Separation: 0.05999999865889549
229251
Stereo Focal Distance: 1
230252
Swap Stereo Eyes: false
231253
Value: false
232254
Focal Point:
233-
X: -0.08608254045248032
234-
Y: -0.20677587389945984
235-
Z: 0.3424459993839264
255+
X: 0
256+
Y: 0
257+
Z: 0
236258
Focal Shape Fixed Size: true
237259
Focal Shape Size: 0.05000000074505806
238260
Invert Z Axis: false
239261
Name: Current View
240262
Near Clip Distance: 0.009999999776482582
241-
Pitch: 0.4603978991508484
263+
Pitch: 0.785398006439209
242264
Target Frame: <Fixed Frame>
243265
Value: Orbit (rviz)
244-
Yaw: 0.8753982782363892
266+
Yaw: 0.785398006439209
245267
Saved: ~
246268
Window Geometry:
247269
Displays:
248270
collapsed: false
249-
Height: 857
271+
Height: 846
250272
Hide Left Dock: false
251-
Hide Right Dock: true
252-
QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
273+
Hide Right Dock: false
274+
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
253275
Selection:
254276
collapsed: false
277+
Time:
278+
collapsed: false
255279
Tool Properties:
256280
collapsed: false
257281
Views:
258-
collapsed: true
259-
Width: 1586
260-
X: 1179
261-
Y: 393
282+
collapsed: false
283+
Width: 1200
284+
X: 74
285+
Y: 60

‎doc/examples/realtime_servo/config/panda_simulated_config.yaml‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
###############################################
2+
# Modify all parameters related to servoing here
3+
###############################################
4+
5+
# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
6+
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
7+
8+
## Properties of outgoing commands
9+
publish_period: 0.034 # 1/Nominal publish rate [seconds]
10+
11+
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
12+
scale:
13+
# Scale parameters are only used if command_in_type=="unitless"
14+
linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
15+
rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
16+
# Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
17+
joint: 0.5
18+
19+
# What type of topic does your robot driver expect?
20+
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
21+
command_out_type: trajectory_msgs/JointTrajectory
22+
23+
# What to publish? Can save some bandwidth as most robots only require positions or velocities
24+
publish_joint_positions: true
25+
publish_joint_velocities: true
26+
publish_joint_accelerations: false
27+
28+
## Plugins for smoothing outgoing commands
29+
use_smoothing: true
30+
smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
31+
32+
# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
33+
# which other nodes can use as a source for information about the planning environment.
34+
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
35+
# then is_primary_planning_scene_monitor needs to be set to false.
36+
is_primary_planning_scene_monitor: true
37+
38+
## MoveIt properties
39+
move_group_name: panda_arm # Often 'manipulator' or 'arm'
40+
planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world'
41+
42+
## Other frames
43+
ee_frame: panda_hand # The name of the end effector link, used to return the EE pose
44+
45+
## Configure handling of singularities and joint limits
46+
lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
47+
hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
48+
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
49+
leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity.
50+
51+
## Topic names
52+
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
53+
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
54+
joint_topic: /joint_states
55+
status_topic: ~/status # Publish status to this topic
56+
command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
57+
58+
## Collision checking for the entire robot body
59+
check_collisions: true # Check collisions?
60+
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
61+
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
62+
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

‎doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py‎

Lines changed: 152 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
import os
2+
import launch
3+
import launch_ros
4+
from ament_index_python.packages import get_package_share_directory
5+
from launch.conditions import IfCondition, UnlessCondition
6+
from launch.substitutions import LaunchConfiguration
7+
from launch_param_builder import ParameterBuilder
8+
from moveit_configs_utils import MoveItConfigsBuilder
9+
10+
11+
def generate_launch_description():
12+
moveit_config = (
13+
MoveItConfigsBuilder("moveit_resources_panda")
14+
.robot_description(file_path="config/panda.urdf.xacro")
15+
.to_moveit_configs()
16+
)
17+
18+
# Launch Servo as a standalone node or as a "node component" for better latency/efficiency
19+
launch_as_standalone_node = LaunchConfiguration(
20+
"launch_as_standalone_node", default="false"
21+
)
22+
23+
# Get parameters for the Servo node
24+
servo_params = {
25+
"moveit_servo": ParameterBuilder("moveit2_tutorials")
26+
.yaml("config/panda_simulated_config.yaml")
27+
.to_dict()
28+
}
29+
30+
# This filter parameter should be >1. Increase it for greater smoothing but slower motion.
31+
low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
32+
33+
# RViz
34+
rviz_config_file = (
35+
get_package_share_directory("moveit2_tutorials")
36+
+ "/config/demo_rviz_config.rviz"
37+
)
38+
rviz_node = launch_ros.actions.Node(
39+
package="rviz2",
40+
executable="rviz2",
41+
name="rviz2",
42+
output="log",
43+
arguments=["-d", rviz_config_file],
44+
parameters=[
45+
moveit_config.robot_description,
46+
moveit_config.robot_description_semantic,
47+
],
48+
)
49+
50+
# ros2_control using FakeSystem as hardware
51+
ros2_controllers_path = os.path.join(
52+
get_package_share_directory("moveit_resources_panda_moveit_config"),
53+
"config",
54+
"ros2_controllers.yaml",
55+
)
56+
ros2_control_node = launch_ros.actions.Node(
57+
package="controller_manager",
58+
executable="ros2_control_node",
59+
parameters=[moveit_config.robot_description, ros2_controllers_path],
60+
output="screen",
61+
)
62+
63+
joint_state_broadcaster_spawner = launch_ros.actions.Node(
64+
package="controller_manager",
65+
executable="spawner",
66+
arguments=[
67+
"joint_state_broadcaster",
68+
"--controller-manager-timeout",
69+
"300",
70+
"--controller-manager",
71+
"/controller_manager",
72+
],
73+
)
74+
75+
panda_arm_controller_spawner = launch_ros.actions.Node(
76+
package="controller_manager",
77+
executable="spawner",
78+
arguments=["panda_arm_controller", "-c", "/controller_manager"],
79+
)
80+
81+
# Launch as much as possible in components
82+
container = launch_ros.actions.ComposableNodeContainer(
83+
name="moveit_servo_demo_container",
84+
namespace="/",
85+
package="rclcpp_components",
86+
executable="component_container_mt",
87+
composable_node_descriptions=[
88+
# Example of launching Servo as a node component
89+
# Launching as a node component makes ROS 2 intraprocess communication more efficient.
90+
launch_ros.descriptions.ComposableNode(
91+
package="moveit_servo",
92+
plugin="moveit_servo::ServoNode",
93+
name="servo_node",
94+
parameters=[
95+
servo_params,
96+
low_pass_filter_coeff,
97+
moveit_config.robot_description,
98+
moveit_config.robot_description_semantic,
99+
moveit_config.robot_description_kinematics,
100+
],
101+
condition=UnlessCondition(launch_as_standalone_node),
102+
),
103+
launch_ros.descriptions.ComposableNode(
104+
package="robot_state_publisher",
105+
plugin="robot_state_publisher::RobotStatePublisher",
106+
name="robot_state_publisher",
107+
parameters=[moveit_config.robot_description],
108+
),
109+
launch_ros.descriptions.ComposableNode(
110+
package="tf2_ros",
111+
plugin="tf2_ros::StaticTransformBroadcasterNode",
112+
name="static_tf2_broadcaster",
113+
parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
114+
),
115+
],
116+
output="screen",
117+
)
118+
# Launch a standalone Servo node.
119+
# As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
120+
servo_node = launch_ros.actions.Node(
121+
package="moveit_servo",
122+
executable="servo_node",
123+
name="servo_node",
124+
parameters=[
125+
servo_params,
126+
low_pass_filter_coeff,
127+
moveit_config.robot_description,
128+
moveit_config.robot_description_semantic,
129+
moveit_config.robot_description_kinematics,
130+
],
131+
output="screen",
132+
condition=IfCondition(launch_as_standalone_node),
133+
)
134+
135+
demo_node = launch_ros.actions.Node(
136+
package="moveit2_tutorials",
137+
executable="pose_tracking_tutorial",
138+
name="pose_tracking_tutorial",
139+
output="screen",
140+
)
141+
142+
return launch.LaunchDescription(
143+
[
144+
rviz_node,
145+
ros2_control_node,
146+
joint_state_broadcaster_spawner,
147+
panda_arm_controller_spawner,
148+
servo_node,
149+
container,
150+
launch.actions.TimerAction(period=10.0, actions=[demo_node]),
151+
]
152+
)

0 commit comments

Comments
 (0)