change detection_lidar_EuclideanClustering.

src/detection/detection_lidar_EuclideanClustering/detection_lidar_EuclideanClustering.pro

@@ -16,12 +16,18 @@ DEFINES += QT_DEPRECATED_WARNINGS
 # You can also select to disable deprecated APIs only up to a certain version of Qt.
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
-SOURCES += main.cpp
+SOURCES += main.cpp \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc
 
 !include(../../../include/ivpcl.pri ) {
     error( "Couldn't find the ivpcl.pri file!" )
 }
 
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -29,6 +35,8 @@ SOURCES += main.cpp
 
 HEADERS += \
     lidar_obstacle_detector/box.hpp \
-    lidar_obstacle_detector/obstacle_detector.hpp
+    lidar_obstacle_detector/obstacle_detector.hpp \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h
 
 LIBS += -lpcl_filters -lpcl_common -lpcl_search -lpcl_segmentation

src/detection/detection_lidar_EuclideanClustering/main.cpp

@@ -8,29 +8,64 @@
 
 #include "ivstdcolorout.h"
 
+#include "objectarray.pb.h"
+
 using namespace lidar_obstacle_detector;
 
 // Pointcloud Filtering Parameters
-bool USE_PCA_BOX;
-bool USE_TRACKING;
-float VOXEL_GRID_SIZE;
-Eigen::Vector4f ROI_MAX_POINT, ROI_MIN_POINT;
-float GROUND_THRESH;
-float CLUSTER_THRESH;
-int CLUSTER_MAX_SIZE, CLUSTER_MIN_SIZE;
-float DISPLACEMENT_THRESH, IOU_THRESH;
+static bool USE_PCA_BOX;
+static bool USE_TRACKING;
+static float VOXEL_GRID_SIZE;
+static Eigen::Vector4f ROI_MAX_POINT, ROI_MIN_POINT;
+static float GROUND_THRESH;
+static float CLUSTER_THRESH;
+static int CLUSTER_MAX_SIZE, CLUSTER_MIN_SIZE;
+static float DISPLACEMENT_THRESH, IOU_THRESH;
+
+static size_t obstacle_id_;
+static std::string bbox_target_frame_, bbox_source_frame_;
+static std::vector<Box> prev_boxes_, curr_boxes_;
+
+static std::shared_ptr<ObstacleDetector<pcl::PointXYZ>> obstacle_detector;
+
+static void * gpa_cluster,*gpa_ground,* gpa_detect;
+
+
+struct Quaternion {
+    double w, x, y, z;
+};
 
-size_t obstacle_id_;
-std::string bbox_target_frame_, bbox_source_frame_;
-std::vector<Box> prev_boxes_, curr_boxes_;
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
 
-std::shared_ptr<ObstacleDetector<pcl::PointXYZ>> obstacle_detector;
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
 
-void * gpa_cluster,*gpa_ground;
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
 
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
 
 void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& cloud_clusters)
 {
+
+    iv::lidar::objectarray  lidarobjvec;
   for (auto& cluster : cloud_clusters)
   {
     // Create Bounding Boxes
@@ -65,7 +100,48 @@ void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& c
 
 //    jsk_bboxes.boxes.emplace_back(transformJskBbox(box, bbox_header, pose_transformed));
 //    autoware_objects.objects.emplace_back(transformAutowareObject(box, bbox_header, pose_transformed));
+
+      iv::lidar::lidarobject * plidarobj = lidarobjvec.add_obj();
+
+      iv::lidar::PointXYZ center;
+      iv::lidar::Dimension xdim;
+      center.set_x(box.position(0));
+      center.set_y(box.position(1));
+      center.set_z(box.position(2));
+      xdim.set_x(box.dimension(0));
+      xdim.set_y(box.dimension(1));
+      xdim.set_z(box.dimension(2));
+
+      EulerAngles xAng;
+      Quaternion xQuat;
+      xQuat.w = box.quaternion.w();
+      xQuat.x = box.quaternion.x();
+      xQuat.y = box.quaternion.y();
+      xQuat.z = box.quaternion.z();
+      xAng = ToEulerAngles(xQuat);
+
+      plidarobj->set_id(box.id);
+      plidarobj->set_tyaw(xAng.yaw);
+      plidarobj->mutable_position()->CopyFrom(center);
+      plidarobj->mutable_centroid()->CopyFrom(center);
+      plidarobj->mutable_dimensions()->CopyFrom(xdim);
+
+
+  }
+
+
+  unsigned int ndatasize = lidarobjvec.ByteSize();
+  std::shared_ptr<char> pstr_data = std::shared_ptr<char>(new char[ndatasize]);
+  if(lidarobjvec.SerializeToArray(pstr_data.get(),static_cast<int>(ndatasize) ))
+  {
+      iv::modulecomm::ModuleSendMsg(gpa_detect,pstr_data.get(),ndatasize);
+  }
+  else
+  {
+      ivstdcolorout("Serialzie detect error.",iv::STDCOLOR_BOLDRED);
   }
+//   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+
 
 
   // Update previous bounding boxes
@@ -213,6 +289,8 @@ int main(int argc, char *argv[])
 
     obstacle_detector = std::make_shared<ObstacleDetector<pcl::PointXYZ>>();
 
+    gpa_detect = iv::modulecomm::RegisterSend("lidar_pointpillar",10000000,1);
+
     gpa_cluster = iv::modulecomm::RegisterSend("lidarpc_cluster",10000000,1);
     gpa_ground  = iv::modulecomm::RegisterSend("lidarpc_ground",10000000,1);
     void * pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);

src/tool/tool_grpcdb_query/grpcdbclient.cpp

@@ -1,5 +1,7 @@
 #include "grpcdbclient.h"
 
+#include <thread>
+
 grpcdbclient::grpcdbclient()
 {
     LoadIni();