Merge branch 'master' of http://192.168.14.36:3000/adc_pilot/modularization

src/common/common/xodr/OpenDrive/ObjectSignal.cpp

@@ -0,0 +1,514 @@
+#include "ObjectSignal.h"
+
+#include <iostream>
+
+signal_positionRoad::signal_positionRoad(double s, double t, double zOffset, double hOffset, double pitch, double roll)
+{
+    ms = s;
+    mt = t;
+    mzOffset = zOffset;
+    mhOffset = hOffset;
+    mpitch = pitch;
+    mroll = roll;
+}
+
+double signal_positionRoad::Gets()
+{
+    return ms;
+}
+
+double signal_positionRoad::Gett()
+{
+    return mt;
+}
+
+double signal_positionRoad::GetzOffset()
+{
+    return mzOffset;
+}
+
+double signal_positionRoad::GethOffset()
+{
+    return mhOffset;
+}
+
+double signal_positionRoad::Getpitch()
+{
+    return mpitch;
+}
+
+double signal_positionRoad::Getroll()
+{
+    return mroll;
+}
+
+void signal_positionRoad::Sets(double s)
+{
+    ms = s;
+}
+
+void signal_positionRoad::Sett(double t)
+{
+    mt = t;
+}
+
+void signal_positionRoad::SetzOffset(double zOffset)
+{
+    mzOffset = zOffset;
+}
+
+void signal_positionRoad::SethOffset(double hOffset)
+{
+    mhOffset = hOffset;
+}
+
+void signal_positionRoad::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void signal_positionRoad::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+signal_positionInertial::signal_positionInertial(double x, double y,double z, double hdg, double pitch, double roll)
+{
+    mx = x;
+    my = y;
+    mz = z;
+    mhdg = hdg;
+    mpitch = pitch;
+    mroll = roll;
+}
+
+double signal_positionInertial::Getx()
+{
+    return mx;
+}
+
+double signal_positionInertial::Gety()
+{
+    return my;
+}
+
+double signal_positionInertial::Getz()
+{
+    return mz;
+}
+
+double signal_positionInertial::Gethdg()
+{
+    return mhdg;
+}
+
+double signal_positionInertial::Getpitch()
+{
+    return mpitch;
+}
+
+double signal_positionInertial::Getroll()
+{
+    return mroll;
+}
+
+void signal_positionInertial::Setx(double x)
+{
+    mx = x;
+}
+
+void signal_positionInertial::Sety(double y)
+{
+    my = y;
+}
+
+void signal_positionInertial::Setz(double z)
+{
+    mz = z;
+}
+
+void signal_positionInertial::Sethdg(double hdg)
+{
+    mhdg = hdg;
+}
+
+void signal_positionInertial::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void signal_positionInertial::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+
+signal_laneValidity::signal_laneValidity(int fromLane,int toLane)
+{
+    mfromLane = fromLane;
+    mtoLane = toLane;
+}
+
+int signal_laneValidity::GetfromLane()
+{
+    return mfromLane;
+}
+
+int signal_laneValidity::GettoLane()
+{
+    return mtoLane;
+}
+
+void signal_laneValidity::SetfromLane(int fromLane)
+{
+    mfromLane = fromLane;
+}
+
+void signal_laneValidity::SettoLane(int toLane)
+{
+    mtoLane = toLane;
+}
+
+
+Signal::Signal(double s, double t, std::string id, std::string name, bool dynamic,string orientation,
+               double zOffset, string type, std::string country, std::string countryRevision,
+               string subtype, double hOffset, double pitch, double roll, double height, double width)
+{
+    ms = s;
+    mt = t;
+    mid = id;
+    mname = name;
+    mdynamic = dynamic;
+    morientation = orientation;
+    mzOffset = zOffset;
+    mtype = type;
+    mcountry = country;
+    mcountryRevision = countryRevision;
+    msubtype = subtype;
+    mhOffset = hOffset;
+    mpitch = pitch;
+    mroll = roll;
+    mheight = height;
+    mwidth = width;
+    mpsignal_laneValidity = 0;
+    mpsignal_positionInertial = 0;
+    mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
+}
+
+Signal::Signal()
+{
+    mpsignal_positionInertial = 0;
+    mpsignal_positionRoad = 0;
+    mpsignal_laneValidity = 0;
+
+}
+Signal::~Signal()
+{
+    if(mpsignal_laneValidity != 0)delete mpsignal_laneValidity;
+    if(mpsignal_positionInertial != 0)delete mpsignal_positionInertial;
+    if(mpsignal_positionRoad != 0)delete mpsignal_positionRoad;
+}
+
+Signal& Signal::operator=(const Signal& x)
+{
+    if (this != &x)
+    {
+        this->ms = x.ms;
+        this->mt = x.mt;
+        this->mid = x.mid;
+        this->mname = x.mname;
+        this->mdynamic = x.mdynamic;
+        this->morientation = x.morientation;
+        this->mzOffset = x.mzOffset;
+        this->mtype = x.mtype;
+        this->mcountry = x.mcountry;
+        this->mcountryRevision = x.mcountryRevision;
+        this->msubtype = x.msubtype;
+        this->mhOffset = x.mhOffset;
+        this->mpitch = x.mpitch;
+        this->mroll = x.mroll;
+        this->mheight = x.mheight;
+        this->mwidth = x.mwidth;
+        this->mpsignal_positionInertial = 0;
+        if(x.mpsignal_positionInertial != 0)
+        {
+            this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
+                                                                      x.mpsignal_positionInertial->Gety(),
+                                                                      x.mpsignal_positionInertial->Getz(),
+                                                                      x.mpsignal_positionInertial->Gethdg(),
+                                                                      x.mpsignal_positionInertial->Getpitch(),
+                                                                      x.mpsignal_positionInertial->Getroll());
+        }
+        this->mpsignal_laneValidity = 0;
+        if(x.mpsignal_laneValidity != 0)
+        {
+            this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
+                                                              x.mpsignal_laneValidity->GettoLane());
+        }
+        this->mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
+    }
+    return *this;
+}
+
+Signal::Signal(const Signal &x)
+{
+    ms = x.ms;
+    mt = x.mt;
+    mid = x.mid;
+    mname = x.mname;
+    mdynamic = x.mdynamic;
+    morientation = x.morientation;
+    mzOffset = x.mzOffset;
+    mtype = x.mtype;
+    mcountry = x.mcountry;
+    mcountryRevision = x.mcountryRevision;
+    msubtype = x.msubtype;
+    mhOffset = x.mhOffset;
+    mpitch = x.mpitch;
+    mroll = x.mroll;
+    mheight = x.mheight;
+    mwidth = x.mwidth;
+    this->mpsignal_positionInertial = 0;
+    if(x.mpsignal_positionInertial != 0)
+    {
+        this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
+                                                                  x.mpsignal_positionInertial->Gety(),
+                                                                  x.mpsignal_positionInertial->Getz(),
+                                                                  x.mpsignal_positionInertial->Gethdg(),
+                                                                  x.mpsignal_positionInertial->Getpitch(),
+                                                                  x.mpsignal_positionInertial->Getroll());
+    }
+    this->mpsignal_laneValidity = 0;
+    if(x.mpsignal_laneValidity != 0)
+    {
+        this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
+                                                          x.mpsignal_laneValidity->GettoLane());
+    }
+    mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
+}
+
+double Signal::Gets()
+{
+    return ms;
+}
+
+double Signal::Gett()
+{
+    return mt;
+}
+
+string Signal::Getid()
+{
+    return mid;
+}
+
+string Signal::Getname()
+{
+    return mname;
+}
+
+bool Signal::Getdynamic()
+{
+    return mdynamic;
+}
+
+string Signal::Getorientation()
+{
+    return morientation;
+}
+
+double Signal::GetzOffset()
+{
+    return mzOffset;
+}
+
+string Signal::Gettype()
+{
+    return mtype;
+}
+
+string Signal::Getcountry()
+{
+    return mcountry;
+}
+
+string Signal::GetcountryRevision()
+{
+    return mcountryRevision;
+}
+
+string Signal::Getsubtype()
+{
+    return msubtype;
+}
+
+double Signal::GethOffset()
+{
+    return mhOffset;
+}
+
+double Signal::Getpitch()
+{
+    return mpitch;
+}
+
+double Signal::Getroll()
+{
+    return mroll;
+}
+
+double Signal::Getheight()
+{
+    return mheight;
+}
+
+double Signal::Getwidth()
+{
+    return mwidth;
+}
+
+signal_positionRoad * Signal::GetpositionRoad()
+{
+    return mpsignal_positionRoad;
+}
+
+signal_positionInertial * Signal::GetpositionInertial()
+{
+    return mpsignal_positionInertial;
+}
+
+void Signal::Sets(double s)
+{
+    ms = s;
+}
+
+void Signal::Sett(double t)
+{
+    mt = t;
+}
+
+void Signal::Setid(std::string id)
+{
+    mid = id;
+}
+
+void Signal::Setname(std::string name)
+{
+    mname = name;
+}
+
+void Signal::Setdynamic(bool dynamic)
+{
+    mdynamic = dynamic;
+}
+
+void Signal::Setorientation(std::string orientation)
+{
+    morientation = orientation;
+}
+
+void Signal::SetzOffset(double zOffset)
+{
+    mzOffset = zOffset;
+}
+
+void Signal::Settype(string type)
+{
+    mtype = type;
+}
+
+void Signal::Setcountry(std::string country)
+{
+    mcountry = country;
+}
+
+void Signal::SetcountryRevision(std::string countryRevision)
+{
+    mcountryRevision = countryRevision;
+}
+
+void Signal::Setsubtype(string subtype)
+{
+    msubtype = subtype;
+}
+
+void Signal::SethOffset(double hOffset)
+{
+    mhOffset = hOffset;
+}
+
+void Signal::Setpitch(double pitch)
+{
+    mpitch = pitch;
+}
+
+void Signal::Setroll(double roll)
+{
+    mroll = roll;
+}
+
+void Signal::Setheight(double height)
+{
+    mheight = height;
+}
+
+void Signal::Setwidth(double width)
+{
+    mwidth = width;
+}
+
+void Signal::SetlaneValidity(int fromLane, int toLane)
+{
+    if(mpsignal_laneValidity == 0)
+    {
+        mpsignal_laneValidity = new signal_laneValidity(fromLane,toLane);
+    }
+    else
+    {
+        mpsignal_laneValidity->SetfromLane(fromLane);
+        mpsignal_laneValidity->SettoLane(toLane);
+    }
+}
+
+void Signal::SetpositionRoad(double s, double t, double zOffset, double hOffset, double pitch,double roll)
+{
+    if(mpsignal_positionRoad == 0)
+    {
+        mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
+    }
+    else
+    {
+        mpsignal_positionRoad->Sets(s);
+        mpsignal_positionRoad->Sett(t);
+        mpsignal_positionRoad->SetzOffset(zOffset);
+        mpsignal_positionRoad->SethOffset(hOffset);
+        mpsignal_positionRoad->Setpitch(pitch);
+        mpsignal_positionRoad->Setroll(roll);
+    }
+}
+
+void Signal::SetpositionInertial(double x, double y, double z, double hdg, double pitch, double roll)
+{
+    if(mpsignal_positionInertial == 0)
+    {
+        mpsignal_positionInertial = new signal_positionInertial(x,y,z,hdg,pitch,roll);
+    }
+    else
+    {
+        mpsignal_positionInertial->Setx(x);
+        mpsignal_positionInertial->Sety(y);
+        mpsignal_positionInertial->Setz(z);
+        mpsignal_positionInertial->Sethdg(hdg);
+        mpsignal_positionInertial->Setpitch(pitch);
+        mpsignal_positionInertial->Setroll(roll);
+    }
+}
+
+signal_laneValidity * Signal::GetlaneValidity()
+{
+    return mpsignal_laneValidity;
+}
+
+
+
+

