change ui/ui_osgviewer for radar view.

src/ui/ui_osgviewer/main.cpp

@@ -62,10 +62,22 @@ typedef struct
 	int id;
 } Car;
 
+typedef struct
+{
+    double mrel_x;
+    double mrel_y;
+    viewer::EntityModel * model;
+} RadarObj;
+
 std::vector<Car*> cars;
 
+std::vector<RadarObj *> gradarobj;
+const int NRADARMX = 100;
+
 Car * gADCIV_car;
 
+viewer::EntityModel *  gtestRadar;
+
 // Car models used for populating the road network
 // path should be relative the OpenDRIVE file
 static const char* carModelsFiles_[] =
@@ -85,6 +97,7 @@ std::vector<osg::ref_ptr<osg::LOD>> carModels_;
 
 #include "modulecomm.h"
 #include "gpsimu.pb.h"
+#include "radarobjectarray.pb.h"
 #include "gnss_coordinate_convert.h"
 
 double glat0 = 39.1201777;
@@ -140,6 +153,42 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
     gvehicle_hdg = hdg;
 }
 
+void ListenRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    static qint64 oldrecvtime;
+    iv::radar::radarobjectarray xradararray;
+    if(!xradararray.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRadar Parse Error."<<std::endl;
+        return;
+    }
+
+    int i;
+    for(i=0;i<xradararray.obj_size();i++)
+    {
+        iv::radar::radarobject * pobj = xradararray.mutable_obj(i);
+        if(i<gradarobj.size())
+        {
+            if(pobj->bvalid())
+            {
+            gradarobj.at(i)->mrel_x = pobj->x();
+            gradarobj.at(i)->mrel_y = pobj->y();
+            }
+            else
+            {
+                gradarobj.at(i)->mrel_x = 10000;
+                gradarobj.at(i)->mrel_y = 10000;
+            }
+        }
+    }
+    for(i=xradararray.obj_size();i<gradarobj.size();i++)
+    {
+        gradarobj.at(i)->mrel_x = 10000;
+        gradarobj.at(i)->mrel_y = 10000;
+    }
+}
+
 
 void log_callback(const char *str)
 {
@@ -230,6 +279,20 @@ int SetupADCIVCar( viewer::Viewer *viewer)
     return 0;
 }
 
+int SetupRadar(viewer::Viewer *viewer)
+{
+    int i;
+    for(i=0;i<NRADARMX;i++)
+    {
+        RadarObj * pRadar = new RadarObj;
+        pRadar->mrel_x = 10000;
+        pRadar->mrel_y = 10000;
+        pRadar->model = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
+        gradarobj.push_back(pRadar);
+    }
+    return 0;
+}
+
 int SetupCars(roadmanager::OpenDrive *odrManager, viewer::Viewer *viewer)
 {
 	if (density < 1E-10)
@@ -365,6 +428,26 @@ void updateADCIVCar(Car * car)
                     head, osg::Vec3(0, 0, 1));
 
         car->model->txNode_->setAttitude(car->model->quat_);
+
+        int k;
+ //       head = head +M_PI/2.0;
+        for(k=0;k<gradarobj.size();k++)
+        {
+            double relx,rely;
+            relx = gradarobj.at(k)->mrel_y*cos(head) + gradarobj.at(k)->mrel_x * sin(head);
+            rely = gradarobj.at(k)->mrel_y*sin(head) - gradarobj.at(k)->mrel_x*cos(head);
+
+            gradarobj.at(k)->model->txNode_->setPosition(osg::Vec3(x+relx, y+rely, z));
+
+            gradarobj.at(k)->model->quat_.makeRotate(
+                        roll, osg::Vec3(1, 0, 0),
+                        pitch, osg::Vec3(0, 1, 0),
+                        head, osg::Vec3(0, 0, 1));
+
+            gradarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
+        }
+
+
     }
 
 }
@@ -414,6 +497,7 @@ int main(int argc, char** argv)
 
 
     void * pa = iv::modulecomm::RegisterRecv("hcp2_gpsimu",Listengpsimu);
+    pa = iv::modulecomm::RegisterRecv("radar",ListenRADAR);
     (void)pa;
 
 	// Use logger callback
@@ -547,6 +631,10 @@ int main(int argc, char** argv)
 			viewer->GetNodeMaskBit(viewer::NodeMask::NODE_MASK_OSI_POINTS) ? "on" : "off");
 
          SetupADCIVCar(viewer);
+         SetupRadar(viewer);
+//         gtestRadar = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
+//         gtestRadar = viewer->AddEntityModel(carModelsFiles_[0], osg::Vec3(0.5, 0.5, 0.5),
+//                                             viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0);
 //        if (SetupCars(odrManager, viewer) == -1)
 //        {
 //            return 4;

src/ui/ui_osgviewer/ui_osgviewer.pro

@@ -21,6 +21,8 @@ SOURCES += \
         ../../include/msgtype/gps.pb.cc \
         ../../include/msgtype/gpsimu.pb.cc \
         ../../include/msgtype/imu.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
         gnss_coordinate_convert.cpp \
         main.cpp \
         ../../../thirdpartylib/esminilib/odrSpiral.cpp \
@@ -69,4 +71,6 @@ HEADERS += \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/radarobject.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
     gnss_coordinate_convert.h

src/ui/ui_osgviewer/viewer.cpp

@@ -1080,7 +1080,7 @@ EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
         // Set dimensions of the entity "box"
 
             // Scale to something car-ish
-            tx->setScale(osg::Vec3(4.5, 1.8, 1.5));
+            tx->setScale(osg::Vec3(0.5, 0.5, 1.5));
             tx->setPosition(osg::Vec3(1.5, 0, 0.75));
 
         tx->addChild(geode);
@@ -1088,17 +1088,19 @@ EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
         bbGroup->setName("BoundingBox");
         bbGroup->addChild(tx);
 
-//		group->addChild(bbGroup);
-//		group->setName(name);
+        group->addChild(bbGroup);
+        group->setName(name);
 
     EntityModel* model;
 //	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
 //	{
-//		model = new CarModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//        model = new CarModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+
 //	}
 //	else
 //	{
-        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
 //	}
 
     model->state_set_ = model->lod_->getOrCreateStateSet(); // Creating material
@@ -1282,17 +1284,17 @@ EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_c
 
 	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
 	{
-		CarModel* vehicle = (CarModel*)entities_.back();
-		CreateRoadSensors(vehicle);
+        CarModel* vehicle = (CarModel*)entities_.back();
+        CreateRoadSensors(vehicle);
 		
-		if (road_sensor)
-		{
-			vehicle->road_sensor_->Show();
-		}
-		else
-		{
-			vehicle->road_sensor_->Hide();
-		}
+        if (road_sensor)
+        {
+            vehicle->road_sensor_->Show();
+        }
+        else
+        {
+            vehicle->road_sensor_->Hide();
+        }
 	}
 
 	return entities_.back();