Skip to content

Commit 664a58d

Browse files
tkimura4pre-commit-ci[bot]
tkimura4
and
pre-commit-ci[bot]
authored
feat(pacmod_interface): prevent gear chattering (#64)
* feat(pacmod_interface): prevent gear chattering Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 689f93c commit 664a58d

File tree

4 files changed

+63
-23
lines changed

4 files changed

+63
-23
lines changed

‎pacmod_interface/README.md

+23-22
Original file line numberDiff line numberDiff line change
@@ -57,25 +57,26 @@
5757

5858
## ROS Parameters
5959

60-
| Name | Type | Description |
61-
| --------------------------------- | ------ | ----------------------------------------------------------------------------------------- |
62-
| `base_frame_id` | string | frame id (assigned in pacmod command, but it does not make sense) |
63-
| `command_timeout_ms` | double | timeout [ms] |
64-
| `loop_rate` | double | loop rate to publish commands |
65-
| `steering_offset` | double | steering wheel angle offset |
66-
| `enable_steering_rate_control` | bool | when enabled, max steering wheel rate is used for steering wheel angular velocity command |
67-
| `emergency_brake` | double | brake pedal for emergency |
68-
| `vgr_coef_a` | double | coefficient to calculate steering wheel angle |
69-
| `vgr_coef_b` | double | coefficient to calculate steering wheel angle |
70-
| `vgr_coef_c` | double | coefficient to calculate steering wheel angle |
71-
| `accel_pedal_offset` | double | accel pedal offset |
72-
| `brake_pedal_offset` | double | brake pedal offset |
73-
| `max_throttle` | double | max accel pedal |
74-
| `max_brake` | double | max brake pedal |
75-
| `max_steering_wheel` | double | max steering wheel angle |
76-
| `max_steering_wheel_rate` | double | max steering wheel angular velocity |
77-
| `min_steering_wheel_rate` | double | min steering wheel angular velocity |
78-
| `steering_wheel_rate_low_vel` | double | min steering wheel angular velocity when velocity is low |
79-
| `steering_wheel_rate_low_stopped` | double | min steering wheel angular velocity when velocity is almost 0 |
80-
| `low_vel_thresh` | double | threshold velocity to decide the velocity is low for `steering_wheel_rate_low_vel` |
81-
| `hazard_thresh_time` | double | threshold time to keep hazard lights |
60+
| Name | Type | Description |
61+
| --------------------------------- | ------ | -------------------------------------------------------------------------------------------- |
62+
| `base_frame_id` | string | frame id (assigned in pacmod command, but it does not make sense) |
63+
| `command_timeout_ms` | double | timeout [ms] |
64+
| `loop_rate` | double | loop rate to publish commands |
65+
| `steering_offset` | double | steering wheel angle offset |
66+
| `enable_steering_rate_control` | bool | when enabled, max steering wheel rate is used for steering wheel angular velocity command |
67+
| `emergency_brake` | double | brake pedal for emergency |
68+
| `vgr_coef_a` | double | coefficient to calculate steering wheel angle |
69+
| `vgr_coef_b` | double | coefficient to calculate steering wheel angle |
70+
| `vgr_coef_c` | double | coefficient to calculate steering wheel angle |
71+
| `accel_pedal_offset` | double | accel pedal offset |
72+
| `brake_pedal_offset` | double | brake pedal offset |
73+
| `max_throttle` | double | max accel pedal |
74+
| `max_brake` | double | max brake pedal |
75+
| `max_steering_wheel` | double | max steering wheel angle |
76+
| `max_steering_wheel_rate` | double | max steering wheel angular velocity |
77+
| `min_steering_wheel_rate` | double | min steering wheel angular velocity |
78+
| `steering_wheel_rate_low_vel` | double | min steering wheel angular velocity when velocity is low |
79+
| `steering_wheel_rate_low_stopped` | double | min steering wheel angular velocity when velocity is almost 0 |
80+
| `low_vel_thresh` | double | threshold velocity to decide the velocity is low for `steering_wheel_rate_low_vel` |
81+
| `hazard_thresh_time` | double | threshold time to keep hazard lights |
82+
| `margin_time_for_gear_change` | double | Minimum time between consecutive gear transition commands, designed to prevent gear chatter. |

‎pacmod_interface/config/pacmod.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -18,3 +18,4 @@
1818
accel_pedal_offset: 0.0
1919
brake_pedal_offset: 0.0
2020
need_separate_engage_sequence: false
21+
margin_time_for_gear_change: 2.0

‎pacmod_interface/include/pacmod_interface/pacmod_interface.hpp

+5
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,8 @@ class PacmodInterface : public rclcpp::Node
161161
int hazard_recover_count_ = 0;
162162
const int hazard_recover_cmd_num_ = 5;
163163

164+
double margin_time_for_gear_change_; // [s]
165+
164166
vehicle_info_util::VehicleInfo vehicle_info_;
165167

166168
// Service
@@ -188,6 +190,8 @@ class PacmodInterface : public rclcpp::Node
188190
rclcpp::Time control_command_received_time_;
189191
rclcpp::Time actuation_command_received_time_;
190192
rclcpp::Time last_shift_inout_matched_time_;
193+
std::shared_ptr<rclcpp::Time> last_time_to_change_gear_ptr_;
194+
uint16_t prev_gear_command_ = pacmod3_msgs::msg::SystemCmdInt::SHIFT_PARK;
191195

192196
/* callbacks */
193197
void callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg);
@@ -221,6 +225,7 @@ class PacmodInterface : public rclcpp::Node
221225
double calculateVariableGearRatio(const double vel, const double steer_wheel);
222226
double calcSteerWheelRateCmd(const double gear_ratio);
223227
uint16_t toPacmodShiftCmd(const autoware_auto_vehicle_msgs::msg::GearCommand & gear_cmd);
228+
uint16_t getGearCmdForPreventChatter(uint16_t gear_command);
224229
uint16_t toPacmodTurnCmd(
225230
const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn,
226231
const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard);