src/common/common/xodr/OpenDrive/ObjectSignal.h

@@ -0,0 +1,163 @@
+#ifndef OBJECTSIGNAL_H
+#define OBJECTSIGNAL_H
+
+#include <vector>
+#include <string>
+
+using std::vector;
+using std::string;
+
+
+//***********************************************************************************
+//Object
+//***********************************************************************************
+class Object
+{
+public:
+	Object(){}
+};
+//----------------------------------------------------------------------------------
+
+
+
+class signal_positionRoad
+{
+private:
+    double ms;
+    double mt;
+    double mzOffset;
+    double mhOffset;
+    double mpitch;
+    double mroll;
+public:
+    signal_positionRoad(double s,double t,double zOffset,double hOffset, double pitch,double roll);
+    double Gets();
+    double Gett();
+    double GetzOffset();
+    double GethOffset();
+    double Getpitch();
+    double Getroll();
+    void Sets(double s);
+    void Sett(double t);
+    void SetzOffset(double zOffset);
+    void SethOffset(double hOffset);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+};
+
+class signal_positionInertial
+{
+private:
+    double mx;
+    double my;
+    double mz;
+    double mhdg;
+    double mpitch;
+    double mroll;
+public:
+    signal_positionInertial(double x,double y,double z,double hdg,double pitch,double roll );
+    double Getx();
+    double Gety();
+    double Getz();
+    double Gethdg();
+    double Getpitch();
+    double Getroll();
+    void Setx(double x);
+    void Sety(double y);
+    void Setz(double z);
+    void Sethdg(double hdg);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+};
+
+class signal_laneValidity
+{
+private:
+    int mfromLane;
+    int mtoLane;
+public:
+    signal_laneValidity(int fromLane,int toLane);
+    int GetfromLane();
+    int GettoLane();
+    void SetfromLane(int fromLane);
+    void SettoLane(int toLane);
+};
+
+
+//***********************************************************************************
+//Signal
+//***********************************************************************************
+class Signal
+{
+private:
+    double ms;
+    double mt;
+    string mid;
+    string mname;
+    bool mdynamic;
+    string morientation;
+    double mzOffset;
+    string mtype;
+    string mcountry;
+    string mcountryRevision;
+    string msubtype;
+    double mhOffset;
+    double mpitch;
+    double mroll;
+    double mheight;
+    double mwidth;
+    signal_positionRoad * mpsignal_positionRoad;
+    signal_positionInertial * mpsignal_positionInertial;
+    signal_laneValidity * mpsignal_laneValidity;
+public:
+    Signal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+           string subtype,double hOffset,double pitch,double roll ,double height,double width);
+    Signal();
+    ~Signal();
+    Signal& operator=(const Signal& x);
+    Signal(const Signal & x);
+    double Gets();
+    double Gett();
+    string Getid();
+    string Getname();
+    bool Getdynamic();
+    string Getorientation();
+    double GetzOffset();
+    string Gettype();
+    string Getcountry();
+    string GetcountryRevision();
+    string Getsubtype();
+    double GethOffset();
+    double Getpitch();
+    double Getroll();
+    double Getheight();
+    double Getwidth();
+    signal_positionRoad * GetpositionRoad();
+    signal_positionInertial * GetpositionInertial();
+    signal_laneValidity * GetlaneValidity();
+    void Sets(double s);
+    void Sett(double t);
+    void Setid(string id);
+    void Setname(string name);
+    void Setdynamic(bool dynamic);
+    void Setorientation(string orientation);
+    void SetzOffset(double zOffset);
+    void Settype(string type);
+    void Setcountry(string country);
+    void SetcountryRevision(string countryRevision);
+    void Setsubtype(string subtype);
+    void SethOffset(double hOffset);
+    void Setpitch(double pitch);
+    void Setroll(double roll);
+    void Setheight(double height);
+    void Setwidth(double width);
+    void SetlaneValidity(int fromLane, int toLane);
+    void SetpositionRoad(double s,double t, double zOffset,double hOffset,double pitch,double roll);
+    void SetpositionInertial(double x,double y, double z, double hdg,double pitch,double roll);
+
+
+};
+//----------------------------------------------------------------------------------
+
+
+#endif

src/driver/driver_map_xodrload/OpenDrive/OpenDrive.cpp → src/common/common/xodr/OpenDrive/OpenDrive.cpp

@@ -299,3 +299,9 @@ void Header::SetXYValues(double north, double south, double east,double west)
 	mEast=east;
 	mWest=west;
 }
+
+void Header::GetLat0Lon0(double &lat0, double &lon0)
+{
+    lat0 = mLat0;
+    lon0 = mLon0;
+}

src/driver/driver_map_xodrload/OpenDrive/OpenDrive.h → src/common/common/xodr/OpenDrive/OpenDrive.h

@@ -181,6 +181,9 @@ public:
 
     void SetAllParams(unsigned short int revMajor, unsigned short int revMinor, string name, float version, string date,
         double north, double south, double east,double west,double lat0,double lon0,double hdg0);
