change vtd_pilot_if

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -26,7 +26,7 @@ vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
 
     mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);
 
-    mpafusion = iv::modulecomm::RegisterSend("fusion",10000000,1);
+    mpafusion = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
 
     mpthread = new std::thread(&vtd_pilot::threadego,this);
     mpthreadfusion = new std::thread(&vtd_pilot::threadobject,this);
@@ -99,7 +99,7 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
                     pobject->set_id(xobj.base.id);
                     double x,y;
                     x = xobj.base.pos.x - mfX;
-                    y = xobj.base.pos.y - mfY;
+                    y = (xobj.base.pos.y - mfY)*(1.0);
                     double relx,rely;
                     double beta  = mfHeading*(-1.0);
                     relx = x*cos(beta) - y*sin(beta);
@@ -118,15 +118,45 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
                     pobject->set_prob(1.0);
                     pobject->set_sensor_type(1);
                     pobject->set_yaw(fobjheading);
+
                     iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
-                    ppointxyz->set_x(relx);
-                    ppointxyz->set_y(rely);
+                    ppointxyz->set_x(rely*(-1.0));
+                    ppointxyz->set_y(relx);
                     ppointxyz->set_z(0);
                     iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
                     pdim->set_x(xobj.base.geo.dimX);
                     pdim->set_y(xobj.base.geo.dimY);
                     pdim->set_z(xobj.base.geo.dimZ);
                     pobject->set_allocated_centroid(ppointxyz);
+//                    int m,n;
+//                    for(m=-30;m<30;m++)
+//                    {
+//                        for(n=-30;n<30;n++)
+//                        {
+//                            iv::fusion::NomalXYZ *pnormal = pobject->add_nomal_centroid();
+//                            pnormal->set_nomal_x((rely+m*0.1)*(-1.0));
+//                            pnormal->set_nomal_y(relx+n*0.1);
+//                        }
+//                    }
+
+                    double dimx,dimy,dimz;
+                    dimx = xobj.base.geo.dimX;
+                    dimy = xobj.base.geo.dimY;
+                    dimz = xobj.base.geo.dimZ;
+                    double xoff,yoff;
+                    for(xoff = (dimx*(-0.5));xoff<=(dimx*0.5);xoff=xoff+0.2)
+                    {
+                        for(yoff = (dimy*(-0.5));yoff<=(dimy*0.5);yoff = yoff+0.2)
+                        {
+                            double reloffx,reloffy;
+                            reloffx = xoff*cos(fobjheading) - yoff*sin(fobjheading) + relx;
+                            reloffy = yoff*sin(fobjheading) + yoff*sin(fobjheading) + rely;
+                            iv::fusion::NomalXYZ *pnormal = pobject->add_nomal_centroid();
+                            pnormal->set_nomal_x((reloffy)*(-1.0));
+                            pnormal->set_nomal_y(reloffx);
+                        }
+                    }
+
                     pobject->set_allocated_dimensions(pdim);
                     pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));
 
@@ -164,7 +194,7 @@ void vtd_pilot::threadobject()
 
             std::cout<<" now : "<<now/1000000<<std::endl;
 
-            int nbytesize = xfusionarray.ByteSizeLong();
+            int nbytesize = xfusionarray.ByteSize();
 
             std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
 

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -41,13 +41,9 @@ private:
     std::thread * mpthread;
     std::thread * mpthreadfusion;
 
-<<<<<<< HEAD
     double mflat0 = 39.1207274;
     double mflon0 = 117.0280033;
-=======
-    double mflat0 = 39.1207274;//39.1235363;
-    double mflon0 = 117.0280032;//117.0272538;
->>>>>>> 4ea24c1d8a6c3d9da100d240cae24c28600e6543
+
 
     iv::gps::gpsimu mgpsimu;
     bool mbupdate_gps = false;