add decition_fromautoware for transfer autoware decition to pilot.

src/decition/decition_fromautoware/.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/decition/decition_fromautoware/ctrlcmd2decition.cpp

@@ -0,0 +1,82 @@
+#include "ctrlcmd2decition.h"
+
+#include <iostream>
+#include <math.h>
+
+ctrlcmd2decition::ctrlcmd2decition()
+{
+    mpLT = new LookupTable();
+
+    ModuleFun xFunctrlcmd = std::bind(&ctrlcmd2decition::ListenCtrlCmd,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpactrlcmd = iv::modulecomm::RegisterRecvPlus("ctrlcmd",xFunctrlcmd);
+    mpadecision = iv::modulecomm::RegisterSend("deciton",1000,1);
+
+}
+
+ctrlcmd2decition::~ctrlcmd2decition()
+{
+    iv::modulecomm::Unregister(mpadecision);
+    iv::modulecomm::Unregister(mpactrlcmd);
+    delete mpLT;
+}
+
+void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::autoware::autowarectrlcmd xctrlcmd;
+    if(!xctrlcmd.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Parse Fail"<<std::endl;
+        return;
+    }
+
+    iv::brain::decition xdecition;
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = 0;
+    double fwheelangle = 0;
+    if(mpLT->IsINterpolationOK())
+    {
+        mpLT->GetTorqueBrake(xctrlcmd.linear_velocity()*3.6,xctrlcmd.linear_acceleration(),ftorque,fbrake);
+    }
+    facc = xctrlcmd.linear_acceleration();
+    fwheelangle = xctrlcmd.steering_angle() * (180.0/M_PI) * 15.0;
+    xdecition.set_accelerator(facc);
+    xdecition.set_brake(fbrake);
+    xdecition.set_leftlamp(false);
+    xdecition.set_rightlamp(false);
+    xdecition.set_speed(xctrlcmd.linear_velocity() * 3.6);
+    xdecition.set_wheelangle(fwheelangle);
+    xdecition.set_wheelspeed(300);
+    xdecition.set_torque(ftorque);
+    xdecition.set_mode(0);
+    xdecition.set_gear(0);
+    xdecition.set_handbrake(0);
+    xdecition.set_grade(0);
+    xdecition.set_engine(0);
+    xdecition.set_brakelamp(false);
+    xdecition.set_ttc(15);
+//    xdecition.set_air_enable(decition->air_enable);
+//    xdecition.set_air_temp(decition->air_temp);
+//    xdecition.set_air_mode(decition->air_mode);
+//    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(0);
+    xdecition.set_home_light(0);
+//    xdecition.set_air_worktime(decition->air_worktime);
+//    xdecition.set_air_offtime(decition->air_offtime);
+//    xdecition.set_air_on(decition->air_on);
+    xdecition.set_door(0);
+    xdecition.set_dippedlight(0);
+
+    int nbytesize = (int)xdecition.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(!xdecition.SerializeToArray(pstr_ptr.get(),nbytesize))
+    {
+        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Serialize Fail. "<<std::endl;
+        return;
+    }
+    iv::modulecomm::ModuleSendMsg(mpadecision,pstr_ptr.get(),nbytesize);
+}

src/decition/decition_fromautoware/ctrlcmd2decition.h

@@ -0,0 +1,29 @@
+#ifndef CTRLCMD2DECITION_H
+#define CTRLCMD2DECITION_H
+
+#include "modulecomm.h"
+
+#include "decition.pb.h"
+#include "autowarectrlcmd.pb.h"
+
+#include "lookuptable.h"
+
+class ctrlcmd2decition
+{
+public:
+    ctrlcmd2decition();
+    ~ctrlcmd2decition();
+
+private:
+    void ListenCtrlCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+
+private:
+    void * mpadecision;
+    void * mpactrlcmd;
+
+private:
+    LookupTable * mpLT;
+};
+
+#endif // CTRLCMD2DECITION_H

src/decition/decition_fromautoware/decition_fromautoware.pro

@@ -0,0 +1,43 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# 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 += \
+        ../../include/msgtype/autowarectrlcmd.pb.cc \
+        ../../include/msgtype/decition.pb.cc \
+        ctrlcmd2decition.cpp \
+        interpolation_2d.cc \
+        lookuptable.cpp \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+HEADERS += \
+    ../../include/msgtype/autowarectrlcmd.pb.h \
+    ../../include/msgtype/decition.pb.h \
+    ctrlcmd2decition.h \
+    interpolation_2d.h \
+    lookuptable.h

src/decition/decition_fromautoware/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_fromautoware/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_fromautoware/lookuptable.cpp

@@ -0,0 +1,55 @@
+#include "lookuptable.h"
+
+LookupTable::LookupTable()
+{
+    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;
+    }
+}
+
+int LookupTable::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 LookupTable::IsINterpolationOK()
+{
+    return mbInterpolation2DOK;
+}

src/decition/decition_fromautoware/lookuptable.h

@@ -0,0 +1,21 @@
+#ifndef LOOKUPTABLE_H
+#define LOOKUPTABLE_H
+
+#include <QFile>
+
+#include "interpolation_2d.h"
+
+class LookupTable
+{
+public:
+    LookupTable();
+private:
+    apollo::control::Interpolation2D mInterpolation2D_torque,mInterpolation2D_brake;
+    bool mbInterpolation2DOK = false;
+
+public:
+    int GetTorqueBrake(double fVeh,double fAcc,double & fTorque, double & fBrake);
+    bool IsINterpolationOK();
+};
+
+#endif // LOOKUPTABLE_H

