Add animated visualization of plan.

Signed-off-by: Jelmer de Wolde <[email protected]>

ros2_ws/src/rcdt_launch/rcdt_launch/moveit.py

@@ -98,6 +98,9 @@ def add_prefix_in_robot_description_semantic(description: dict, prefix: str) ->
     xml_dict = xmltodict.parse(description["robot_description_semantic"])
     ee_parent_link = xml_dict["robot"]["end_effector"]["@parent_link"]
     xml_dict["robot"]["end_effector"]["@parent_link"] = f"{prefix}/{ee_parent_link}"
+    for group in xml_dict["robot"]["group"]:
+        if "link" in group:
+            group["link"]["@name"] = f"{prefix}/{group['link']['@name']}"
     for disable_collision in xml_dict["robot"]["disable_collisions"]:
         link1 = disable_collision["@link1"]
         link2 = disable_collision["@link2"]

ros2_ws/src/rcdt_launch/rcdt_launch/robot.py

@@ -794,8 +794,7 @@ def __init__(  # noqa: PLR0913
             Rviz.add_motion_planning_plugin(self.namespace)
             Rviz.add_planning_scene(self.namespace)
             Rviz.add_robot_state(self.namespace)
-        if graspnet:
-            Rviz.add_markers()
+            Rviz.add_trajectory(self.namespace)
 
     def create_launch_description(self) -> list[RegisteredLaunchDescription]:
         """Create the launch description with specific elements for an arm.

ros2_ws/src/rcdt_launch/rcdt_launch/rviz.py

@@ -15,6 +15,7 @@ class Rviz:
         load_motion_planning_plugin (bool): Whether to load the motion planning plugin.
         load_planning_scene (bool): Whether to load the planning scene display.
         load_robot_state (bool): Whether to load the robot state display.
+        load_trajectory (bool): Whether to load the trajectory display.
         load_point_cloud (bool): Whether to load point cloud displays.
         moveit_namespaces (list[str]): A list of the namespaces where MoveIt is launched.
     """
@@ -24,6 +25,7 @@ class Rviz:
     load_motion_planning_plugin: bool = False
     load_planning_scene: bool = False
     load_robot_state: bool = False
+    load_trajectory: bool = False
     load_point_cloud: bool = False
     moveit_namespaces: list[str] = []
 
@@ -197,6 +199,26 @@ def add_robot_state(namespace: str) -> None:
             }
         )
 
+    @staticmethod
+    def add_trajectory(namespace: str) -> None:
+        """Add the trajectory display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_trajectory:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/Trajectory",
+                "Robot Description": f"{namespace}_robot_description",
+                "Name": f"{namespace}_trajectory",
+                "Trajectory Topic": f"/{namespace}/display_planned_path_custom",
+                "State Display Time": "0.5s",
+            }
+        )
+
     @staticmethod
     def add_map(topic: str) -> None:
         """Add a map to the RViz configuration.

ros2_ws/src/rcdt_moveit/include/moveit_manager.hpp

@@ -15,8 +15,8 @@
 #include <rcdt_messages/srv/express_pose_in_other_frame.hpp>
 #include <rcdt_messages/srv/move_hand_to_pose.hpp>
 #include <rcdt_messages/srv/move_to_configuration.hpp>
+#include <rcdt_messages/srv/pose_stamped_srv.hpp>
 #include <rcdt_messages/srv/transform_goal_pose.hpp>
-#include <rcdt_messages/srv/visualize_grasp_pose.hpp>
 #include <rclcpp/node.hpp>
 #include <rviz_visual_tools/rviz_visual_tools.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -30,7 +30,7 @@ typedef rcdt_messages::srv::ExpressPoseInOtherFrame ExpressPoseInOtherFrame;
 typedef rcdt_messages::srv::TransformGoalPose TransformGoalPose;
 typedef rcdt_messages::srv::MoveToConfiguration MoveToConf;
 typedef rcdt_messages::srv::MoveHandToPose MoveHandToPose;
-typedef rcdt_messages::srv::VisualizeGraspPose VisualizeGraspPose;
+typedef rcdt_messages::srv::PoseStampedSrv PoseStampedSrv;
 typedef moveit_msgs::srv::ServoCommandType ServoCommandType;
 typedef std_srvs::srv::Trigger Trigger;
 typedef geometry_msgs::msg::PoseStamped PoseStamped;
