feat: add option to use emergency brake commands from vehicle_cmd_gate and … (#79)

add option to use emergency brake commands from vehicle_cmd_gate and not set emergency brake

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

pacmod_interface/config/pacmod.param.yaml

@@ -3,6 +3,7 @@
     base_frame_id: "base_link"
     command_timeout_ms: 1000
     loop_rate: 30.0
+    use_external_emergency_brake: false
     emergency_brake: 0.7
     max_throttle: 0.4
     max_brake: 0.8

pacmod_interface/include/pacmod_interface/pacmod_interface.hpp

@@ -136,6 +136,7 @@ class PacmodInterface : public rclcpp::Node
   double brake_pedal_offset_;  // offset of brake pedal value
 
   double emergency_brake_;              // brake command when emergency [m/s^2]
+  bool use_external_emergency_brake_;   // set to true to not use emergency_brake_
   double max_throttle_;                 // max throttle [0~1]
   double max_brake_;                    // max throttle [0~1]
   double max_steering_wheel_;           // max steering wheel angle [rad]

pacmod_interface/src/pacmod_interface/pacmod_interface.cpp

@@ -37,6 +37,7 @@ PacmodInterface::PacmodInterface()
 
   /* parameters for emergency stop */
   emergency_brake_ = declare_parameter("emergency_brake", 0.7);
+  use_external_emergency_brake_ = declare_parameter("use_external_emergency_brake", false);
 
   /* vehicle parameters */
   vgr_coef_a_ = declare_parameter("vgr_coef_a", 15.713);
@@ -408,7 +409,10 @@ void PacmodInterface::publishCommands()
   if (t_out >= 0 && (control_cmd_delta_time_ms > t_out || actuation_cmd_delta_time_ms > t_out)) {
     timeouted = true;
   }
-  if (is_emergency_ || timeouted) {
+  /* check emergency and timeout */
+  const bool emergency_brake_needed =
+    (is_emergency_ && !use_external_emergency_brake_) || timeouted;
+  if (emergency_brake_needed) {
     RCLCPP_ERROR(
       get_logger(), "Emergency Stopping, emergency = %d, timeouted = %d", is_emergency_, timeouted);
     desired_throttle = 0.0;