Refactor image buffers and resize logic

sivral · sivral · commit 5f16f6a9bf75 · 2026-02-03T15:03:57.000+01:00
diff --git a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp
@@ -105,7 +105,8 @@ class RobotReadServiceImpl final
 
   void FillProtoImage(
     const sensor_msgs::msg::Image& ros,
-    proto_aegis_grpc::v1::Image* out); 
+    proto_aegis_grpc::v1::Image* out,
+    cv::Mat& resize_buffer); 
 
     std::shared_ptr<rclcpp::Node> node_;
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
@@ -131,6 +132,12 @@ class RobotReadServiceImpl final
 
     uint32_t target_img_width_;
     uint32_t target_img_height_;
+
+    cv::Mat scene_resized_;
+    cv::Mat right_resized_;
+    cv::Mat left_resized_;
+
+    bool enable_image_resize_;
 };
 
 } // namespace aegis_grpc
diff --git a/aegis_grpc/src/robot_read_service.cpp b/aegis_grpc/src/robot_read_service.cpp
@@ -9,11 +9,12 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
   DeclareROSParameter("topic_pose", std::string("/tcp_pose"), "[str] Init; Sub: topic with the TCP pose data.");
   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] Camera scene image topic");
-  DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_raw"), "[str] Camera scene image topic");
-  DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_raw"), "[str] Camera scene image topic");
-  DeclareROSParameter("target_image_width", 64, "[int] Init; TODO");
-  DeclareROSParameter("target_image_height", 64, "[int] Init; TODO");
+  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("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.");
 
   pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
       node_->get_parameter("topic_pose").as_string(), 10,
@@ -41,6 +42,7 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)
                 std::placeholders::_1));
   target_img_width_ = node_->get_parameter("target_image_width").as_int();
   target_img_height_ = node_->get_parameter("target_image_height").as_int();
+  enable_image_resize_ = node_->get_parameter("enable_image_resize").as_bool();
 }
 
 template <class T>
@@ -141,7 +143,7 @@ grpc::Status RobotReadServiceImpl::GetCameraSceneImage(
     (void)request;
     {
       std::lock_guard<std::mutex> lock(image_scene_mutex_);
-      FillProtoImage(image_scene_data_, response);
+      FillProtoImage(image_scene_data_, response, scene_resized_);
     }
     return grpc::Status::OK;
 }
@@ -154,7 +156,7 @@ grpc::Status RobotReadServiceImpl::GetCameraRightImage(
     (void)request;
     {
       std::lock_guard<std::mutex> lock(image_right_mutex_);
-      FillProtoImage(image_right_data_, response);
+      FillProtoImage(image_right_data_, response, right_resized_);
     }
     return grpc::Status::OK;
 }
@@ -167,7 +169,7 @@ grpc::Status RobotReadServiceImpl::GetCameraLeftImage(
     (void)request;
     {
       std::lock_guard<std::mutex> lock(image_left_mutex_);
-      FillProtoImage(image_left_data_, response);
+      FillProtoImage(image_left_data_, response, left_resized_);
     }
     return grpc::Status::OK;
 }
@@ -204,15 +206,15 @@ grpc::Status RobotReadServiceImpl::GetRobotVision(
     (void) request;
     {
     std::lock_guard<std::mutex> lock(image_scene_mutex_);
-    FillProtoImage(image_scene_data_, response->mutable_image_scene());
+    FillProtoImage(image_scene_data_, response->mutable_image_scene(), scene_resized_);
     }
     {
     std::lock_guard<std::mutex> lock(image_right_mutex_);
-    FillProtoImage(image_right_data_, response->mutable_image_right());
+    FillProtoImage(image_right_data_, response->mutable_image_right(), right_resized_);
     }
     {
     std::lock_guard<std::mutex> lock(image_left_mutex_);
-    FillProtoImage(image_left_data_, response->mutable_image_left());
+    FillProtoImage(image_left_data_, response->mutable_image_left(), left_resized_);
     }
     return grpc::Status::OK;
 }
