Visualize gripper pose in Rviz.

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

ros2_ws/src/rcdt_franka_moveit_config/config/fr3.srdf

@@ -37,6 +37,13 @@ are defined
         <joint name="fr3_joint6" />
         <joint name="fr3_joint7" />
     </group>
+    <group name="franka_hand">
+        <joint name="fr3_joint8" />
+        <joint name="fr3_hand_joint" />
+        <joint name="fr3_hand_tcp_joint" />
+        <joint name="fr3_finger_joint1" />
+        <joint name="fr3_finger_joint2" />
+    </group>
     <!--GROUP_STATES:
     Purpose: Define a named state for a particular group, in terms of joint values. This is
     useful to define states like 'folded arms'-->
@@ -61,6 +68,8 @@ are defined
 
     <!--END_EFFECTOR:
     Purpose: Represent information about an end effector.-->
+    <end_effector name="franka_hand" parent_link="fr3_link7" group="franka_hand"
+        parent_group="fr3_arm" />
     <!--DISABLE_COLLISIONS:
     By default it is assumed that any link of the robot could potentially come into
     collision with any other link in the robot. This tag disables collision checking between a

ros2_ws/src/rcdt_launch/rcdt_launch/moveit.py

@@ -96,6 +96,8 @@ def add_prefix_in_robot_description_semantic(description: dict, prefix: str) ->
         prefix (str): The prefix to add to each link.
     """
     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 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

@@ -790,7 +790,12 @@ def __init__(  # noqa: PLR0913
 
         if moveit:
             Moveit.add(self.namespace, self.robot_description, self.platform)
+            Rviz.moveit_namespaces.append(self.namespace)
             Rviz.add_motion_planning_plugin(self.namespace)
+            Rviz.add_planning_scene(self.namespace)
+            Rviz.add_robot_state(self.namespace)
+        if graspnet:
+            Rviz.add_markers()
 
     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

@@ -13,13 +13,17 @@ class Rviz:
         yaml (dict): The default RViz configuration.
         displays (list): The list of displays in the RViz configuration.
         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_point_cloud (bool): Whether to load point cloud displays.
         moveit_namespaces (list[str]): A list of the namespaces where MoveIt is launched.
     """
 
     yaml: dict = get_yaml(get_file_path("rcdt_utilities", ["rviz"], "default.rviz"))
     displays: list = yaml["Visualization Manager"]["Displays"]
     load_motion_planning_plugin: bool = False
+    load_planning_scene: bool = False
+    load_robot_state: bool = False
     load_point_cloud: bool = False
     moveit_namespaces: list[str] = []
 
@@ -142,7 +146,6 @@ def add_motion_planning_plugin(namespace: str) -> None:
         """
         if not Rviz.load_motion_planning_plugin:
             return
-        Rviz.moveit_namespaces.append(namespace)
         Rviz.displays.append(
             {
                 "Enabled": True,
@@ -154,6 +157,46 @@ def add_motion_planning_plugin(namespace: str) -> None:
             }
         )
 
+    @staticmethod
+    def add_planning_scene(namespace: str) -> None:
+        """Add the planning scene display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_planning_scene:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/PlanningScene",
+                "Move Group Namespace": namespace,
+                "Robot Description": f"{namespace}_robot_description",
+                "Name": f"{namespace}_planning_scene",
+                "Planning Scene Topic": f"/{namespace}/planning_scene",
+            }
+        )
+
+    @staticmethod
+    def add_robot_state(namespace: str) -> None:
+        """Add the robot state display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_robot_state:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/RobotState",
+                "Robot Description": f"{namespace}_robot_description",
+                "Robot State Topic": f"/{namespace}/display_robot_state",
+                "Name": f"{namespace}_robot_state",
+                "TF Prefix": namespace,
+            }
+        )
+
     @staticmethod
     def add_map(topic: str) -> None:
         """Add a map to the RViz configuration.

ros2_ws/src/rcdt_moveit/include/moveit_manager.hpp

@@ -16,7 +16,9 @@
 #include <rcdt_messages/srv/move_to_configuration.hpp>
 #include <rcdt_messages/srv/transform_goal_pose.hpp>
 #include <rclcpp/node.hpp>
+#include <rviz_visual_tools/rviz_visual_tools.hpp>
 #include <std_srvs/srv/trigger.hpp>
+#include <string>
 
 typedef rcdt_messages::srv::AddObject AddObject;
 typedef rcdt_messages::srv::AddMarker AddMarker;
