fix(pacmod_interface): revert "update engage_sequence (#45)" (#67)

Revert "feat(pacmod_interface): update engage_sequence (#45)" This reverts commit 3db50a2. Signed-off-by: tomoya.kimura <[email protected]>
tier4 · Nov 15, 2023 · f369080 · f369080
1 parent 664a58d
commit f369080
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 27 deletions.
diff --git 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
@@ -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
@@ -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,19 +514,19 @@ 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);
     shift_cmd_pub_->publish(shift_cmd);
   }
 
   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
@@ -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