Added cras_bag_tools plugin. Fixed a few C/Python bugs.

peci1 · peci1 · commit 2c6c04dd8e08 · 2025-08-20T04:08:55.000+02:00
diff --git a/include/robot_model_renderer/c_api.h b/include/robot_model_renderer/c_api.h
@@ -11,6 +11,7 @@
 
 #include <stdbool.h>
 #include <stddef.h>
+#include <stdint.h>
 
 #ifdef __cplusplus
 extern "C"
@@ -145,39 +146,39 @@ struct robot_model_renderer_RobotModelRendererConfig
 
 struct ros_Time
 {
-  unsigned int sec;
-  unsigned int nsec;
+  uint32_t sec;
+  uint32_t nsec;
 };
 
 struct std_msgs_Header
 {
-  unsigned int seq;
+  uint32_t seq;
   ros_Time stamp;
   const char* frame_id;
 };
 
 struct sensor_msgs_RegionOfInterest
 {
-  unsigned int x_offset;
-  unsigned int y_offset;
-  unsigned int height;
-  unsigned int width;
-  bool do_rectify;
+  uint32_t x_offset;
+  uint32_t y_offset;
+  uint32_t height;
+  uint32_t width;
+  uint8_t do_rectify;
 };
 
 struct sensor_msgs_CameraInfo
 {
   std_msgs_Header header;
-  unsigned int height;
-  unsigned int width;
+  uint32_t height;
+  uint32_t width;
   const char* distortion_model;
   size_t D_count;
   double* D;
   double K[9];
   double R[9];
   double P[12];
-  unsigned int binning_x;
-  unsigned int binning_y;
+  uint32_t binning_x;
+  uint32_t binning_y;
   sensor_msgs_RegionOfInterest roi;
 };
 
diff --git a/package.xml b/package.xml
@@ -47,6 +47,8 @@ SPDX-FileCopyrightText: Czech Technical University in Prague
   <build_export_depend>libopencv-dev</build_export_depend>
 
   <exec_depend>assimp</exec_depend>
+  <exec_depend>cras_bag_tools</exec_depend>
+  <exec_depend>cras_py_common</exec_depend>
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>glut</exec_depend>
   <exec_depend>image_transport</exec_depend>
@@ -71,6 +73,7 @@ SPDX-FileCopyrightText: Czech Technical University in Prague
   <doc_depend>rosdoc_lite</doc_depend>
 
   <export>
+    <cras_bag_tools filters="robot_model_renderer.cras_bag_tools_plugin" />
     <nodelet plugin="${prefix}/nodelet.xml" />
     <rosdoc config="rosdoc.yaml"/>
   </export>
