提交图像处理

TrackingRosEx.cpp

@@ -293,6 +293,9 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     SDK_LOG(SDK_INFO, "right_left_camera_len = %d", m_right_left_camera_len);
     nh.param<int32_t>("front_camera_len", m_front_camera_len, 0);
     SDK_LOG(SDK_INFO, "front_camera_len = %d", m_front_camera_len);
+    nh.param<int32_t>("send_camera_image", m_send_camera_image, 0);
+    SDK_LOG(SDK_INFO, "send_camera_image = %d", m_send_camera_image);
+    
 
     YAML::Node config = YAML::LoadFile(file.c_str());
     auto cfg = config["TRACKING"];
@@ -312,6 +315,10 @@ void TrackingRos::Init(ros::NodeHandle& nh)
     m_pubBoundingBoxes = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/tracking_bbox", 100);
     m_pubMarker = nh.advertise<visualization_msgs::MarkerArray>("/tracking_loc", 100);
 
+    m_pubCameraFront = nh.advertise("/CameraFront", 10);
+    m_pubCameraRight = nh.advertise("/CameraRight", 10);
+    m_pubCameraLeft = nh.advertise("/CameraLeft", 10);
+
     m_objsQueue.set_max_num_items(2);
 
     m_isTrackingRun = true;
@@ -457,6 +464,13 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                 objsPtr->array.push_back(obj);
             }
         }
+        cv::Mat sendImage[3];
+        if (m_send_camera_image == 1 && msg->Images.size() == 3)
+        {
+            sendImage[0] = cv_bridge::toCvCopy(msg->Images[0], "bgr8")->image;
+            sendImage[1] = cv_bridge::toCvCopy(msg->Images[1], "bgr8")->image;
+            sendImage[2] = cv_bridge::toCvCopy(msg->Images[2], "bgr8")->image;
+        }
         //SDK_LOG(SDK_INFO, "camera msg [0] size = %d,[1] = %d,[2] = %d",msg->reses[0].results.size(), msg->reses[1].results.size(), msg->reses[2].results.size());
         for (int i = 0; i < 3; i++)
         {
@@ -476,6 +490,11 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                     float bottom_center_y = float(y2);
                     float x = 0.0f;
                     float y = 0.0f;
+                    if (m_send_camera_image == 1 && msg->Images.size() == 3)
+                    {
+                        cv::Rect rect(x1,y1, x2 - x1, y2 - y1);
+                        cv::rectangle(sendImage[i], rect, cv::Scalar(0, 0, 255), 5, cv::LINE_8, 0);
+                    }
                     if (i == 0)
                     {
                         if(m_camera_one_center_type == 1 || m_camera_one_center_type == 2)
@@ -568,6 +587,11 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                                 continue;
                         }
                     }
+                    if (m_send_camera_image == 1 && msg->Images.size() == 3)
+                    {
+                        cv::Point2f point(bottom_center_x, bottom_center_y);
+                        cv::circle(sendImage[i], point, (int)3, cv::Scalar(0, 255, 0), 1);
+                    }
                     if(i == 0 && m_camera_one_center_type != 2)
                         objMsg.x = objMsg.x + (objMsg.l / 2);
                     int duplicate = 0;
@@ -609,11 +633,25 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
                             //SDK_LOG(SDK_INFO, "send camera i = %d, x = %f,y = %f",i, objMsg.x, objMsg.y);
                             objsPtr->array.push_back(objMsg);
                         }
+                        if (m_send_camera_image == 1 && msg->Images.size() == 3)
+                        {
+                            char name[512] = {};
+                            sprintf(name, "[%.2f,%.2f]", objMsg.x, objMsg.y);
+                            cv::putText(sendImage[i], name, cv::Point(bottom_center_x, bottom_center_y), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 23, 0), 1, 1);
+                        }
                     }
                 }
             }
         }
-
+        if (m_send_camera_image == 1 && msg->Images.size() == 3)
+        {
+            sensor_msgs::ImagePtr img_cameraFront = cv_bridge::CvImage(std_msgs::Header(), "bgr8", sendImage[0]).toImageMsg();
+            m_pubCameraFront.publish(img_cameraFront);
+            sensor_msgs::ImagePtr img_cameraRight = cv_bridge::CvImage(std_msgs::Header(), "bgr8", sendImage[1]).toImageMsg();
+            m_pubCameraRight.publish(img_cameraRight);
+            sensor_msgs::ImagePtr img_cameraLeft = cv_bridge::CvImage(std_msgs::Header(), "bgr8", sendImage[2]).toImageMsg();
+            m_pubCameraLeft.publish(img_cameraLeft);
+        }
     }
     //SDK_LOG(SDK_INFO, "m_objsQueue push size = %d", objsPtr->array.size());
     m_objsQueue.push(objsPtr);

TrackingRosEx.h

@@ -51,6 +51,9 @@ public:
     ros::Publisher m_pubCloud;//发送点云数据
     ros::Publisher m_pubBoundingBoxes;//发送3D框信息
     ros::Publisher m_pubMarker;//发送marker信息
+    ros::Publisher m_pubCameraFront;//发送前相机的图
+    ros::Publisher m_pubCameraRight;//发送右相机的图
+    ros::Publisher m_pubCameraLeft;//发送左相机的图
 
     std::vector<std::vector<double>> m_trans;
     std::vector<std::vector<double>> m_kitti2origin;
@@ -67,4 +70,5 @@ public:
     int m_camera_debug_type = 0;//调试相机类型是否修改为7，0是正常，1是调试为7
     int m_right_left_camera_len = 15;//左右相机的显示距离米数
     int m_front_camera_len = 0;//-前相机过滤的距离米数,0是不过滤，70就是过滤70米，但不过滤行人和机动车
+    int m_send_camera_image = 0;//是否发送相机图像，0是不发送，1是发送
 };