change catkin/rostopilot. add lgsvl/3d_truth for pilot run.

src/ros/catkin/src/opendrive_if/src/main.cpp

@@ -15,7 +15,7 @@ void threadtest()
     while(1)
     {
         visualization_msgs::Marker marker;
-        marker.header.frame_id = "/my_frame";
+        marker.header.frame_id = "/map";
         marker.header.stamp = ros::Time::now();
         marker.ns = "basic_shapes";
         marker.id = 0;
@@ -29,7 +29,7 @@ void threadtest()
      //   marker.pose.orientation.y = 0.0;
      //   marker.pose.orientation.z = 0.0;
      //   marker.pose.orientation.w = 1.0;
-         marker.scale.x = 0.01;
+         marker.scale.x = 0.1;
       //   marker.scale.y = 0.3;
     //     marker.scale.z = 1.0;
          marker.color.r = 0.0f;

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

@@ -12,20 +12,20 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
-#include<cv_bridge/cv_bridge.h>
-#include<sensor_msgs/image_encodings.h>
-#include<image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_transport/image_transport.h>
 
 #include <nav_msgs/Odometry.h>
 
-#include  "autoware_msgs/VehicleCmd.h"
+#include "autoware_msgs/VehicleCmd.h"
 #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 <geometry_msgs/TwistStamped.h>
 
 #include <visualization_msgs/Marker.h>
 
@@ -42,126 +42,129 @@ static std::string _point_msgname = "lidarpc_ros";
 static std::string _image_topic = "/image_raw";
 static std::string _image_msgname = "pic_ros";
 
-static std::string  _odom_topic = "/odom";
+static std::string _odom_topic = "/odom";
 static std::string _odom_msgname = "odom_ros";
 
-static std::string  _twistraw_topic = "/twist_raw";
+static std::string _twistraw_topic = "/twist_raw";
 
-static std::string  _ctrlcmd_topic = "/ctrl_raw";
-static std::string  _ctrlcmd_msgname = "ctrlcmd";
+static std::string _ctrlcmd_topic = "/ctrl_raw";
+static std::string _ctrlcmd_msgname = "ctrlcmd";
 static bool _buse_autowarectrlcmd = true;
 
-  ros::Subscriber points_sub;
-  ros::Subscriber odom_sub;
+static std::string _detect_msgname = "lidar_pointpillar";
+static bool _b_usetruthdetect = true;
 
-  ros::Subscriber twist_sub;
-  ros::Subscriber ctrlcmd_sub;
+ros::Subscriber points_sub;
+ros::Subscriber odom_sub;
+ros::Subscriber lgsvldetect_sub;
 
-  void * gpa_lidar;
-  void * gpa_image;
-  void * gpa_odom;
-  void * gpa_ctrlcmd;
+ros::Subscriber twist_sub;
+ros::Subscriber ctrlcmd_sub;
 
+void *gpa_lidar;
+void *gpa_image;
+void *gpa_odom;
+void *gpa_ctrlcmd;
+void * gpa_detect;
 
-ros::Publisher  test_cmd_pub;
+ros::Publisher test_cmd_pub;
 ros::Publisher lgsvl_state_pub;
 ros::Publisher lgsvl_cmd_pub;
 
-
-static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
+static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
 {
-    pcl::PointCloud<pcl::PointXYZI>  point_cloud;
+    pcl::PointCloud<pcl::PointXYZI> point_cloud;
 
     ros::Time current_scan_time = input->header.stamp;
     static ros::Time previous_scan_time = current_scan_time;
 
     pcl::fromROSMsg(*input, point_cloud);
 
-        char * strOut = new char[4+sizeof(point_cloud.header.frame_id.size()) + 4+8+point_cloud.width * sizeof(pcl::PointXYZI)];
+    char *strOut = new char[4 + sizeof(point_cloud.header.frame_id.size()) + 4 + 8 + point_cloud.width * sizeof(pcl::PointXYZI)];
 
-    int * pHeadSize = (int *)strOut;
-    *pHeadSize = 4 + point_cloud.header.frame_id.size()+4+8;
-    memcpy(strOut+4,point_cloud.header.frame_id.c_str(),point_cloud.header.frame_id.size());
-    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud.header.frame_id.size()));
+    int *pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud.header.frame_id.size() + 4 + 8;
+    memcpy(strOut + 4, point_cloud.header.frame_id.c_str(), point_cloud.header.frame_id.size());
+    pcl::uint32_t *pu32 = (pcl::uint32_t *)(strOut + 4 + sizeof(point_cloud.header.frame_id.size()));
     *pu32 = point_cloud.header.seq;
