add table look-up pro

src/decition/decition_brain_sf/decition/adc_adapter/base_adapter.cpp

@@ -6,16 +6,67 @@
 #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);
+            str = str.trimmed();
+            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;
+    }
+
 }
-iv::decition::BaseAdapter::~BaseAdapter(){
 
+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(){
+
+}
 
  iv::decition::Decition  iv::decition::BaseAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  path , float dSpeed, float obsDistacne ,
                                                                       float  obsSpeed,float accAim,float accNow  , bool changingDangWei, Decition *decition){

src/decition/decition_brain_sf/decition/adc_adapter/base_adapter.h

@@ -9,6 +9,8 @@
 #include<vector>
 #include <decition/adc_tools/gnss_coordinate_convert.h>
 
+#include <decition/adc_adapter/interpolation_2d.h>
+
 namespace iv {
 namespace decition {
 
@@ -28,6 +30,12 @@ namespace 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_sf/decition/adc_adapter/hapo_adapter.cpp

@@ -199,6 +199,17 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     lastTorque=(*decition)->torque;
 
 
+
+
+
+    if(IsINterpolationOK())
+    {
+        double ftorque,fbrake;
+        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
+        (*decition)->brake = fbrake;
+
+        (*decition)->torque= ftorque;
+    }
 //    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
 //                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
 

src/decition/decition_brain_sf/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_sf/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/decition/decition_brain_sf/decition/decition.pri

@@ -25,7 +25,8 @@ HEADERS += \
     $$PWD/adc_adapter/bus_adapter.h \
     $$PWD/fanyaapi.h \
     $$PWD/adc_adapter/hapo_adapter.h \
-    $$PWD/adc_adapter/sightseeing_adapter.h
+    $$PWD/adc_adapter/sightseeing_adapter.h \
+    $$PWD/adc_adapter/interpolation_2d.h
 
 SOURCES += \
     $$PWD/decide_gps_00.cpp \
@@ -51,4 +52,5 @@ SOURCES += \
     $$PWD/adc_adapter/bus_adapter.cpp \
     $$PWD/fanyaapi.cpp \
     $$PWD/adc_adapter/hapo_adapter.cpp \
-    $$PWD/adc_adapter/sightseeing_adapter.cpp
+    $$PWD/adc_adapter/sightseeing_adapter.cpp \
+    $$PWD/adc_adapter/interpolation_2d.cc