change vtd_autoware.

src/ros/catkin/src/vtd_ros/CMakeLists.txt

@@ -3,6 +3,8 @@ project(vtd_ros)
 
 include(${CMAKE_SOURCE_DIR}/adcpilot.cmake)
 
+
+find_package(autoware_build_flags REQUIRED)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -10,6 +12,9 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   pluginlib
   sensor_msgs
+  autoware_msgs
+  std_msgs
+  tf
 )
 find_package(Boost REQUIRED)
 find_package(Protobuf REQUIRED)
@@ -72,6 +77,7 @@ include_directories(
   ${CMAKE_CURRENT_BINARY_DIR}/..
   ${catkin_INCLUDE_DIRS} include
   src/VTD
+  ${catkin_INCLUDE_DIRS} 
   ${Boost_INCLUDE_DIR}
   ${autoware_msgs_INCLUDE_DIRS}
 )

src/ros/catkin/src/vtd_ros/package.xml

@@ -51,7 +51,9 @@
   <build_depend>sensor_msgs</build_depend>
   <run_depend>sensor_msgs</run_depend>
 
-
+    <build_depend>autoware_msgs</build_depend>
+   <run_depend>autoware_msgs</run_depend>
+    <build_depend>tf</build_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/ros/catkin/src/vtd_ros/src/vtd_autoware.cpp

@@ -3,9 +3,14 @@
 #include <iostream>
 #include "vtd.pb.h"
 
+#include "RDBHandler.hh"
+
+#include "tf/transform_datatypes.h"
+
 #define DEFAULT_BUFFER      204800
 
 vtd_autoware::vtd_autoware()
+: private_nh_("~")
 {
 
 }
@@ -15,6 +20,176 @@ vtd_autoware::~vtd_autoware()
 
 }
 