diff --git a/src/robot_model_renderer/cras_bag_tools_plugin.py b/src/robot_model_renderer/cras_bag_tools_plugin.py
@@ -0,0 +1,125 @@
+# SPDX-License-Identifier: BSD-3-Clause
+# SPDX-FileCopyrightText: Czech Technical University in Prague
+
+import copy
+import os
+import sys
+
+import rospy
+from cras import image_encodings
+from cras_bag_tools import DeserializedMessageFilter
+from cras_bag_tools.message_filter import fix_connection_header
+from sensor_msgs.msg import Image
+from robot_model_renderer import RobotModelRenderer, RobotModelRendererConfig, RenderingMode
+
+
+class RenderCameraMask(DeserializedMessageFilter):
+    def __init__(self, camera_info_topic, mask_topic=None, description_param=None, description_file=None,
+                 encoding=image_encodings.MONO8, tf_timeout=1.5,
+                 min_stamp=None, max_stamp=None, include_time_ranges=None, exclude_time_ranges=None, **kwargs):
+        super(RenderCameraMask, self).__init__(
+            include_topics=[camera_info_topic, "/tf", "/tf_static"], min_stamp=min_stamp, max_stamp=max_stamp,
+            include_time_ranges=include_time_ranges, exclude_time_ranges=exclude_time_ranges)
+
+        self.camera_info_topic = camera_info_topic
+        self.mask_topic = mask_topic
+        if mask_topic is None:
+            self.mask_topic = camera_info_topic.replace("/camera_info", "/image_raw/mask")
+        self.tf_timeout = rospy.Duration(tf_timeout)
+        self.description_param = description_param
+
+        self._model_from_file = None
+        if description_file is not None:
+            if os.path.exists(description_file):
+                try:
+                    with open(description_file, "r") as f:
+                        self._model_from_file = f.read()
+                    print("Loaded URDF model from file " + description_file)
+                except Exception as e:
+                    print('Could not read URDF model from file %s: %s' % (description_file, str(e)), file=sys.stderr)
+            else:
+                print('URDF model file "%s" does not exist.' % (description_file,), file=sys.stderr)
+
+        self._config = RobotModelRendererConfig()
+        self._config.renderingMode = RenderingMode.MASK
+        self._config.drawOutline = True
+        self._config.outlineColor = [1.0, 1.0, 1.0, 1.0]
+        self._config.outlineWidth = 10.0
+        self._config.renderImageScale = 0.25
+        self._config.maxRenderImageSize = 2048
+        for k, v in kwargs.items():
+            setattr(self._config, k, v)
+        self._encoding = encoding
+
+        self.renderer = None
+        self._failed_msgs = []
+
+    def set_bag(self, bag):
+        super(RenderCameraMask, self).set_bag(bag)
+
+        model = self._model_from_file
+        if model is None:
+            if self.description_param is None:
+                self.description_param = "robot_description"
+            model = self._get_param(self.description_param)
+            if model is not None:
+                print("Read URDF model from ROS parameter " + self.description_param)
+
+        if model is None:
+            raise RuntimeError("RenderCameraMask requires robot URDF either as a file or ROS parameter.")
+
+        self.renderer = RobotModelRenderer(model, self._config, self._encoding)
+
+    def filter(self, topic, msg, stamp, header):
+        if self.renderer is None:
+            return None
+
+        to_try = []
+        for m, s, h, e in self._failed_msgs:
+            if stamp - s < self.tf_timeout:
+                to_try.append((m, s, h))
+            else:
+                print("Failed transforming robot for mask rendering at time %s: %r" % (m.header.stamp, e),
+                      file=sys.stderr)
+        self._failed_msgs = []
+
+        if topic.lstrip('/') == 'tf':
+            for transform in msg.transforms:
+                self.renderer.setTransform(transform, "robot_model_renderer.cras_bag_tools_plugin", False)
+        elif topic.lstrip('/') == 'tf_static':
+            for transform in msg.transforms:
+                self.renderer.setTransform(transform, "robot_model_renderer.cras_bag_tools_plugin", True)
+        else:
+            to_try.append((msg, stamp, header))
+        result = [(topic, msg, stamp, header)]
+
+        for msg, stamp, header in to_try:
+            img, err, link_err = self.renderer.render(msg)
+
+            may_succeed_later = any([e.maySucceedLater for e in link_err]) if link_err is not None else False
+            if may_succeed_later:
+                self._failed_msgs.append((msg, stamp, header, err))
+            elif img is not None:
+                header = fix_connection_header(copy.copy(header), self.mask_topic, Image._type, Image._md5sum, Image)
+                result.append((self.mask_topic, img, stamp, header))
+            else:
+                print("Failed rendering robot mask for time %s: %r" % (msg.header.stamp, err), file=sys.stderr)
+
+        return result
+
+    def reset(self):
+        self.renderer = None
+        self._failed_msgs = []
+        super(RenderCameraMask, self).reset()
+
+    def _str_params(self):
+        parts = [
+            'camera_info_topic=' + self.camera_info_topic,
+            'mask_topic=' + self.mask_topic
+        ]
+        if self.description_param != "robot_description":
+            parts.append('description_param=' + self.description_param)
+        parent_params = super(RenderCameraMask, self)._str_params()
+        if len(parent_params) > 0:
+            parts.append(parent_params)
+        return ", ".join(parts)
diff --git a/src/robot_model_renderer/msg.py b/src/robot_model_renderer/msg.py
@@ -1,10 +1,11 @@
 # SPDX-License-Identifier: BSD-3-Clause
 # SPDX-FileCopyrightText: Czech Technical University in Prague
-
-from ctypes import c_bool, c_char_p, c_double, c_size_t, c_uint, c_uint32
+import ctypes
+from ctypes import c_bool, c_char_p, c_double, c_size_t, c_uint, c_uint32, c_uint8
 from ctypes import cast, POINTER, Structure
 import sys
 
