change src/decition/decition_brake. add interpolation_2d. and add test/testinterpolation2d for test inter.

src/decition/decition_brain/decition/adc_adapter/base_adapter.cpp

@@ -6,11 +6,62 @@
 #include <decition/adc_adapter/base_adapter.h>
 
 
-
+#include <QFile>
 
 iv::decition::BaseAdapter::BaseAdapter(){
 
+    std::vector<std::tuple<double, double, double>> xvectortable_torque,xvectortable_brake;
+    QFile xFile;
+    xFile.setFileName("velacctable.txt");
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        QByteArray ba = xFile.readAll();
+        QString strba;
+        strba.append(ba);
+        QStringList strlinelist =strba.split("\n");// strba.split(QRegExp("[\t ;]+"));
+        int nline = strlinelist.size();
+        int i;
+        for(i=0;i<nline;i++)
+        {
+            QString str = strlinelist.at(i);
+            QStringList strvaluelist = str.split(QRegExp("[\t ;]+"));
+            if(strvaluelist.size()>=4)
+            {
+                double vel,acc,torque,brake;
+                vel = QString(strvaluelist.at(0)).toDouble();
+                acc = QString(strvaluelist.at(1)).toDouble();
+                torque = QString(strvaluelist.at(2)).toDouble();
+                brake = QString(strvaluelist.at(3)).toDouble();
+                xvectortable_torque.push_back(std::make_tuple(vel,acc,torque));
+                xvectortable_brake.push_back(std::make_tuple(vel,acc,brake));
+            }
+        }
+
+    }
+    xFile.close();
+
+    if(xvectortable_torque.size()>=4)
+    {
+        mInterpolation2D_torque.Init(xvectortable_torque);
+        mInterpolation2D_brake.Init(xvectortable_brake);
+        mbInterpolation2DOK = true;
+    }
 }
+
+
+int iv::decition::BaseAdapter::GetTorqueBrake(double fVeh, double fAcc, double &fTorque, double &fBrake)
+{
+    if(mbInterpolation2DOK == false)return -1;
+    fTorque = mInterpolation2D_torque.Interpolate(std::make_pair(fVeh,fAcc));
+    fBrake = mInterpolation2D_brake.Interpolate(std::make_pair(fVeh,fAcc));
+    return 0;
+}
+
+bool iv::decition::BaseAdapter::IsINterpolationOK()
+{
+    return mbInterpolation2DOK;
+}
+
 iv::decition::BaseAdapter::~BaseAdapter(){
 
 }

src/decition/decition_brain/decition/adc_adapter/base_adapter.h

@@ -8,6 +8,7 @@
 #include <common/obstacle_type.h>
 #include<vector>
 #include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/interpolation_2d.h>
 
 namespace iv {
 namespace decition {
@@ -23,11 +24,19 @@ namespace decition {
 
 
 
+
+
         virtual  iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
                                                           float  obsSpeed,float accAim,float accNow, bool changingDangWei,Decition *decition);
 
 
         private:
+            apollo::control::Interpolation2D mInterpolation2D_torque,mInterpolation2D_brake;
+            bool mbInterpolation2DOK = false;
+
+        public:
+            int GetTorqueBrake(double fVeh,double fAcc,double & fTorque, double & fBrake);
+            bool IsINterpolationOK();
 
 
         };

src/decition/decition_brain/decition/adc_adapter/ge3_adapter.cpp

@@ -91,7 +91,14 @@ iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_g
 
 
 
+    if(IsINterpolationOK())
+    {
+        double ftorque,fbrake;
+        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
+        (*decition)->brake = fbrake;
 
+        (*decition)->torque= ftorque;
+    }
 
 
     lastTorque=(*decition)->torque;

src/decition/decition_brain/decition/decition.pri

@@ -1,4 +1,5 @@
 HEADERS += \
+    $$PWD/adc_adapter/interpolation_2d.h \
     $$PWD/adc_adapter/yuhesen_adapter.h  \
     $$PWD/decition_type.h \
     $$PWD/decition_maker.h \
@@ -28,6 +29,7 @@ HEADERS += \
     $$PWD/adc_adapter/hapo_adapter.h
 
 SOURCES += \
+    $$PWD/adc_adapter/interpolation_2d.cc \
     $$PWD/adc_adapter/yuhesen_adapter.cpp \
     $$PWD/decide_gps_00.cpp \
     $$PWD/brain.cpp \

src/test/testinperplation2d/.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/testinperplation2d/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/test/testinperplation2d/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/test/testinperplation2d/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+    return a.exec();
+}

