提交发送点云框逻辑

ros2/TrackingRos2.cpp

@@ -22,7 +22,7 @@ namespace juefx_tracking {
                               this, std::placeholders::_1));
         std::string pub_topic = "perception/sensors_fusion/tracking/objects";
         m_tracking_res = this->create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>(pub_topic, 10);
-
+        m_pubMarker = this->create_publisher<visualization_msgs::msg::MarkerArray>("output/sensors_fusion_obj_markers", 10);
         //获取yaml文件
         std::string root_dir = this->declare_parameter<std::string>("root_dir", std::string(""));
         int run_mode = this->declare_parameter<int>("run_mode", 1);
@@ -73,6 +73,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr)
 {
     if(!objsPtr)
         return;
+    visualization_msgs::msg::MarkerArray makerArray = {};
     autoware_auto_perception_msgs::msg::TrackedObjects sendObjs = {};
     sendObjs.header = objsPtr->header;
     uint64_t timestamp = (unsigned long long)objsPtr->header.stamp.sec * 1e6 + (unsigned long long)objsPtr->header.stamp.nanosec * 1e-3;
@@ -269,7 +270,34 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr)
                 //    last_type,predict[0], predict[1], predict[2], predict[4], predict[5], predict[6],
                 //    obj.type,obj.x, obj.y, obj.z, obj.l, obj.w, obj.h, obj.rot_y, obj.v_x, obj.v_y, obj.v_z, obj.Lat, obj.Long);
             }
+            geometry_msgs::msg::Point& pos = obj.kinematics.pose_with_covariance.pose.position;
+            geometry_msgs::msg::Vector3& dimensions = obj.shape.dimensions;
+
+            visualization_msgs::msg::Marker marker = {};
+            marker.lifetime = rclcpp::Duration(1);
+            marker.header.frame_id = "/rslidar";
+            marker.type = visualization_msgs::msg::Marker::CUBE;
+            marker.action = visualization_msgs::msg::Marker::ADD;
+            marker.ns = "lane";
+            marker.id = 0;
 
+            marker.pose.position.x = pos.x;
+            marker.pose.position.y = pos.y;
+            marker.pose.position.z = pos.z;
+            Eigen::AngleAxisd V3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
+            Eigen::Quaterniond R_quat(V3);
+            marker.pose.orientation.x = R_quat.x();
+            marker.pose.orientation.y = R_quat.y();
+            marker.pose.orientation.z = R_quat.z();
+            marker.pose.orientation.w = R_quat.w();
+            marker.scale.x = dimensions.x;
+            marker.scale.y = dimensions.y;
+            marker.scale.z = dimensions.z;
+            marker.color.r = 0.0f;
+            marker.color.g = 0.0f;
+            marker.color.b = 0.5f;
+            marker.color.a = 0.5;
+            makerArray.markers.emplace_back(marker);
             // jsk_recognition_msgs::BoundingBox box = {};
             // box.label = obj.type;
             // box.value = obj.obj_id;
@@ -322,6 +350,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr)
     //SDK_LOG(SDK_INFO, "calculate objs size = %d", sendObjs.array.size());
 
     m_tracking_res->publish(sendObjs);
+    m_pubMarker->publish(makerArray);
     // m_pubBoundingBoxes.publish(sendBoxes);
     // objsPtr->cloud.header.frame_id = "/rslidar";
     // m_pubCloud.publish(objsPtr->cloud);

ros2/TrackingRos2.h

@@ -34,6 +34,7 @@ class TrackingRos2 : public rclcpp::Node {
 
     rclcpp::Subscription<wit_perception_msgs::msg::DetectedObjectsWithFeatureEx>::SharedPtr m_fusion_res;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr m_tracking_res;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pubMarker;
 
     void ReceiveFusionResMsg(const wit_perception_msgs::msg::DetectedObjectsWithFeatureEx & msg);