change ivmapmake_sharemem, lower cpu load. change tool_trace2vectormap, add speedmode to vel.

src/ros/catkin/README.md

@@ -43,6 +43,8 @@ ros消息转换到我们系统
 
 如果需要编译单独一个工程，用colcon build --packages-select YOUR_PKG_NAME 
 
+编译使用colcon build，如果编译有问题可以先使用source install/setup.bash,在colcon build.
+
 colcon编译后如果依赖qt需要进行修改，依据当前qt库的位置。
  patchelf --set-rpath "/opt/qt/5.13.2/gcc_64/lib:/opt/ros/melodic/lib:" controllertocan 
 

src/ros/catkin/src/controllertocan/CMakeLists.txt

@@ -35,6 +35,7 @@ find_package(catkin REQUIRED COMPONENTS
   pluginlib
   sensor_msgs
   adc_system_msg
+  autoware_msgs
 )
 find_package(Boost REQUIRED)
 find_package(Protobuf REQUIRED)

src/tool/ivmapmake_sharemem/gps_collect.cpp

@@ -2,6 +2,7 @@
 #include <iomanip>
 #include<QDebug>
 #include <iomanip>
+#include <thread>
 // General constants.
 #define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
 #define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
@@ -404,7 +405,7 @@ void GPSSensor::processSensor()
             }
 //            char *buf = new char[100];
 //            memset(buf,0,100);
-            std::cout<<"run "<<std::endl;
+//            std::cout<<"run "<<std::endl;
 
             if (data->ins_heading_angle < 0.0)
             {
@@ -421,11 +422,15 @@ void GPSSensor::processSensor()
             if (x > (jianju*jianju))
             {
                 fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<std::endl;
-
+                std::cout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<std::endl;
                 gps_index++;
                 ServiceCarStatus.location->gps_x = data->gps_x;
                 ServiceCarStatus.location->gps_y = data->gps_y;
             }
+            else
+            {
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            }
         }
         fout.close();                  //关闭文件
     }
@@ -795,11 +800,15 @@ GPS_Collect::GPS_Collect(QWidget *parent):
 
 
     timer = new QTimer(this);
-    connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot1()));
+
     connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot2()));
     connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot3()));
     timer->start(20);
 
+    QTimer * timer2 = new QTimer(this);
+    connect(timer2, SIGNAL(timeout()), this, SLOT(timeoutslot1()));
+    timer2->start(100);
+
     myview = new MyView(this);
     myview->setObjectName(QStringLiteral("graphicsView"));
     myview->setGeometry(QRect(820, 10,500, 585));
