1

vision_detect/vision_detect/detect_bottle_service.py

@@ -288,6 +288,22 @@ def crop_mask_bbox(depth_img, mask, box):
     return depth_crop, mask_crop, (max(0, x_min), max(0, y_min))
 
 
+def calculate_grav_width(mask, K, depth):
+    """计算重心宽度"""
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    if contours:
+        box = cv2.boxPoints(cv2.minAreaRect(contours[0]))
+        if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
+            point_diff = box[1] - box[0]
+        else:
+            point_diff = box[1] - box[2]
+
+        grab_width = depth * np.sqrt(point_diff[0]**2 / K[0]**2 + point_diff[1]**2 / K[4]**2)
+        return grab_width
+    else:
+        return 0.0
+
+
 class DetectNode(Node):
     def __init__(self, name, mode):
         super().__init__(name)
@@ -505,6 +521,9 @@ class DetectNode(Node):
 
             x, y, z, rw, rx, ry, rz = self.calculate_function(mask_crop, depth_crop, intrinsics, self.source_model)
 
+            grab_width = calculate_grav_width(mask, self.K, z)
+            z = z + grab_width * 0.45
+
             if (x, y, z) == (None, None, None):
                 self.get_logger().error("have wrong pose")
                 continue

vision_test/include/vision_test/VisionDetectNode.hpp

@@ -1,11 +1,10 @@
-#include <map>
 #include <string>
 #include <nlohmann/json.hpp>
 #include <Eigen/Dense>
 
 #include "rclcpp/rclcpp.hpp"
-#include <message_filters/subscriber.hpp>
-#include <message_filters/sync_policies/approximate_time.hpp>
+#include "message_filters/subscriber.hpp"
+#include "message_filters/sync_policies/approximate_time.hpp"
 
 #include "sensor_msgs/msg/image.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
@@ -39,7 +38,7 @@ class VisionDetectNode: public rclcpp::Node {
         message_filters::Subscriber<sensor_msgs::msg::Image> depth_img_sub;
         std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_sub;
 
-        rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info;
+        rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub;
 
         rclcpp::Service<interfaces::srv::VisionObjectRecognition>::SharedPtr detect_server;
         
@@ -52,11 +51,13 @@ class VisionDetectNode: public rclcpp::Node {
         void init_subscriber();
 
         void service_callback(
-            const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> request, 
+            const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request, 
             std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
         );
 
-        void topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> msg);
+        void topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> &msg);
+
+        void camera_info_callback(const std::shared_ptr<sensor_msgs::msg::CameraInfo> &msg);
 
         void seg_img();
         void seg_color();

vision_test/src/VisionDetectNode.cpp

@@ -1,14 +1,8 @@
-#include <string>
-
-#include <nlohmann/json.hpp>
-#include <Eigen/Dense>
-
-#include "rclcpp/rclcpp.hpp"
 #include "vision_test/VisionDetectNode.hpp"
 #include "vision_test/utils.hpp"
 
 using json = nlohmann::json;
-
+using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image>;
 
 VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
     /**/
@@ -22,6 +16,7 @@ VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
     if (this->GetCameraMode == "Service") {
         this->GetCameraConfigs = json::parse(this->get_parameter("ServiceConfigs").as_string());
         this->init_service();
+        this->init_subscriber();
     }
     else if (this->GetCameraMode == "Topic") {
         this->GetCameraConfigs = json::parse(this->get_parameter("TopicConfigs").as_string());
@@ -31,6 +26,7 @@ VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
         RCLCPP_WARN(this->get_logger(), "Unkonw GetCameraMode, Use Service");
         this->GetCameraConfigs = json::parse(this->get_parameter("ServiceConfigs").as_string());
         this->init_service();
+        this->init_subscriber();
     }
 
     if (this->DetectMode == "Detect") {
@@ -120,7 +116,7 @@ void VisionDetectNode::init_calibration_mat() {
 void VisionDetectNode::init_service() {
     /*Init_service*/
     detect_server = this->create_service<interfaces::srv::VisionObjectRecognition>(
-        GetCameraConfigs["service_name"].get<std::string>(), 
+        this->GetCameraConfigs["service_name"].get<std::string>(), 
         std::bind(
             &VisionDetectNode::service_callback, 
             this, 
@@ -136,15 +132,34 @@ void VisionDetectNode::init_service() {
 
 void VisionDetectNode::init_topic() {
     /*Init Topic*/
-    color_img_sub.subscribe(this, GetCameraConfigs["color_image_topic_name"].get<std::string>());
-    depth_img_sub.subscribe(this, GetCameraConfigs["depth_image_topic_name"].get<std::string>());
+    this->color_img_sub.subscribe(this, this->GetCameraConfigs["color_image_topic_name"].get<std::string>());
+    this->depth_img_sub.subscribe(this, this->GetCameraConfigs["depth_image_topic_name"].get<std::string>());
+    this->sync_sub = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
+        SyncPolicy(10), 
+        color_img_sub, depth_img_sub
+    );
+
+    this->sync_sub->registerCallback(&VisionDetectNode::topic_callback, this);
+
+    this->camera_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+        this->GetCameraConfigs["camera_info_topic_name"].get<std::string>(), 
+        10,
+        std::bind(&VisionDetectNode::camera_info_callback, this, std::placeholders::_1)
+    );
 
     RCLCPP_INFO(this->get_logger(), "Init topic done");
+    // Done
 }
 
 
 void VisionDetectNode::init_publisher() {
     /*Init Publisher*/
+    pub_detect_img = this->create_publisher<sensor_msgs::msg::Image>("/image/detect_image", 10);
+
+    if (GetCameraMode == "Topic") {
+        pub_pose_list = this->create_publisher<interfaces::msg::PoseArrayClassAndID>("/pose/cv_detect_pose", 10);
+    }
+
     RCLCPP_INFO(this->get_logger(), "Init publisher done");
 }
 
@@ -156,7 +171,7 @@ void VisionDetectNode::init_subscriber() {
 
 
 void VisionDetectNode::service_callback(
-    const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> request, 
+    const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request, 
     std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
 ) {
     /**/
@@ -164,12 +179,18 @@ void VisionDetectNode::service_callback(
 }
 
 
-void VisionDetectNode::topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> msg) {
+void VisionDetectNode::topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> &msg) {
     /**/
 
 }
 
 
+void VisionDetectNode::camera_info_callback(const std::shared_ptr<sensor_msgs::msg::CameraInfo> &msg) {
+    /**/
+    
+}
+
+
 void VisionDetectNode::seg_img() {
     /**/