+import genpy
 import rospy
 from geometry_msgs.msg import Transform, TransformStamped
 from sensor_msgs.msg import CameraInfo, RegionOfInterest
@@ -29,7 +30,7 @@ def __init__(self, secs=0, nsecs=0):
         if isinstance(secs, _RosTime):
             c = secs
             super(RosTime, self).__init__(c.sec, c.nsec)
-        elif isinstance(secs, rospy.Time):
+        elif isinstance(secs, (rospy.Time, genpy.rostime.Time)):
             c = secs
             super(RosTime, self).__init__(c.secs, c.nsecs)
         else:
@@ -55,7 +56,7 @@ class std_msgs_Header(Header):
     def __init__(self, header=None, *args, **kwargs):
         if header is not None and isinstance(header, _std_msgs_Header):
             super(std_msgs_Header, self).__init__(header.seq, RosTime(header.stamp), _decode(header.frame_id))
-        if header is not None and isinstance(header, Header):
+        if header is not None:
             super(std_msgs_Header, self).__init__(header.seq, RosTime(header.stamp), header.frame_id)
         else:
             super(std_msgs_Header, self).__init__(*args, **kwargs)
@@ -65,23 +66,24 @@ def from_param(cls, obj):
         c = _std_msgs_Header()
         c.seq = obj.seq
         c.stamp = RosTime.from_param(obj.stamp)
-        c.frame_id = obj.frame_id.encode("utf-8")
+        obj._frame_id = obj.frame_id.encode("utf-8")
+        c.frame_id = obj._frame_id
         return c
 
 
 class _sensor_msgs_RegionOfInterest(Structure):
     _fields_ = [
-        ("x_offset", c_uint),
-        ("y_offset", c_uint),
-        ("height", c_uint),
-        ("width", c_uint),
-        ("do_rectify", c_bool),
+        ("x_offset", c_uint32),
+        ("y_offset", c_uint32),
+        ("height", c_uint32),
+        ("width", c_uint32),
+        ("do_rectify", c_uint8),
     ]
 
 
 class sensor_msgs_RegionOfInterest(RegionOfInterest):
     def __init__(self, roi=None, *args, **kwargs):
-        if roi is not None and isinstance(roi, (_sensor_msgs_RegionOfInterest, RegionOfInterest)):
+        if roi is not None:
             super(sensor_msgs_RegionOfInterest, self).__init__(
                 roi.x_offset, roi.y_offset, roi.height, roi.width, roi.do_rectify)
         else:
@@ -101,16 +103,16 @@ def from_param(cls, obj):
 class _sensor_msgs_CameraInfo(Structure):
     _fields_ = [
         ("header", _std_msgs_Header),
-        ("height", c_uint),
-        ("width", c_uint),
+        ("height", c_uint32),
+        ("width", c_uint32),
         ("distortion_model", c_char_p),
         ("D_count", c_size_t),
         ("D", POINTER(c_double)),
         ("K", c_double * 9),
         ("R", c_double * 9),
         ("P", c_double * 12),
-        ("binning_x", c_uint),
-        ("binning_y", c_uint),
+        ("binning_x", c_uint32),
+        ("binning_y", c_uint32),
         ("roi", _sensor_msgs_RegionOfInterest),
     ]
 
@@ -119,17 +121,14 @@ class sensor_msgs_CameraInfo(CameraInfo):
     def __init__(self, camInfo=None, *args, **kwargs):
         if camInfo is not None and isinstance(camInfo, _sensor_msgs_CameraInfo):
             super(sensor_msgs_CameraInfo, self).__init__(
-                std_msgs_Header(camInfo.header), camInfo.height, camInfo.width, _decode(camInfo.distortion_model),
-                list(cast(camInfo.D, POINTER(c_double))), camInfo.K, camInfo.R, camInfo.P,
-                camInfo.binning_x, camInfo.binning_y, sensor_msgs_RegionOfInterest(camInfo.roi))
-        elif camInfo is not None and isinstance(camInfo, CameraInfo):
+                std_msgs_Header(header=camInfo.header), camInfo.height, camInfo.width,
+                _decode(camInfo.distortion_model), list(cast(camInfo.D, POINTER(c_double))), camInfo.K, camInfo.R,
+                camInfo.P, camInfo.binning_x, camInfo.binning_y, sensor_msgs_RegionOfInterest(roi=camInfo.roi))
+        elif camInfo is not None:
             super(sensor_msgs_CameraInfo, self).__init__(
-                std_msgs_Header(camInfo.header), camInfo.height, camInfo.width, camInfo.distortion_model,
+                std_msgs_Header(header=camInfo.header), camInfo.height, camInfo.width, camInfo.distortion_model,
                 camInfo.D, camInfo.K, camInfo.R, camInfo.P, camInfo.binning_x, camInfo.binning_y,
-                sensor_msgs_RegionOfInterest(camInfo.roi))
-        elif camInfo is not None and isinstance(camInfo, CameraInfo):
-            for f in CameraInfo.__slots__:
-                setattr(self, f, getattr(camInfo, f))
+                sensor_msgs_RegionOfInterest(roi=camInfo.roi))
         else:
             super(sensor_msgs_CameraInfo, self).__init__(*args, **kwargs)
 