@@ -927,12 +936,22 @@ void GPS_Collect::timeoutslot3(){
     xxx=ServiceCarStatus.location->gps_x;
     yyy=ServiceCarStatus.location->gps_y;
     if((xxx==0)&&(yyy==0)){return;};
+
+    if(pointf.size()>0)
+    {
+        double dis = sqrt(pow(xxx - pointf[pointf.size()-1].x(),2)+pow(yyy - pointf[pointf.size()-1].y(),2));
+        if(dis<0.05)
+        {
+            return;
+        }
+    }
     //qDebug() << xxx;
     //qDebug()<< ServiceCarStatus.location->gps_x;
     pointf_t.setX(xxx);
     pointf_t.setY(yyy);
     // mutex1.lock();
     pointf.push_back(pointf_t);
+//    std::cout<<"printf size "<<pointf.size()<<std::endl;
     // mutex1.unlock();
    // qDebug() << pointf_t.x();
 

src/tool/tool_trace2vectormap/dialogsetvel.cpp

@@ -0,0 +1,69 @@
+#include "dialogsetvel.h"
+#include "ui_dialogsetvel.h"
+
+#include <iostream>
+
+DialogSetVel::DialogSetVel(std::map<int,double> * pmapmodevel,QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogSetVel)
+{
+    ui->setupUi(this);
+    mpmapmodevel = pmapmodevel;
+    UpdateCB();
+
+    setWindowTitle("Set SpeedMode Velocity");
+}
+
+DialogSetVel::~DialogSetVel()
+{
+    delete ui;
+}
+
+void DialogSetVel::UpdateCB()
+{
+    ui->comboBox->clear();
+    for(auto& x:*mpmapmodevel)
+    {
+        char strout[100];
+        snprintf(strout,100,"%d",x.first);
+        ui->comboBox->addItem(strout);
+    }
+}
+
+void DialogSetVel::on_comboBox_currentIndexChanged(int index)
+{
+    int key = ui->comboBox->currentText().toInt();
+    std::map<int,double>::iterator it = mpmapmodevel->find(key);
+    if(it == mpmapmodevel->end())
+    {
+        std::cout<<" not found this item."<<std::endl;
+        return;
+    }
+    (void)index;
+    ui->lineEdit_SpeedMode->setText(QString::number(it->first));
+    ui->lineEdit_Vel->setText(QString::number(it->second));
+    
+}
+
+void DialogSetVel::on_pushButton_Add_clicked()
+{
+    int key =ui->lineEdit_SpeedMode->text().toInt();
+    double value = ui->lineEdit_Vel->text().toDouble();
+    (*mpmapmodevel)[key] = value;
+    UpdateCB();
+}
+
+void DialogSetVel::on_pushButton_Del_clicked()
+{
+    int key =ui->lineEdit_SpeedMode->text().toInt();
+    mpmapmodevel->erase(key);
+    UpdateCB();
+}
+
+void DialogSetVel::on_pushButton_Change_clicked()
+{
+    int key =ui->lineEdit_SpeedMode->text().toInt();
+    double value = ui->lineEdit_Vel->text().toDouble();
+    (*mpmapmodevel)[key] = value;
+    UpdateCB();
+}

src/tool/tool_trace2vectormap/dialogsetvel.h

@@ -0,0 +1,38 @@
+#ifndef DIALOGSETVEL_H
+#define DIALOGSETVEL_H
+
+#include <QDialog>
+
+#include <map>
+
+namespace Ui {
+class DialogSetVel;
+}
+
+class DialogSetVel : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogSetVel(std::map<int,double> * pmapmodevel, QWidget *parent = nullptr);
+    ~DialogSetVel();
+
+private slots:
+    void on_comboBox_currentIndexChanged(int index);
+    
+    void on_pushButton_Add_clicked();
+
+    void on_pushButton_Del_clicked();
+
+    void on_pushButton_Change_clicked();
+
+private:
+    Ui::DialogSetVel *ui;
+
+private:
+    std::map<int,double> * mpmapmodevel;
+private:
+    void UpdateCB();
+};
+
+#endif // DIALOGSETVEL_H

src/tool/tool_trace2vectormap/dialogsetvel.ui

@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogSetVel</class>
+ <widget class="QDialog" name="DialogSetVel">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>586</width>
+    <height>400</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QComboBox" name="comboBox">
+   <property name="geometry">
+    <rect>
+     <x>70</x>
+     <y>40</y>
+     <width>181</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_SpeedMode">
+   <property name="geometry">
+    <rect>
+     <x>154</x>
+     <y>120</y>
+     <width>121</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_Vel">
+   <property name="geometry">
+    <rect>
+     <x>420</x>
+     <y>120</y>
+     <width>121</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>120</y>
+     <width>111</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Speed Mode</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>320</x>
+     <y>120</y>
+     <width>111</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Vel(km/h)</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_Add">
+   <property name="geometry">
+    <rect>
+     <x>60</x>
+     <y>220</y>
+     <width>121</width>
+     <height>51</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Add</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_Del">
+   <property name="geometry">
+    <rect>
+     <x>236</x>
+     <y>220</y>
+     <width>121</width>
+     <height>51</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Del</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_Change">
+   <property name="geometry">
+    <rect>
+     <x>405</x>
+     <y>220</y>
+     <width>121</width>
+     <height>51</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Change</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/tool_trace2vectormap/mainwindow.cpp

@@ -3,6 +3,7 @@
 
 #include <QMessageBox>
 #include <QFileDialog>