+void vtd_autoware::createROSPubSub()
+{
+    pub_objects_ = nh_.advertise<autoware_msgs::DetectedObjectArray>("/detection/lidar_detector/objects", 1);
+}
+
+
+void vtd_autoware::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> & xvectorrdbdata,autoware_msgs::DetectedObjectArray  & xobjectsarray)
+{
+    int i;
+    int j;
+    std::cout<<" obj size: "<<xvectorrdbdata.size()<<std::endl;
+    for(i=0;i<(int)xvectorrdbdata.size();i++)
+    {
+        iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
+        for(j=0;j<prdbdata->mrdbitem_size();j++)
+        {
+            iv::vtd::rdbitem * pitem = prdbdata->mutable_mrdbitem(j);
+            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
+            {
+                RDB_OBJECT_STATE_t xobj;
+    //            std::cout<<" structure RDB_OBJECT_STATE_t size: "<<sizeof(xobj)<<"data size: "<<xrdbitem.mnpkgdatasize<<std::endl;
+                memcpy(&xobj,pitem->pkgdata().data(),sizeof(RDB_OBJECT_STATE_t));
+   //             std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
+                if(xobj.base.id == 1)
+                {
+                    mfX = xobj.base.pos.x;
+                    mfY = xobj.base.pos.y;
+                    mfHeading = xobj.base.pos.h;
+                }
+                else
+                {
+                    autoware_msgs::DetectedObject * pobject = NULL;
+    
+                    int k;
+                    for(k=0;k<xobjectsarray.objects.size();k++)
+                    {
+                        if(xobjectsarray.objects[k].id== xobj.base.id)
+                        {
+                            pobject = &xobjectsarray.objects[k];
+                            break;
+                        }
+                    }
+                    if(pobject  == NULL)
+                    {
+                        autoware_msgs::DetectedObject xobject;
+                        xobjectsarray.objects.push_back(xobject);
+                        pobject =  &xobjectsarray.objects[xobjectsarray.objects.size() -1];
+                        
+                    }
+                    pobject->id = xobj.base.id;
+                    double x,y;
+                    x = xobj.base.pos.x - mfX;
+                    y = (xobj.base.pos.y - mfY)*(1.0);
+                    double relx,rely;
+                    double beta  = mfHeading*(-1.0);
+                    relx = x*cos(beta) - y*sin(beta);
+                    rely = x*sin(beta) + y*cos(beta);
+                    double vx,vy;
+                    vx = xobj.ext.speed.x;
+                    vy = xobj.ext.speed.y;
+                    vx = vx - mfvx;
+                    vy = vy - mfvy;
+                    double relvx,relvy;
+                    relvx = vx*cos(beta) - vy*sin(beta);
+                    relvy = vx*sin(beta) + vy*sin(beta);
+                    double fobjheading = xobj.base.pos.h;
+                    fobjheading = fobjheading - mfHeading;
+                    pobject->score = 1.0;
+                    pobject->label = "car";
+
+
+                     //   object.header = in_header;
+                    pobject->valid = true;
+                    pobject->pose_reliable = true;
+
+                    pobject->pose.position.x = x;
+                    pobject->pose.position.y = y;
+                    pobject->pose.position.z = xobj.base.pos.z;
+
+    // Trained this way
+                     float yaw = fobjheading;//detections[i * OUTPUT_NUM_BOX_FEATURE_ + 6];
+  //  yaw += M_PI/2;
+   // yaw = std::atan2(std::sin(yaw), std::cos(yaw));
+                    geometry_msgs::Quaternion q = tf::createQuaternionMsgFromYaw(-yaw);
+                    pobject->pose.orientation = q;
+
+
+    // Again: Trained this way
+                    pobject->dimensions.x = xobj.base.geo.dimX;
+                    pobject->dimensions.y = xobj.base.geo.dimY;
+                    pobject->dimensions.z = xobj.base.geo.dimZ;
+
+                    pobject->velocity.linear.x = sqrt(pow(relvx,2)+pow(relvy,2));
+
+
+//                    std::cout<<" x: "<<relx<<" y: "<<rely<<std::endl;
+                }
+
+
+            }
+        }
+    }
+
+}
+
+
+void vtd_autoware::threadobject()
+{
+    std::vector<iv::vtd::rdbdata> xvectorrdbdata;
+
+    int nseq = 1;
+    
+
+    int64_t nlastsharetime = std::chrono::system_clock::now().time_since_epoch().count();
+//    int nmaxobjid = 2;
+    int nshareinter = 20;  //100 ms share a data.
+    bool bshare = false;
+    while(mbRun)
+    {
+        int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+        int64_t timediff = abs(now-nlastsharetime)/1000000;
+        if(timediff >= nshareinter)
+        {
+            bshare = true;
+        }
+        if(bshare)
+        {
+            autoware_msgs::DetectedObjectArray objects;
+          //  iv::fusion::fusionobjectarray  xfusionarray;
+          //  ConvertToObjectArray(xvectorrdbdata,xfusionarray);
+            ConvertToObjectArray(xvectorrdbdata,objects);
+
+            pub_objects_.publish(objects);
+
+                objects.header.frame_id = "vtd";
+            objects.header.seq = nseq;
+//          objects.header.stamp = 1;
+
+            //share object data.
+            bshare = false;
+            nlastsharetime = now;
+            xvectorrdbdata.clear();
+        }
+        std::unique_lock<std::mutex> lk(mmutexcvrdb);
+        if(mcvrdb.wait_for(lk,std::chrono::milliseconds(10)) == std::cv_status::timeout)
+        {
+            lk.unlock();
+            continue;
+        }
+        else
+        {
+            lk.unlock();
+
+        }
+        mmutexrdb.lock();
+        int i;
+        for(i=0;i<(int)mvectorrdbdata.size();i++)
+        {
+            xvectorrdbdata.push_back(mvectorrdbdata[i]);
+        }
+        mvectorrdbdata.clear();
+        mmutexrdb.unlock();
+
+
+
+
+    }
+}
+
+
 void vtd_autoware::UpdateVTD(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     (void)index;
@@ -24,7 +199,10 @@ void vtd_autoware::UpdateVTD(const char * strdata,const unsigned int nSize,const
     iv::vtd::rdbdata xrdbdata;
     if(xrdbdata.ParseFromArray(strdata,nSize))
     {
-
+        mmutexrdb.lock();
+        mvectorrdbdata.push_back(xrdbdata);
+        mmutexrdb.unlock();
+        mcvrdb.notify_all();
     }
 }
 

src/ros/catkin/src/vtd_ros/src/vtd_autoware.h

@@ -5,10 +5,15 @@
 #include <mutex>
 #include <condition_variable>
 #include <iostream>
+#include <vector>
 
 #include "modulecomm.h"
 
+#include "autoware_msgs/DetectedObjectArray.h"
 
+#include "vtd.pb.h"
+
+#include <ros/ros.h>
 
 class vtd_autoware
 {
@@ -16,10 +21,15 @@ public:
     vtd_autoware();
     ~vtd_autoware();
 
+public:
+    void createROSPubSub();
+
 private:
     void UpdateVTD(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
-  
+    void threadobject();
+
+    void ConvertToObjectArray(std::vector<iv::vtd::rdbdata> & xvectorrdbdata,autoware_msgs::DetectedObjectArray  & xobjectsarray);
 
 private:
     void * mpfromvtd;
@@ -32,6 +42,22 @@ private:
     double mflat0 = 39.1207274;
     double mflon0 = 117.0280033;
 
+        double mfX,mfY,mfHeading,mfvx,mfvy;
+
+    bool mbRun = true;
+
+        std::vector<iv::vtd::rdbdata> mvectorrdbdata;
+    std::mutex mmutexrdb;
+    std::mutex mmutexcvrdb;
+    std::condition_variable mcvrdb;
+
+    ros::Publisher pub_objects_;
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+
+
+
 };
 
 #endif // VTD_PILOT_H