Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(pacmod_interface): add option to receive steering wheel command directly #85

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions pacmod_interface/config/pacmod.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -19,3 +19,5 @@
accel_pedal_offset: 0.0
brake_pedal_offset: 0.0
margin_time_for_gear_change: 2.0
enable_pub_steer: true
convert_steer_command: true
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,13 @@ class PacmodInterface : public rclcpp::Node

double margin_time_for_gear_change_; // [s]

// steer command conversion
// if false, it is expected to be converted from and published actuation_status in
// raw_vehicle_cmd_converter
bool enable_pub_steer_ = true; // flag to publish steer_cmd
// if false, it is expected to be converted from and published actuation_status in
bool convert_steer_command_ = true; // flag to convert steer command

autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

// Service
Expand Down
30 changes: 23 additions & 7 deletions pacmod_interface/src/pacmod_interface/pacmod_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,10 @@ PacmodInterface::PacmodInterface()
/* parameter for preventing gear chattering */
margin_time_for_gear_change_ = declare_parameter("margin_time_for_gear_change", 2.0);

/* parameter for steering conversion */
enable_pub_steer_ = declare_parameter("enable_pub_steer", true);
convert_steer_command_ = declare_parameter("convert_steer_command", true);

/* initialize */
prev_steer_cmd_.header.stamp = this->now();
prev_steer_cmd_.command = 0.0;
Expand Down Expand Up @@ -321,7 +325,7 @@ void PacmodInterface::callbackPacmodRpt(
}

/* publish current status */
{
if (enable_pub_steer_) {
autoware_vehicle_msgs::msg::SteeringReport steer_msg;
steer_msg.stamp = header.stamp;
steer_msg.steering_tire_angle = current_steer;
Expand All @@ -330,11 +334,14 @@ void PacmodInterface::callbackPacmodRpt(

/* publish control status */
{
// NOTE:
// actuation status must be the same data format as the input data.
// raw_vehicle_cmd_converter converts from steering_wheel_angle to steering_tire_angle.
ActuationStatusStamped actuation_status;
actuation_status.header = header;
actuation_status.status.accel_status = accel_rpt_ptr_->output;
actuation_status.status.brake_status = brake_rpt_ptr_->output;
actuation_status.status.steer_status = current_steer;
actuation_status.status.steer_status = current_steer_wheel;
actuation_status_pub_->publish(actuation_status);
}

Expand Down Expand Up @@ -396,11 +403,20 @@ void PacmodInterface::publishCommands()
const double current_steer_wheel = steer_wheel_rpt_ptr_->output;

/* calculate desired steering wheel */
double adaptive_gear_ratio = calculateVariableGearRatio(current_velocity, current_steer_wheel);
double desired_steer_wheel =
(control_cmd_ptr_->lateral.steering_tire_angle - steering_offset_) * adaptive_gear_ratio;
desired_steer_wheel =
std::min(std::max(desired_steer_wheel, -max_steering_wheel_), max_steering_wheel_);
const double adaptive_gear_ratio =
calculateVariableGearRatio(current_velocity, current_steer_wheel);
const double desired_steer_wheel = std::invoke([&]() -> double {
double desired_steer_wheel{0.0};
if (convert_steer_command_) {
// NOTE:
// It is assumed that steer_cmd is send as actuation_cmd without being converted from
// raw_vehicle_cmd_converter.
desired_steer_wheel = (actuation.steer_cmd - steering_offset_) * adaptive_gear_ratio;
} else {
desired_steer_wheel = actuation_cmd_ptr_->actuation.steer_cmd;
}
return std::min(std::max(desired_steer_wheel, -max_steering_wheel_), max_steering_wheel_);
});

/* check clear flag */
bool clear_override = false;
Expand Down
Loading