+#include <iostream>
 
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
@@ -25,6 +26,32 @@ MainWindow::MainWindow(QWidget *parent)
     ui->comboBox_RIghtColor->addItem("White");
     ui->comboBox_RIghtColor->addItem("Yellow");
     ui->comboBox_RIghtColor->setCurrentIndex(0);
+
+    mmapmodevel[0] = 30.0;
+    mmapmodevel[1] = 30.0;
+    mmapmodevel[4] = 15.0;
+    mmapmodevel[5] = 15.0;
+    mmapmodevel[14] = 15.0;
+    mmapmodevel[15] = 15.0;
+    mmapmodevel[16] = 10.0;
+    mmapmodevel[17] = 10.0;
+    mmapmodevel[18] = 5.0;
+    mmapmodevel[11] = 60.0;
+//    if(mmapmodevel.find(9) == mmapmodevel.end())
+//    {
+//        std::cout<<" not have 9"<<std::endl;
+//    }
+//    std::map<int,double>::iterator it;
+//    it = mmapmodevel.find(11);
+//    if(mmapmodevel.find(11) == mmapmodevel.end())
+//    {
+//        std::cout<<" not have 11"<<std::endl;
+//    }
+//    else
+//    {
+//        std::cout<<mmapmodevel.find(11)->second<<std::endl;
+//    }
+
 }
 
 MainWindow::~MainWindow()
@@ -53,6 +80,7 @@ void MainWindow::on_pushButton_Convert_clicked()
     xparam.fDisRight = ui->lineEdit_RIghtDis->text().toDouble();
     xparam.fSpan = ui->lineEdit_Span->text().toDouble();
     xparam.fVelLim = ui->lineEdit_VelLim->text().toDouble()/3.6;
+    xparam.mapmodevel = mmapmodevel;
     if(ui->comboBox_LeftColor->currentText() == "White")
     {
         xparam.strColorLeft = "W";
@@ -85,3 +113,9 @@ void MainWindow::on_pushButton_Convert_clicked()
         QMessageBox::warning(this,"Warning",strinfo);
     }
 }
+
+void MainWindow::on_pushButton_SetVel_clicked()
+{
+    DialogSetVel dlgSetVel(&mmapmodevel);
+    dlgSetVel.exec();
+}

src/tool/tool_trace2vectormap/mainwindow.h

@@ -4,6 +4,7 @@
 #include <QMainWindow>
 
 #include "trace2vectormap.h"
+#include "dialogsetvel.h"
 
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
@@ -20,7 +21,13 @@ public:
 private slots:
     void on_pushButton_Convert_clicked();
 
+    void on_pushButton_SetVel_clicked();
+
 private:
     Ui::MainWindow *ui;
+
+    std::map<int,double> mmapmodevel;
+
+
 };
 #endif // MAINWINDOW_H

src/tool/tool_trace2vectormap/mainwindow.ui

@@ -263,7 +263,7 @@
    <widget class="QLineEdit" name="lineEdit_VelLim">
     <property name="geometry">
      <rect>
-      <x>169</x>
+      <x>240</x>
       <y>380</y>
       <width>121</width>
       <height>41</height>
@@ -278,12 +278,25 @@
      <rect>
       <x>30</x>
       <y>380</y>
-      <width>121</width>
+      <width>171</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Default LimVel(km/h)</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="pushButton_SetVel">
+    <property name="geometry">
+     <rect>
+      <x>430</x>
+      <y>381</y>
+      <width>221</width>
       <height>41</height>
      </rect>
     </property>
     <property name="text">
-     <string>LimVel(km/h)</string>
+     <string>Setting LimVel</string>
     </property>
    </widget>
   </widget>

src/tool/tool_trace2vectormap/tool_trace2vectormap.pro

@@ -17,16 +17,19 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
     ../../common/common/math/gnss_coordinate_convert.cpp \
+    dialogsetvel.cpp \
     main.cpp \
     mainwindow.cpp \
     trace2vectormap.cpp
 
 HEADERS += \
     ../../common/common/math/gnss_coordinate_convert.h \
+    dialogsetvel.h \
     mainwindow.h \
     trace2vectormap.h
 
 FORMS += \
