change rostopilot. add Detect3D to objectarray convert.

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -133,6 +133,6 @@ include_directories(
 
 ## Declare a C++ executable
 add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc   ./../../../../include/msgtype/odom.pb.cc  ./../../../../include/msgtype/decition.pb.cc
-./../../../../include/msgtype/autowarectrlcmd.pb.cc  )
+./../../../../include/msgtype/autowarectrlcmd.pb.cc   ./../../../../include/msgtype/objectarray.pb.cc ./../../../../include/msgtype/object.pb.cc)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}  Qt5Core Qt5Xml modulecomm)
 

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -22,13 +22,19 @@
 #include "autoware_msgs/ControlCommandStamped.h"
 #include "lgsvl_msgs/VehicleControlData.h"
 #include "lgsvl_msgs/VehicleStateData.h"
+#include "lgsvl_msgs/BoundingBox3D.h"
+#include "lgsvl_msgs/Detection3DArray.h"
 
 #include  <geometry_msgs/TwistStamped.h>
 
+#include <visualization_msgs/Marker.h>
+
 #include "rawpic.pb.h"
 #include "odom.pb.h"
 #include "decition.pb.h"
 #include "autowarectrlcmd.pb.h"
+#include "object.pb.h"
+#include "objectarray.pb.h"
 
 static std::string _point_topic = "/points_raw";
 static std::string _point_msgname = "lidarpc_ros";
@@ -306,6 +312,71 @@ void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned
 
 }
 
+#include <cmath>
+
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // 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;
+}
+
+int CovertBoundingToObject(lgsvl_msgs::Detection3DArray xDetect, iv::lidar::objectarray & xobjarray)
+{
+    int nobjsize = xDetect.detections.size();
+    int i;
+    for(i=0;i<nobjsize;i++)
+    {
+        iv::lidar::lidarobject * pobj = xobjarray.add_obj();
+        lgsvl_msgs::Detection3D * pdetect =  &xDetect.detections[i];
+        pobj->set_velocity_linear_x(pdetect->velocity.linear.x);
+        pobj->set_type_name(pdetect->label.data());
+        pobj->set_id(pdetect->id);
+        iv::lidar::PointXYZ * pPoint = new iv::lidar::PointXYZ;
+        pPoint->set_x(pdetect->bbox.position.position.x);
+        pPoint->set_y(pdetect->bbox.position.position.y);
+        pPoint->set_z(pdetect->bbox.position.position.z);
+        pobj->set_allocated_centroid(pPoint);
+        iv::lidar::Dimension * pdim = new iv::lidar::Dimension;
+        pdim->set_x(pdetect->bbox.size.x);
+        pdim->set_y(pdetect->bbox.size.y);
+        pdim->set_z(pdetect->bbox.size.z);
+        pobj->set_allocated_dimensions(pdim);
+        Quaternion quat;
+        quat.x = pdetect->bbox.position.orientation.x;
+        quat.y = pdetect->bbox.position.orientation.y;
+        quat.z = pdetect->bbox.position.orientation.z;
+        quat.w = pdetect->bbox.position.orientation.w;
+        EulerAngles EA = ToEulerAngles(quat);
+        pobj->set_tyaw(EA.yaw);
+    }
+    return 0;
+}
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "rostopilot");
@@ -331,8 +402,8 @@ int main(int argc, char *argv[])
     private_nh.getParam("autoware_twist_raw",_twistraw_topic);
     std::cout<<"  _twist_topic : "<<_twistraw_topic<<std::endl;
 
-    private_nh.getParam("ctrlcmd_node",_ctrlcmd_topic);
-    private_nh.getParam("ctrlcmd_msgname",_ctrlcmd_msgname);
+    private_nh.getParam("_ctrlcmd_node",_ctrlcmd_topic);
+    private_nh.getParam("_ctrlcmd_msgname",_ctrlcmd_msgname);
     private_nh.getParam("useautowrectrl",_buse_autowarectrlcmd);
     std::cout<<"  ctrlcmd_topic: "<<_ctrlcmd_topic<<std::endl;
     std::cout<<"  ctrlcmd_msgname: "<<_ctrlcmd_msgname<<std::endl;
@@ -361,6 +432,10 @@ ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic,1,ctrlrawCallback);
    void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
   //   std::thread * pnew = new std::thread(testvh);
 
+  visualization_msgs::Marker marker;
+
+  marker.type = visualization_msgs::Marker::LINE_LIST;
+
    ros::spin();
    /*
 	while (ros::ok())