@@ -237,15 +239,15 @@ grpc::Status RobotReadServiceImpl::GetAll(
     }
     {
         std::lock_guard<std::mutex> lock(image_scene_mutex_);
-        FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene());
+        FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene(), scene_resized_);
     }
     {
         std::lock_guard<std::mutex> lock(image_right_mutex_);
-        FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right());
+        FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right(), right_resized_);
     }
     {
         std::lock_guard<std::mutex> lock(image_left_mutex_);
-        FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left());
+        FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left(), left_resized_);
     }
     return grpc::Status::OK;
 }
@@ -293,33 +295,45 @@ void RobotReadServiceImpl::FillProtoJointState(
     for (const auto& v : ros.effort) out->add_effort(v);
 }
 
-void RobotReadServiceImpl::FillProtoImage(
-  const sensor_msgs::msg::Image& ros,
-  proto_aegis_grpc::v1::Image* out) {
-    cv_bridge::CvImagePtr cv_ptr;
+  void RobotReadServiceImpl::FillProtoImage(
+    const sensor_msgs::msg::Image& ros,
+    proto_aegis_grpc::v1::Image* out,
+    cv::Mat& buffer)
+  {
+    cv_bridge::CvImageConstPtr cv_ptr;
     try {
-        cv_ptr = cv_bridge::toCvCopy(ros, ros.encoding);
+      cv_ptr = cv_bridge::toCvCopy(ros, ros.encoding);
     } catch (cv_bridge::Exception& e) {
-        RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what());
-        return;
+      RCLCPP_ERROR(node_->get_logger(), "cv_bridge: %s", e.what());
+      return;
     }
-    cv::Mat image_resized;
-    cv::resize(cv_ptr->image, image_resized, cv::Size(target_img_width_, target_img_height_));
 
-    out->set_height(image_resized.rows);
-    out->set_width(image_resized.cols);
-    out->set_step(image_resized.step);
+    const cv::Mat* src = &cv_ptr->image;
 
-    if (ros.encoding == "bgr8") {
-        out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);
-    } else if (ros.encoding == "bayer_rggb8") {
-        out->set_encoding(proto_aegis_grpc::v1::Image::BAYER_RGGB8);
+    if (enable_image_resize_) {
+      cv::resize(
+        cv_ptr->image,
+        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);
+      out->set_encoding(proto_aegis_grpc::v1::Image::UNKNOWN);
     }
 
-    out->set_data(image_resized.data, image_resized.total() * image_resized.elemSize());
-}
+    out->set_height(src->rows);
+    out->set_width(src->cols);
+    out->set_step(src->step);
+    out->set_data(src->data, src->total() * src->elemSize());
+  }
 
 
 } // namespace aegis_grpc

Original file line number	Diff line number	Diff line change
`@@ -9,11 +9,12 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)`
`9`	`9`	`DeclareROSParameter("topic_pose", std::string("/tcp_pose"), "[str] Init; Sub: topic with the TCP pose data.");`
`10`	`10`	`DeclareROSParameter("topic_wrench", std::string("/wrench"), "[str] Init; Sub: topic with the F/T data.");`
`11`	`11`	`DeclareROSParameter("topic_joints", std::string("/joint_states"), "[str] Init; Sub: topic with the joint states.");`
`12`		`- DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"), "[str] Camera scene image topic");`
`13`		`- DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_raw"), "[str] Camera scene image topic");`
`14`		`- DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_raw"), "[str] Camera scene image topic");`
`15`		`- DeclareROSParameter("target_image_width", 64, "[int] Init; TODO");`
`16`		`- DeclareROSParameter("target_image_height", 64, "[int] Init; TODO");`
	`12`	`+ DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"), "[str] Init; Sub: Camera scene image topic.");`
	`13`	`+ DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_raw"), "[str] Init; Sub: Camera scene image topic.");`
	`14`	`+ DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_raw"), "[str] Init; Sub:Camera scene image topic.");`
	`15`	`+ DeclareROSParameter("target_image_width", 64, "[int] Init; Target output image width in pixels.");`
	`16`	`+ DeclareROSParameter("target_image_height", 64, "[int] Init; Target output image height in pixels.");`
	`17`	`+ DeclareROSParameter("enable_image_resize", true, "[bool] Init; Enable resizing images before sending over gRPC.");`
