From f3690802c12d1f6335df9f9d695f594ddbac04c6 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 15 Nov 2023 13:15:39 +0900 Subject: [PATCH] fix(pacmod_interface): revert "update engage_sequence (#45)" (#67) Revert "feat(pacmod_interface): update engage_sequence (#45)" This reverts commit 3db50a2abc68576308289cc84d83d14376144d64. Signed-off-by: tomoya.kimura --- pacmod_interface/config/pacmod.param.yaml | 1 - .../pacmod_interface/pacmod_interface.hpp | 4 +--- .../src/pacmod_interface/pacmod_interface.cpp | 24 ++++++------------- .../pacmod_steer_test/pacmod_steer_test.cpp | 12 +++++----- 4 files changed, 14 insertions(+), 27 deletions(-) diff --git a/pacmod_interface/config/pacmod.param.yaml b/pacmod_interface/config/pacmod.param.yaml index 8b5e8c0..c9a0da7 100644 --- a/pacmod_interface/config/pacmod.param.yaml +++ b/pacmod_interface/config/pacmod.param.yaml @@ -17,5 +17,4 @@ vgr_coef_c: 0.042 accel_pedal_offset: 0.0 brake_pedal_offset: 0.0 - need_separate_engage_sequence: false margin_time_for_gear_change: 2.0 diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index a137a44..e34da0f 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -153,9 +153,7 @@ class PacmodInterface : public rclcpp::Node double steering_wheel_rate_stopped_; // [rad/s] double low_vel_thresh_; // [m/s] - bool enable_steering_rate_control_; // use steering angle speed for command [rad/s] - bool need_separate_engage_sequence_; // when you use a newer version of firmware than 3.3, it - // must be true + bool enable_steering_rate_control_; // use steering angle speed for command [rad/s] double hazard_thresh_time_; int hazard_recover_count_ = 0; diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 53cf183..698b80c 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -58,9 +58,6 @@ PacmodInterface::PacmodInterface() /* parameters for turn signal recovery */ hazard_thresh_time_ = declare_parameter("hazard_thresh_time", 0.20); // s - /* parameter for engage sequence */ - need_separate_engage_sequence_ = declare_parameter("need_separate_engage_sequence", false); - /* parameter for preventing gear chattering */ margin_time_for_gear_change_ = declare_parameter("margin_time_for_gear_change", 2.0); @@ -458,19 +455,12 @@ void PacmodInterface::publishCommands() } } - const auto & sep_engage = need_separate_engage_sequence_; - /* publish accel cmd */ { pacmod3_msgs::msg::SystemCmdFloat accel_cmd; accel_cmd.header.frame_id = base_frame_id_; accel_cmd.header.stamp = current_time; - /* - For changing to auto mode, it needs to be following two messages - 1. Send a msg with clear override bit high and enable bit low. - 2. Send a msg after that, with clear override bit low, and enable bit high. - */ - accel_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + accel_cmd.enable = engage_cmd_; accel_cmd.ignore_overrides = false; accel_cmd.clear_override = clear_override; accel_cmd.command = std::max(0.0, std::min(desired_throttle, max_throttle_)); @@ -482,7 +472,7 @@ void PacmodInterface::publishCommands() pacmod3_msgs::msg::SystemCmdFloat brake_cmd; brake_cmd.header.frame_id = base_frame_id_; brake_cmd.header.stamp = current_time; - brake_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + brake_cmd.enable = engage_cmd_; brake_cmd.ignore_overrides = false; brake_cmd.clear_override = clear_override; brake_cmd.command = std::max(0.0, std::min(desired_brake, max_brake_)); @@ -494,7 +484,7 @@ void PacmodInterface::publishCommands() pacmod3_msgs::msg::SteeringCmd steer_cmd; steer_cmd.header.frame_id = base_frame_id_; steer_cmd.header.stamp = current_time; - steer_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + steer_cmd.enable = engage_cmd_; steer_cmd.ignore_overrides = false; steer_cmd.clear_override = clear_override; steer_cmd.rotation_rate = calcSteerWheelRateCmd(adaptive_gear_ratio); @@ -510,7 +500,7 @@ void PacmodInterface::publishCommands() pacmod3_msgs::msg::SteeringCmd raw_steer_cmd; raw_steer_cmd.header.frame_id = base_frame_id_; raw_steer_cmd.header.stamp = current_time; - raw_steer_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + raw_steer_cmd.enable = engage_cmd_; raw_steer_cmd.ignore_overrides = false; raw_steer_cmd.clear_override = clear_override; raw_steer_cmd.command = desired_steer_wheel; @@ -524,7 +514,7 @@ void PacmodInterface::publishCommands() pacmod3_msgs::msg::SystemCmdInt shift_cmd; shift_cmd.header.frame_id = base_frame_id_; shift_cmd.header.stamp = current_time; - shift_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + shift_cmd.enable = engage_cmd_; shift_cmd.ignore_overrides = false; shift_cmd.clear_override = clear_override; shift_cmd.command = getGearCmdForPreventChatter(desired_shift); @@ -532,11 +522,11 @@ void PacmodInterface::publishCommands() } if (turn_indicators_cmd_ptr_ && hazard_lights_cmd_ptr_) { - /* publish turn cmd */ + /* publish shift cmd */ pacmod3_msgs::msg::SystemCmdInt turn_cmd; turn_cmd.header.frame_id = base_frame_id_; turn_cmd.header.stamp = current_time; - turn_cmd.enable = clear_override && sep_engage ? false : engage_cmd_; + turn_cmd.enable = engage_cmd_; turn_cmd.ignore_overrides = false; turn_cmd.clear_override = clear_override; turn_cmd.command = diff --git a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp index b61072d..0b35e65 100644 --- a/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp +++ b/pacmod_interface/src/pacmod_steer_test/pacmod_steer_test.cpp @@ -234,7 +234,7 @@ void PacmodSteerTest::publishCommands() pacmod3_msgs::msg::SystemCmdFloat accel_cmd; accel_cmd.header.frame_id = base_frame_id_; accel_cmd.header.stamp = current_time; - accel_cmd.enable = clear_override ? false : engage_cmd_; + accel_cmd.enable = engage_cmd_; accel_cmd.ignore_overrides = false; accel_cmd.clear_override = clear_override; accel_cmd.command = getAccel(); @@ -246,7 +246,7 @@ void PacmodSteerTest::publishCommands() pacmod3_msgs::msg::SystemCmdFloat brake_cmd; brake_cmd.header.frame_id = base_frame_id_; brake_cmd.header.stamp = current_time; - brake_cmd.enable = clear_override ? false : engage_cmd_; + brake_cmd.enable = engage_cmd_; brake_cmd.ignore_overrides = false; brake_cmd.clear_override = clear_override; brake_cmd.command = getBrake(); @@ -258,7 +258,7 @@ void PacmodSteerTest::publishCommands() pacmod3_msgs::msg::SteeringCmd steer_cmd; steer_cmd.header.frame_id = base_frame_id_; steer_cmd.header.stamp = current_time; - steer_cmd.enable = clear_override ? false : engage_cmd_; + steer_cmd.enable = engage_cmd_; steer_cmd.ignore_overrides = false; steer_cmd.clear_override = clear_override; steer_cmd.command = testSteerCommand(); // desired_steer_wheel; @@ -271,18 +271,18 @@ void PacmodSteerTest::publishCommands() pacmod3_msgs::msg::SystemCmdInt shift_cmd; shift_cmd.header.frame_id = base_frame_id_; shift_cmd.header.stamp = current_time; - shift_cmd.enable = clear_override ? false : engage_cmd_; + shift_cmd.enable = engage_cmd_; shift_cmd.ignore_overrides = false; shift_cmd.clear_override = clear_override; shift_cmd.command = pacmod3_msgs::msg::SystemCmdInt::SHIFT_FORWARD; // always drive shift shift_cmd_pub_->publish(shift_cmd); } - /* publish turn cmd */ + /* publish shift cmd */ pacmod3_msgs::msg::SystemCmdInt turn_cmd; turn_cmd.header.frame_id = base_frame_id_; turn_cmd.header.stamp = current_time; - turn_cmd.enable = clear_override ? false : engage_cmd_; + turn_cmd.enable = engage_cmd_; turn_cmd.ignore_overrides = false; turn_cmd.clear_override = clear_override; turn_cmd.command = pacmod3_msgs::msg::SystemCmdInt::TURN_HAZARDS; // for safety