change for autoware. test ok.

src/decition/decition_fromautoware/ctrlcmd2decition.cpp

@@ -60,6 +60,7 @@ void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSi
     double ftorque = 0;
     double fbrake = 0;
     double fwheelangle = 0;
+    std::cout<<" cmd acc: "<<xctrlcmd.linear_acceleration()<<std::endl;
     if(xctrlcmd.linear_velocity() > 1.0)
     {
         if(xctrlcmd.linear_velocity()>mfVelNow)
@@ -115,6 +116,12 @@ void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSi
         double fNeed = fRollForce + fVehWeight*facc;
 
         ftorque = fNeed/fRatio;
+        fbrake = 0;
+        if(ftorque<0)
+        {
+            fbrake = facc;
+            ftorque = 0;
+        }
     }
 //    facc = xctrlcmd.linear_acceleration();
     fwheelangle = xctrlcmd.steering_angle() * (180.0/M_PI) * 15.0;

src/ros2/src/pilot_autoware_bridge/include/pilot_autoware_bridge/pilot_autoware_bridge_core.hpp

@@ -206,6 +206,8 @@ private:
 
     bool mbTestSim = false;
 
+    double mfacc = 0;
+
 
 };
 

src/ros2/src/pilot_autoware_bridge/src/pilot_autoware_bridge_core.cpp

@@ -271,6 +271,8 @@ void pilot_autoware_bridge::callbackTimerGPS()
             double flat = xgpsimu.lat();
             double flon = xgpsimu.lon();
 
+            mfacc = xgpsimu.acce_x();
+
             double pitch,roll,yaw;
             pitch = 0;
             roll = 0;
@@ -305,7 +307,7 @@ void pilot_autoware_bridge::callbackTimerGPS()
             publish_tf(msg);
 
             autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
-            velocity.longitudinal_velocity = 0;
+            velocity.longitudinal_velocity = sqrt(pow(vn,2)+pow(ve,2));
             velocity.lateral_velocity = 0.0F;
             velocity.heading_rate = 0;
             publish_velocity(velocity);
@@ -482,7 +484,7 @@ void pilot_autoware_bridge::publish_acceleration()
     AccelWithCovarianceStamped msg;
     msg.header.frame_id = "/base_link";
     msg.header.stamp = get_clock()->now();
-    msg.accel.accel.linear.x = 0;//vehicle_model_ptr_->getAx();
+    msg.accel.accel.linear.x = mfacc;// 0;//vehicle_model_ptr_->getAx();
 
     using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
     constexpr auto COV = 0.001;
@@ -512,6 +514,8 @@ void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int
     double flat = xgpsimu.lat();
     double flon = xgpsimu.lon();
 
+    mfacc = xgpsimu.acce_x();
+
     double pitch,roll,yaw;
     pitch = 0;
     roll = 0;

src/tool/simple_planning_simulator/simmodel_shenlan.cpp

@@ -86,6 +86,11 @@ void simmodel_shenlan::CalcModel()
         {
             facc = 0;
         }
+        if((facc<0)&&(mvel <= 0)&&(mshift == SHIFT_GEER::DRIVE))
+        {
+            mvel = 0;
+            facc = 0;
+        }
         }
 
         if(mshift == SHIFT_GEER::PARK)