Fix image handling

Author: sivral

aegis_grpc/include/aegis_grpc/robot_read_service.hpp

@@ -106,7 +106,7 @@ class RobotReadServiceImpl final
   void FillProtoImage(
     const sensor_msgs::msg::Image& ros,
     proto_aegis_grpc::v1::Image* out,
-    cv::Mat& resize_buffer); 
+    cv::Mat& resize_buffer);
 
     std::shared_ptr<rclcpp::Node> node_;
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;

aegis_grpc/proto_aegis_grpc/v1/sensor_msgs.proto

@@ -25,12 +25,6 @@ message JointState {
 message Image {
   uint32 height = 1;
   uint32 width = 2;
-  enum Encoding {
-    UNKNOWN = 0;
-    BGR8 = 1;
-    BAYER_RGGB8 = 2;
-  }
-  Encoding encoding = 3;
-  uint32 step = 4;
-  bytes data = 5;
+  uint32 step = 3;
+  bytes data = 4;
 }

aegis_grpc/python_client/aegis_grpc_client/grpc_client.py

@@ -219,16 +219,11 @@ def _joints_to_array(self, joints: sensor_msgs_pb2.JointState) -> np.ndarray:
         ).T
 
     def _image_to_array(self, img: sensor_msgs_pb2.Image) -> np.ndarray:
-        arr = np.frombuffer(img.data, dtype=np.uint8)
-
-        if img.encoding == sensor_msgs_pb2.Image.BGR8:
-            arr = arr.reshape(img.height, img.width, 3)
-        elif img.encoding == sensor_msgs_pb2.Image.BAYER_RGGB8:
-            arr = arr.reshape(img.height, img.width)
-        else:
-            raise ValueError(f"Unsupported image encoding: {img.encoding}")
-
-        return arr
+        return np.moveaxis(
+            np.frombuffer(img.data, dtype=np.uint8).reshape(img.height, img.width, 3),
+            -1,
+            0,
+        )
 
     # ==================== RobotControlService Methods ====================
 

aegis_grpc/src/robot_read_service.cpp

@@ -10,8 +10,8 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
   DeclareROSParameter("topic_wrench", std::string("/wrench"), "[str] Init; Sub: topic with the F/T data.");
   DeclareROSParameter("topic_joints", std::string("/joint_states"), "[str] Init; Sub: topic with the joint states.");
   DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"), "[str] Init; Sub: Camera scene image topic.");
-  DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_raw"), "[str] Init; Sub: Camera scene image topic.");
-  DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_raw"), "[str] Init; Sub:Camera scene image topic.");
+  DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_color"), "[str] Init; Sub: Camera scene image topic.");
+  DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_color"), "[str] Init; Sub:Camera scene image topic.");
   DeclareROSParameter("target_image_width", 64, "[int] Init; Target output image width in pixels.");
   DeclareROSParameter("target_image_height", 64, "[int] Init; Target output image height in pixels.");
   DeclareROSParameter("enable_image_resize", true, "[bool] Init; Enable resizing images before sending over gRPC.");
@@ -300,39 +300,30 @@ void RobotReadServiceImpl::FillProtoJointState(
     proto_aegis_grpc::v1::Image* out,
     cv::Mat& buffer)
   {
-    cv_bridge::CvImageConstPtr cv_ptr;
-    try {
-      cv_ptr = cv_bridge::toCvCopy(ros, ros.encoding);
-    } catch (cv_bridge::Exception& e) {
-      RCLCPP_ERROR(node_->get_logger(), "cv_bridge: %s", e.what());
-      return;
-    }
+    const cv::Mat src(
+      ros.height,
+      ros.width,
+      CV_8UC3,
+      const_cast<uint8_t*>(ros.data.data()),
+      ros.step
+    );
 
-    const cv::Mat* src = &cv_ptr->image;
+    const cv::Mat* img = &src;
 
     if (enable_image_resize_) {
       cv::resize(
-        cv_ptr->image,
+        src,
         buffer,
         cv::Size(target_img_width_, target_img_height_),
-        0, 0, cv::INTER_LINEAR);
-      src = &buffer;
-    }
-
-    if (ros.encoding == "bayer_rggb8") {
-      cv::cvtColor(*src, buffer, cv::COLOR_BayerRG2BGR);
-      src = &buffer;
-      out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);
-    } else if (ros.encoding == "bgr8") {
-      out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);
-    } else {
-      out->set_encoding(proto_aegis_grpc::v1::Image::UNKNOWN);
+        0, 0, cv::INTER_LINEAR
+      );
+      img = &buffer;
     }
 
-    out->set_height(src->rows);
-    out->set_width(src->cols);
-    out->set_step(src->step);
-    out->set_data(src->data, src->total() * src->elemSize());
+    out->set_height(img->rows);
+    out->set_width(img->cols);
+    out->set_step(img->step);
+    out->set_data(img->data, img->total() * img->elemSize());
   }