add test/carmakerservie for ipg xodr to rd5.

src/decition/decition_brain_localplan/brain_localplan_gpstype.cpp

@@ -45,6 +45,8 @@ void brain_localplan_gpstype::threadplan()
     std::shared_ptr<iv::gps::gpsimu> pgpsimu_ptr = nullptr;
     std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr = nullptr;
 
+     bool bNewMap = false;
+
     int nnogps = 0;
     while(mbRun)
     {
@@ -67,7 +69,7 @@ void brain_localplan_gpstype::threadplan()
             continue;
         }
 
-        bool bNewMap = false;
+
 
         if(mbMapUpdate)
         {
@@ -128,6 +130,12 @@ void brain_localplan_gpstype::threadplan()
 
         std::shared_ptr<iv::decision::localplan> plocalplan_ptr = mpplan->GetPlan(pmapglobal_ptr,pgpsimu_ptr,pfusion_ptr,bNewMap);
 
+        //If have Decision, set new Map False;
+        if(plocalplan_ptr->mlocalpoint_size()>0)
+        {
+            bNewMap = false;
+        }
+
         //Share local plan
 
 

src/decition/decition_brain_localplan/localplan_base.cpp

@@ -21,3 +21,9 @@ std::shared_ptr<iv::decision::localplan> localplan_base::GetPlan(std::shared_ptr
 
     return plan_ptr;
 }
+
+
+bool localplan_base::IsUseCircle()
+{
+    return  mbUseCircle;
+}

src/decition/decition_brain_localplan/localplan_base.h

@@ -11,9 +11,14 @@ public:
     localplan_base();
     virtual ~localplan_base();
 
+    bool IsUseCircle();
+
 public:
     virtual std::shared_ptr<iv::decision::localplan> GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
-                                                             std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap);;
+                                                             std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap);
+
+private:
+    bool mbUseCircle = true;
 };
 
 #endif // LOCALPLAN_BASE_H

src/decition/decition_brain_localplan/localplan_follow.cpp

@@ -1,5 +1,7 @@
 #include "localplan_follow.h"
 
+#include <math.h>
+
 localplan_follow::localplan_follow()
 {
 
@@ -10,6 +12,61 @@ localplan_follow::~localplan_follow()
 
 }
 
+void localplan_follow::GetPointDisDiff(iv::map::pointglobal * pointnow,iv::map::pointglobal * pointobj,
+                        double & fDis, double & fHeadingDiff)
+{
+    fDis = sqrt(pow(pointnow->gps_x() - pointobj->gps_x(),2) + pow(pointnow->gps_y() - pointobj->gps_y(),2));
+
+    fHeadingDiff = pointnow->ins_heading_angle() - pointobj->ins_heading_angle();
+    if(fHeadingDiff>180)fHeadingDiff = fHeadingDiff - 360.0;
+    if(fHeadingDiff<-180)fHeadingDiff = fHeadingDiff + 360.0;
+
+}
+
+int localplan_follow::GetNearPointIndex(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                      bool bNewMap)
+{
+    static int nLastIndex = -1;
+    if(bNewMap)nLastIndex = -1;
+
+    if(pmap_ptr->point_size() < 230)
+    {
+        return  -1;
+    }
+
+    bool bCircleRun = false;
+
+    //If Use Circle Run, Check is a circle
+    if(IsUseCircle())
+    {
+        iv::map::pointglobal * plastpoint = pmap_ptr->mutable_point(pmap_ptr->point_size()-1);
+        iv::map::pointglobal * pfirstpoint = pmap_ptr->mutable_point(0);
+        double fDisDiff,fHeadingDiff;
+        GetPointDisDiff(plastpoint,pfirstpoint,fDisDiff,fHeadingDiff);
+        if((fabs(fHeadingDiff)<30) && (fDisDiff<10))
+        {
+            bCircleRun = true;
+        }
+    }
+
+    if(nLastIndex >= 0)
+    {
+        int nserchfromindex = nLastIndex - 100;
+        int nserchtoindex = nLastIndex + 100;
+        if(nserchfromindex<0)nserchfromindex = 0;
+        int i;
+        for(i=nserchfromindex;i<nserchtoindex;i++)
+        {
+
+        }
+    }
+
+    if(nLastIndex == -1)
+    {
+
+    }
+}
+
 std::shared_ptr<iv::decision::localplan> localplan_follow::GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
                                                                    std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap)
 {
@@ -19,5 +76,7 @@ std::shared_ptr<iv::decision::localplan> localplan_follow::GetPlan(std::shared_p
     (void)bNewMap;
     std::shared_ptr<iv::decision::localplan> plan_ptr = std::shared_ptr<iv::decision::localplan>(new iv::decision::localplan);
 
+
+
     return plan_ptr;
 }

src/decition/decition_brain_localplan/localplan_follow.h

@@ -12,6 +12,13 @@ public:
 public:
     virtual std::shared_ptr<iv::decision::localplan> GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
                                                              std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap);
