add autowarewithpilot.launch.

src/decition/decition_fromautoware/ctrlcmd2decition.cpp

@@ -11,15 +11,37 @@ ctrlcmd2decition::ctrlcmd2decition()
     mpactrlcmd = iv::modulecomm::RegisterRecvPlus("ctrlcmd",xFunctrlcmd);
     mpadecision = iv::modulecomm::RegisterSend("deciton",1000,1);
 
+    ModuleFun xFunGPSIMU = std::bind(&ctrlcmd2decition::ListenGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",xFunGPSIMU);
+
 }
 
 ctrlcmd2decition::~ctrlcmd2decition()
 {
+    iv::modulecomm::Unregister(mpagpsimu);
     iv::modulecomm::Unregister(mpadecision);
     iv::modulecomm::Unregister(mpactrlcmd);
     delete mpLT;
 }
 
+void ctrlcmd2decition::ListenGPSIMU(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        mfVelNow = sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
+    }
+    else
+    {
+        std::cout<<" ctrlcmd2decition::ListenGPSIMU Parse Fail ." <<std::endl;
+    }
+}
+
 void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     (void)index;
@@ -38,11 +60,52 @@ void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSi
     double ftorque = 0;
     double fbrake = 0;
     double fwheelangle = 0;
+    if(xctrlcmd.linear_velocity() > 1.0)
+    {
+        if(xctrlcmd.linear_velocity()>mfVelNow)
+        {
+            if(xctrlcmd.linear_velocity()>(mfVelNow + 0.1))
+            {
+                if(xctrlcmd.linear_velocity()>(mfVelNow + 0.3))
+                {
+                    facc =0.3+ 1.0* (xctrlcmd.linear_velocity() -mfVelNow)/xctrlcmd.linear_velocity() ;
+                }
+                else
+                    facc =1.6 * (xctrlcmd.linear_velocity() -mfVelNow)/xctrlcmd.linear_velocity() ;
+            }
+            else
+            {
+                facc = 0.0;
+            }
+        }
+        else
+        {
+            if(xctrlcmd.linear_velocity()>(mfVelNow-0.3))
+            {
+                facc = 0.0;
+            }
+            else
+            {
+                facc = -1.5;
+            }
+        }
+    }
+    else
+    {
+        if(xctrlcmd.linear_velocity()>mfVelNow)
+        {
+            facc = 1.0;
+        }
+        else
+        {
+            facc = -1.0;
+        }
+    }
     if(mpLT->IsINterpolationOK())
     {
-        mpLT->GetTorqueBrake(xctrlcmd.linear_velocity()*3.6,xctrlcmd.linear_acceleration(),ftorque,fbrake);
+        mpLT->GetTorqueBrake(xctrlcmd.linear_velocity()*3.6,facc,ftorque,fbrake);
     }
-    facc = xctrlcmd.linear_acceleration();
+//    facc = xctrlcmd.linear_acceleration();
     fwheelangle = xctrlcmd.steering_angle() * (180.0/M_PI) * 15.0;
     xdecition.set_accelerator(facc);
     xdecition.set_brake(fbrake);
@@ -52,10 +115,10 @@ void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSi
     xdecition.set_wheelangle(fwheelangle);
     xdecition.set_wheelspeed(300);
     xdecition.set_torque(ftorque);
-    xdecition.set_mode(0);
-    xdecition.set_gear(0);
+    xdecition.set_mode(1);
+    xdecition.set_gear(1);
     xdecition.set_handbrake(0);
-    xdecition.set_grade(0);
+    xdecition.set_grade(1);
     xdecition.set_engine(0);
     xdecition.set_brakelamp(false);
     xdecition.set_ttc(15);

src/decition/decition_fromautoware/ctrlcmd2decition.h

@@ -5,6 +5,7 @@
 
 #include "decition.pb.h"
 #include "autowarectrlcmd.pb.h"