+
+
+    void GetLat0Lon0(double & lat0,double & lon0);
 };
 
 

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlParser.cpp → src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -75,7 +75,7 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 
 	int checker=TIXML_SUCCESS;
 	
-	checker+=node->QueryStringAttribute("name",&name);
+//	checker+=node->QueryStringAttribute("name",&name);
 	checker+=node->QueryDoubleAttribute("length",&length);
 	checker+=node->QueryStringAttribute("id",&id);
 	checker+=node->QueryStringAttribute("junction",&junction);
@@ -85,6 +85,11 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 		cout<<"Error parsing Road attributes"<<endl;
 		return false;
 	}
+
+    if(node->QueryStringAttribute("name",&name) != TIXML_SUCCESS)
+    {
+        name = "";
+    }
 	//fill in
 	mOpenDrive->AddRoad(name, length, id, junction);
 	Road* road = mOpenDrive->GetLastAddedRoad();
@@ -910,8 +915,117 @@ bool OpenDriveXmlParser::ReadObjects (Road* road, TiXmlElement *node)
 
 bool OpenDriveXmlParser::ReadSignals (Road* road, TiXmlElement *node)
 {
-	return true;
+
+    TiXmlElement *subNode = node->FirstChildElement("signal");
+    while (subNode)
+    {
+        ReadSignal(road, subNode);
+        subNode=subNode->NextSiblingElement("signal");
+    }
+    return true;
+
+
+}
+
+bool OpenDriveXmlParser::ReadSignal(Road *road, TiXmlElement *node)
+{
+    double s;
+    double t;
+    string id;
+    string name;
+    bool dynamic;
+    string strdynamic;
+    string orientation;
+    double zOffset;
+    string type;
+    string country;
+    string countryRevision;
+    string subtype;
+    double hOffset;
+    double pitch;
+    double roll;
+    double height;
+    double width;
+
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("t",&t);
+    checker+=node->QueryStringAttribute("id",&id);
+    checker+=node->QueryStringAttribute("name",&name);
+    checker+=node->QueryStringAttribute("dynamic",&strdynamic);
+    checker+=node->QueryStringAttribute("orientation",&orientation);
+    checker+=node->QueryDoubleAttribute("zOffset",&zOffset);
+    checker+=node->QueryStringAttribute("type",&type);
+    checker+=node->QueryStringAttribute("country",&country);
+    checker+=node->QueryStringAttribute("countryRevision",&countryRevision);
+    checker+=node->QueryStringAttribute("subtype",&subtype);
+    checker+=node->QueryDoubleAttribute("hOffset",&hOffset);
+    checker+=node->QueryDoubleAttribute("pitch",&pitch);
+    checker+=node->QueryDoubleAttribute("roll",&roll);
+    checker+=node->QueryDoubleAttribute("height",&height);
+    checker+=node->QueryDoubleAttribute("width",&width);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Lane Signals attributes"<<endl;
+        return false;
+    }
+    if(strncmp(strdynamic.data(),"no",256) == 0)dynamic = false;
+    else dynamic = true;
+    road->AddSignal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,subtype,hOffset,
+                    pitch,roll,height,width);
+
+    Signal * pSignal = road->GetSignal(road->GetSignalCount() - 1);
+    TiXmlElement * subNode;
+    //Proceed to Signals
+    subNode=node->FirstChildElement("validity");
+    if (subNode)
+    {
+        ReadSignal_laneValidity(pSignal, subNode);
+    }
+    subNode=node->FirstChildElement("positionInertial");
+    if(subNode)
+    {
+        ReadSignal_positionInertial(pSignal,subNode);
+    }
+    return true;
 }
+
+bool OpenDriveXmlParser::ReadSignal_laneValidity(Signal *pSignal, TiXmlElement *node)
+{
+    int fromLane;
+    int toLane;
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryIntAttribute("fromLane",&fromLane);
+    checker+=node->QueryIntAttribute("toLane",&toLane);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing laneValidity attributes"<<endl;
+        return false;
+    }
+    pSignal->SetlaneValidity(fromLane,toLane);
+    return true;
+}
+
+bool OpenDriveXmlParser::ReadSignal_positionInertial(Signal *pSignal, TiXmlElement *node)
+{
+    double x,y,z,hdg,pitch,roll;
+    int checker=TIXML_SUCCESS;
+    checker+=node->QueryDoubleAttribute("x",&x);
+    checker+=node->QueryDoubleAttribute("y",&y);
+    checker+=node->QueryDoubleAttribute("z",&z);
+    checker+=node->QueryDoubleAttribute("hdg",&hdg);
+    checker+=node->QueryDoubleAttribute("pitch",&pitch);
+    checker+=node->QueryDoubleAttribute("roll",&roll);
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing positionInertial attributes"<<endl;
+        return false;
+    }
+    pSignal->SetpositionInertial(x,y,z,hdg,pitch,roll);
+    return true;
+}
+
+
 //--------------
 
 bool OpenDriveXmlParser::ReadSurface (Road* road, TiXmlElement *node)

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlParser.h → src/common/common/xodr/OpenDrive/OpenDriveXmlParser.h

@@ -64,6 +64,9 @@ public:
 
 	bool ReadObjects (Road* road, TiXmlElement *node);
 	bool ReadSignals (Road* road, TiXmlElement *node);
+    bool ReadSignal(Road * road,TiXmlElement * node);
+    bool ReadSignal_positionInertial(Signal * pSignal, TiXmlElement *node);
+    bool ReadSignal_laneValidity(Signal * pSignal,TiXmlElement * node);
 	//--------------
 
 	bool ReadSurface (Road* road, TiXmlElement *node);

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlWriter.cpp → src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -920,8 +920,83 @@ bool OpenDriveXmlWriter::WriteSignals (TiXmlElement *node, Road* road)
 	TiXmlElement* nodeSignals = new TiXmlElement("signals");
 	node->LinkEndChild(nodeSignals);
 
+    unsigned int lSignalSectionCount = road->GetSignalCount();
+    for(unsigned int i=0; i<lSignalSectionCount; i++)
+    {
+        WriteSignal(nodeSignals, road->GetSignal(i));
+    }
+
 	return true;
 }
+
+bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
+{
+    TiXmlElement* nodeSignal = new TiXmlElement("signal");
+    node->LinkEndChild(nodeSignal);
+
+    nodeSignal->SetAttribute("s",pSignal->Gets());
+    nodeSignal->SetAttribute("t",pSignal->Gett());
+    nodeSignal->SetAttribute("id",pSignal->Getid());
+    nodeSignal->SetAttribute("name",pSignal->Getname());
+    if(pSignal->Getdynamic() == true)
+    nodeSignal->SetAttribute("dynamic","yes");
+    else
+        nodeSignal->SetAttribute("dynamic","no");
+    nodeSignal->SetAttribute("orientation",pSignal->Getorientation());
+    nodeSignal->SetAttribute("zOffset",pSignal->GetzOffset());
+    nodeSignal->SetAttribute("type",pSignal->Gettype());
+    nodeSignal->SetAttribute("country",pSignal->Getcountry());
+    nodeSignal->SetAttribute("countryRevision",pSignal->GetcountryRevision());
+    nodeSignal->SetAttribute("subtype",pSignal->Getsubtype());
+    nodeSignal->SetAttribute("hOffset",pSignal->GethOffset());
+    nodeSignal->SetAttribute("pitch",pSignal->Getpitch());
+    nodeSignal->SetAttribute("roll",pSignal->Getroll());
+    nodeSignal->SetAttribute("height",pSignal->Getheight());
+    nodeSignal->SetAttribute("width",pSignal->Getwidth());
+
+    signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
+    if(psignal_lanevalidity != 0)
+    {
+        WriteSignal_laneValidity(nodeSignal,psignal_lanevalidity);
+    }
+
+    signal_positionInertial * psignal_positionInertial = pSignal->GetpositionInertial();
+    if(psignal_positionInertial != 0)
+    {
+        WriteSignal_positionInertial(nodeSignal,psignal_positionInertial);
+    }
+
+    return true;
+}
+
+bool OpenDriveXmlWriter::WriteSignal_positionInertial(TiXmlElement *node, signal_positionInertial *pSignal_positionInertial)
+{
+    TiXmlElement* nodepositionInertial = new TiXmlElement("positionInertial");
+
+    node->LinkEndChild(nodepositionInertial);
+
+    nodepositionInertial->SetAttribute("x",pSignal_positionInertial->Getx());
+    nodepositionInertial->SetAttribute("y",pSignal_positionInertial->Gety());
+    nodepositionInertial->SetAttribute("z",pSignal_positionInertial->Getz());
+    nodepositionInertial->SetAttribute("hdg",pSignal_positionInertial->Gethdg());
+    nodepositionInertial->SetAttribute("pitch",pSignal_positionInertial->Getpitch());
+    nodepositionInertial->SetAttribute("roll",pSignal_positionInertial->Getroll());
+
+    return true;
+}
+
+bool OpenDriveXmlWriter::WriteSignal_laneValidity(TiXmlElement *node, signal_laneValidity *pSignal_laneValidity)
+{
+    TiXmlElement* nodelaneValidity = new TiXmlElement("validity");
+
+    node->LinkEndChild(nodelaneValidity);
+
+    nodelaneValidity->SetAttribute("fromLane",pSignal_laneValidity->GetfromLane());
+    nodelaneValidity->SetAttribute("toLane",pSignal_laneValidity->GettoLane());
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteSurface (TiXmlElement *node, Road* road)

src/driver/driver_map_xodrload/OpenDrive/OpenDriveXmlWriter.h → src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.h

@@ -65,6 +65,9 @@ public:
 
 	bool WriteObjects (TiXmlElement *node, Road* road);
 	bool WriteSignals (TiXmlElement *node, Road* road);
+    bool WriteSignal(TiXmlElement * node, Signal * pSignal);
+    bool WriteSignal_positionInertial(TiXmlElement * node, signal_positionInertial * pSignal_positionInertial);
+    bool WriteSignal_laneValidity(TiXmlElement * node, signal_laneValidity * pSignal_laneValidity);
 	//--------------
 
 	bool WriteSurface (TiXmlElement *node, Road* road);

src/driver/driver_map_xodrload/OpenDrive/Road.cpp → src/common/common/xodr/OpenDrive/Road.cpp

@@ -610,12 +610,18 @@ unsigned int Road::AddObject()
 	return index;
 }
 //-------------