-    memcpy(strOut+4+sizeof(point_cloud.header.frame_id.size()) + 4,&point_cloud.header.stamp,8);
-    pcl::PointXYZI * p;
-    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud.header.frame_id.size()) + 4+8 );
-    memcpy(p,point_cloud.points.data(),point_cloud.width * sizeof(pcl::PointXYZI));
+    memcpy(strOut + 4 + sizeof(point_cloud.header.frame_id.size()) + 4, &point_cloud.header.stamp, 8);
+    pcl::PointXYZI *p;
+    p = (pcl::PointXYZI *)(strOut + 4 + sizeof(point_cloud.header.frame_id.size()) + 4 + 8);
+    memcpy(p, point_cloud.points.data(), point_cloud.width * sizeof(pcl::PointXYZI));
 
-    iv::modulecomm::ModuleSendMsg(gpa_lidar,strOut,4+sizeof(point_cloud.header.frame_id.size()) + 4+8+point_cloud.width * sizeof(pcl::PointXYZI));
+    iv::modulecomm::ModuleSendMsg(gpa_lidar, strOut, 4 + sizeof(point_cloud.header.frame_id.size()) + 4 + 8 + point_cloud.width * sizeof(pcl::PointXYZI));
     delete strOut;
- //   pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
-  //  int scan_points_num = filtered_scan_ptr->size();
+    //   pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
+    //  int scan_points_num = filtered_scan_ptr->size();
 }
 
-
 #ifndef CV_IMWRITE_JPEG_QUALITY
 #define CV_IMWRITE_JPEG_QUALITY 1
 #endif
 
 int gindex = 0;
 int gcompress = 1;
-void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
+void imageCalllback(const sensor_msgs::ImageConstPtr &msg)
 {
-//	ROS_INFO("Received \n");
-	try{
-
-        cv::Mat mat1 = cv_bridge::toCvShare(msg, "bgr8")->image ;
-     //   cv::imshow( "video", mat1 );
-      //  cv::waitKey(30);
-
-        
-                    iv::vision::rawpic pic;
-            
-            qint64 time = QDateTime::currentMSecsSinceEpoch();
-            QTime xg;
-            xg.start();
-            pic.set_time(time);
-            pic.set_index(gindex);gindex++;
-
-
-            pic.set_elemsize(mat1.elemSize());
-            pic.set_width(mat1.cols);
-            pic.set_height(mat1.rows);
-            pic.set_mattype(mat1.type());
-
-
-            std::vector<int> param = std::vector<int>(2);
-            param[0] = CV_IMWRITE_JPEG_QUALITY;
-            param[1] = 95; // default(95) 0-100
-            std::vector<unsigned char> buff;
-            if(gcompress == 1)
-            {
-                cv::imencode(".jpg", mat1, buff, param);
-                pic.set_picdata(buff.data(),buff.size());
-                buff.clear();
-                pic.set_type(2);
-            }
-            else
-            {
-                pic.set_picdata(mat1.data,mat1.rows*mat1.cols*mat1.elemSize());
-                pic.set_type(1);
-            }
-
-             int nSize = pic.ByteSize();
-             std::shared_ptr<char>  str_ptr = std::shared_ptr<char>(new char[nSize]);
-            bool bser = pic.SerializeToArray(str_ptr.get(),nSize);
-            if(bser)iv::modulecomm::ModuleSendMsg(gpa_image,str_ptr.get(),nSize);
-            else
-            {
-                std::cout<<"imageCalllback "<< "serialize error. "<<std::endl;
-            }
+    //	ROS_INFO("Received \n");
+    try
+    {
+
+        cv::Mat mat1 = cv_bridge::toCvShare(msg, "bgr8")->image;
+        //   cv::imshow( "video", mat1 );
+        //  cv::waitKey(30);
+
+        iv::vision::rawpic pic;
+
+        qint64 time = QDateTime::currentMSecsSinceEpoch();
+        QTime xg;
+        xg.start();
+        pic.set_time(time);
+        pic.set_index(gindex);
+        gindex++;
+
+        pic.set_elemsize(mat1.elemSize());
+        pic.set_width(mat1.cols);
+        pic.set_height(mat1.rows);
+        pic.set_mattype(mat1.type());
+
+        std::vector<int> param = std::vector<int>(2);
+        param[0] = CV_IMWRITE_JPEG_QUALITY;
+        param[1] = 95; // default(95) 0-100
+        std::vector<unsigned char> buff;
+        if (gcompress == 1)
+        {
+            cv::imencode(".jpg", mat1, buff, param);
+            pic.set_picdata(buff.data(), buff.size());
+            buff.clear();
+            pic.set_type(2);
+        }
+        else
+        {
+            pic.set_picdata(mat1.data, mat1.rows * mat1.cols * mat1.elemSize());
+            pic.set_type(1);
+        }
+
+        int nSize = pic.ByteSize();
+        std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nSize]);
+        bool bser = pic.SerializeToArray(str_ptr.get(), nSize);
+        if (bser)
+            iv::modulecomm::ModuleSendMsg(gpa_image, str_ptr.get(), nSize);
+        else
+        {
+            std::cout << "imageCalllback "
+                      << "serialize error. " << std::endl;
+        }
     }