+    dialogsetvel.ui \
     mainwindow.ui
 
 # Default rules for deployment.

src/tool/tool_trace2vectormap/trace2vectormap.cpp

@@ -55,6 +55,13 @@ int trace2vectormap::convert()
             double flon = QString(badata[1]).toDouble();
             double flat = QString(badata[2]).toDouble();
             double fheading = QString(badata[5]).toDouble();
+            int nspeedmode = QString(badata[7]).toInt();
+            double fspeed = xparam.fVelLim;
+            std::map<int,double>::iterator it = xparam.mapmodevel.find(nspeedmode);
+            if(it !=xparam.mapmodevel.end())
+            {
+                fspeed = it->second;
+            }
 //            std::cout<<"lat: "<<flat<<" lon:"<<flon<<" heading:"<<fheading<<std::endl;
             double x,y,fyaw;
             llh2xyy(flon,flat,fheading,x,y,fyaw,xparam.fLon0,xparam.fLat0);
@@ -64,6 +71,7 @@ int trace2vectormap::convert()
             xpoint.ly = x;
             xpoint.fyaw = fyaw;
             xpoint.h = 0;
+            xpoint.fspeed = fspeed;
             npid++;
             xvectorpoint.push_back(xpoint);
         }
@@ -75,6 +83,7 @@ int trace2vectormap::convert()
         iv::vectormap::node xnode;
         xnode.nid = i;
         xnode.pid = xvectorpoint[i].pid;
+        xnode.fspeed = xvectorpoint[i].fspeed;
         xvectornode.push_back(xnode);
     }
 
@@ -91,6 +100,7 @@ int trace2vectormap::convert()
         xlane.lno = xparam.Lno;
         xlane.blid = 0;
         xlane.flid = 0;
+        xlane.fspeed = xvectornode[i-1].fspeed;
         xvectorlane.push_back(xlane);
         iv::vectormap::dtlane xdtlane;
         xdtlane.did = i;
@@ -267,13 +277,14 @@ int trace2vectormap::convert()
 
     for(i=0;i<(int)xvectorlane.size();i++)
     {
+//        std::map<int,double>::iterator it = xparam.mapmodevel.find()
         snprintf(strline,1000,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%d,%d\n",
                  xvectorlane[i].lnid,xvectorlane[i].did,xvectorlane[i].blid,
                  xvectorlane[i].flid,xvectorlane[i].bnid,xvectorlane[i].fnid,
                  0,0,0,0,
                  0,0,0,0,
                  1,xvectorlane[i].lcnt,xvectorlane[i].lno,0,
-                 xparam.fVelLim,xparam.fVelLim,0,0);
+                 xvectorlane[i].fspeed/3.6,xvectorlane[i].fspeed/3.6,0,0);
         xFileLane.write(strline);
     }
     xFileLane.close();
@@ -301,3 +312,5 @@ int trace2vectormap::convert()
 
     return 0;
 }
+
+

src/tool/tool_trace2vectormap/trace2vectormap.h

@@ -2,6 +2,8 @@
 #define TRACE2VECTORMAP_H
 
 #include <string>
+#include <map>
+#include <vector>
 
 namespace iv {
 struct trace2vectormap_param
@@ -18,6 +20,7 @@ struct trace2vectormap_param
     std::string strColorLeft;
     std::string strColorRight;
     double fVelLim;
+    std::map<int,double> mapmodevel;
 };
 
 namespace vectormap {
@@ -25,6 +28,7 @@ struct node
 {
     int nid;
     int pid;
+    double fspeed;
 };
 
 struct point
@@ -34,6 +38,7 @@ struct point
     double bx;
     double ly;
     double fyaw;
+    double fspeed;
 };
 
 struct line
@@ -64,6 +69,7 @@ struct lane
     double span;
     int lcnt;
     int lno;
+    double fspeed;
 };
 
 struct dtlane
@@ -93,6 +99,7 @@ public:
 private:
     iv::trace2vectormap_param mparam;
 
+
 public:
     int convert();
     void llh2xyy(double flon,double flat,double fhead,double & x,double & y,double & fyaw,double flon0,double flat0);