src/decition/decition_fromautoware/main.cpp

@@ -0,0 +1,9 @@
+#include <QCoreApplication>
+#include "ctrlcmd2decition.h"
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    ctrlcmd2decition xc2d;
+    (void)xc2d;
+    return a.exec();
+}

src/include/proto/autowarectrlcmd.proto

@@ -0,0 +1,11 @@
+syntax = "proto2";
+
+package iv.autoware;
+
+message autowarectrlcmd
+{
+  required double linear_velocity = 1;
+  required double linear_acceleration =2 ;//#m/s^2
+  required double steering_angle = 3;
+ 
+};

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -132,6 +132,7 @@ include_directories(
 
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc   ./../../../../include/msgtype/odom.pb.cc  ./../../../../include/msgtype/decition.pb.cc )
+add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc   ./../../../../include/msgtype/odom.pb.cc  ./../../../../include/msgtype/decition.pb.cc
+./../../../../include/msgtype/autowarectrlcmd.pb.cc  )
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}  Qt5Core Qt5Xml modulecomm)
 

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -19,6 +19,7 @@
 #include <nav_msgs/Odometry.h>
 
 #include  "autoware_msgs/VehicleCmd.h"
+#include "autoware_msgs/ControlCommandStamped.h"
 #include "lgsvl_msgs/VehicleControlData.h"
 #include "lgsvl_msgs/VehicleStateData.h"
 
@@ -27,6 +28,7 @@
 #include "rawpic.pb.h"
 #include "odom.pb.h"
 #include "decition.pb.h"
+#include "autowarectrlcmd.pb.h"
 
 static std::string _point_topic = "/points_raw";
 static std::string _point_msgname = "lidarpc_ros";
@@ -39,14 +41,20 @@ static std::string _odom_msgname = "odom_ros";
 
 static std::string  _twistraw_topic = "/twist_raw";
 
+static std::string  _ctrlcmd_topic = "/ctrl_raw";
+static std::string  _ctrlcmd_msgname = "ctrlcmd";
+static bool _buse_autowarectrlcmd = true;
+
   ros::Subscriber points_sub;
   ros::Subscriber odom_sub;
 
   ros::Subscriber twist_sub;
+  ros::Subscriber ctrlcmd_sub;
 
   void * gpa_lidar;
   void * gpa_image;
   void * gpa_odom;
+  void * gpa_ctrlcmd;
 
 
 ros::Publisher  test_cmd_pub;
@@ -214,6 +222,25 @@ void  twistrawCalllback(const  geometry_msgs::TwistStampedConstPtr  & msg)
     xcmd.twist_cmd.twist = msg->twist;
     test_cmd_pub.publish(xcmd);
 }
+
+void ctrlrawCallback(const autoware_msgs::ControlCommandStampedConstPtr & msg)
+{
+    iv::autoware::autowarectrlcmd xcmd;
+    xcmd.set_linear_acceleration(msg->cmd.linear_acceleration);
+    xcmd.set_linear_velocity(msg->cmd.linear_velocity);
+    xcmd.set_steering_angle(msg->cmd.steering_angle);
+    int nbytesize = (int)xcmd.ByteSize();
+    std::shared_ptr<char>  str_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(xcmd.SerializeToArray(str_ptr.get(),nbytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpa_ctrlcmd,str_ptr.get(),nbytesize);
+    }
+    else
+    {
+        std::cout<<" ctrlrawCallback Serialize fail."<<std::endl;
+    }
+}
+
 void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 
@@ -304,6 +331,13 @@ int main(int argc, char *argv[])
     private_nh.getParam("autoware_twist_raw",_twistraw_topic);
     std::cout<<"  _twist_topic : "<<_twistraw_topic<<std::endl;
 
+    private_nh.getParam("_ctrlcmd_node",_ctrlcmd_topic);
+    private_nh.getParam("_ctrlcmd_msgname",_ctrlcmd_msgname);
+    private_nh.getParam("useautowrectrl",_buse_autowarectrlcmd);
+    std::cout<<"  ctrlcmd_topic: "<<_ctrlcmd_topic<<std::endl;
+    std::cout<<"  ctrlcmd_msgname: "<<_ctrlcmd_msgname<<std::endl;
+    std::cout<<"  buseautowrectrl: "<<_buse_autowarectrlcmd<<std::endl;
+
     test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
                 "/vehicle_cmd", 10);
     lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state",10);
@@ -312,6 +346,7 @@ int main(int argc, char *argv[])
 	gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
     gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(),20000000,1);
     gpa_odom = iv::modulecomm::RegisterSend(_odom_msgname.data(),10000,1);
+    gpa_ctrlcmd = iv::modulecomm::RegisterSend(_ctrlcmd_msgname.data(),1000,1);
 
   points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
 
@@ -321,6 +356,7 @@ int main(int argc, char *argv[])
     odom_sub = private_nh.subscribe(_odom_topic,1,odomCalllback);
 
 twist_sub = private_nh.subscribe(_twistraw_topic,1,twistrawCalllback);
+ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic,1,ctrlrawCallback);
 
    void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
   //   std::thread * pnew = new std::thread(testvh);