‎pacmod_interface/src/pacmod_interface/pacmod_interface.cpp

+34-1
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,9 @@ PacmodInterface::PacmodInterface()
6161
/* parameter for engage sequence */
6262
need_separate_engage_sequence_ = declare_parameter("need_separate_engage_sequence", false);
6363

64+
/* parameter for preventing gear chattering */
65+
margin_time_for_gear_change_ = declare_parameter("margin_time_for_gear_change", 2.0);
66+
6467
/* initialize */
6568
prev_steer_cmd_.header.stamp = this->now();
6669
prev_steer_cmd_.command = 0.0;
@@ -524,7 +527,7 @@ void PacmodInterface::publishCommands()
524527
shift_cmd.enable = clear_override && sep_engage ? false : engage_cmd_;
525528
shift_cmd.ignore_overrides = false;
526529
shift_cmd.clear_override = clear_override;
527-
shift_cmd.command = desired_shift;
530+
shift_cmd.command = getGearCmdForPreventChatter(desired_shift);
528531
shift_cmd_pub_->publish(shift_cmd);
529532
}
530533

@@ -601,6 +604,36 @@ uint16_t PacmodInterface::toPacmodShiftCmd(
601604
return SystemCmdInt::SHIFT_NONE;
602605
}
603606

607+
uint16_t PacmodInterface::getGearCmdForPreventChatter(uint16_t gear_command)
608+
{
609+
// first time to change gear
610+
if (!last_time_to_change_gear_ptr_) {
611+
// send gear change command
612+
last_time_to_change_gear_ptr_ = std::make_shared<rclcpp::Time>(this->now());
613+
prev_gear_command_ = gear_command;
614+
return gear_command;
615+
}
616+
617+
// no gear change
618+
if (gear_command == prev_gear_command_) {
619+
return gear_command;
620+
}
621+
622+
const auto time_from_last_gear_change = (this->now() - *last_time_to_change_gear_ptr_).seconds();
623+
if (time_from_last_gear_change < margin_time_for_gear_change_) {
624+
// hold current gear
625+
RCLCPP_INFO_STREAM(get_logger(), "current_gear_command: " << static_cast<int>(gear_command));
626+
RCLCPP_INFO_STREAM(get_logger(), "prev_gear_command: " << static_cast<int>(prev_gear_command_));
627+
RCLCPP_INFO_STREAM(get_logger(), "send prev_gear_command for preventing gear-chattering");
628+
629+
return prev_gear_command_;
630+
}
631+
// send gear change command
632+
last_time_to_change_gear_ptr_ = std::make_shared<rclcpp::Time>(this->now());
633+
prev_gear_command_ = gear_command;
634+
return gear_command;
635+
}
636+
604637
std::optional<int32_t> PacmodInterface::toAutowareShiftReport(
605638
const pacmod3_msgs::msg::SystemRptInt & shift)
606639
{

0 commit comments

Comments
 (0)