+
+private:
+    int GetNearPointIndex(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                          bool bNewMap);
+
+    inline void GetPointDisDiff(iv::map::pointglobal * pointnow,iv::map::pointglobal * pointobj,
+                         double & fDis, double & fHeadingDiff);
 };
 
 #endif // LOCALPLAN_FOLLOW_H

src/driver/driver_map_xodrload/main.cpp

@@ -14,6 +14,8 @@
 
 #include "mapdata.pb.h"
 
+#include "mapglobal.pb.h"
+
 #include "v2x.pb.h"
 
 #include "modulecomm.h"
@@ -50,13 +52,14 @@ bool gbExtendMap = true;
 static bool gbSideEnable = false;
 static bool gbSideLaneEnable = false;
 
-void * gpa;
-void * gpasrc;
-void * gpmap;
-void * gpagps;
-void * gpasimple;
-void * gpav2x;
+static void * gpa;
+static void * gpasrc;
+static void * gpmap;
+static void * gpagps;
+static void * gpasimple;
+static void * gpav2x;
 static void * gpanewtrace;
+static void * gpamapglobal;
 
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
@@ -334,7 +337,60 @@ public:
     int lane;
 };
 
-xodrobj gsrc;
+static xodrobj gsrc;
+
+void ShareMapGlobal(std::vector<PlanPoint> & xvectorPlan)
+{
+    iv::map::mapglobal xglobal;
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int >(xvectorPlan.size());
+    for(i=0;i<nsize;i++)
+    {
+        iv::map::pointglobal * ppoint = xglobal.add_point();
+        double gps_lon,gps_lat,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xvectorPlan[i].x,xvectorPlan[i].y,xvectorPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
+        ppoint->set_gps_lat(gps_lat);
+        ppoint->set_gps_lng(gps_lon);
+        ppoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y;
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        ppoint->set_gps_x(gps_x);
+        ppoint->set_gps_y(gps_y);
+        ppoint->set_gps_z(0);
+
+        ppoint->set_mfcurvature(xvectorPlan[i].mfCurvature);
+        ppoint->set_mfdistolaneleft(xvectorPlan[i].mfDisToLaneLeft);
+        ppoint->set_mfdistoroadleft(xvectorPlan[i].mfDisToRoadLeft);
+        ppoint->set_mflanewidth(xvectorPlan[i].mWidth);
+        ppoint->set_mfroadwidth(xvectorPlan[i].mfRoadWidth);
+        ppoint->set_speed(xvectorPlan[i].speed);
+        unsigned int nlanecount = static_cast<unsigned int>(xvectorPlan[i].mVectorLaneWidth.size());
+        unsigned int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            ppoint->add_mfvectorlanewidth(xvectorPlan[i].mVectorLaneWidth[j]);
+        }
+        ppoint->set_mlanecount(xvectorPlan[i].mLaneCount);
+        ppoint->set_mlanecur(xvectorPlan[i].mLaneCur);
+
+
+    }
+
+    int bytesize = static_cast<int>(xglobal.ByteSizeLong()) ;
+
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[bytesize]);
+    if(xglobal.SerializePartialToArray(pstr_ptr.get(),bytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpamapglobal,pstr_ptr.get(),static_cast<unsigned int>(bytesize) );
+    }
+    else
+    {
+        std::cout<<" ShareMapGlobal  Serialzie Fail."<<std::endl;
+    }
+
+}
 
 void ShareMap(std::vector<iv::GPSData> navigation_data)
 {
@@ -554,6 +610,7 @@ void CalcSide(std::vector<PlanPoint> & xPlan)
     return;
 }
 