`17`	`18`
`18`	`19`	`pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(`
`19`	`20`	`node_->get_parameter("topic_pose").as_string(), 10,`
`@@ -41,6 +42,7 @@ RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node)`
`41`	`42`	`std::placeholders::_1));`
`42`	`43`	`target_img_width_ = node_->get_parameter("target_image_width").as_int();`
`43`	`44`	`target_img_height_ = node_->get_parameter("target_image_height").as_int();`
	`45`	`+ enable_image_resize_ = node_->get_parameter("enable_image_resize").as_bool();`
`44`	`46`	`}`
`45`	`47`
`46`	`48`	`template <class T>`
`@@ -141,7 +143,7 @@ grpc::Status RobotReadServiceImpl::GetCameraSceneImage(`
`141`	`143`	`(void)request;`
`142`	`144`	`{`
`143`	`145`	`std::lock_guard<std::mutex> lock(image_scene_mutex_);`
`144`		`- FillProtoImage(image_scene_data_, response);`
	`146`	`+ FillProtoImage(image_scene_data_, response, scene_resized_);`
`145`	`147`	`}`
`146`	`148`	`return grpc::Status::OK;`
`147`	`149`	`}`
`@@ -154,7 +156,7 @@ grpc::Status RobotReadServiceImpl::GetCameraRightImage(`
`154`	`156`	`(void)request;`
`155`	`157`	`{`
`156`	`158`	`std::lock_guard<std::mutex> lock(image_right_mutex_);`
`157`		`- FillProtoImage(image_right_data_, response);`
	`159`	`+ FillProtoImage(image_right_data_, response, right_resized_);`
`158`	`160`	`}`
`159`	`161`	`return grpc::Status::OK;`
`160`	`162`	`}`
`@@ -167,7 +169,7 @@ grpc::Status RobotReadServiceImpl::GetCameraLeftImage(`
`167`	`169`	`(void)request;`
`168`	`170`	`{`
`169`	`171`	`std::lock_guard<std::mutex> lock(image_left_mutex_);`
`170`		`- FillProtoImage(image_left_data_, response);`
	`172`	`+ FillProtoImage(image_left_data_, response, left_resized_);`
`171`	`173`	`}`
`172`	`174`	`return grpc::Status::OK;`
`173`	`175`	`}`
`@@ -204,15 +206,15 @@ grpc::Status RobotReadServiceImpl::GetRobotVision(`
`204`	`206`	`(void) request;`
`205`	`207`	`{`
`206`	`208`	`std::lock_guard<std::mutex> lock(image_scene_mutex_);`
`207`		`- FillProtoImage(image_scene_data_, response->mutable_image_scene());`
	`209`	`+ FillProtoImage(image_scene_data_, response->mutable_image_scene(), scene_resized_);`
`208`	`210`	`}`
`209`	`211`	`{`
`210`	`212`	`std::lock_guard<std::mutex> lock(image_right_mutex_);`
`211`		`- FillProtoImage(image_right_data_, response->mutable_image_right());`
	`213`	`+ FillProtoImage(image_right_data_, response->mutable_image_right(), right_resized_);`
`212`	`214`	`}`
`213`	`215`	`{`
`214`	`216`	`std::lock_guard<std::mutex> lock(image_left_mutex_);`
`215`		`- FillProtoImage(image_left_data_, response->mutable_image_left());`
	`217`	`+ FillProtoImage(image_left_data_, response->mutable_image_left(), left_resized_);`
`216`	`218`	`}`
`217`	`219`	`return grpc::Status::OK;`
`218`	`220`	`}`
`@@ -237,15 +239,15 @@ grpc::Status RobotReadServiceImpl::GetAll(`
`237`	`239`	`}`
`238`	`240`	`{`
`239`	`241`	`std::lock_guard<std::mutex> lock(image_scene_mutex_);`
`240`		`- FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene());`
	`242`	`+ FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene(), scene_resized_);`
`241`	`243`	`}`
`242`	`244`	`{`
`243`	`245`	`std::lock_guard<std::mutex> lock(image_right_mutex_);`
`244`		`- FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right());`
	`246`	`+ FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right(), right_resized_);`