+#include "gpsimu.pb.h"
 
 #include "lookuptable.h"
 
@@ -16,14 +17,18 @@ public:
 
 private:
     void ListenCtrlCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 
 private:
     void * mpadecision;
     void * mpactrlcmd;
+    void * mpagpsimu;
 
 private:
     LookupTable * mpLT;
+
+    double mfVelNow = 0;
 };
 
 #endif // CTRLCMD2DECITION_H

src/decition/decition_fromautoware/decition_fromautoware.pro

@@ -20,7 +20,8 @@ SOURCES += \
         ctrlcmd2decition.cpp \
         interpolation_2d.cc \
         lookuptable.cpp \
-        main.cpp
+        main.cpp \
+    ../../include/msgtype/gpsimu.pb.cc
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
@@ -40,4 +41,5 @@ HEADERS += \
     ../../include/msgtype/decition.pb.h \
     ctrlcmd2decition.h \
     interpolation_2d.h \
-    lookuptable.h
+    lookuptable.h \
+    ../../include/msgtype/gpsimu.pb.h

src/driver/driver_autoware_computewaypoints/computewaypoints.cpp

@@ -1,10 +1,14 @@
 #include "computewaypoints.h"
 
+#include "xmlparam.h"
+
 #include <limits>
 #include <memory>
 
 ComputeWayPoints::ComputeWayPoints()
 {
+    iv::xmlparam::Xmlparam xp("./driver_autoware_computewaypoints.xml");
+    mfVel = xp.GetParam("Vel",15.0);
     ModuleFun xFunGPSIMU = std::bind(&ComputeWayPoints::ListenGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",xFunGPSIMU);
 
@@ -89,7 +93,7 @@ void ComputeWayPoints::ListenNewTraceMap(const char *strdata, const unsigned int
     (void)dt;
     (void)strmemname;
 
-    const double fspeedlimt = 30.0/3.6;   //Speeed Limit
+    const double fspeedlimt = mfVel/3.6;   //Speeed Limit
     const double fdacc = -0.5;   //Acc Limit
 
     iv::map::tracemap xtracemap;

src/driver/driver_autoware_computewaypoints/computewaypoints.h

@@ -47,6 +47,8 @@ private:
     void * mpanewtrace;
     void * mpawaypoints;
 
+    double mfVel = 15.0;
+
 };
 
 #endif // COMPUTEWAYPOINTS_H

src/ros/catkin/launch/autowarewithpilot.launch

@@ -0,0 +1,28 @@
+<!-- -->
+<launch>
+  <arg name="is_linear_interpolation" default="True"/>
+  <arg name="publishes_for_steering_robot" default="True"/>
+  <arg name="add_virtual_end_waypoints" default="False"/>
+  <arg name="const_lookahead_distance" default="4.0"/>
+  <arg name="const_velocity" default="5.0"/>
+  <arg name="lookahead_ratio" default="2.0"/>
+  <arg name="minimum_lookahead_distance" default="6.0"/>
+
+  <!-- 0 = waypoints, 1 = provided constant velocity -->
+  <arg name="velocity_source" default="0"/>
+
+  <!-- rosrun waypoint_follower pure_pursuit -->
+
+  <node pkg="pilottoros" type="pilottoros_node" name="pilottoros"  output="screen">
+    <param name="points_node" value="points_raw"/>
+    <param name="points_msgname" value="lidar_pc"/>
+    <param name="image_node" value="image_raw"/>
+    <param name="image_msgname" value="picfront"/>
+    <param name="waypoints_node" value="/final_waypoints"/>
+    <param name="waypoints_msgname" value="waypoints"/>
+    <param name="use_rtk_odom" value="True"/>
+    <param name="use_pilot_waypoints" value="True"/>    
+  </node>
+  <node pkg="rostopilot" type="rostopilot_node" name="rostopilot"  output="screen"> 
+  </node>
+</launch>

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

@@ -331,8 +331,8 @@ 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("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;