+
 void SetPlan(xodrobj xo)
 {
 
@@ -843,6 +900,8 @@ void SetPlan(xodrobj xo)
     xfile_1.close();
     ShareMap(mapdata);
 
+    ShareMapGlobal(xPlan);
+
     int nnewmapsize = xtrace.ByteSize();
     std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
     if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
@@ -1194,6 +1253,7 @@ int main(int argc, char *argv[])
     gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
     gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
     gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
+    gpamapglobal = iv::modulecomm::RegisterSend("mapglobal",20000000,1);
 
 
     gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);

src/include/proto3/carmaker.proto

@@ -0,0 +1,63 @@
+// Copyright 2015 gRPC authors.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+syntax = "proto3";
+
+option java_multiple_files = true;
+option java_package = "io.grpc.adc.ipg";
+option java_outer_classname = "IPGMsgProto";
+option objc_class_prefix = "HLW";
+
+package iv.ipg;
+
+// The Upload service definition.
+service Carmaker {
+  // Sends a Upload
+  rpc convertreq(cvtReq) returns (cvtReply) {}
+  rpc convertwork(workReq) returns (workReply) {}
+  
+}
+
+
+message workReq {
+  int32 mnRes = 1; //0 No Data  1 Have Work Data  -1 Convert Fail
+  int64 workid = 2;
+  bytes xdata = 3;  
+}
+
+message workReply {
+  int32 mnRes = 1;  //0 no work  1 have work
+  int64 workid = 2;
+  string strInputName = 3;
+  string strOutputName = 4;
+  string strcmd = 5;
+  bytes data_Input = 6;
+  bytes data_Output = 7;
+}
+
+message cvtReq
+{
+  string strInputName = 1;
+  string strOutputName = 2;
+  string strcmd = 3;
+  bytes data_Input = 4;
+  bytes data_Output = 5;
+  int32 mCvtTimeLimms = 6; //default 30s
+}
+
+message cvtReply {
+  int32 mnRes = 1; //0 Work OK  -1 Convert Fail  -2 No CARMAKER CONVERT SERVICE
+  bytes xdata = 2;  
+
+}

src/test/carmakerservice/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/test/carmakerservice/carmakerimpl.cpp

@@ -0,0 +1,14 @@
+#include "carmakerimpl.h"
+
+
+Status CarmakerImpl::convertreq(ServerContext* context, const iv::ipg::cvtReq* request,
+                iv::ipg::cvtReply* reply)
+{
+
+}
+
+Status CarmakerImpl::convertwork(ServerContext* context, const iv::ipg::workReq * request,
+                   iv::ipg::workReply* reply)
+{
+
+}

src/test/carmakerservice/carmakerimpl.h

@@ -0,0 +1,30 @@
+#ifndef CARMAKERIMPL_H
+#define CARMAKERIMPL_H
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <grpcpp/grpcpp.h>
+#include <grpcpp/health_check_service_interface.h>
+#include <grpcpp/ext/proto_server_reflection_plugin.h>
+
+#include "carmaker.grpc.pb.h"
+
+#include "carmaker.pb.h"
+
+using grpc::Server;
+using grpc::ServerBuilder;
+using grpc::ServerContext;
+using grpc::Status;
+
+
+class CarmakerImpl   final: public iv::ipg::Carmaker::Service
+{
+    Status convertreq(ServerContext* context, const iv::ipg::cvtReq* request,
+                    iv::ipg::cvtReply* reply) override;
+    Status convertwork(ServerContext* context, const iv::ipg::workReq * request,
+                       iv::ipg::workReply* reply) override;
+};
+
+#endif // CARMAKERIMPL_H