`245`	`247`	`}`
`246`	`248`	`{`
`247`	`249`	`std::lock_guard<std::mutex> lock(image_left_mutex_);`
`248`		`- FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left());`
	`250`	`+ FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left(), left_resized_);`
`249`	`251`	`}`
`250`	`252`	`return grpc::Status::OK;`
`251`	`253`	`}`
`@@ -293,33 +295,45 @@ void RobotReadServiceImpl::FillProtoJointState(`
`293`	`295`	`for (const auto& v : ros.effort) out->add_effort(v);`
`294`	`296`	`}`
`295`	`297`
`296`		`-void RobotReadServiceImpl::FillProtoImage(`
`297`		`- const sensor_msgs::msg::Image& ros,`
`298`		`- proto_aegis_grpc::v1::Image* out) {`
`299`		`- cv_bridge::CvImagePtr cv_ptr;`
	`298`	`+ void RobotReadServiceImpl::FillProtoImage(`
	`299`	`+ const sensor_msgs::msg::Image& ros,`
	`300`	`+ proto_aegis_grpc::v1::Image* out,`
	`301`	`+ cv::Mat& buffer)`
	`302`	`+ {`
	`303`	`+ cv_bridge::CvImageConstPtr cv_ptr;`
`300`	`304`	`try {`
`301`		`- cv_ptr = cv_bridge::toCvCopy(ros, ros.encoding);`
	`305`	`+ cv_ptr = cv_bridge::toCvCopy(ros, ros.encoding);`
`302`	`306`	`} catch (cv_bridge::Exception& e) {`
`303`		`- RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what());`
`304`		`- return;`
	`307`	`+ RCLCPP_ERROR(node_->get_logger(), "cv_bridge: %s", e.what());`
	`308`	`+ return;`
`305`	`309`	`}`
`306`		`- cv::Mat image_resized;`
`307`		`- cv::resize(cv_ptr->image, image_resized, cv::Size(target_img_width_, target_img_height_));`
`308`	`310`
`309`		`- out->set_height(image_resized.rows);`
`310`		`- out->set_width(image_resized.cols);`
`311`		`- out->set_step(image_resized.step);`
	`311`	`+ const cv::Mat* src = &cv_ptr->image;`
`312`	`312`
`313`		`- if (ros.encoding == "bgr8") {`
`314`		`- out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);`
`315`		`- } else if (ros.encoding == "bayer_rggb8") {`
`316`		`- out->set_encoding(proto_aegis_grpc::v1::Image::BAYER_RGGB8);`
	`313`	`+ if (enable_image_resize_) {`
	`314`	`+ cv::resize(`
	`315`	`+ cv_ptr->image,`
	`316`	`+ buffer,`
	`317`	`+ cv::Size(target_img_width_, target_img_height_),`
	`318`	`+ 0, 0, cv::INTER_LINEAR);`
	`319`	`+ src = &buffer;`
	`320`	`+ }`
	`321`	`+`
	`322`	`+ if (ros.encoding == "bayer_rggb8") {`
	`323`	`+ cv::cvtColor(*src, buffer, cv::COLOR_BayerRG2BGR);`
	`324`	`+ src = &buffer;`
	`325`	`+ out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);`
	`326`	`+ } else if (ros.encoding == "bgr8") {`
	`327`	`+ out->set_encoding(proto_aegis_grpc::v1::Image::BGR8);`
`317`	`328`	`} else {`
`318`		`- out->set_encoding(proto_aegis_grpc::v1::Image::UNKNOWN);`
	`329`	`+ out->set_encoding(proto_aegis_grpc::v1::Image::UNKNOWN);`
`319`	`330`	`}`
`320`	`331`
`321`		`- out->set_data(image_resized.data, image_resized.total() * image_resized.elemSize());`
`322`		`-}`
	`332`	`+ out->set_height(src->rows);`
	`333`	`+ out->set_width(src->cols);`
	`334`	`+ out->set_step(src->step);`
	`335`	`+ out->set_data(src->data, src->total() * src->elemSize());`
	`336`	`+ }`
`323`	`337`
`324`	`338`
`325`	`339`	`} // namespace aegis_grpc`