track add arrow

TrackingRos.cpp

@@ -3,6 +3,7 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <jsk_recognition_msgs/BoundingBoxArray.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <tf2/LinearMath/Quaternion.h>
 #include <math.h>
 #include "LogBase.h"
 #include <yaml-cpp/yaml.h>
@@ -678,7 +679,7 @@ void TrackingRos::ThreadTrackingProcess()
                         obj.acc_y = iter.second->get_acceleration_y(0.1);
                         iter.second->GetColorInfo(obj.color_name);
 #ifdef _ABUZHABI_
-						double lon = 0.0f;
+                        double lon = 0.0f;
                         double lat = 0.0f;
                         double loc_lon[8] = {};
                         double loc_lat[8] = {};
@@ -713,6 +714,9 @@ void TrackingRos::ThreadTrackingProcess()
                 }
                 if (is_need_send == 1)
                 {
+                    //暂存rot_y
+                    float rot_y_angle_temp = obj.rot_y;
+
                     //修正rot_y值,变为角度
                     while (obj.rot_y >= _PI_)
                     {
@@ -805,21 +809,29 @@ void TrackingRos::ThreadTrackingProcess()
                         marker.id = i;
                         marker.header.frame_id = "/Pandar64";
                         marker.action = visualization_msgs::Marker::ADD;
-                        marker.type = visualization_msgs::Marker::SPHERE;
+                        marker.type = visualization_msgs::Marker::ARROW;
                         marker.pose.position.x = obj.x;
                         marker.pose.position.y = obj.y;
-                        marker.pose.position.z = obj.z + obj.h / 2;
-                        marker.pose.orientation.x = 0.0;
-                        marker.pose.orientation.y = 0.0;
-                        marker.pose.orientation.z = 0.0;
-                        marker.pose.orientation.w = 1.0;
-                        marker.scale.x = 0.5;
-                        marker.scale.y = 0.5;
-                        marker.scale.z = 0.5;
+                        //marker.pose.position.z = obj.z + obj.h / 2;
+                        marker.pose.position.z = obj.z;
+                        //marker.pose.orientation.x = 0.0;
+                        //marker.pose.orientation.y = 0.0;
+                        //marker.pose.orientation.z = 0.0;
+                        //marker.pose.orientation.w = 1.0;
+                        tf2::Quaternion myquater;
+                        myquater.setRPY( 0, 0, rot_y_angle_temp );
+                        //myquater.normalize();
+                        marker.pose.orientation.x = myquater.x();
+                        marker.pose.orientation.y = myquater.y();
+                        marker.pose.orientation.z = myquater.z();
+                        marker.pose.orientation.w = myquater.w();
+                        marker.scale.x = 6;
+                        marker.scale.y = 1;
+                        marker.scale.z = 0.1;
                         marker.color.a = 1.0;
                         marker.color.r = 1.0;
-                        marker.color.g = 1.0;
-                        marker.color.b = 1.0;
+                        marker.color.g = 0;
+                        marker.color.b = 0;
                         marker.lifetime = ros::Duration(0.5);
                         sendMarkers.markers.emplace_back(marker);
                         i += 1;