src/test/carmakerservice/carmakerservice.pro

@@ -0,0 +1,39 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    carmaker.grpc.pb.cc \
+    ../../include/msgtype/carmaker.pb.cc \
+    carmakerimpl.cpp \
+    worktask.cpp
+
+
+HEADERS += \
+    carmaker.grpc.pb.h \
+    ../../include/msgtype/carmaker.pb.h \
+    carmakerimpl.h \
+    worktask.h
+
+
+INCLUDEPATH += $$PWD/../../../src/include/msgtype
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivgrpc.pri ) {
+    error( "Couldn't find the ivgrpc.pri file!" )
+}

src/test/carmakerservice/main.cpp

@@ -0,0 +1,25 @@
+#include <QCoreApplication>
+
+#include <iostream>
+
+#include "signal.h"
+
+
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        std::cout<<" sig out. "<<std::endl;
+        exit(0);
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    signal(SIGINT,signal_handler);
+
+    return a.exec();
+}

src/test/carmakerservice/worktask.cpp

@@ -0,0 +1,122 @@
+#include "worktask.h"
+
+WorkTask::WorkTask()
+{
+}
+
+int64_t WorkTask::AddTask(std::string strInputName,std::string strOutputName,std::string strcmd,std::shared_ptr<char> xdata_Input,int nInputSize)
+{
+    iv::TaskUnit xTU;
+    mMutex.lock();
+    xTU.mnTaskID = mnIDNow++;
+    xTU.mbConverting = false;
+    xTU.mstrInputName = strInputName;
+    xTU.mstrOutputName = strOutputName;
+    xTU.mstrcmd = strcmd;
+    xTU.mdata_Input = xdata_Input;
+    xTU.mnInputSize = nInputSize;
+    xTU.mdata_Output = nullptr;
+    xTU.mnOutputSize = 0;
+    xTU.mbComplete = false;
+    xTU.mbConverting = false;
+    mvecterTask.push_back(xTU);
+    mMutex.unlock();
+    return xTU.mnTaskID;
+}
+
+void WorkTask::DeleteTask(int mnID)
+{
+    mMutex.lock();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(mvecterTask.size()) ;
+    for(i=0;i<nsize;i++)
+    {
+        if(mvecterTask[i].mnTaskID == mnID)
+        {
+            mvecterTask.erase(mvecterTask.begin()+i);
+            break;
+        }
+    }
+    mMutex.unlock();
+}
+
+int WorkTask::ReqTask(std::string & strInputName,std::string & strOutputName,std::string & strcmd,std::shared_ptr<char> & xdata_Input,int & nInputSize,int64_t & nID) //if 1 a task . if 0 no task
+{
+    int nrtn = 0;
+    mMutex.lock();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(mvecterTask.size()) ;
+    for(i=0;i<nsize;i++)
+    {
+        if(mvecterTask[i].mbConverting == false)
+        {
+            mvecterTask[i].mbConverting = true;
+            nID = mvecterTask[i].mnTaskID;
+            strInputName = mvecterTask[i].mstrInputName;
+            strOutputName = mvecterTask[i].mstrOutputName;
+            strcmd = mvecterTask[i].mstrcmd;
+            xdata_Input = mvecterTask[i].mdata_Input;
+            nInputSize = mvecterTask[i].mnInputSize;
+            nrtn = 1;
+            break;
+
+        }
+    }
+    mMutex.unlock();
+}
+
+void WorkTask::SetTaskRes(int nID,int ncvtRes,std::shared_ptr<char> xdata_Output,int nOutputSize)
+{
+    mMutex.lock();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(mvecterTask.size()) ;
+    for(i=0;i<nsize;i++)
+    {
+        if(mvecterTask[i].mnTaskID == nID)
+        {
+            mvecterTask[i].mnCvtRes = ncvtRes;
+            mvecterTask[i].mdata_Output = xdata_Output;
+            mvecterTask[i].mnOutputSize = nOutputSize;
+            mvecterTask[i].mbComplete = true;
+            break;
+        }
+    }
+    mMutex.unlock();
+}
+
+int WorkTask::GetTaskRes(int nID,int & ncvtRes,std::shared_ptr<char> & xdata_Output, int & nOutputSize)
+{
+    mMutex.lock();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(mvecterTask.size()) ;
+    for(i=0;i<nsize;i++)
+    {
+        if(mvecterTask[i].mnTaskID == nID)
+        {
+            ncvtRes = mvecterTask[i].mnCvtRes;
+            xdata_Output = mvecterTask[i].mdata_Output;
+            nOutputSize = mvecterTask[i].mnOutputSize;
+            mvecterTask.erase(mvecterTask.begin()+i);
+            break;
+        }
+    }
+    mMutex.unlock();
+}
+
+bool WorkTask::InqureTaskComplete(int nID)
+{
+    bool bComplete = false;
+    mMutex.lock();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int>(mvecterTask.size()) ;
+    for(i=0;i<nsize;i++)
+    {
+        if(mvecterTask[i].mnTaskID == nID)
+        {
+            bComplete = mvecterTask[i].mbComplete;
+            break;
+        }
+    }
+    mMutex.unlock();
+    return bComplete;
+}