@@ -139,9 +138,11 @@ def from_param(cls, obj):
         c.header = std_msgs_Header.from_param(obj.header)
         c.height = obj.height
         c.width = obj.width
-        c.distortion_model = obj.distortion_model.encode('utf-8')
+        obj._distortion_model = obj.distortion_model.encode('utf-8')
+        c.distortion_model = obj._distortion_model
         c.D_count = len(obj.D)
-        c.D = cast((c_double * len(obj.D))(*obj.D), POINTER(c_double))
+        obj._D = cast((c_double * len(obj.D))(*obj.D), POINTER(c_double))
+        c.D = obj._D
         c.K = (c_double * 9)(*obj.K)
         c.R = (c_double * 9)(*obj.R)
         c.P = (c_double * 12)(*obj.P)
@@ -169,8 +170,15 @@ def __init__(self, tf=None, *args, **kwargs):
             self.rotation.y = tf.rotation[1]
             self.rotation.z = tf.rotation[2]
             self.rotation.w = tf.rotation[3]
-        elif tf is not None and isinstance(tf, Transform):
-            super(geometry_msgs_Transform, self).__init__(tf.translation, tf.rotation)
+        elif tf is not None:
+            super(geometry_msgs_Transform, self).__init__()
+            self.translation.x = tf.translation.x
+            self.translation.y = tf.translation.y
+            self.translation.z = tf.translation.z
+            self.rotation.x = tf.rotation.x
+            self.rotation.y = tf.rotation.y
+            self.rotation.z = tf.rotation.z
+            self.rotation.w = tf.rotation.w
         else:
             super(geometry_msgs_Transform, self).__init__(*args, **kwargs)
 
@@ -194,22 +202,23 @@ class geometry_msgs_TransformStamped(TransformStamped):
     def __init__(self, tf=None, *args, **kwargs):
         if tf is not None and isinstance(tf, _geometry_msgs_TransformStamped):
             super(geometry_msgs_TransformStamped, self).__init__()
-            self.header = std_msgs_Header(tf.header)
+            self.header = std_msgs_Header(header=tf.header)
             self.child_frame_id = _decode(tf.child_frame_id)
-            self.transform = geometry_msgs_Transform(tf.transform)
-        if tf is not None and isinstance(tf, TransformStamped):
+            self.transform = geometry_msgs_Transform(tf=tf.transform)
+        elif tf is not None:
             super(geometry_msgs_TransformStamped, self).__init__()
-            self.header = std_msgs_Header(tf.header)
+            self.header = std_msgs_Header(header=tf.header)
             self.child_frame_id = tf.child_frame_id
-            self.transform = geometry_msgs_Transform(tf.transform)
+            self.transform = geometry_msgs_Transform(tf=tf.transform)
         else:
             super(geometry_msgs_TransformStamped, self).__init__(*args, **kwargs)
 
     @classmethod
     def from_param(cls, obj):
         c = _geometry_msgs_TransformStamped()
         c.header = std_msgs_Header.from_param(obj.header)
-        c.child_frame_id = obj.child_frame_id.encode('utf-8')
+        obj._child_frame_id = obj.child_frame_id.encode('utf-8')
+        c.child_frame_id = obj._child_frame_id
         c.transform = geometry_msgs_Transform.from_param(obj.transform)
         return c
 
diff --git a/src/robot_model_renderer/renderer.py b/src/robot_model_renderer/renderer.py
diff --git a/src/robot_model_renderer/types.py b/src/robot_model_renderer/types.py
