Merge branch 'issue/emergency_handler_twist_angular_value' into 'master'

bug fix: twist angular value in /vehicle_cmd is remaining during error state See merge request autowarefoundation/autoware.ai/common!63
autowarefoundation · Nov 25, 2019 · c0fc751 · c0fc751
2 parents bebcb65 + aa71180
commit c0fc751
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 2 deletions.
diff --git a/emergency_handler/include/emergency_handler/emergency_planner.h b/emergency_handler/include/emergency_handler/emergency_planner.h
@@ -26,6 +26,7 @@
 #include <autoware_system_msgs/DiagnosticStatusArray.h>
 
 static constexpr double EMERGENCY_PLANNER_RATE = 50.0;
+static constexpr double EMERGENCY_PLANNER_STOP_THRESH = 0.5;
 
 class EmergencyPlannerFeedback
 {

diff --git a/emergency_handler/src/emergency_stop_planner.cpp b/emergency_handler/src/emergency_stop_planner.cpp
@@ -28,8 +28,19 @@ void SemiEmergencyStopPlanner::get_feedback_from_emergency_planner(EmergencyPlan
 {
   epf->is_vehicle_cmd_updated = true;
   epf->vehicle_cmd.header.stamp = ros::Time::now();
-  epf->vehicle_cmd.ctrl_cmd.linear_velocity =
-  std::max(epf->vehicle_cmd.ctrl_cmd.linear_velocity - max_dec_for_spdctl_, 0.0);
+
+  double new_vel = std::max(epf->vehicle_cmd.ctrl_cmd.linear_velocity - max_dec_for_spdctl_, 0.0);
+
+  if (epf->vehicle_cmd.ctrl_cmd.linear_velocity > EMERGENCY_PLANNER_STOP_THRESH)
+  {
+    epf->vehicle_cmd.twist_cmd.twist.angular.z *= (new_vel / epf->vehicle_cmd.ctrl_cmd.linear_velocity);
+  }
+  else
+  {
+    epf->vehicle_cmd.twist_cmd.twist.angular.z = 0;
+  }
+
+  epf->vehicle_cmd.ctrl_cmd.linear_velocity = new_vel;
   epf->vehicle_cmd.twist_cmd.twist.linear.x = epf->vehicle_cmd.ctrl_cmd.linear_velocity;
   epf->vehicle_cmd.emergency = (epf->vehicle_cmd.ctrl_cmd.linear_velocity == 0.0) ? 1 : 0;
 }
@@ -46,5 +57,6 @@ void EmergencyStopPlanner::get_feedback_from_emergency_planner(EmergencyPlannerF
   epf->vehicle_cmd.emergency = 1;
   epf->vehicle_cmd.ctrl_cmd.linear_velocity = 0;
   epf->vehicle_cmd.twist_cmd.twist.linear.x = 0;
+  epf->vehicle_cmd.twist_cmd.twist.angular.z = 0;
   //  epf->vehicle_cmd.ctrl_cmd.steering_angle // No update use the previous steering angle
 }