Fix Servo pose tracking tutorial (#973)

Author: sea-bass

doc/examples/realtime_servo/config/panda_simulated_config.yaml

@@ -6,13 +6,14 @@
 # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
 
 ## Properties of outgoing commands
-publish_period: 0.034  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]
 
-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.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  linear:  0.2  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.8  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
   joint: 0.5
 
@@ -27,36 +28,36 @@ publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
 
 # 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: true
+check_octomap_collisions: false  # Check collision against the octomap (if a 3D sensor plugin is available)
 
 ## MoveIt properties
-move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
-planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
-
-## Other frames
-ee_frame: panda_hand  # The name of the end effector link, used to return the EE pose
+move_group_name: panda_arm  # Often 'manipulator' or 'arm'
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
-hard_stop_singularity_threshold: 30.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.
-leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity.
+lower_singularity_threshold: 30.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 50.0  # Stop when the condition number hits this
+leaving_singularity_threshold_multiplier: 2.0  # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
+# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
+# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
+# If moving quickly, make these values larger.
+joint_limit_margins: [0.12]
 
 ## 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_topic: /joint_states
-status_topic: ~/status # Publish status to this topic
-command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
+joint_command_in_topic: ~/delta_joint_cmds  # Topic for incoming joint angle commands
+joint_topic: /joint_states  # Get joint states from this tpoic
+status_topic: ~/status  # Publish status to this topic
+command_out_topic: /panda_arm_controller/joint_trajectory  # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
-collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
-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]
+check_collisions: true  # Check collisions?
+collision_check_rate: 10.0  # [Hz] Collision-checking can easily bog down a CPU if done too often.
+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]

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

@@ -12,6 +12,8 @@ def generate_launch_description():
     moveit_config = (
         MoveItConfigsBuilder("moveit_resources_panda")
         .robot_description(file_path="config/panda.urdf.xacro")
+        .joint_limits(file_path="config/hard_joint_limits.yaml")
+        .robot_description_kinematics()
         .to_moveit_configs()
     )
 
@@ -27,8 +29,9 @@ def generate_launch_description():
         .to_dict()
     }
 
-    # This filter parameter should be >1. Increase it for greater smoothing but slower motion.
-    low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
+    # This sets the update rate and planning group name for the acceleration limiting filter.
+    acceleration_filter_update_period = {"update_period": 0.01}
+    planning_group_name = {"planning_group_name": "panda_arm"}
 
     # RViz
     rviz_config_file = (
@@ -96,10 +99,12 @@ def generate_launch_description():
                 name="servo_node",
                 parameters=[
                     servo_params,
-                    low_pass_filter_coeff,
+                    acceleration_filter_update_period,
+                    planning_group_name,
                     moveit_config.robot_description,
                     moveit_config.robot_description_semantic,
                     moveit_config.robot_description_kinematics,
+                    moveit_config.joint_limits,
                 ],
                 condition=UnlessCondition(launch_as_standalone_node),
             ),
@@ -126,10 +131,12 @@ def generate_launch_description():
         name="servo_node",
         parameters=[
             servo_params,
-            low_pass_filter_coeff,
+            acceleration_filter_update_period,
+            planning_group_name,
             moveit_config.robot_description,
             moveit_config.robot_description_semantic,
             moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
         ],
         output="screen",
         condition=IfCondition(launch_as_standalone_node),
@@ -150,6 +157,6 @@ def generate_launch_description():
             panda_arm_controller_spawner,
             servo_node,
             container,
-            launch.actions.TimerAction(period=10.0, actions=[demo_node]),
+            launch.actions.TimerAction(period=8.0, actions=[demo_node]),
         ]
     )

doc/examples/realtime_servo/realtime_servo_tutorial.rst

@@ -200,9 +200,11 @@ The user can use status for higher-level decision making.
 See :moveit_codedir:`moveit_servo/demos <moveit_ros/moveit_servo/demos/cpp_interface>` for complete examples of using the C++ interface.
 The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch <moveit_ros/moveit_servo/launch>`.
 
-    - ``ros2 launch moveit_servo demo_joint_jog.launch.py``
-    - ``ros2 launch moveit_servo demo_twist.launch.py``
-    - ``ros2 launch moveit_servo demo_pose.launch.py``
+.. code-block:: bash
+
+    ros2 launch moveit_servo demo_joint_jog.launch.py
+    ros2 launch moveit_servo demo_twist.launch.py
+    ros2 launch moveit_servo demo_pose.launch.py
 
 
 Using the ROS API
@@ -222,7 +224,11 @@ selected using the *command_out_type* parameter, and published on the topic spec
 
 The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType <srv/ServoCommandType.srv>`.
 
-From cli : ``ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"``
+From the CLI:
+
+.. code-block:: bash
+
+    ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"
 
 Programmatically:
 
@@ -248,10 +254,22 @@ Similarly, servoing can be paused using the pause service ``<node_name>/pause_se
 
 When using the ROS interface, the status of Servo is available on the topic ``/<node_name>/status``, see definition :moveit_msgs_codedir:`ServoStatus <msg/ServoStatus.msg>`.
 
-Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``.
+Launch ROS interface demo:
+
+.. code-block:: bash
+
+    ros2 launch moveit_servo demo_ros_api.launch.py
 
 Once the demo is running, the robot can be teleoperated through the keyboard.
 
-Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``.
+Launch the keyboard demo:
+
+.. code-block:: bash
+
+    ros2 run moveit_servo servo_keyboard_input
 
 An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example <examples/realtime_servo/src/pose_tracking_tutorial.cpp>`.
+
+.. code-block:: bash
+
+    ros2 launch moveit2_tutorials pose_tracking_tutorial.launch.py

doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp

@@ -208,7 +208,7 @@ int main(int argc, char* argv[])
   executor->add_node(node);
 
   // Spin the node.
-  std::thread executor_thread([&]() { executor->spin(); });
+  std::thread executor_thread([&executor]() { executor->spin(); });
 
   visualization_msgs::msg::MarkerArray marray;
   std::vector<Eigen::Vector3d> path = getPath();
@@ -231,20 +231,25 @@ int main(int argc, char* argv[])
   // Create the ROS message.
   auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
   request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE;
-  auto response = switch_input_client->async_send_request(request);
+  auto response_future = switch_input_client->async_send_request(request);
+  if (response_future.wait_for(std::chrono::duration<double>(3.0)) == std::future_status::timeout)
+  {
+    RCLCPP_ERROR_STREAM(node->get_logger(), "Timed out waiting for MoveIt servo command switching request.");
+  }
+  const auto response = response_future.get();
   if (response.get()->success)
   {
-    RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Pose");
+    RCLCPP_INFO_STREAM(node->get_logger(), "Switched to command input type: Pose");
   }
   else
   {
-    RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Pose");
+    RCLCPP_ERROR_STREAM(node->get_logger(), "Could not switch MoveIt servo command input type.");
   }
 
   // Follow the trajectory
 
   const double publish_period = 0.15;
-  rclcpp::WallRate rate(1 / publish_period);
+  rclcpp::WallRate rate(1.0 / publish_period);
 
   // The path needs to be reversed since the last point in the path is where we want to start.
   std::reverse(path.begin(), path.end());