-unsigned int Road::AddSignal()
+unsigned int Road::AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+                             string subtype,double hOffset,double pitch,double roll ,double height,double width)
 {
 	// Check the first method in the group for details
 
 	unsigned int index=GetSignalCount();
-	mSignalsVector.push_back(Signal());	
+    Signal x(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+             subtype,hOffset,pitch,roll,height,width);
+    mSignalsVector.push_back(x);
+
+//    mSignalsVector.push_back(Signal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+//                                    subtype,hOffset,pitch,roll,height,width));
 	mLastAddedSignal=index;
 	return index;
 }

src/driver/driver_map_xodrload/OpenDrive/Road.h → src/common/common/xodr/OpenDrive/Road.h

@@ -229,7 +229,8 @@ public:
 	unsigned int AddCrossfall (string side, double s, double a, double b, double c, double d);
 	unsigned int AddLaneSection(double s);
 	unsigned int AddObject();
-	unsigned int AddSignal();
+    unsigned int AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
+                           string subtype,double hOffset,double pitch,double roll ,double height,double width);
 
 	/**
 	 * Methods used to clone child records in the respective vectors

src/driver/driver_map_xodrload/OpenDrive/RoadGeometry.h → src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -30,7 +30,7 @@ protected:
 	double mHdg;
 	double mLength;
 	double mS2;
-    short int mGeomType;	//0-line, 1-spiral, 2-arc 3-poly3 4-parampoly3
+    short int mGeomType;	//0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
 public:
 	/**
 	 * Constructor that initializes the base properties of teh record

src/driver/driver_map_xodrload/OpenDrive/ObjectSignal.h

@@ -1,28 +0,0 @@
-#ifndef OBJECTSIGNAL_H
-#define OBJECTSIGNAL_H
-
-
-
-//***********************************************************************************
-//Object
-//***********************************************************************************
-class Object
-{
-public:
-	Object(){}
-};
-//----------------------------------------------------------------------------------
-
-
-//***********************************************************************************
-//Signal
-//***********************************************************************************
-class Signal
-{
-public:
-	Signal(){}
-};
-//----------------------------------------------------------------------------------
-
-
-#endif

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -19,19 +19,19 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp     \
     ../../include/msgtype/v2x.pb.cc \
-OpenDrive/OpenDriveXmlWriter.cpp \
-OpenDrive/OtherStructures.cpp \
-OpenDrive/OpenDrive.cpp \
-OpenDrive/Road.cpp \
-OpenDrive/Junction.cpp \
-OpenDrive/Lane.cpp \
-OpenDrive/OpenDriveXmlParser.cpp \
-OpenDrive/ObjectSignal.cpp \
-OpenDrive/RoadGeometry.cpp \
-TinyXML/tinyxml.cpp \
-TinyXML/tinyxmlparser.cpp \
-TinyXML/tinystr.cpp \
-TinyXML/tinyxmlerror.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp \
+    ../../common/common/xodr/OpenDrive/OtherStructures.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDrive.cpp \
+    ../../common/common/xodr/OpenDrive/Road.cpp \
+    ../../common/common/xodr/OpenDrive/Junction.cpp \
+    ../../common/common/xodr/OpenDrive/Lane.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.cpp \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.cpp \
+    ../../common/common/xodr/TinyXML/tinyxml.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlparser.cpp \
+    ../../common/common/xodr/TinyXML/tinystr.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlerror.cpp \
     fresnl.cpp \
     const.cpp \
     polevl.c \
@@ -46,17 +46,17 @@ TinyXML/tinyxmlerror.cpp \
 HEADERS += \
     ../../../include/ivexit.h \
     ../../include/msgtype/v2x.pb.h \
-OpenDrive/OpenDriveXmlParser.h \
-OpenDrive/RoadGeometry.h \
-OpenDrive/Road.h \
-OpenDrive/Junction.h \
-OpenDrive/OpenDriveXmlWriter.h \
-OpenDrive/Lane.h \
-OpenDrive/OtherStructures.h \
-OpenDrive/OpenDrive.h \
-OpenDrive/ObjectSignal.h \
-TinyXML/tinyxml.h \
-TinyXML/tinystr.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.h \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.h \
+    ../../common/common/xodr/OpenDrive/Road.h \
+    ../../common/common/xodr/OpenDrive/Junction.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.h \
+    ../../common/common/xodr/OpenDrive/Lane.h \
+    ../../common/common/xodr/OpenDrive/OtherStructures.h \
+    ../../common/common/xodr/OpenDrive/OpenDrive.h \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.h \
+    ../../common/common/xodr/TinyXML/tinyxml.h \
+    ../../common/common/xodr/TinyXML/tinystr.h \
     mconf.h \
     globalplan.h \
     gps_type.h \
@@ -84,4 +84,6 @@ TinyXML/tinystr.h \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/common/xodr
+
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -19,6 +19,8 @@ extern "C"
 #include "dubins.h"
 }
 
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
 /**
  * @brief GetLineDis 获得点到直线Geometry的距离。
  * @param pline
@@ -1916,7 +1918,9 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         }
     }
 
-    Road * pRoad = xps.mpRoad;
+
+
+
 
 
     if(xps.mnStartLaneSel > 0)
@@ -1927,14 +1931,64 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         {
             xvvPP.push_back(xvectorPP.at(nvsize-1-i));
         }
+        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
         return xvvPP;
     }
 
 //    pRoad->GetLaneSection(xps.msectionid)
 
+    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
     return xvectorPP;
 }
 
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
+{
+    if(pRoad->GetSignalCount() == 0)return;
+    int nsigcount = pRoad->GetSignalCount();
+    int i;
+    for(i=0;i<nsigcount;i++)
+    {
+        Signal * pSig = pRoad->GetSignal(i);
+        int nfromlane = -100;
+        int ntolane = 100;
+        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
+        if(pSig_laneValidity != 0)
+        {
+            nfromlane = pSig_laneValidity->GetfromLane();
+            ntolane = pSig_laneValidity->GettoLane();
+        }
+        if((nlane < 0)&&(nfromlane >= 0))
+        {
+            continue;
+        }
+        if((nlane > 0)&&(ntolane<=0))
+        {
+            continue;
+        }
+
+        double s = pSig->Gets();
+        double fpos = s/pRoad->GetRoadLength();
+        if(nlane > 0)fpos = 1.0 -fpos;
+        int npos = fpos *xvectorPP.size();
+        if(npos <0)npos = 0;
+        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos > 0)npos--;
+            else break;
+        }
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos < (xvectorPP.size()-1))npos++;
+            else break;
+        }
+        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
+
+
+    }
+}
+
 static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
                                           Road * pRoad_obj,GeometryBlock * pgeob_obj,
                                            const double x_now,const double y_now,const double head,
@@ -1949,6 +2003,7 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
   int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
   int i;
   std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP);
+
   if(xpathsection[0].mainsel < 0)
   {
   for(i=indexstart;i<xvPP.size();i++)
@@ -2255,6 +2310,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
         int nindexstart = indexinroadpoint(xvPP,nearx,neary);
         int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
         int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
+        AddSignalMark(pRoad,nlane,xvPP);
         if((nindexend >nindexstart)&&(lr_start == 2))
         {
             bNeedDikstra = false;

src/driver/driver_map_xodrload/globalplan.h

@@ -24,6 +24,8 @@ public:
     double mfDisToLaneLeft;
     int mnLaneori = 0;  //from Right 0
     int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/main.cpp

@@ -467,12 +467,30 @@ void SetPlan(xodrobj xo)
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
-        data->roadMode = 0;
-        data->roadOri = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
 
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
+
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
+
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
 
         mapdata.push_back(data);
 
@@ -576,12 +594,29 @@ void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
-        data->roadMode = 0;
-        data->roadOri = 0;
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
 
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
 
         mapdata.push_back(data);
 

src/include/proto/scheduler.proto

@@ -14,6 +14,7 @@ message scheduler_unit
 message scheduler
 {
 	repeated scheduler_unit mscheduler_unit = 1;
+	required int32 mcyble = 2;
 };
 
 

src/include/proto/switch.proto

@@ -0,0 +1,243 @@
+syntax = "proto2";
+
+package iv.Switch;
+
+//前视障碍物开关命令
+message CtrlCmd_FrontCamera_Obs{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//障碍物类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;			//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+    required bool size = 9;			//尺寸
+    required bool bearing_xyz = 10;		//方位（X/Y/Z 三轴信息）
+    required bool relativeSpeed = 11;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 12;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 13;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 14;		//参考点（目标中心点）
+    required bool contourPoint = 15;		//轮廓点
+    required bool framePoint = 16;		//外包框顶点
+    required bool obstacleMotionPrediction = 17;	//障碍物运动预测轨迹
+};
+
+//前视车道线开关命令
+message  CtrlCmd_FrontCamera_Laneline{
+    required bool No = 1;		//车道线编号
+    required bool type = 2;		//车道线类型
+    required bool color = 3;		//车道线颜色
+    required bool curvature = 4;	//车道线曲率
+    required bool geometryInf = 5;	//车道线几何信息
+};
+
+//前视交通标线开关命令
+message CtrlCmd_FrontCamera_Trafficline{
+    required bool type = 1;	//类型信息
+    required bool posInf = 2;	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//前视可行驶区域开关命令
+message  CtrlCmd_FrontCamera_DriveableArea{
+    required bool type = 1;		//类型信息
+    required bool boundPoint = 2;	//可行驶域拟合信息
+};
+
+//前视交通标志开关命令
+message CtrlCmd_FrontCamera_TrafficSign{
+    required bool type = 1;	//类型信息
+    required bool posInf = 2;	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//前视交通信号灯开关命令
+message CtrlCmd_FrontCamera_TrafficLight{
+    required bool type = 1;   	//交通信号灯状态信息
+    required bool posInf = 2; 	//方位信息包含(关键点坐标,附着车道 ID)
+};
+
+//环视车位信息开关命令
+message CtrlCmd_AroundCamera_Carpos{
+    required bool carAngle = 1;               		//车位角点坐标
+    required bool carWidhtDeep = 2;           	//车位宽度/深度
+    required bool carconfidenceDegree = 3;    	//置信度
+};
+
+//环视障碍物信息开关命令
+message CtrlCmd_AroundCamera_Obs{
+    required bool type = 1;			//障碍物类型信息
+    required bool posinf = 2;			//障碍物置信度
+    required bool confidenceDegree = 3;		//障碍物方位信息
+};
+
+//毫米波雷达障碍物信息开关命令
+message CtrlCmd_MWRader_Obs{
+    required bool ID = 1;			//障碍物ID
+    required bool confidenceDegree = 2;		//置信度
+    required bool state = 3;			//状态位
+    required bool lifeCycle = 4;			//生命周期
+    required bool courseAngle = 5;		//航向角
+    required bool bearing = 6;			//方位
+    required bool relativeSpeed = 7;		//相对速度
+    required bool absoluteSpeed = 8;		//绝对速度
+    required bool relativeAcceleration = 9;	//相对加速度
+    required bool obstacleMotionPrediction = 10;	//障碍物运动预测轨迹
+};
+
+//激光雷达车道线信息开关命令
+message CtrlCmd_LRader_Laneline{
+    required bool No = 1;		//车道线 ID
+    required bool geometryInf = 2;	//车道线空间坐标信息
+};
+
+//激光雷达障碍物信息开关命令
+message CtrlCmd_LRader_Obs{
+    required bool ID = 1;			//障碍物 ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;			//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool size = 8;			//尺寸
+    required bool bearing_xyz = 9;		//方位（X/Y/Z 三轴信息）
+    required bool relativeSpeed = 10;		//相对速度
+    required bool absoluteSpeed = 11;		//绝对速度
+    required bool relativeAcceleration = 12;	//相对加速度
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//感知融合毫米波激光雷达前视融合信息开关命令
+message CtrlCmd_MWLRaderFrontCamera{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;		//生命周期
+    required bool route = 6;			//所处车道标志
+    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+    required bool size = 9;			//尺寸
+    required bool relativeSpeed = 10;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 11;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 12;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//毫米波雷达前视融合信息开关命令
+message CtrlCmd_MWRaderFrontCamera{
+    required bool ID = 1;			//障碍物ID
+    required bool type = 2;			//类型
+    required bool confidenceDegree = 3;		//置信度
+    required bool state = 4;			//状态位
+    required bool lifeCycle = 5;		//生命周期
+    required bool route = 6;			//所处车道标志
+//    required bool courseAngle = 7;		//航向角
+    required bool bearing = 8;			//方位
+//    required bool size = 9;			//尺寸
+    required bool relativeSpeed = 10;		//相对速度（X/Y 两轴信息）
+    required bool absoluteSpeed = 11;		//绝对速度（X/Y两轴信息）
+    required bool relativeAcceleration = 12;	//相对加速度（X/Y 两轴信息）
+    required bool referencePoint = 13;		//参考点（目标中心点）
+    required bool contourPoint = 14;		//轮廓点
+    required bool framePoint = 15;		//外包框顶点
+    required bool obstacleMotionPrediction = 16;	//障碍物运动预测轨迹
+};
+
+//超声波雷达环视融合信息开关命令
+message CtrlCmd_URaderAroundCamera{
+    required bool carAngle = 1;		//车位角点坐标
+    required bool carWidhtDeep = 2;		//车位宽度/深度
+    required bool carconfidenceDegree = 3;	//车位置信度
+    required bool obstype = 4;			//障碍物类型信息
+    required bool obsposinf = 5;		//障碍物置信度
+    required bool obsconfidenceDegree = 6;	//障碍物方位信息
+};
+
+// 环境建模开关命令
+message CtrlCmd_EnvironmentModeling{
+    required bool grid = 1;	//栅格图
+};
+
+//融合定位模块开关命令
+message CtrlCmd_FusionPositioning{
+    required bool time = 1;		//时间信息
+    required bool state = 2;		//纬度、经度、海拔、定位状态
+    required bool distance = 3;		//距离左右侧车道线距离
+    required bool yawAngle = 4;	//偏航角
+    required bool angleOfPitch = 5;	//俯仰角
+    required bool rollAngle = 6;	//滚转角
+};
+
+//显示开关命令
+message  ToDispControlCmd{
+    required CtrlCmd_FrontCamera_Obs FrontCamera_Obs=1;                           		//前视障碍物开关命令
+    required CtrlCmd_FrontCamera_Laneline FrontCamera_Laneline=2;           		//前视车道线开关命令
+    required CtrlCmd_FrontCamera_Trafficline FrontCamera_Trafficline=3;      		//前视交通标线开关命令
+    required CtrlCmd_FrontCamera_DriveableArea FrontCamera_DriveableArea=4;  	//前视可行驶区域开关命令
+    required CtrlCmd_FrontCamera_TrafficSign FrontCamera_TrafficSign=5;      		//前视交通标志开关命令
+    required CtrlCmd_FrontCamera_TrafficLight FrontCamera_TrafficLight=6;    		//前视交通信号灯开关命令
+    required CtrlCmd_AroundCamera_Carpos AroundCamera_Carpos=7;              		//环视车位信息开关命令
+    required CtrlCmd_AroundCamera_Obs AroundCamera_Obs=8;                    		//环视障碍物信息开关命令
+    required CtrlCmd_MWRader_Obs MWRader_Obs=9;                              		//毫米波雷达障碍物信息开关命令
+    required CtrlCmd_LRader_Laneline LRader_Laneline=10;                      		//激光雷达车道线信息开关命令
+    required CtrlCmd_LRader_Obs LRader_Obs=11;                                			//激光雷达障碍物信息开关命令
+    required CtrlCmd_MWLRaderFrontCamera MWLRaderFrontCamera=12;              	//感知融合毫米波激光雷达前视融合信息开关命令
+    required CtrlCmd_MWRaderFrontCamera MWRaderFrontCamera=13;                	//毫米波雷达前视融合信息开关命令
+    required CtrlCmd_URaderAroundCamera URaderAroundCamera=14;                	//超声波雷达环视融合信息开关命令
+    required CtrlCmd_EnvironmentModeling EnvironmentModeling=15;              		//环境建模开关命令
+    required CtrlCmd_FusionPositioning FusionPositioning=16;                  		//融合定位模块开关命令
+};
+
+//数据解析层模块开关
+message dataAnalysisCmd
+{
+    required bool frontViewCameraDataAnalysisModule = 1;         	//前视三目摄像头数据解析模块
+    required bool ViewAroundCameraDataAnalysisModule = 2;        	//环视摄像头数据解析模块
+    required bool MWRDataAnalysisModule = 3;                     	//毫米波雷达数据解析模块
+    required bool lidarDataAnalysisModule = 4;                   		//激光雷达数据解析模块
+    required bool URDataAnalysisModule = 5;                      		//超声波雷达数据解析模块
+    required bool GpsImuDataAnalysisModule = 6;                  	//GPS/IMU 数据解析模块
+    required bool vehicleStatusDataAnalysisModule = 7;           	//车辆状态信息数据解析模块
+};
+
+//数据处理层模块开关
+message dataProcessingCmd
+{
+    required bool frontViewThreeEyeCamera = 1;                   //前视三目摄像头图像处理模块
+    required bool visualCamera = 2;                              	//环视摄像头图像处理模块
+    required bool MMWRadar = 3;                                  	//毫米波雷达数据处理模块
+    required bool laserRadar = 4;                                	//激光雷达点云数据处理模块
+};
+
+//感知融合层模块开关
+message perceptualFusion
+{
+    required bool  spaceRegistrationModule = 1;                 //空间配准模块
+    required bool  perceptualFusionModule = 2;                  //感知融合模块
+    required bool  fusionPositioningModule = 3;                  //融合定位模块
+    required bool  timeRegistrationModule = 4;                   //时间配准模块
+    required bool  environmentModelingModule = 5;         //环境建模模块
+    required bool  lidarAndMayConstructModule = 6;         //激光雷达即时定位与地图构建处理模块
+};
+
+//功能开关命令
+message fucSetControlCmd
+{
+    required dataAnalysisCmd DataAnalysis =1;         	//数据解析层模块
+    required dataProcessingCmd DataProcessing =2;     	//数据处理层模块
+    required perceptualFusion PerceptualFusion =3;    	//感知融合层模块
+};
+
+//系统设置命令
+message settingsCmd
+{
+    required fucSetControlCmd fucSetControl =1;   	//功能设置
+    required ToDispControlCmd dispSetControl =2;  	//显示设置
+};

src/tool/IVSysMan/IVSysMan.pro

@@ -45,6 +45,7 @@ contains(QMAKE_HOST.os, Windows){
 
 
 SOURCES += \
+    ../../include/msgtype/switch.pb.cc \
         main.cpp \
         mainwindow.cpp \
     sysman.cpp \
@@ -58,6 +59,7 @@ SOURCES += \
     progunit.cpp
 
 HEADERS += \
+    ../../include/msgtype/switch.pb.h \
         mainwindow.h \
     sysman.h \
     progmon.h \

src/tool/IVSysMan/mainwindow.cpp

@@ -1,6 +1,8 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
+#include <iostream>
+
 extern std::string gstrxmlpath;
 
 
@@ -10,6 +12,10 @@ extern iv::Ivlog  * ivlog;
 static bool gbupdate =  false;
 static std::string gstrivsyspath;
 
+static iv::Switch::settingsCmd  gsettingcmd;
+static bool gbupdatesetting = false;
+QMutex gMutexSetting;
+
 
 
 void ListenChange(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -23,6 +29,24 @@ void ListenChange(const char * strdata,const unsigned int nSize,const unsigned i
 
 }
 
+void ListenSysSwitch(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::Switch::settingsCmd x;
+    if(!x.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenSysSwitch Parse error."<<std::endl;
+        return;
+    }
+    gMutexSetting.lock();
+    gsettingcmd.CopyFrom(x);
+    gbupdatesetting = true;
+    gMutexSetting.unlock();
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+}
+
 
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent),
@@ -89,9 +113,14 @@ MainWindow::MainWindow(QWidget *parent) :
     mpivexit = iv::ivexit::RegIVExitCmd();
 
     mpa = iv::modulecomm::RegisterRecv("ivsyschange",ListenChange);
+    mpaswitch = iv::modulecomm::RegisterRecv("sys_switch",ListenSysSwitch);
     QTimer * timerchange = new QTimer(this);
     connect(timerchange,SIGNAL(timeout()),this,SLOT(onTimerChangeXML()));
     timerchange->start(100);
+
+    QTimer * timerswitch = new QTimer(this);
+    connect(timerswitch,SIGNAL(timeout()),this,SLOT(onTimerSysSwitch()));
+    timerswitch->start(50);
 }
 
 MainWindow::~MainWindow()
@@ -239,4 +268,79 @@ void MainWindow::ChangeXML(std::string strxmlpath)
 //    pLabel->setGeometry(30,30,100,30);
 }
 
+void MainWindow::ProcdataAnalysisCmd(const iv::Switch::dataAnalysisCmd *pdataAnalysisCmd)
+{
+    if(pdataAnalysisCmd->mwrdataanalysismodule())
+    {
+        mPM->StartProc("driver_can_nvidia_agx");
+    }
+    else
+    {
+        mPM->StopProc("driver_can_nvidia_agx");
+    }
+    if(pdataAnalysisCmd->lidardataanalysismodule())
+    {
+        mPM->StartProc("driver_lidar_vlp32c");
+        mPM->StartProc("driver_lidar_vlp16","-s vlp16_left.yaml -n driver_lidar_vlp32_l");
+        mPM->StartProc("driver_lidar_vlp16","-s vlp16_right.yaml -n driver_lidar_vlp32_r");
+    }
+    else
+    {
+        mPM->StopProc("driver_lidar_vlp32c");
+        mPM->StopProc("driver_lidar_vlp16","-s vlp16_left.yaml -n driver_lidar_vlp32_l");
+        mPM->StopProc("driver_lidar_vlp16","-s vlp16_right.yaml -n driver_lidar_vlp32_r");
+    }
+    if(pdataAnalysisCmd->gpsimudataanalysismodule())
+    {
+        mPM->StartProc("detection_state_delphi_ins500d");
+    }
+    else
+    {
+        mPM->StartProc("detection_state_delphi_ins500d");
+    }
+}
+
+void MainWindow::ProcperceptualFusion(const iv::Switch::perceptualFusion *pperceptualFusion)
+{
+    if(pperceptualFusion->spaceregistrationmodule())
+    {
+        mPM->StartProc("driver_lidar_merge");
+    }
+    else
+    {
+        mPM->StopProc("driver_lidar_merge");
+    }
+    if(pperceptualFusion->fusionpositioningmodule())
+    {
+        mPM->StartProc("detection_ndt_matching_gpu_multi");
+    }
+    else
+    {
+        mPM->StopProc("detection_ndt_matching_gpu_multi");
+    }
+    if(pperceptualFusion->lidarandmayconstructmodule())
+    {
+        mPM->StartProc("adcndtmultimapping");
+    }
+    else
+    {
+        mPM->StopProc("adcndtmultimapping");
+    }
+}
+
+void MainWindow::onTimerSysSwitch()
+{
+    if(gbupdatesetting == false)return;
+    iv::Switch::settingsCmd pswitch;
+
+    gMutexSetting.lock();
+    pswitch.CopyFrom(gsettingcmd);
+    gbupdatesetting = false;
+    gMutexSetting.unlock();
+
+    iv::Switch::fucSetControlCmd xcmd = pswitch.fucsetcontrol();
+    ProcdataAnalysisCmd(&xcmd.dataanalysis());
+    ProcperceptualFusion(&xcmd.perceptualfusion());
+}
+
 

src/tool/IVSysMan/mainwindow.h

@@ -18,6 +18,8 @@
 
 #include "modulecomm.h"
 
+#include "switch.pb.h"
+
 namespace Ui {
 class MainWindow;
 }
@@ -54,6 +56,8 @@ private slots:
 
     void onTimerChangeXML();
 
+    void onTimerSysSwitch();
+
 
 
 private:
@@ -69,6 +73,8 @@ private:
 
     void * mpivexit;
 
+    void * mpaswitch;
+
     int mnTestIndex = 0;
 
     void * mpa;
@@ -76,6 +82,9 @@ private:
 public:
     void ChangeXML(std::string strxmlpath);
 
+    void ProcdataAnalysisCmd(const iv::Switch::dataAnalysisCmd * pdataAnalysisCmd);
+    void ProcperceptualFusion(const iv::Switch::perceptualFusion * pperceptualFusion);
+
 
 
     //add tjc

src/tool/IVSysMan/progmon.cpp

@@ -318,8 +318,68 @@ void ProgMon::restartProc(ProgUnit *pu){
     ivlog->info("start program: AppGroup - %s; AppDir - %s; AppName - %s; StartArgs - %s;", pu->strgroup.c_str(), pu->strappdir.c_str(), pu->strappname.c_str(), pu->strargs.c_str());
 }
 
+ProgUnit * ProgMon::FindProcByName(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = 0;
+    int i;
+    int nsize =  mvectorprog.size();
+    for(i=0;i<nsize;i++)
+    {
+        ProgUnit * putem = &mvectorprog.at(i);
+        if(strargs.size()<1)
+        {
+            if(strappname == putem->strappname)
+            {
+                pu = putem;
+                break;
+            }
+        }
+        else
+        {
+            if((strappname == putem->strappname)&&(strargs == putem->strargs))
+            {
+                pu = putem;
+                break;
+            }
+        }
+    }
+
+    return pu;
+}
+
+void ProgMon::StartProc(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = FindProcByName(strappname,strargs);
+
+    if(pu == 0)
+    {
+        qDebug("StartProc can't find app = %s args = %s",strappname.data(),strargs.data());
+        return;
+    }
+
+    StartProc(pu);
+}
+
+void ProgMon::StopProc(std::string strappname, std::string strargs)
+{
+    ProgUnit * pu = FindProcByName(strappname,strargs);
+
+    if(pu == 0)
+    {
+        qDebug("StopProc can't find app = %s args = %s",strappname.data(),strargs.data());
+        return;
+    }
+
+    StopProc(pu);
+}
+
 void ProgMon::StartProc(ProgUnit *pu)
 {
+    if(pu->mbRun)
+    {
+        qDebug("process %s is running. not need start.",pu->strappname.data());
+        return;
+    }
     pu->mProcess = new QProcess(this);
 
     connect(pu->mProcess,SIGNAL(started()),this,SLOT(onProcessStarted()));
@@ -352,6 +412,11 @@ void ProgMon::StopProc(ProgUnit *pu)
 {
     if(pu == 0)return;
     if(pu->mProcess == 0)return;
+    if(!pu->mbRun)
+    {
+        qDebug("process %s is not running. not need stop.",pu->strappname.data());
+        return;
+    }
     pu->mProcess->kill();
     if(!checkStartState(pu)){
         return;

src/tool/IVSysMan/progmon.h

@@ -62,9 +62,14 @@ public:
 
     std::vector<ProgUnit> loadprogunit(std::string path);
 
+    void StartProc(std::string strappname,std::string strargs="");
+    void StopProc(std::string strappname,std::string strargs="");
+
 private:
     QMutex mMutex;
 
+    ProgUnit * FindProcByName(std::string strappname,std::string strargs);
+
 signals:
     /* Signal when proc started */
     void SigProcStarted(ProgUnit * pu);

