change too/map_lanetoxodr. change ObjectSignal ADD some markings code,but not complete.

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

@@ -2,6 +2,72 @@
 
 #include <iostream>
 
+Object_markings_marking::Object_markings_marking(std::string color, double spaceLength, double lineLength,
+                                                 double startOffset, double stopOffset)
+{
+    mcolor = color;
+    mspaceLength = spaceLength;
+    mlineLength = lineLength;
+    mstartOffset = startOffset;
+    mstopOffset = stopOffset;
+}
+
+Object_markings::Object_markings(Object_markings_marking xmarking)
+{
+    mmarking.push_back(xmarking);
+}
+
+vector<Object_markings_marking> * Object_markings::GetMarkingVector()
+{
+    return &mmarking;
+}
+
+Object_markings_marking* Object_markings::GetMarking(unsigned int i)
+{
+    if((mmarking.size() == 0) ||(i>=mmarking.size()))
+    {
+        return NULL;
+    }
+    return &(mmarking.at(i));
+}
+
+unsigned int Object_markings::GetMarkingCount()
+{
+    return mmarking.size();
+}
+
+Object_markings_marking*			Object_markings::GetLastMarking()
+{
+    if(mmarking.size() == 0)return NULL;
+    return &mmarking.at(mmarking.size() -1);
+}
+
+unsigned int Object_markings::AddMarking(string color,double spaceLength,
+                        double lineLength,double startOffset,double stopOffset)
+{
+    mmarking.push_back(Object_markings_marking(color,spaceLength,lineLength,startOffset,stopOffset));
+    return mmarking.size()-1;
+}
+
+unsigned int Object_markings::CloneMarking(unsigned int index)
+{
+    if(index<(mmarking.size()-1))
+        mmarking.insert(mmarking.begin()+index+1, mmarking[index]);
+    else if(index==mmarking.size()-1)
+        mmarking.push_back(mmarking[index]);
+    mnLastAddedmarking=index+1;
+    return mnLastAddedmarking;
+}
+
+void Object_markings::DeleteMarking(unsigned int index)
+{
+    if((mmarking.size() == 0) ||(index>=mmarking.size()))
+    {
+    }
+
+    mmarking.erase(mmarking.begin() + index);
+}
+
 Object_outlines_outline_cornerLocal::Object_outlines_outline_cornerLocal(double u,
                                                                          double v,double z,double height)
 {

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

@@ -8,6 +8,52 @@ using std::vector;
 using std::string;
 
 
+
+class Object_markings_marking
+{
+private:
+    vector<string> mside;
+    vector<string> mwight;
+    vector<double> mwidth;
+    string mcolor;
+    vector<double> mzOffset;
+    double mspaceLength;
+    double mlineLength;
+    double mstartOffset;
+    double mstopOffset;
+
+
+
+public:
+    Object_markings_marking(string color,double spaceLength,
+                            double lineLength,double startOffset,double stopOffset);
+};
+
+class Object_markings
+{
+private:
+    vector<Object_markings_marking> mmarking;
+
+private:
+    int mnLastAddedmarking;
+
+public:
+    Object_markings(Object_markings_marking xmarking);
+
+    vector<Object_markings_marking> * GetMarkingVector();
+    Object_markings_marking* GetMarking(unsigned int i);
+    unsigned int GetMarkingCount();
+    Object_markings_marking*			GetLastMarking();
+    unsigned int AddMarking(string color,double spaceLength,
+                            double lineLength,double startOffset,double stopOffset);
+    unsigned int CloneMarking(unsigned int index);
+    void DeleteMarking(unsigned int index);
+
+
+
+
+};
+
 class Object_outlines_outline_cornerLocal
 {
 private:

src/controller/adcapollompc/adcapollompc.pro

@@ -104,6 +104,9 @@ SOURCES += \
         ../../../thirdpartylib/apollo/modules/planning/proto/sl_boundary.pb.cc \
         ../../../thirdpartylib/apollo/modules/planning/proto/task_config.pb.cc \
         ../../../thirdpartylib/apollo/modules/routing/proto/routing.pb.cc \
+        ../../include/msgtype/gps.pb.cc \
+        ../../include/msgtype/gpsimu.pb.cc \
+        ../../include/msgtype/imu.pb.cc \
         adc_mpc_controller.cc \
         adcapollompc_impl.cpp \
         main.cpp
@@ -125,6 +128,9 @@ HEADERS += \
     ../../../thirdpartylib/apollo/modules/control/controller/mpc_controller.h \
     ../../../thirdpartylib/apollo/modules/map/proto/map_rsu.pb.h \
     ../../../thirdpartylib/apollo/modules/planning/proto/planning.pb.h \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/imu.pb.h \
     adcapollompc_impl.h
 
 

src/controller/adcapollompc/adcapollompc_impl.cpp

@@ -1,5 +1,7 @@
 #include "adcapollompc_impl.h"
 
+#include "modules/common/math/quaternion.h"
+
 adcapollompc_impl::adcapollompc_impl(std::string strconfpath)
 {
     FLAGS_enable_map_reference_unify = false;
@@ -15,4 +17,44 @@ adcapollompc_impl::adcapollompc_impl(std::string strconfpath)
         << "Failed to open file " << strconfpath;
 
     mMPCController.Init(injector,&xConf);
+
+    ModuleFun funfusion = std::bind(&adcapollompc_impl::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("li_ra_fusion",funfusion);
+
+}
+
+void adcapollompc_impl::UpdateGPSIMU(const char *strdata,
+                                     const unsigned int nSize, const unsigned int index, const QDateTime *dt,
+                                     const char *strmemname)
+{
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize) == false)
+    {
+        std::cout<<"adcapollompc_impl::UpdateGPSIMU Parse Error."<<std::endl;
+        return;
+    }
+
+   apollo::localization::Pose * pPose = new apollo::localization::Pose;
+
+   apollo::common::PointENU * pPENU = new apollo::common::PointENU;
+   pPENU->set_x(0);
+   pPENU->set_y(0);
+   pPENU->set_z(0);
+
+   double fheading = xgpsimu.heading();
+   pPose->set_heading(fheading);
+   apollo::common::Quaternion * pQuaternion = new apollo::common::Quaternion;
+//   Eigen::Quaternion<double > eq = apollo::common::math::HeadingToQuaternion(fheading);
+//   pQuaternion->set_qw(eq.coeffs();
+
+   pPose->set_allocated_orientation(pQuaternion);
+   pPose->set_allocated_position(pPENU);
+
+
+   mapollolocalize.set_allocated_pose(pPose);
+
+
 }

src/controller/adcapollompc/adcapollompc_impl.h

@@ -17,6 +17,10 @@
 
 #include "modules/common/configs/config_gflags.h"
 
+#include "modulecomm.h"
+
+#include "gpsimu.pb.h"
+
 class adcapollompc_impl
 {
 public:
@@ -25,6 +29,20 @@ public:
 private:
     apollo::common::VehicleStateProvider * mvehicle_state;
     apollo::control::MPCController mMPCController;
+
+private:
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+
+private:
+    void * mpagpsimu;
+
+    apollo::localization::LocalizationEstimate mapollolocalize;
+
+    double mfLat0 = 39;
+    double mfLon0 = 117;
+    double mfX0 = 0;
+    double mfY0 = 0;
 };
 
 #endif // ADCAPOLLOMPC_IMPL_H