-    catch( cv_bridge::Exception& e )
+    catch (cv_bridge::Exception &e)
     {
-        ROS_ERROR( "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str() );
+        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
     }
 }
 
-void  odomCalllback(const  nav_msgs::OdometryConstPtr  & msg)
+void odomCalllback(const nav_msgs::OdometryConstPtr &msg)
 {
-  //  std::cout<<" position x "<<msg->pose.pose.position.x<<std::endl;
+    //  std::cout<<" position x "<<msg->pose.pose.position.x<<std::endl;
     qint64 msgtime = msg->header.stamp.toSec();
     msgtime = msgtime * 1000;
     iv::ros::pose_position_def xpose_position;
@@ -187,75 +190,75 @@ void  odomCalllback(const  nav_msgs::OdometryConstPtr  & msg)
     xodom.mutable_pose_orientation()->CopyFrom(xpose_orientation);
     xodom.mutable_twist_angula()->CopyFrom(twist_angular);
     xodom.mutable_twist_linear()->CopyFrom(twist_linear);
- //   std::cout<<" pose x : "<<xodom.pose_position().x()<<std::endl;
+    //   std::cout<<" pose x : "<<xodom.pose_position().x()<<std::endl;
 
-     int nSize = xodom.ByteSize();
-    std::shared_ptr<char>  str_ptr = std::shared_ptr<char>(new char[nSize]);
-     bool bser = xodom.SerializeToArray(str_ptr.get(),nSize);
-     if(bser)iv::modulecomm::ModuleSendMsg(gpa_odom,str_ptr.get(),nSize);
-     else
+    int nSize = xodom.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nSize]);
+    bool bser = xodom.SerializeToArray(str_ptr.get(), nSize);
+    if (bser)
+        iv::modulecomm::ModuleSendMsg(gpa_odom, str_ptr.get(), nSize);
+    else
     {
-         std::cout<<"odomCalllback "<< "serialize error. "<<std::endl;
+        std::cout << "odomCalllback "
+                  << "serialize error. " << std::endl;
     }
-
 }
 
