change tool/map_lanetoxodr. solve a rtk fit error. because crosspoint calculate, computer precison, change ==0 to <=0.01

src/decition/decition_brain/decition/adc_adapter/interpolation_2d.cc

@@ -0,0 +1,120 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+#include "interpolation_2d.h"
+
+#include <cmath>
+#include <iostream>
+
+//#include "cyber/common/log.h"
+
+namespace {
+
+const double kDoubleEpsilon = 1.0e-6;
+
+}  // namespace
+
+namespace apollo {
+namespace control {
+
+bool Interpolation2D::Init(const DataType &xyz) {
+  if (xyz.empty()) {
+    std::cout << "empty input."<<std::endl;
+    return false;
+  }
+  for (const auto &t : xyz) {
+    xyz_[std::get<0>(t)][std::get<1>(t)] = std::get<2>(t);
+  }
+  return true;
+}
+
+double Interpolation2D::Interpolate(const KeyType &xy) const {
+  double max_x = xyz_.rbegin()->first;
+  double min_x = xyz_.begin()->first;
+  if (xy.first >= max_x - kDoubleEpsilon) {
+    return InterpolateYz(xyz_.rbegin()->second, xy.second);
+  }
+  if (xy.first <= min_x + kDoubleEpsilon) {
+    return InterpolateYz(xyz_.begin()->second, xy.second);
+  }
+
+  auto itr_after = xyz_.lower_bound(xy.first);
+  auto itr_before = itr_after;
+  if (itr_before != xyz_.begin()) {
+    --itr_before;
+  }
+
+  double x_before = itr_before->first;
+  double z_before = InterpolateYz(itr_before->second, xy.second);
+  double x_after = itr_after->first;
+  double z_after = InterpolateYz(itr_after->second, xy.second);
+
+  double x_diff_before = std::fabs(xy.first - x_before);
+  double x_diff_after = std::fabs(xy.first - x_after);
+
+  return InterpolateValue(z_before, x_diff_before, z_after, x_diff_after);
+}
+
+double Interpolation2D::InterpolateYz(const std::map<double, double> &yz_table,
+                                      double y) const {
+  if (yz_table.empty()) {
+    std::cout << "Unable to interpolateYz because yz_table is empty."<<std::endl;
+    return y;
+  }
+  double max_y = yz_table.rbegin()->first;
+  double min_y = yz_table.begin()->first;
+  if (y >= max_y - kDoubleEpsilon) {
+    return yz_table.rbegin()->second;
+  }
+  if (y <= min_y + kDoubleEpsilon) {
+    return yz_table.begin()->second;
+  }
+
+  auto itr_after = yz_table.lower_bound(y);
+  auto itr_before = itr_after;
+
+  if (itr_before != yz_table.begin()) {
+    --itr_before;
+  }
+
+  double y_before = itr_before->first;
+  double z_before = itr_before->second;
+  double y_after = itr_after->first;
+  double z_after = itr_after->second;
+
+  double y_diff_before = std::fabs(y - y_before);
+  double y_diff_after = std::fabs(y - y_after);
+
+  return InterpolateValue(z_before, y_diff_before, z_after, y_diff_after);
+}
+
+double Interpolation2D::InterpolateValue(const double value_before,
+                                         const double dist_before,
+                                         const double value_after,
+                                         const double dist_after) const {
+  if (dist_before < kDoubleEpsilon) {
+    return value_before;
+  }
+  if (dist_after < kDoubleEpsilon) {
+    return value_after;
+  }
+  double value_gap = value_after - value_before;
+  double value_buff = value_gap * dist_before / (dist_before + dist_after);
+  return value_before + value_buff;
+}
+
+}  // namespace control
+}  // namespace apollo

src/decition/decition_brain/decition/adc_adapter/interpolation_2d.h