src/tool/adciv_replay/mainwindow.cpp

@@ -162,7 +162,7 @@ void MainWindow::onTimerReplay()
     QDateTime dtx = QDateTime::currentDateTime();
 
     qint64 timediff = mdtrpstart.msecsTo(dtx);
-    qDebug("diff is %d",timediff);
+ //   qDebug("diff is %d",timediff);
 
     bool bR= true;
 

src/tool/tool_scheduler/Readme.md

@@ -0,0 +1,3 @@
+1.本工程用于调度。
+2.设置调度的方法有两种，一种时接收scheduler.proto格式的消息，另外一种是从界面里选取xml文件。
+3.调度是单元是一系列的经纬度和停留时间，参考本工程的xml文件。

src/tool/tool_scheduler/ivscheduler.cpp

@@ -2,6 +2,9 @@
 
 #include <iostream>
 
+
+static ivscheduler * givs;
+
 static QMutex gMutexGPSIMU;
 
 static iv::gps::gpsimu ggpsimu;
@@ -26,7 +29,7 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
     iv::gps::gpsimu xgpsimu;
     if(!xgpsimu.ParseFromArray(strdata,nSize))
     {
-        std::cout<<"ListenRaw Parse error."<<std::endl;
+        std::cout<<"Listengpsimu Parse error."<<std::endl;
         return;
     }
     (void)&index;