-
 void testvh()
 {
-     int x = 0;
-     while(1)
-     {
-
-         lgsvl_msgs::VehicleStateData xstate;
-         xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
-         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
-         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_HIGH;
-         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_PARKING;
-         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
-         xstate.hand_brake_active = true;
-         xstate.horn_active = true;
-         xstate.autonomous_mode_active = true;
-         lgsvl_state_pub.publish(xstate);
-         x++;
-         std::this_thread::sleep_for(std::chrono::milliseconds(10));
-     }
+    int x = 0;
+    while (1)
+    {
+
+        lgsvl_msgs::VehicleStateData xstate;
+        xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
+        xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+        xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_HIGH;
+        xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_PARKING;
+        xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+        xstate.hand_brake_active = true;
+        xstate.horn_active = true;
+        xstate.autonomous_mode_active = true;
+        lgsvl_state_pub.publish(xstate);
+        x++;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
 }
 
-void  twistrawCalllback(const  geometry_msgs::TwistStampedConstPtr  & msg)
+void twistrawCalllback(const geometry_msgs::TwistStampedConstPtr &msg)
 {
     autoware_msgs::VehicleCmd xcmd;
     xcmd.twist_cmd.twist = msg->twist;
     test_cmd_pub.publish(xcmd);
 }
 
-void ctrlrawCallback(const autoware_msgs::ControlCommandStampedConstPtr & msg)
+void ctrlrawCallback(const autoware_msgs::ControlCommandStampedConstPtr &msg)
 {
     iv::autoware::autowarectrlcmd xcmd;
     xcmd.set_linear_acceleration(msg->cmd.linear_acceleration);
     xcmd.set_linear_velocity(msg->cmd.linear_velocity);
     xcmd.set_steering_angle(msg->cmd.steering_angle);
     int nbytesize = (int)xcmd.ByteSize();
-    std::shared_ptr<char>  str_ptr = std::shared_ptr<char>(new char[nbytesize]);
-    if(xcmd.SerializeToArray(str_ptr.get(),nbytesize))
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if (xcmd.SerializeToArray(str_ptr.get(), nbytesize))
     {
-        iv::modulecomm::ModuleSendMsg(gpa_ctrlcmd,str_ptr.get(),nbytesize);
+        iv::modulecomm::ModuleSendMsg(gpa_ctrlcmd, str_ptr.get(), nbytesize);
     }
     else
     {
-        std::cout<<" ctrlrawCallback Serialize fail."<<std::endl;
+        std::cout << " ctrlrawCallback Serialize fail." << std::endl;
     }
 }
 
-void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+void UpdateDecition(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
 
     static int x = 0;
     iv::brain::decition xdecition;
-    std::cout<<" nSize : "<<nSize<<std::endl;
-    if(!xdecition.ParseFromArray(strdata,nSize))
+    std::cout << " nSize : " << nSize << std::endl;
+    if (!xdecition.ParseFromArray(strdata, nSize))
     {
-        std::cout<<" UpdateDecition Parse error."<<std::endl;
+        std::cout << " UpdateDecition Parse error." << std::endl;
         return;
     }
     /*
@@ -265,64 +268,68 @@ void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned
          xcmd.ctrl_cmd.linear_velocity = 30.0;
          test_cmd_pub.publish(xcmd);
     */
-   lgsvl_msgs::VehicleStateData xstate;
-   lgsvl_msgs::VehicleControlData xctrl;
-    if(xdecition.leftlamp() == true)
+    lgsvl_msgs::VehicleStateData xstate;
+    lgsvl_msgs::VehicleControlData xctrl;
+    if (xdecition.leftlamp() == true)
     {
         xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
     }
     else
     {
-        if(xdecition.rightlamp() == true)
-        { 
+        if (xdecition.rightlamp() == true)
+        {
             xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_RIGHT;
         }
         else
             xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_OFF;
     }
-         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
-         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_OFF;
-         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_DRIVE;
-         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
-         xstate.hand_brake_active = false;
-         xstate.horn_active = true;
-         xstate.autonomous_mode_active = true;
-    
-    if(xdecition.brake() < 0.00001)
+    xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+    xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_OFF;
+    xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_DRIVE;
+    xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+    xstate.hand_brake_active = false;
+    xstate.horn_active = true;
+    xstate.autonomous_mode_active = true;
+
+    if (xdecition.brake() < 0.00001)
     {
-        xctrl.acceleration_pct = xdecition.accelerator() /5.0;
-        if(xctrl.acceleration_pct >1.0)xctrl.acceleration_pct = 1.0;
-        if(xctrl.acceleration_pct<0.0)xctrl.acceleration_pct = 0.0;
+        xctrl.acceleration_pct = xdecition.accelerator() / 5.0;
+        if (xctrl.acceleration_pct > 1.0)
+            xctrl.acceleration_pct = 1.0;
+        if (xctrl.acceleration_pct < 0.0)
+            xctrl.acceleration_pct = 0.0;
         xctrl.braking_pct = 0.0;
     }
     else
     {
         xctrl.acceleration_pct = 0.0;
-        xctrl.braking_pct = xdecition.brake()/100.0;
+        xctrl.braking_pct = xdecition.brake() / 100.0;
     }
 
     //        xctrl.acceleration_pct = 0.05;
-     //   xctrl.braking_pct = 0;
-    xctrl.target_gear =  lgsvl_msgs::VehicleControlData::GEAR_DRIVE;
-    xctrl.target_wheel_angle = 0.1 *  (-1)* xdecition.wheelangle() *M_PI/180.0;
-    xctrl.target_wheel_angular_rate = 300 * M_PI/180.0;
+    //   xctrl.braking_pct = 0;
+    xctrl.target_gear = lgsvl_msgs::VehicleControlData::GEAR_DRIVE;
+    xctrl.target_wheel_angle = 0.1 * (-1) * xdecition.wheelangle() * M_PI / 180.0;
+    xctrl.target_wheel_angular_rate = 300 * M_PI / 180.0;
 
     lgsvl_state_pub.publish(xstate);
     lgsvl_cmd_pub.publish(xctrl);
-
 }
 
 #include <cmath>
 
-struct Quaternion {
+struct Quaternion
+{
     double w, x, y, z;
 };
 
-struct EulerAngles {
+struct EulerAngles
+{
     double roll, pitch, yaw;
 };
 
-EulerAngles ToEulerAngles(Quaternion q) {
+EulerAngles ToEulerAngles(Quaternion q)
+{
     EulerAngles angles;
 
     // roll (x-axis rotation)
@@ -345,23 +352,23 @@ EulerAngles ToEulerAngles(Quaternion q) {
     return angles;
 }
 
-int CovertBoundingToObject(lgsvl_msgs::Detection3DArray xDetect, iv::lidar::objectarray & xobjarray)
+int ConvertBoundingToObject(lgsvl_msgs::Detection3DArray xDetect, iv::lidar::objectarray &xobjarray)
 {
     int nobjsize = xDetect.detections.size();
     int i;
-    for(i=0;i<nobjsize;i++)
+    for (i = 0; i < nobjsize; i++)
     {
-        iv::lidar::lidarobject * pobj = xobjarray.add_obj();
-        lgsvl_msgs::Detection3D * pdetect =  &xDetect.detections[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;
+        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;
+        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);
@@ -373,71 +380,106 @@ int CovertBoundingToObject(lgsvl_msgs::Detection3DArray xDetect, iv::lidar::obje
         quat.w = pdetect->bbox.position.orientation.w;
         EulerAngles EA = ToEulerAngles(quat);
         pobj->set_tyaw(EA.yaw);
+        pobj->set_mntype(0);
     }
     return 0;
 }
 
-int main(int argc, char *argv[])
+void lgsvldetectCalllback(const  lgsvl_msgs::Detection3DArray    &msg)
 {
-	ros::init(argc, argv, "rostopilot");
-	ros::NodeHandle n;
-	ros::Rate loop_rate(100);
-
-	    ros::NodeHandle private_nh("~");
-    private_nh.getParam("points_node",_point_topic);
-	private_nh.getParam("points_msgname",_point_msgname);
-    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
-	std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
+     iv::lidar::objectarray xlidararray;
+    std::cout<<" receceive lgsvl box: "<<msg.detections.size()<<std::endl;
 
-    private_nh.getParam("image_node",_image_topic);
-	private_nh.getParam("image_msgname",_image_msgname);
-    std::cout<<"_image_topic is "<<_image_topic<<std::endl;
-	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
+    ConvertBoundingToObject(msg,xlidararray);
 
-        private_nh.getParam("odom_node",_odom_topic);
-	private_nh.getParam("odom_msgname",_odom_msgname);
-    std::cout<<"_odom_topic is "<<_odom_topic<<std::endl;
-	std::cout<<"_odom_msgname : "<<_odom_msgname<<std::endl;
-
-    private_nh.getParam("autoware_twist_raw",_twistraw_topic);
-    std::cout<<"  _twist_topic : "<<_twistraw_topic<<std::endl;
+    if(_b_usetruthdetect)
+    {
+        int nbytesize = xlidararray.ByteSize();
+        std::shared_ptr<char> pstr_pstr = std::shared_ptr<char>(new char[nbytesize]);
+        if(xlidararray.SerializeToArray(pstr_pstr.get(),nbytesize))
+        {
+            iv::modulecomm::ModuleSendMsg(gpa_detect,pstr_pstr.get(),nbytesize);
+        }
+        else
+        {
+            std::cout<<" lgsvldetectCalllback  SerializeToArray Fail."<<std::endl;
+        }
+    }
 
-    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;
-    std::cout<<"  buseautowrectrl: "<<_buse_autowarectrlcmd<<std::endl;
+    
+}
 
-    test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
-                "/vehicle_cmd", 10);
-    lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state",10);
-    lgsvl_cmd_pub = private_nh.advertise<lgsvl_msgs::VehicleControlData>("/vehicle_ctrl",10);
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "rostopilot");
+    ros::NodeHandle n;
+    ros::Rate loop_rate(100);
+
+    ros::NodeHandle private_nh("~");
+    private_nh.getParam("points_node", _point_topic);
+    private_nh.getParam("points_msgname", _point_msgname);
+    std::cout << "_point_topic is " << _point_topic << std::endl;
+    std::cout << "_point_msgname : " << _point_msgname << std::endl;
+
+    private_nh.getParam("image_node", _image_topic);
+    private_nh.getParam("image_msgname", _image_msgname);
+    std::cout << "_image_topic is " << _image_topic << std::endl;
+    std::cout << "_image_msgname : " << _image_msgname << std::endl;
+
+    private_nh.getParam("odom_node", _odom_topic);
+    private_nh.getParam("odom_msgname", _odom_msgname);
+    std::cout << "_odom_topic is " << _odom_topic << std::endl;
+    std::cout << "_odom_msgname : " << _odom_msgname << std::endl;
+
+    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("useautowrectrl", _buse_autowarectrlcmd);
+    std::cout << "  ctrlcmd_topic: " << _ctrlcmd_topic << std::endl;
+    std::cout << "  ctrlcmd_msgname: " << _ctrlcmd_msgname << std::endl;
+    std::cout << "  buseautowrectrl: " << _buse_autowarectrlcmd << std::endl;
+
+    private_nh.getParam("detect_msgname", _detect_msgname);
+    private_nh.getParam("use_lgsvl_detect", _b_usetruthdetect);
+
+    test_cmd_pub = private_nh.advertise<autoware_msgs::VehicleCmd>(
+        "/vehicle_cmd", 10);
+    lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state", 10);
+    lgsvl_cmd_pub = private_nh.advertise<lgsvl_msgs::VehicleControlData>("/vehicle_ctrl", 10);
+
+    gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(), 20000000, 1);
+    gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(), 20000000, 1);
+    gpa_odom = iv::modulecomm::RegisterSend(_odom_msgname.data(), 10000, 1);
+    gpa_ctrlcmd = iv::modulecomm::RegisterSend(_ctrlcmd_msgname.data(), 1000, 1);
+    
+    if(_b_usetruthdetect)
+    {
+        gpa_detect = iv::modulecomm::RegisterSend(_detect_msgname.data(),1000000,1);
+    }
 
-	gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
-    gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(),20000000,1);
-    gpa_odom = iv::modulecomm::RegisterSend(_odom_msgname.data(),10000,1);
-    gpa_ctrlcmd = iv::modulecomm::RegisterSend(_ctrlcmd_msgname.data(),1000,1);
+    points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
 
-  points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
+    image_transport::ImageTransport it(private_nh);
+    image_transport::Subscriber sub = it.subscribe(_image_topic, 1, imageCalllback);
 
-      image_transport::ImageTransport it(private_nh);
-    image_transport::Subscriber sub = it.subscribe( _image_topic, 1, imageCalllback );
+    odom_sub = private_nh.subscribe(_odom_topic, 1, odomCalllback);
 
-    odom_sub = private_nh.subscribe(_odom_topic,1,odomCalllback);
+    twist_sub = private_nh.subscribe(_twistraw_topic, 1, twistrawCalllback);
+    ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic, 1, ctrlrawCallback);
 
-twist_sub = private_nh.subscribe(_twistraw_topic,1,twistrawCalllback);
-ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic,1,ctrlrawCallback);
+    lgsvldetect_sub = private_nh.subscribe("/simulator/ground_truth/3d_detections",1,lgsvldetectCalllback);
 
