Move debug pub in service

launch/config.rviz

@@ -70,7 +70,7 @@ Visualization Manager:
       Shaft Length: 0.23000000417232513
       Shaft Radius: 0.009999999776482582
       Shape: Axes
-      Topic: /cosyobject_poses
+      Topic: /ros_cosypose/pose_debug
       Unreliable: false
       Value: true
     - Class: rviz/Camera
@@ -78,7 +78,7 @@ Visualization Manager:
       Image Rendering: background and overlay
       Image Topic: /usb_cam_0/image_rect_color
       Name: Camera
-      Overlay Alpha: 0.5
+      Overlay Alpha: 0.25
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
@@ -91,7 +91,7 @@ Visualization Manager:
       Zoom Factor: 1
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: cosyobject_markers
+      Marker Topic: /ros_cosypose/marker_debug
       Name: MarkerArray
       Namespaces:
         "": true

launch/singleview_loop.launch

 <launch>
   <arg name="dataset" default="ycbv"/>
+  <arg name="debug" default="false"/>
+  <arg name="debug_models" default="bop_datasets/$(arg dataset)/models/"/> <!-- Relative path from cosypose local_data directory -->
+
   <arg name="camera_id" default="0" />
   <arg name="camera_name" default="usb_cam_$(arg camera_id)"/>
   <arg name="bringup_camera" default="true"/>
 
   <arg name="allow_tracking" default="false"/>
 
-  <!-- Start camera and service -->
+  <!-- Start service and camera -->
   <include file="$(find ros_cosypose)/launch/singleview_service.launch">
     <arg name="dataset" value="$(arg dataset)"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="debug_models" value="$(arg debug_models)"/>
+
     <arg name="camera_id" value="$(arg camera_id)"/>
     <arg name="camera_name" value="$(arg camera_name)"/>
     <arg name="bringup_camera" value="$(arg bringup_camera)"/>

launch/singleview_rviz.launch

-<launch>
-  <arg name="dataset" default="ycbv"/>
-  <arg name="camera_id" default="0" />
-  <arg name="camera_name" default="usb_cam_$(arg camera_id)"/>
-  <arg name="bringup_camera" default="true"/>
-  <arg name="allow_tracking" default="false"/>
-
-  <!-- Start camera, service and loop -->
-  <include file="$(find ros_cosypose)/launch/singleview_loop.launch">
-    <arg name="dataset" value="$(arg dataset)"/>
-    <arg name="camera_id" value="$(arg camera_id)"/>
-    <arg name="camera_name" value="$(arg camera_name)"/>
-    <arg name="bringup_camera" value="$(arg bringup_camera)"/>
-    <arg name="allow_tracking" value="$(arg allow_tracking)"/>
-  </include>
-
-  <!-- RViz -->
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ros_cosypose)/launch/config.rviz"/>
-
-  <node name="$(anon rviz_translator.py)" pkg="ros_cosypose" type="rviz_translator.py">
-    <param name="in_object_topic" value="cosyobject_list"/>
-    <param name="out_poses_topic" value="cosyobject_poses"/>
-    <param name="out_marker_topic" value="cosyobject_markers"/>
-    <param name="models_relative_path" value="bop_datasets/$(arg dataset)/models/"/> <!-- Relative to cosypose local_data directory -->
-  </node>
-</launch>

launch/singleview_service.launch

 <launch>
     <arg name="dataset" default="ycbv"/>
+    <arg name="debug" default="false"/>
+    <arg name="debug_models" default="bop_datasets/$(arg dataset)/models/"/> <!-- Relative path from cosypose local_data directory -->
+
     <arg name="camera_id" default="0" />
     <arg name="camera_name" default="usb_cam_$(arg camera_id)"/>
     <arg name="bringup_camera" default="true"/>
 
+
     <!-- Start camera -->
     <group if="$(arg bringup_camera)">
       <include file="$(find ros_cosypose)/launch/cameras_bringup.launch">
           <arg name="usbcam_ids" value="$(arg camera_id)"/>
       </include>