src/test/testinperplation2d/mainwindow.cpp

@@ -0,0 +1,91 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+#include <QFile>
+
+MainWindow::MainWindow(QWidget *parent)
+    : QMainWindow(parent)
+    , ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    ui->horizontalSlider_vel->setRange(0,1000);
+    ui->horizontalSlider_acc->setRange(0,1000);
+    ui->horizontalSlider_acc->setValue(500);
+    ui->horizontalSlider_vel->setValue(0);
+
+    std::vector<std::tuple<double, double, double>> xvectortable_torque,xvectortable_brake;
+    QFile xFile;
+    xFile.setFileName("velacctable.txt");
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        QByteArray ba = xFile.readAll();
+        QString strba;
+        strba.append(ba);
+        QStringList strlinelist =strba.split("\n");// strba.split(QRegExp("[\t ;]+"));
+        int nline = strlinelist.size();
+        int i;
+        for(i=0;i<nline;i++)
+        {
+            QString str = strlinelist.at(i);
+            QStringList strvaluelist = str.split(QRegExp("[\t ;]+"));
+            if(strvaluelist.size()>=4)
+            {
+                double vel,acc,torque,brake;
+                vel = QString(strvaluelist.at(0)).toDouble();
+                acc = QString(strvaluelist.at(1)).toDouble();
+                torque = QString(strvaluelist.at(2)).toDouble();
+                brake = QString(strvaluelist.at(3)).toDouble();
+                xvectortable_torque.push_back(std::make_tuple(vel,acc,torque));
+                xvectortable_brake.push_back(std::make_tuple(vel,acc,brake));
+            }
+        }
+
+    }
+    xFile.close();
+
+    if(xvectortable_torque.size()>0)
+    {
+        mInterpolation2D_torque.Init(xvectortable_torque);
+        mInterpolation2D_brake.Init(xvectortable_brake);
+        mbInterpolation2DOK = true;
+    }
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+
+void MainWindow::on_horizontalSlider_vel_valueChanged(int value)
+{
+    double fvalue = value;
+    fvalue = fvalue * 0.1;
+    ui->lineEdit_vel->setText(QString::number(fvalue));
+    updateinterp();
+}
+
+void MainWindow::on_horizontalSlider_acc_valueChanged(int value)
+{
+    double fvalue = value;
+    fvalue = (fvalue - 500)/50.0;
+    ui->lineEdit_acc->setText(QString::number(fvalue));
+    updateinterp();
+}
+
+void MainWindow::updateinterp()
+{
+    double fvel = ui->horizontalSlider_vel->value();
+    fvel = fvel*0.1;
+    double facc = ui->horizontalSlider_acc->value();
+    facc = (facc -500)/50.0;
+    double ftorque,fbrake;
+    if(mbInterpolation2DOK)
+    {
+        ftorque = mInterpolation2D_torque.Interpolate(std::make_pair(fvel,facc));
+        fbrake = mInterpolation2D_brake.Interpolate(std::make_pair(fvel,facc));
+        ui->lineEdit_torque->setText(QString::number(ftorque,'f',3));
+        ui->lineEdit_brake->setText(QString::number(fbrake,'f',3));
+    }
+}

src/test/testinperplation2d/mainwindow.h

@@ -0,0 +1,32 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include "interpolation_2d.h"
+
+QT_BEGIN_NAMESPACE
+namespace Ui { class MainWindow; }
+QT_END_NAMESPACE
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    MainWindow(QWidget *parent = nullptr);
+    ~MainWindow();
+
+private slots:
+    void on_horizontalSlider_vel_valueChanged(int value);
+
+    void on_horizontalSlider_acc_valueChanged(int value);
+
+private:
+    Ui::MainWindow *ui;
+
+    apollo::control::Interpolation2D mInterpolation2D_torque,mInterpolation2D_brake;
+    bool mbInterpolation2DOK = false;
+
+    void updateinterp();
+};
+#endif // MAINWINDOW_H