src/test/carmakerservice/worktask.h

@@ -0,0 +1,50 @@
+#ifndef WORKTASK_H
+#define WORKTASK_H
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+
+namespace iv
+{
+class TaskUnit
+{
+public:
+    int64_t mnTaskID;
+    std::string mstrInputName;
+    std::string mstrOutputName ;
+    std::string mstrcmd;
+    std::shared_ptr<char> mdata_Input;
+    int mnInputSize;
+    std::shared_ptr<char> mdata_Output;
+    int mnOutputSize;
+    bool mbConverting;
+    int mnCvtRes ;
+    bool mbComplete = false;
+
+
+};
+}
+
+
+class WorkTask
+{
+public:
+    WorkTask();
+
+    int64_t AddTask(std::string strInputName,std::string strOutputName,std::string strcmd,std::shared_ptr<char> xdata_Input,int nInputSize);
+    void DeleteTask(int mnID);
+    int ReqTask(std::string & strInputName,std::string & strOutputName,std::string & strcmd,std::shared_ptr<char> & xdata_Input,int & nInputSize,int64_t &nID); //if 1 a task . if 0 no task
+    void SetTaskRes(int nID,int ncvtRes,std::shared_ptr<char> xdata_Output,int nOutputSize);
+    int GetTaskRes(int nID,int & ncvtRes,std::shared_ptr<char> & xdata_Output, int & nOutputSize);
+    bool InqureTaskComplete(int nID);
+
+
+private:
+    std::vector<iv::TaskUnit> mvecterTask;
+    std::mutex mMutex;
+    int64_t mnIDNow = 0;
+};
+
+#endif // WORKTASK_H

src/tool/map_lanetoxodr/function/cdaproc.cpp

@@ -232,7 +232,7 @@ int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr, iv::map::cdadraw * pcdadraw
     finlinex[2] = flinelen+finsectlen;finliney[2] = 0;finlinehdg[2] = M_PI;
     finlinex[3] = fRoadLen*0.5;finliney[3] = finsectlen*0.5;finlinehdg[3] = 3.0*M_PI/2.0;
 
-    for(i=0;i<4;i++)
+    for(i=0;i<2;i++)
     {
         char strroadid[100];
         nroadid++;