@@ -57,6 +57,7 @@ class MoveitManager {
   const moveit::core::JointModelGroup *jmg_tcp;
   std::string base_frame = "map";
   std::string marker_topic = "/rviz_markers";
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
   rviz_visual_tools::RvizVisualTools rviz_visual_tools;
   moveit_visual_tools::MoveItVisualTools moveit_visual_tools;
   PoseStamped goal_pose;
@@ -100,10 +101,18 @@ class MoveitManager {
   void add_marker(const std::shared_ptr<AddMarker::Request> request,
                   std::shared_ptr<AddMarker::Response> response);
 
-  rclcpp::Service<VisualizeGraspPose>::SharedPtr visualize_grasp_pose_service;
-  void visualize_grasp_pose(
-      const std::shared_ptr<VisualizeGraspPose::Request> request,
-      std::shared_ptr<VisualizeGraspPose::Response> response);
+  rclcpp::Service<PoseStampedSrv>::SharedPtr visualize_grasp_pose_service;
+  void
+  visualize_grasp_pose(const std::shared_ptr<PoseStampedSrv::Request> request,
+                       std::shared_ptr<PoseStampedSrv::Response> response);
+
+  rclcpp::Service<PoseStampedSrv>::SharedPtr create_plan_service;
+  void create_plan(const std::shared_ptr<PoseStampedSrv::Request> request,
+                   std::shared_ptr<PoseStampedSrv::Response> response);
+
+  rclcpp::Service<Trigger>::SharedPtr visualize_plan_service;
+  void visualize_plan(const std::shared_ptr<Trigger::Request> request,
+                      std::shared_ptr<Trigger::Response> response);
 
   rclcpp::Service<Trigger>::SharedPtr clear_markers_service;
   void clear_markers(const std::shared_ptr<Trigger::Request> request,

ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp

@@ -4,6 +4,7 @@
 
 #include "moveit_manager.hpp"
 #include <moveit/move_group_interface/move_group_interface.hpp>
+#include <moveit/robot_state/robot_state.hpp>
 #include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node_options.hpp>
@@ -30,6 +31,10 @@ MoveitManager::MoveitManager(rclcpp::Node::SharedPtr node_)
 
   moveit_visual_tools.loadMarkerPub(false);
   moveit_visual_tools.loadRobotStatePub("display_robot_state");
+  moveit_visual_tools.loadTrajectoryPub("display_planned_path_custom");
+
+  auto link_tcp = jmg_tcp->getLinkModelNames().back();
+  move_group.setEndEffectorLink(link_tcp);
 
   initialize_clients();
   initialize_services();
@@ -69,10 +74,17 @@ void MoveitManager::initialize_services() {
   add_marker_service = node->create_service<AddMarker>(
       "~/add_marker", std::bind(&MoveitManager::add_marker, this, _1, _2));
 
-  visualize_grasp_pose_service = node->create_service<VisualizeGraspPose>(
+  visualize_grasp_pose_service = node->create_service<PoseStampedSrv>(
       "~/visualize_grasp_pose",
       std::bind(&MoveitManager::visualize_grasp_pose, this, _1, _2));
 
+  create_plan_service = node->create_service<PoseStampedSrv>(
+      "~/create_plan", std::bind(&MoveitManager::create_plan, this, _1, _2));
+
+  visualize_plan_service = node->create_service<Trigger>(
+      "~/visualize_plan",
+      std::bind(&MoveitManager::visualize_plan, this, _1, _2));
+
   clear_markers_service = node->create_service<Trigger>(
       "~/clear_markers",
       std::bind(&MoveitManager::clear_markers, this, _1, _2));
@@ -201,8 +213,8 @@ void MoveitManager::add_marker(
 };
 
 void MoveitManager::visualize_grasp_pose(
-    const std::shared_ptr<VisualizeGraspPose::Request> request,
-    std::shared_ptr<VisualizeGraspPose::Response> response) {
+    const std::shared_ptr<PoseStampedSrv::Request> request,
+    std::shared_ptr<PoseStampedSrv::Response> response) {
 
   // Broadcast a tf frame at the desired Tool Center Point location:
   TransformStamped tf;
@@ -232,7 +244,34 @@ void MoveitManager::visualize_grasp_pose(
   // Publish the End Effector marker:
   std::vector<double> positions = {0.04};
   moveit_visual_tools.publishEEMarkers(arm_end_in_base_frame.pose, jmg_hand,
-                                       positions, rviz_visual_tools::ORANGE);
+                                       positions, rviz_visual_tools::BLUE);
+  response->success = true;
+}
+
+void MoveitManager::create_plan(
+    const std::shared_ptr<PoseStampedSrv::Request> request,
+    std::shared_ptr<PoseStampedSrv::Response> response) {
+
+  move_group.setPoseTarget(request->pose);
+  auto error_code = move_group.plan(plan);
+  if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
+    RCLCPP_ERROR(node->get_logger(), "Failed to generate plan.");
+  }
+
+  // Visualize the goal state in RViz:
+  auto goal_positions =
+      plan.trajectory.joint_trajectory.points.back().positions;
+  moveit::core::RobotState goal_state(move_group.getRobotModel());
+  goal_state.setJointGroupPositions(jmg_arm, goal_positions);
+  moveit_visual_tools.publishRobotState(goal_state, rviz_visual_tools::ORANGE);
+  response->success = true;
+}
+
+void MoveitManager::visualize_plan(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
+  moveit::core::RobotState state(move_group.getRobotModel());
+  moveit_visual_tools.publishTrajectoryPath(plan.trajectory, state);
   response->success = true;
 }
 

ros2_ws/src/rcdt_utilities/launch/rviz.launch.py

@@ -29,7 +29,14 @@ def launch_setup(context: LaunchContext) -> list:
     if os.path.isfile(file_name):
         arguments.extend(["--display-config", file_name])
 
-    if Rviz.load_motion_planning_plugin or Rviz.load_planning_scene:
+    if any(
+        [
+            Rviz.load_motion_planning_plugin,
+            Rviz.load_planning_scene,
+            Rviz.load_robot_state,
+            Rviz.load_trajectory,
+        ]
+    ):
         for namespace in Rviz.moveit_namespaces:
             configuration = Moveit.configurations[namespace]