diff --git a/pacmod_interface/config/pacmod.param.yaml b/pacmod_interface/config/pacmod.param.yaml index c9a0da7..cc94ac4 100644 --- a/pacmod_interface/config/pacmod.param.yaml +++ b/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 diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index 69699cb..33ea910 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/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] diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 044f155..a143437 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/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;