@@ -37,14 +39,19 @@ struct Action {
 
 class MoveitManager {
 public:
-  MoveitManager(rclcpp::Node::SharedPtr node);
+  MoveitManager(rclcpp::Node::SharedPtr node, std::string group_arm,
+                std::string group_hand);
 
 private:
   rclcpp::Node::SharedPtr node;
   rclcpp::Node::SharedPtr client_node;
   moveit::planning_interface::MoveGroupInterface move_group;
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-  const moveit::core::JointModelGroup *joint_model_group;
+  const moveit::core::JointModelGroup *joint_model_group_arm;
+  const moveit::core::JointModelGroup *joint_model_group_hand;
+  std::string base_frame = "map";
+  std::string marker_topic = "/rviz_markers";
+  rviz_visual_tools::RvizVisualTools rviz_visual_tools;
   moveit_visual_tools::MoveItVisualTools moveit_visual_tools;
   PoseStamped goal_pose;
 
@@ -87,6 +94,10 @@ class MoveitManager {
   void add_marker(const std::shared_ptr<AddMarker::Request> request,
                   std::shared_ptr<AddMarker::Response> response);
 
+  rclcpp::Service<Trigger>::SharedPtr visualize_gripper_pose_service;
+  void visualize_gripper_pose(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,
                      std::shared_ptr<Trigger::Response> response);

ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp

@@ -8,24 +8,30 @@
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node_options.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <vector>
 
 using std::placeholders::_1;
 using std::placeholders::_2;
 
-MoveitManager::MoveitManager(rclcpp::Node::SharedPtr node_)
+MoveitManager::MoveitManager(rclcpp::Node::SharedPtr node_,
+                             std::string group_arm, std::string group_hand)
     : node(node_),
       move_group(
           node,
           moveit::planning_interface::MoveGroupInterface::Options(
-              "fr3_arm",
+              group_arm,
               moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION,
               node->get_namespace())),
-      moveit_visual_tools(node, "franka/fr3_link0", "/rviz_markers") {
+      rviz_visual_tools(base_frame, marker_topic, node),
+      moveit_visual_tools(node, base_frame, marker_topic) {
+
+  joint_model_group_arm =
+      move_group.getRobotModel()->getJointModelGroup(group_arm);
+  joint_model_group_hand =
+      move_group.getRobotModel()->getJointModelGroup(group_hand);
 
   moveit_visual_tools.loadMarkerPub(false);
-  move_group.setEndEffectorLink("franka/fr3_hand_tcp");
-  joint_model_group =
-      move_group.getRobotModel()->getJointModelGroup("franka/fr3_arm");
+  moveit_visual_tools.loadRobotStatePub("display_robot_state");
 
   initialize_clients();
   initialize_services();
@@ -65,6 +71,10 @@ void MoveitManager::initialize_services() {
   add_marker_service = node->create_service<AddMarker>(
       "~/add_marker", std::bind(&MoveitManager::add_marker, this, _1, _2));
 
+  visualize_gripper_pose_service = node->create_service<Trigger>(
+      "~/visualize_gripper_pose",
+      std::bind(&MoveitManager::visualize_gripper_pose, this, _1, _2));
+
   clear_markers_service = node->create_service<Trigger>(
       "~/clear_markers",
       std::bind(&MoveitManager::clear_markers, this, _1, _2));
@@ -85,12 +95,14 @@ void MoveitManager::add_object(
     return;
   }
 
+  std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
   solid_primitive.dimensions = {request->d1, request->d2, request->d3};
   collision_object.primitives.push_back(solid_primitive);
   collision_object.primitive_poses.push_back(request->pose.pose);
   collision_object.operation = collision_object.ADD;
   collision_object.id = "object";
-  planning_scene_interface.applyCollisionObject(collision_object);
+  collision_objects.push_back(collision_object);
+  planning_scene_interface.addCollisionObjects(collision_objects);
   response->success = true;
 };
 
@@ -156,7 +168,8 @@ bool MoveitManager::plan_and_execute(std::string planning_type) {
 
   moveit_visual_tools.deleteAllMarkers("Path");
   moveit_visual_tools.deleteAllMarkers("Sphere");
-  moveit_visual_tools.publishTrajectoryLine(plan.trajectory, joint_model_group);
+  moveit_visual_tools.publishTrajectoryLine(plan.trajectory,
+                                            joint_model_group_arm);
   moveit_visual_tools.trigger();
 
   error_code = move_group.execute(plan);
@@ -170,7 +183,7 @@ bool MoveitManager::plan_and_execute(std::string planning_type) {
 PoseStamped MoveitManager::change_frame_to_base(PoseStamped pose) {
   auto request = std::make_shared<ExpressPoseInOtherFrame::Request>();
   request->pose = pose;
-  request->target_frame = "franka/fr3_link0";
+  request->target_frame = base_frame;
   auto future = express_pose_in_other_frame_client->async_send_request(request);
   rclcpp::spin_until_future_complete(client_node, future);
   auto response = future.get();
@@ -186,6 +199,21 @@ void MoveitManager::add_marker(
   response->success = true;
 };
 
+void MoveitManager::visualize_gripper_pose(
+    const std::shared_ptr<Trigger::Request> request,
+    std::shared_ptr<Trigger::Response> response) {
+
+  PoseStamped pose_hand;
+  pose_hand.header.frame_id =
+      joint_model_group_hand->getEndEffectorParentGroup().second;
+  auto pose_map = change_frame_to_base(pose_hand);
+
+  std::vector<double> positions = {0.04};
+  moveit_visual_tools.publishEEMarkers(pose_map.pose, joint_model_group_hand,
+                                       positions, rviz_visual_tools::ORANGE);
+  response->success = true;
+}
+
 void MoveitManager::clear_markers(
     const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
     std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
@@ -199,7 +227,7 @@ int main(int argc, char **argv) {
   rclcpp::NodeOptions node_options;
   node_options.automatically_declare_parameters_from_overrides(true);
   auto node = std::make_shared<rclcpp::Node>("moveit_manager", node_options);
-  auto moveit_manager = MoveitManager(node);
+  auto moveit_manager = MoveitManager(node, "fr3_arm", "franka_hand");
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(node);
   executor.spin();

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

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

ros2_ws/src/rcdt_utilities/src_py/simple_gui.py

@@ -12,12 +12,13 @@
 from nicegui import app, ui
 from PIL import Image as PIL
 from rcdt_messages.msg import Grasp
-from rcdt_messages.srv import GenerateGraspnetGrasp
+from rcdt_messages.srv import AddObject, GenerateGraspnetGrasp
 from rcdt_utilities.cv_utils import cv2_image_to_ros_image, ros_image_to_cv2_image
 from rcdt_utilities.launch_utils import spin_node
 from rclpy.node import Node
 from rclpy.wait_for_message import wait_for_message
 from sensor_msgs.msg import CameraInfo, Image
+from std_srvs.srv import Trigger
 
 TIMEOUT = 3
 
@@ -70,8 +71,8 @@ def get_message(self, node: Node) -> object:
         return message
 
 
-class ManipulatePose(Node):
-    """Node to manipulate poses in different frames and apply transformations."""
+class UI(Node):
+    """Node of the UI."""
 
     def __init__(self):
         """Initialize the node."""
@@ -84,13 +85,29 @@ def __init__(self):
             GenerateGraspnetGrasp, "/graspnet/generate"
         )
 
+        self.clear_objects_client = self.create_client(
+            Trigger, "/franka1/moveit_manager/clear_objects"
+        )
+
+        self.add_object_client = self.create_client(
+            AddObject, "/franka1/moveit_manager/add_object"
+        )
+
+        self.visualize_gripper_pose_service = self.create_client(
+            Trigger, "/franka1/moveit_manager/visualize_gripper_pose"
+        )
+
         self.setup_gui()
 
     def setup_gui(self) -> None:
         """Setup the GUI pages."""
 
         @ui.page("/")
         def page():
+            with ui.row():
+                ui.button("Add Object", on_click=self.add_object)
+                ui.button("Clear Objects", on_click=self.clear_objects)
+                ui.button("Publish Robot State", on_click=self.visualize_gripper_pose)
             with ui.row():
                 color_image = ui.image(self.camera_view.color).classes("w-32")
                 depth_image = ui.image(self.camera_view.depth).classes("w-32")
@@ -111,6 +128,34 @@ def page():
             grasp = ui.label("No grasp generated.").classes("m-2")
             ui.button("Generate Grasp", on_click=lambda: self.generate_grasp(grasp))
 
+    def visualize_gripper_pose(self) -> None:
+        """Visualize the gripper pose in Rviz."""
+        if self.visualize_gripper_pose_service.call(Trigger.Request(), TIMEOUT) is None:
+            self.get_logger().error("Failed to call visualize gripper pose service.")
+        else:
+            self.get_logger().info(
+                "Successfully called visualize gripper pose service."
+            )
+
+    def clear_objects(self) -> None:
+        """Clear objects from the planning scene."""
+        if self.clear_objects_client.call(Trigger.Request(), TIMEOUT) is None:
+            self.get_logger().error("Failed to call clear objects service.")
+        else:
+            self.get_logger().info("Successfully called clear objects service.")
+
+    def add_object(self) -> None:
+        """Add an object to the planning scene."""
+        request = AddObject.Request()
+        request.shape = "SPHERE"
+        request.pose.header.frame_id = "franka1/fr3_link0"
+        request.d1 = 0.1  # radius
+
+        if self.add_object_client.call(request, TIMEOUT) is None:
+            self.get_logger().error("Failed to call add object service.")
+        else:
+            self.get_logger().info("Successfully called add object service.")
+
     def update_image(self, ui: ui.image, image_type: str) -> None:
         match image_type:
             case "color":
@@ -158,7 +203,7 @@ def ros_main(args: list | None = None) -> None:
         args (list | None): Command line arguments, defaults to None.
     """
     rclpy.init(args=args)
-    node = ManipulatePose()
+    node = UI()
     spin_node(node)