@@ -39,10 +42,32 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
     gMutexGPSIMU.unlock();
 }
 
-ivscheduler::ivscheduler(std::string strmemname)
+void Listenscheduler(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+    iv::scheduler pscheduler;
+    if(!pscheduler.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Listenscheduler Parse error."<<std::endl;
+        return;
+    }
+
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+    givs->setscheduler(&pscheduler);
+
+}
+
+ivscheduler::ivscheduler(std::string strmemname,std::string strschedulername)
 {
+    givs = this;
     mstrmemname = strmemname;
+    mstrschedulername = strschedulername;
     iv::modulecomm::RegisterRecv(mstrmemname.data(),Listengpsimu);
+    iv::modulecomm::RegisterRecv(mstrschedulername.data(),Listenscheduler);
     mpadst = iv::modulecomm::RegisterSend("xodrreq",1000,1);
 }
 
@@ -80,12 +105,14 @@ void ivscheduler::run()
                         mnLastSendObj = QDateTime::currentMSecsSinceEpoch();
                         SendObj(mfLonObj,mfLatObj);
                         mnstep = 1;
+                        emit updatestep(mnstep);
                     }
                         break;
                     case 1:
                         if(IsVehicleMoving(&xgpsimu))
                         {
                             mnstep = 2;
+                            emit updatestep(mnstep);
                         }
                         else
                         {
@@ -100,6 +127,7 @@ void ivscheduler::run()
                         {
                             mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
                             mnstep = 3;
+                            emit updatestep(mnstep);
                         }
                         //Decide Vechicle Running.
                         break;
@@ -108,6 +136,7 @@ void ivscheduler::run()
                         {
                             mnArrivingTime = QDateTime::currentMSecsSinceEpoch();
                             mnstep = 3;
+                            emit updatestep(mnstep);
                         }
                         //Decide Vehiclde Arrive End Point;
                         break;
@@ -117,12 +146,14 @@ void ivscheduler::run()
                         if((QDateTime::currentMSecsSinceEpoch() - mnArrivingTime)>= (pscheduler_unit->mstopsecond()*1000) )
                         {
                             mnstep = 4;
+                            emit updatestep(mnstep);
                         }
                     }
                         //Count Stop Time;
                         break;
                     case 4:
                         mnstep = 0;