src/test/testinperplation2d/mainwindow.ui

@@ -0,0 +1,150 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>923</width>
+    <height>602</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <widget class="QLabel" name="label">
+    <property name="geometry">
+     <rect>
+      <x>70</x>
+      <y>40</y>
+      <width>141</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Vel</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_2">
+    <property name="geometry">
+     <rect>
+      <x>70</x>
+      <y>110</y>
+      <width>151</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Acc</string>
+    </property>
+   </widget>
+   <widget class="QSlider" name="horizontalSlider_vel">
+    <property name="geometry">
+     <rect>
+      <x>180</x>
+      <y>50</y>
+      <width>501</width>
+      <height>16</height>
+     </rect>
+    </property>
+    <property name="orientation">
+     <enum>Qt::Horizontal</enum>
+    </property>
+   </widget>
+   <widget class="QSlider" name="horizontalSlider_acc">
+    <property name="geometry">
+     <rect>
+      <x>180</x>
+      <y>120</y>
+      <width>501</width>
+      <height>16</height>
+     </rect>
+    </property>
+    <property name="orientation">
+     <enum>Qt::Horizontal</enum>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_3">
+    <property name="geometry">
+     <rect>
+      <x>180</x>
+      <y>210</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Torque</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_4">
+    <property name="geometry">
+     <rect>
+      <x>177</x>
+      <y>280</y>
+      <width>67</width>
+      <height>17</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Brake</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_torque">
+    <property name="geometry">
+     <rect>
+      <x>300</x>
+      <y>200</y>
+      <width>191</width>
+      <height>41</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_brake">
+    <property name="geometry">
+     <rect>
+      <x>300</x>
+      <y>270</y>
+      <width>191</width>
+      <height>41</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_vel">
+    <property name="geometry">
+     <rect>
+      <x>720</x>
+      <y>40</y>
+      <width>141</width>
+      <height>41</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_acc">
+    <property name="geometry">
+     <rect>
+      <x>720</x>
+      <y>110</y>
+      <width>141</width>
+      <height>41</height>
+     </rect>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>923</width>
+     <height>28</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/test/testinperplation2d/testinperplation2d.pro

@@ -0,0 +1,33 @@
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+CONFIG += c++11
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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 it uses 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 += \
+    interpolation_2d.cc \
+    main.cpp \
+    mainwindow.cpp
+
+HEADERS += \
+    interpolation_2d.h \
+    mainwindow.h
+
+FORMS += \
+    mainwindow.ui
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target

src/test/testinperplation2d/velacctable.txt

@@ -0,0 +1,27 @@
+0.0	0.0	0.0	0.0
+0.0	-1.0	0.0	30.0
+0.0	-10.0	0.0	30.0
+0.0	1.0	50	0.0
+0.0	2.0	100	0.0
+0.0	0.1	50	0.0
+0.0	10.0	1000	0.0
+5.0	-10.0	0.0	100.0
+5.0	-5.0	0.0	80.0
+5.0	-3.0	0.0	50.0
+5.0	-1.0	0.0	30.0
+5.0	-0.5	0.0	10.0
+5.0	-0.1	0.0	0.0
+5.0	-0.05	30.0	0.0
+5.0	0.0	35.0	0.0
+5.0	1.0	50.0	0.0
+5.0	3.0	300.0	0.0
+5.0	5.0	1500.0	0.0
+10.0	-10.0	0.0	100.0
+10.0	-5.0	0.0	80.0
+10.0	-1.0	0.0	5.0
+10.0	-0.5	0.0	0.0
+10.0	-0.3	30.0	0.0
+10.0	-0.1	31.0	0.0
+10.0	0.0	36.0	0.0
+10.0	1.0	100.0	0.0
+10.0	5.0	1500.0	0.0