-   void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
-  //   std::thread * pnew = new std::thread(testvh);
+    void *pdecition = iv::modulecomm::RegisterRecv("deciton", UpdateDecition);
+    //   std::thread * pnew = new std::thread(testvh);
 
-  visualization_msgs::Marker marker;
+    visualization_msgs::Marker marker;
 
-  marker.type = visualization_msgs::Marker::LINE_LIST;
+    marker.type = visualization_msgs::Marker::LINE_LIST;
 
-   ros::spin();
-   /*
+    ros::spin();
+    /*
 	while (ros::ok())
 	{
         ros::spinOnce();

src/tool/bqev_lidar_cnn_detect_view/mainwindow.cpp

@@ -366,7 +366,10 @@ void MainWindow::paintEvent(QPaintEvent *)
         QColor color;
         color.setRgb(0,255,0);
         painter->setPen(color);
-        painter->drawText(QPoint(lo.centroid().x()*10.0,lo.centroid().y()*(-10.0)),lo.type_name().data());
+        char strinfo[1000];
+        snprintf(strinfo,1000,"%s:%3.1f|%3.1f",lo.type_name().data(),lo.tyaw(),lo.velocity_linear_x());
+        painter->drawText(QPoint(lo.centroid().x()*10.0,lo.centroid().y()*(-10.0)),strinfo);
+ //       painter->drawText(QPoint(lo.centroid().x()*10.0,lo.centroid().y()*(-10.0)),lo.type_name().data());
     }