-
       <!-- Rectify image -->
       <node name="$(anon image_proc)" pkg="image_proc" type="image_proc" ns="$(arg camera_name)"/>
     </group>
@@ -17,5 +20,12 @@
     <!-- ros_cosypose service -->
     <node name="$(anon pose_estimation.py)" pkg="ros_cosypose" type="pose_estimation.py">
       <param name="dataset" value="$(arg dataset)"/>
+      <param name="debug" value="$(arg debug)"/>
+      <param name="debug_models" value="$(arg debug_models)"/>
     </node>
+
+    <!-- RViz (if debug)-->
+    <group if="$(arg debug)">
+      <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ros_cosypose)/launch/config.rviz"/>
+    </group>
 </launch>

scripts/pose_estimation.py

 import rospy
-import time
-import sys
 
 import torch
+import numpy as np
 import pandas as pd
 from cosypose.utils.tensor_collection import PandasTensorCollection
-import numpy as np
 
 from ros_cosypose.rigid_object_predictor import RigidObjectPredictor
 from ros_cosypose.image_utils import imgmsg_to_array, make_cameras
+from ros_cosypose.rviz_translator import RVizTranslator
 
-from std_msgs.msg import Header
 from geometry_msgs.msg import Pose, Point, Quaternion
 from ros_cosypose.msg import CosyObjectArray, CosyObject
 from ros_cosypose.srv import GetPrediction, GetPredictionResponse
@@ -21,7 +19,7 @@ from torch.backends import cudnn
 cudnn.benchmark = True
 
 class PredictionService:
-    def __init__(self, dataset):
+    def __init__(self, dataset, debug, debug_models):
         self.dataset = dataset
 
         if(dataset == "ycbv"):
@@ -46,6 +44,10 @@ class PredictionService:
             detection_th=0.9
         )
 
+        self.debug_converter = None
+        if(debug):
+            self.debug_converter = RVizTranslator(debug_models)
+
     def pose_estimation(self, req):
         # Prepare camera infos
         cam_K = req.K
@@ -90,6 +92,11 @@ class PredictionService:
             poseROS = Pose(position=Point(*poseKDL.p) , orientation=Quaternion(*poseKDL.M.GetQuaternion()))
             objects_array.objects.append(CosyObject(label=label, pose=poseROS))
 
+        # Publish debug objects
+        if(self.debug_converter != None):
+            self.debug_converter.pub(objects_array)
+
+
         return objects_array
 
 
@@ -97,7 +104,10 @@ if __name__ == '__main__':
     rospy.init_node('pose_estimation', anonymous=True)
 
     dataset = rospy.get_param('~dataset', "ycbv")
-    prediction_srv = PredictionService(dataset)
+    debug = rospy.get_param('~debug', "false")
+    debug_models = rospy.get_param('~debug_models', "")
+
+    prediction_srv = PredictionService(dataset, debug, debug_models)
 
     rospy.Service('pose_estimation', GetPrediction, prediction_srv.pose_estimation)
     rospy.spin()

scripts/rviz_translator.py → src/ros_cosypose/rviz_translator.py

 import rospy
-import time
-import sys
 
-from geometry_msgs.msg import PoseArray, Pose
+from geometry_msgs.msg import PoseArray
 from ros_cosypose.msg import CosyObjectArray, CosyObject
 
 from cosypose.config import LOCAL_DATA_DIR
 from visualization_msgs.msg import MarkerArray, Marker
 
 class RVizTranslator:
-    def incomming_objects_cb(self, data):
+    """
+    Args:
+        models_relative_path (String): Relative to path (from cosypose
+            local_data directory) to the objects model.
+            If empty, the 3D  models won't be fetched and sent.
+        topic_prefix (String): prefix for the topics name
+    """
+    def __init__(self, models_relative_path="", topic_prefix="ros_cosypose/"):
+        self.models_relative_path = models_relative_path
+
+        self.poses_pub = rospy.Publisher(topic_prefix+"pose_debug", PoseArray, queue_size=1)
+        if(self.models_relative_path != ""):
+            self.marker_pub = rospy.Publisher(topic_prefix+"marker_debug", MarkerArray, queue_size=1)
+        else:
+            self.marker_pub = None
+
+    """
+    Convert a CosyObjectArray into object displayable by RViz and broadcast them.
+
+    This method will create PoseArray and Markers for plotting the positions and
+    models of the objet in rviz.
+    Calling this method will automatically delete the previously created markers
+    and replace them with the new one
+
+    Args:
+        cosyobjects (CosyObjectArray): data to 'plot'
+    """
+    def pub(self, cosyobjects):
         # Pose array (frames)
-        pose_array = PoseArray(header=data.header)
-        for object in data.objects:
+        pose_array = PoseArray(header=cosyobjects.header)
+        for object in cosyobjects.objects:
             pose_array.poses.append(object.pose)
         self.poses_pub.publish(pose_array)
 
         # 3D Models
-        if(self.models_relative_path != ''):
+        if(self.marker_pub):
             marker_array = MarkerArray()
-            for i in range(len(data.objects)):
-                marker = Marker(header=data.header)
+            for i in range(len(cosyobjects.objects)):
+                marker = Marker(header=cosyobjects.header)
                 marker.id = i
                 marker.type = 10 # Custom mesh
                 marker.action = 0 # Add/modify
@@ -28,19 +53,19 @@ class RVizTranslator:
                 marker.color.g = 0.8
                 marker.color.b = 0.8
                 marker.color.a = 0.8
-                marker.pose = data.objects[i].pose
+                marker.pose = cosyobjects.objects[i].pose
                 marker.scale.x = .001 # mm -> m
                 marker.scale.y = .001 # mm -> m
                 marker.scale.z = .001 # mm -> m
-                marker.mesh_resource = 'file://' + str(LOCAL_DATA_DIR) + '/' + self.models_relative_path + data.objects[i].label + ".ply"
+                marker.mesh_resource = 'file://' + str(LOCAL_DATA_DIR) + '/' + self.models_relative_path + cosyobjects.objects[i].label + ".ply"
                 marker_array.markers.append(marker)
             self.marker_pub.publish(marker_array)
 
             # delete unused 3D model previously created
             try:
                 delete_marker_array = MarkerArray()
-                for i in range(len(data.objects), self.marker_count):
-                    marker = Marker(header=data.header)
+                for i in range(len(cosyobjects.objects), self.marker_count):
+                    marker = Marker(header=cosyobjects.header)
                     marker.id = i
                     marker.action = 2 # delete
                     marker_array.markers.append(marker)
@@ -48,27 +73,8 @@ class RVizTranslator:
             except:
                 pass
             finally:
-                self.marker_count = len(data.objects)
+                self.marker_count = len(cosyobjects.objects)
         else:
             marker_array = MarkerArray()
             marker_array.markers.append(Marker(action=3)) # deleteAll
             self.marker_pub.publish(marker_array)
-
-
-    def run(self):
-        rospy.init_node('object_pose_estimation', anonymous=True)
-
-        self.in_object_topic = rospy.get_param('~in_object_topic', "cosyobject_list")
-        self.out_poses_topic = rospy.get_param('~out_poses_topic', "cosyobject_poses")
-        self.out_marker_topic = rospy.get_param('~out_marker_topic', "cosyobject_markers")
-
-        # If no path (or empty path is given)  the 3D models won't be sent
-        self.models_relative_path = rospy.get_param('~models_relative_path', '') # Relative to cosypose local_data directory
-
-        self.poses_pub = rospy.Publisher(self.out_poses_topic, PoseArray, queue_size=1)
-        self.marker_pub = rospy.Publisher(self.out_marker_topic, MarkerArray, queue_size=1)
-        rospy.Subscriber(self.in_object_topic, CosyObjectArray, self.incomming_objects_cb)
-        rospy.spin()
-
-if __name__ == '__main__':
-    RVizTranslator().run()