@@ -0,0 +1,73 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+/**
+ * @namespace apollo::control
+ * @brief apollo::control
+ */
+namespace apollo {
+namespace control {
+/**
+ * @class Interpolation2D
+ *
+ * @brief linear interpolation from key (double, double) to one double value.
+ */
+class Interpolation2D {
+ public:
+  typedef std::vector<std::tuple<double, double, double>> DataType;
+  typedef std::pair<double, double> KeyType;
+
+  Interpolation2D() = default;
+
+  /**
+   * @brief initialize Interpolation2D internal table
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  bool Init(const DataType &xyz);
+
+  /**
+   * @brief linear interpolate from 2D key (double, double) to one double value.
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  double Interpolate(const KeyType &xy) const;
+
+ private:
+  double InterpolateYz(const std::map<double, double> &yz_table,
+                       double y) const;
+
+  double InterpolateValue(const double value_before, const double dist_before,
+                          const double value_after,
+                          const double dist_after) const;
+
+  std::map<double, std::map<double, double>> xyz_;
+};
+
+}  // namespace control
+}  // namespace apollo

src/tool/controller_torquebrake_get/controller_torquebrake_get.pro

@@ -16,11 +16,17 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/imu.pb.cc \
     ../../include/msgtype/torquebrake.pb.cc \
     main.cpp \
     mainwindow.cpp
 
 HEADERS += \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/imu.pb.h \
     ../../include/msgtype/torquebrake.pb.h \
     mainwindow.h
 

src/tool/controller_torquebrake_get/mainwindow.cpp

@@ -9,8 +9,12 @@ MainWindow::MainWindow(QWidget *parent)
 
     ui->lineEdit_Brake->setText("0.0");
     ui->lineEdit_Torque->setText("35.0");
+    ui->lineEdit_speedlimit->setText(QString::number(mfSpeedLimit));
+    ui->lineEdit_torqueatlimit->setText(QString::number(mfTorqueAtLimit));
 
     mpa = iv::modulecomm::RegisterSend("torquebrake",1000,1);
+    ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
 
     mpTimer = new QTimer();
     connect(mpTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
@@ -28,7 +32,14 @@ MainWindow::~MainWindow()
 void MainWindow::onTimer()
 {
     iv::controller::torquebrake xtb;
-    xtb.set_torque(mfTorque);
+    if((mfSpeedNow > mfSpeedLimit) && (mfBrake == 0.0))
+    {
+        xtb.set_torque(mfTorqueAtLimit);
+    }
+    else
+    {
+        xtb.set_torque(mfTorque);
+    }
     xtb.set_brake(mfBrake);
     xtb.set_enable(mbEnable);
 
@@ -39,6 +50,19 @@ void MainWindow::onTimer()
         iv::modulecomm::ModuleSendMsg(mpa,strbuf_ptr.get(),ndatasize);
     }
 
+    if(mbEnable)
+    {
+        if(mbGPSIMUUpdate)
+        {
+            iv::gps::gpsimu xgpsimu;
+            mMutexGPSIMU.lock();
+            xgpsimu.CopyFrom(mgpsimu);
+            mbGPSIMUUpdate = false;
+            mMutexGPSIMU.unlock();
+
+        }
+    }
+
 }
 
 
@@ -55,5 +79,45 @@ void MainWindow::on_checkBox_clicked()
     }
     mfTorque = ui->lineEdit_Torque->text().toDouble();
     mfBrake = ui->lineEdit_Brake->text().toDouble();
+    mfSpeedLimit = ui->lineEdit_speedlimit->text().toDouble();
+    mfTorqueAtLimit = ui->lineEdit_torqueatlimit->text().toDouble();
+
+}
+
+void MainWindow::UpdateGPSIMU(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)strdata;
+    (void)nSize;
+    (void)index;
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        qDebug("UpdateGPSIMU Parse Error.");
+        return;
+    }
+
+    mfSpeedNow = xgpsimu.speed();
+    mMutexGPSIMU.lock();
+    mgpsimu.CopyFrom(xgpsimu);
+    mbGPSIMUUpdate = true;
+    mMutexGPSIMU.unlock();
+}
+
+void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
+{
+    char strout[1000];
+    snprintf(strout,1000,"%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t",
+            xgpsimu.speed(),xgpsimu.acce_x(),mfTorque,mfBrake,
+            xgpsimu.acc_calc(),xgpsimu.acce_y(),xgpsimu.acce_z());
+    ui->plainTextEdit->appendPlainText(strout);
+}
+
+void MainWindow::on_pushButton_ClearRecord_clicked()
+{
+    ui->plainTextEdit->clear();
+}
+
+void MainWindow::on_pushButton_SaveRecord_clicked()
+{
 
 }

src/tool/controller_torquebrake_get/mainwindow.h

@@ -5,9 +5,12 @@
 #include <QTimer>
 
 #include "torquebrake.pb.h"
+#include "gpsimu.pb.h"
 #include "modulecomm.h"
 #include <memory>
 
+#include <QMutex>
+
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
 QT_END_NAMESPACE
@@ -25,6 +28,10 @@ private slots:
 
     void on_checkBox_clicked();
 
+    void on_pushButton_ClearRecord_clicked();
+
+    void on_pushButton_SaveRecord_clicked();
+
 private:
     Ui::MainWindow *ui;
 
@@ -33,7 +40,20 @@ private:
     double mfTorque = 35.0;
     double mfBrake = 0;
     bool mbEnable = false;
+    double mfSpeedLimit = 30;
+    double mfTorqueAtLimit = 30;
+
+    double mfSpeedNow = 100.0;
 
     void * mpa;
+    void * mpagpsimu;
+
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    QMutex mMutexGPSIMU;
+    iv::gps::gpsimu mgpsimu;
+    bool mbGPSIMUUpdate = false;
+
+    void UpdatePlainText(iv::gps::gpsimu & xgpsimu);
+
 };
 #endif // MAINWINDOW_H

src/tool/controller_torquebrake_get/mainwindow.ui

@@ -27,8 +27,8 @@
    <widget class="QCheckBox" name="checkBox">
     <property name="geometry">
      <rect>
-      <x>560</x>
-      <y>100</y>
+      <x>540</x>
+      <y>120</y>
       <width>211</width>
       <height>61</height>
      </rect>
@@ -109,6 +109,52 @@
      </rect>
     </property>
    </widget>
+   <widget class="QLabel" name="label_3">
+    <property name="geometry">
+     <rect>
+      <x>60</x>
+      <y>110</y>
+      <width>131</width>
+      <height>21</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>速度上限值(km/h)</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_speedlimit">
+    <property name="geometry">
+     <rect>
+      <x>200</x>
+      <y>100</y>
+      <width>131</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_4">
+    <property name="geometry">
+     <rect>
+      <x>60</x>
+      <y>151</y>
+      <width>111</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>超限发送扭矩</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_torqueatlimit">
+    <property name="geometry">
+     <rect>
+      <x>198</x>
+      <y>157</y>
+      <width>131</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">

src/tool/map_lanetoxodr/geofit.cpp

@@ -750,7 +750,7 @@ std::vector<geobase> geofit::getgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals
 
 int geofit::getcrosspoint(double A, double B, double C, double x, double y, double &xc, double &yc)
 {
-    if((A*x  + B* y +C) == 0)
+    if((A*x  + B* y +C) <0.01)
     {
         xc = x;
         yc = y;