+                        emit updatestep(mnstep);
                         mnprocess++;
                         break;
                     default:
@@ -147,11 +178,13 @@ void ivscheduler::run()
     }
 }
 
-void ivscheduler::setscheduler(iv::scheduler *pscheduler, int ncyclecount)
+void ivscheduler::setscheduler(iv::scheduler *pscheduler)
 {
     mMutex.lock();
-    mpscheduler = pscheduler;
-    mncyclecount = ncyclecount;
+    if(mpscheduler != 0)delete mpscheduler;
+    mpscheduler = new iv::scheduler;
+    mpscheduler->CopyFrom(*pscheduler);
+    mncyclecount = pscheduler->mcyble();
     mncycle = 0;
     mnprocess = 0;
     mnstep = 0;

src/tool/tool_scheduler/ivscheduler.h

@@ -15,11 +15,11 @@ class ivscheduler : public QThread
 {
         Q_OBJECT
 public:
-    ivscheduler(std::string strmemname);
+    ivscheduler(std::string strmemname = "hcp2_gpsimu",std::string strschedulername = "scheduler");
 
 
 public:
-    void setscheduler(iv::scheduler * pscheduler,int ncyclecount = 1);
+    void setscheduler(iv::scheduler * pscheduler);
     void run();
 
     void GetProcess(int & nProc, int & nProcTotal);
@@ -28,6 +28,7 @@ public:
 signals:
     void updategps(double flon,double flat,double fheading);
     void updatestate(int ncycle,int ncyclecount,int nprocess,int nprocesscount);
+    void updatestep(int nstep);
 
 
 private:
@@ -53,6 +54,7 @@ private:
     QMutex mMutex;
 
     std::string mstrmemname;
+    std::string mstrschedulername;
     void * mpadst;
 
 private:

src/tool/tool_scheduler/main.cpp

@@ -2,9 +2,30 @@
 
 #include <QApplication>
 
+#include "xmlparam.h"
+
+std::string gstrgpsmsg;
+std::string gstrschmsg;
+
 int main(int argc, char *argv[])
 {
     QApplication a(argc, argv);
+
+    std::string strparapath;
+    if(argc<2)
+    {
+
+        strparapath = "./tool_scheduler.xml";
+    }
+    else
+    {
+
+        strparapath = argv[2];
+    }
+
+    iv::xmlparam::Xmlparam xp(strparapath);
+    gstrgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+    gstrschmsg = xp.GetParam("schmsg","scheduler");
     MainWindow w;
     w.show();
     return a.exec();

src/tool/tool_scheduler/mainwindow.cpp

@@ -4,17 +4,23 @@
 #include <QFileDialog>
 #include <QMessageBox>
 
+extern std::string gstrgpsmsg;
+extern std::string gstrschmsg;
+
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
     , ui(new Ui::MainWindow)
 {
     ui->setupUi(this);
 
-    mpivsch = new ivscheduler("hcp2_gpsimu");
+    mpivsch = new ivscheduler(gstrgpsmsg,gstrschmsg);
     mpivsch->start();
 
     connect(mpivsch,SIGNAL(updategps(double,double,double)),this,SLOT(onUpdatePos(double,double,double)));
     connect(mpivsch,SIGNAL(updatestate(int,int,int,int)),this,SLOT(onUpdateState(int,int,int,int)));
+    connect(mpivsch,SIGNAL(updatestep(int)),this,SLOT(onUpdateStep(int)));
+
+    setWindowTitle("Scheduler");
 }
 
 MainWindow::~MainWindow()
@@ -32,6 +38,9 @@ void MainWindow::on_pushButton_Start_clicked()
     }
 
 
+    int ncycle = ui->lineEdit_Cycle->text().toInt();
+    if(ncycle<1)ncycle = 1;
+    mpscheduler->set_mcyble(ncycle);
     mpivsch->setscheduler(mpscheduler);
 }
 
@@ -143,3 +152,27 @@ void MainWindow::onUpdateState(int ncycle, int ncyclecount, int nprocess, int np
     snprintf(strout,100,"%d/%d",nprocess,nprocesscount);
     ui->lineEdit_Process->setText(strout);
 }
+
+void MainWindow::onUpdateStep(int nstep)
+{
+    switch(nstep)
+    {
+    case 0:
+        ui->lineEdit_Step->setText("Init...");
+        break;
+    case 1:
+        ui->lineEdit_Step->setText("Decide Vehcile Moving...");
+        break;
+    case 2:
+        ui->lineEdit_Step->setText("Go to Destination...");
+        break;
+    case 3:
+        ui->lineEdit_Step->setText("Arrived. Stoping...");
+        break;
+    case 4:
+        ui->lineEdit_Step->setText("Complete...");
+        break;
+    }
+
+
+}

src/tool/tool_scheduler/mainwindow.h

@@ -27,6 +27,7 @@ private slots:
 
     void onUpdatePos(double flon,double flat,double fheading);
     void onUpdateState(int ncycle,int ncyclecount,int nprocess,int nprocesscount);
+    void onUpdateStep(int nstep);
 
 private:
     Ui::MainWindow *ui;

src/tool/tool_scheduler/mainwindow.ui

@@ -188,6 +188,29 @@
      <string>Process</string>
     </property>
    </widget>
+   <widget class="QLineEdit" name="lineEdit_Step">
+    <property name="geometry">
+     <rect>
+      <x>240</x>
+      <y>504</y>
+      <width>401</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>110</x>
+      <y>508</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Step</string>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">

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

@@ -1047,6 +1047,101 @@ void Viewer::SetCameraMode(int mode)
 	rubberbandManipulator_->setMode(camMode_);
 }
 
+EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
+{
+
+    osg::ref_ptr<osg::Group> group = 0;
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(new osg::ShapeDrawable(new osg::Box()));
+
+        osg::ref_ptr<osg::Group> bbGroup = new osg::Group;
+        osg::ref_ptr<osg::PositionAttitudeTransform> tx = new osg::PositionAttitudeTransform;
+
+        osg::Material* material = new osg::Material();
+
+        // Set color of vehicle based on its index
+        double* color;
+        double b = 1.5;  // brighness
+        color = color_blue;
+
+        material->setDiffuse(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+        material->setAmbient(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+
+        if (group == 0)
+        {
+            // If no model loaded, make a shaded copy of bounding box as model
+            osg::ref_ptr<osg::Geode> geodeCopy = dynamic_cast<osg::Geode*>(geode->clone(osg::CopyOp::DEEP_COPY_ALL));
+            group = new osg::Group;
+            tx->addChild(geodeCopy);
+            geodeCopy->setNodeMask(NodeMask::NODE_MASK_ENTITY_MODEL);
+        }
+
+        // Set dimensions of the entity "box"
+
+            // Scale to something car-ish
+            tx->setScale(osg::Vec3(0.5, 0.5, 1.5));
+            tx->setPosition(osg::Vec3(1.5, 0, 0.75));
+
+        tx->addChild(geode);
+        tx->getOrCreateStateSet()->setAttribute(material);
+        bbGroup->setName("BoundingBox");
+        bbGroup->addChild(tx);
+
+        group->addChild(bbGroup);
+        group->setName(name);
+
+    EntityModel* model;
+//	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
+//	{
+        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->state_set_ = model->lod_->getOrCreateStateSet(); // Creating material
+    model->blend_color_ = new osg::BlendColor(osg::Vec4(1, 1, 1, 1));
+    model->state_set_->setAttributeAndModes(model->blend_color_);
+    model->blend_color_->setDataVariance(osg::Object::DYNAMIC);
+
+    osg::BlendFunc* bf = new osg::BlendFunc(osg::BlendFunc::CONSTANT_ALPHA, osg::BlendFunc::ONE_MINUS_CONSTANT_ALPHA);
+    model->state_set_->setAttributeAndModes(bf);
+    model->state_set_->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
+    model->state_set_->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+
+    entities_.push_back(model);
+
+//	// Focus on first added car
+//	if (entities_.size() == 1)
+//	{
+//		currentCarInFocus_ = 0;
+//		rubberbandManipulator_->setTrackNode(entities_.back()->txNode_,
+//			rubberbandManipulator_->getMode() == osgGA::RubberbandManipulator::CAMERA_MODE::RB_MODE_TOP ? false : true);
+//		nodeTrackerManipulator_->setTrackNode(entities_.back()->txNode_);
+//	}
+
+//	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
+//	{
+//		CarModel* vehicle = (CarModel*)entities_.back();
+//		CreateRoadSensors(vehicle);
+
+//		if (road_sensor)
+//		{
+//			vehicle->road_sensor_->Show();
+//		}
+//		else
+//		{
+//			vehicle->road_sensor_->Hide();
+//		}
+//	}
+
+    return entities_.back();
+}
+
 EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
 	bool road_sensor, std::string name, OSCBoundingBox* boundingBox)
 {
@@ -1189,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();

src/ui/ui_osgviewer/viewer.hpp

@@ -317,6 +317,7 @@ namespace viewer
 		int GetEntityInFocus() { return currentCarInFocus_; }
 		EntityModel* AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
 			bool road_sensor, std::string name, OSCBoundingBox *boundingBox);
+                EntityModel* AddRadarModel(osg::Vec3 trail_color,std::string name);
 		void RemoveCar(std::string name);
 		int LoadShadowfile(std::string vehicleModelFilename);
 		int AddEnvironment(const char* filename);