Skip to content

Commit

Permalink
chore(pacmod_interface): separate pacmod callback
Browse files Browse the repository at this point in the history 
Signed-off-by: tomoya.kimura <[email protected]>
  • Loading branch information
@tkimura4
tkimura4 committed Nov 20, 2023
1 parent f369080 commit 1504d9f
Show file tree
Hide file tree
Showing 2 changed files with 93 additions and 68 deletions.
53 changes: 25 additions & 28 deletions pacmod_interface/include/pacmod_interface/pacmod_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,6 @@
#include <tier4_vehicle_msgs/msg/steering_wheel_status_stamped.hpp>
#include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

#include <algorithm>
#include <cmath>
#include <cstdlib>
Expand All @@ -66,12 +62,6 @@ class PacmodInterface : public rclcpp::Node
PacmodInterface();

private:
typedef message_filters::sync_policies::ApproximateTime<
pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::WheelSpeedRpt,
pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::SystemRptFloat,
pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::GlobalRpt>
PacmodFeedbacksSyncPolicy;

/* subscribers */
// From Autoware
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
Expand All @@ -87,16 +77,13 @@ class PacmodInterface : public rclcpp::Node
// From Pacmod
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr rear_door_rpt_sub_;

std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>
steer_wheel_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>
wheel_speed_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> accel_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> brake_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> shift_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> turn_rpt_sub_;
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>> global_rpt_sub_;
std::unique_ptr<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>> pacmod_feedbacks_sync_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr steer_wheel_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::WheelSpeedRpt>::SharedPtr wheel_speed_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr accel_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr brake_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr shift_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr turn_rpt_sub_;
rclcpp::Subscription<pacmod3_msgs::msg::GlobalRpt>::SharedPtr global_rpt_sub_;

/* publishers */
// To Pacmod
Expand Down Expand Up @@ -129,6 +116,14 @@ class PacmodInterface : public rclcpp::Node
/* ros param */
std::string base_frame_id_;
int command_timeout_ms_; // vehicle_cmd timeout [ms]
bool is_steer_wheel_rpt_received_ = false;
bool is_wheel_speed_rpt_received_ = false;
bool is_accel_rpt_received_ = false;
bool is_brake_rpt_received_ = false;
bool is_shift_rpt_received_ = false;
bool is_turn_rpt_received_ = false;
bool is_global_rpt_received_ = false;

bool is_pacmod_rpt_received_ = false;
bool is_pacmod_enabled_ = false;
bool is_clear_override_needed_ = false;
Expand Down Expand Up @@ -206,17 +201,19 @@ class PacmodInterface : public rclcpp::Node
const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg);
void callbackEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg);
void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt);
void callbackPacmodRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_cmd_rpt,
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);
void callbackSteerWheelRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt);
void callbackWheelSpeedRpt(
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt);
void callbackAccelRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt);
void callbackBrakeRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt);
void callbackGearRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt);
void callbackTurnRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt);
void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);

/* functions */
void publishCommands();
void publishVehicleStatus();
double calculateVehicleVelocity(
const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt,
const pacmod3_msgs::msg::SystemRptInt & shift_rpt);
Expand Down
108 changes: 68 additions & 40 deletions pacmod_interface/src/pacmod_interface/pacmod_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,32 +96,20 @@ PacmodInterface::PacmodInterface()
rear_door_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
"/pacmod/rear_pass_door_rpt", 1, std::bind(&PacmodInterface::callbackRearDoor, this, _1));

steer_wheel_rpt_sub_ =
std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
this, "/pacmod/steering_rpt");
wheel_speed_rpt_sub_ =
std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>(
this, "/pacmod/wheel_speed_rpt");
accel_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
this, "/pacmod/accel_rpt");
brake_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
this, "/pacmod/brake_rpt");
shift_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
this, "/pacmod/shift_rpt");
turn_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
this, "/pacmod/turn_rpt");
global_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>>(
this, "/pacmod/global_rpt");

pacmod_feedbacks_sync_ =
std::make_unique<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>>(
PacmodFeedbacksSyncPolicy(10), *steer_wheel_rpt_sub_, *wheel_speed_rpt_sub_, *accel_rpt_sub_,
*brake_rpt_sub_, *shift_rpt_sub_, *turn_rpt_sub_, *global_rpt_sub_);

pacmod_feedbacks_sync_->registerCallback(std::bind(
&PacmodInterface::callbackPacmodRpt, this, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6,
std::placeholders::_7));
steer_wheel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
"/pacmod/steering_rpt", 1, std::bind(&PacmodInterface::callbackSteerWheelRpt, this, _1));
wheel_speed_rpt_sub_ = create_subscription<pacmod3_msgs::msg::WheelSpeedRpt>(
"/pacmod/wheel_speed_rpt", 1, std::bind(&PacmodInterface::callbackWheelSpeedRpt, this, _1));
accel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
"/pacmod/accel_rpt", 1, std::bind(&PacmodInterface::callbackAccelRpt, this, _1));
brake_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
"/pacmod/brake_rpt", 1, std::bind(&PacmodInterface::callbackBrakeRpt, this, _1));
shift_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
"/pacmod/shift_rpt", 1, std::bind(&PacmodInterface::callbackGearRpt, this, _1));
turn_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
"/pacmod/turn_rpt", 1, std::bind(&PacmodInterface::callbackTurnRpt, this, _1));
global_rpt_sub_ = create_subscription<pacmod3_msgs::msg::GlobalRpt>(
"/pacmod/global_rpt", 1, std::bind(&PacmodInterface::callbackGlobalRpt, this, _1));

/* publisher */
// To pacmod
Expand Down Expand Up @@ -241,24 +229,57 @@ void PacmodInterface::callbackRearDoor(
door_status_pub_->publish(toAutowareDoorStatusMsg(*rear_door_rpt));
}

void PacmodInterface::callbackPacmodRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr shift_rpt,
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
void PacmodInterface::callbackSteerWheelRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt)
{
is_pacmod_rpt_received_ = true;
is_steer_wheel_rpt_received_ = true;
steer_wheel_rpt_ptr_ = steer_wheel_rpt;
}

void PacmodInterface::callbackWheelSpeedRpt(
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt)
{
is_wheel_speed_rpt_received_ = true;
wheel_speed_rpt_ptr_ = wheel_speed_rpt;
}

void PacmodInterface::callbackAccelRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt)
{
is_accel_rpt_received_ = true;
accel_rpt_ptr_ = accel_rpt;
}

void PacmodInterface::callbackBrakeRpt(
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt)
{
is_brake_rpt_received_ = true;
brake_rpt_ptr_ = brake_rpt;
gear_cmd_rpt_ptr_ = shift_rpt;
global_rpt_ptr_ = global_rpt;
}

void PacmodInterface::callbackGearRpt(
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt)
{
is_shift_rpt_received_ = true;
gear_cmd_rpt_ptr_ = gear_rpt;
}

void PacmodInterface::callbackTurnRpt(
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt)
{
is_turn_rpt_received_ = true;
turn_rpt_ptr_ = turn_rpt;
}

void PacmodInterface::callbackGlobalRpt(
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
{
is_global_rpt_received_ = true;
global_rpt_ptr_ = global_rpt;
}

void PacmodInterface::publishVehicleStatus()
{
is_pacmod_enabled_ =
steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled;
RCLCPP_DEBUG(
Expand Down Expand Up @@ -293,7 +314,7 @@ void PacmodInterface::callbackPacmodRpt(
autoware_auto_vehicle_msgs::msg::ControlModeReport control_mode_msg;
control_mode_msg.stamp = header.stamp;

if (global_rpt->enabled && is_pacmod_enabled_) {
if (global_rpt_ptr_->enabled && is_pacmod_enabled_) {
control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
} else {
control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL;
Expand Down Expand Up @@ -344,18 +365,23 @@ void PacmodInterface::callbackPacmodRpt(
{
autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport turn_msg;
turn_msg.stamp = header.stamp;
turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt);
turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt_ptr_);
turn_indicators_status_pub_->publish(turn_msg);

autoware_auto_vehicle_msgs::msg::HazardLightsReport hazard_msg;
hazard_msg.stamp = header.stamp;
hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt);
hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt_ptr_);
hazard_lights_status_pub_->publish(hazard_msg);
}
}

void PacmodInterface::publishCommands()
{
is_pacmod_rpt_received_ = is_steer_wheel_rpt_received_ && is_wheel_speed_rpt_received_ &&
is_accel_rpt_received_ && is_brake_rpt_received_ &&
is_shift_rpt_received_ && is_turn_rpt_received_ &&
is_global_rpt_received_;

/* guard */
if (!actuation_cmd_ptr_ || !control_cmd_ptr_ || !is_pacmod_rpt_received_ || !gear_cmd_ptr_) {
RCLCPP_INFO_THROTTLE(
Expand All @@ -365,6 +391,8 @@ void PacmodInterface::publishCommands()
return;
}

publishVehicleStatus();

const rclcpp::Time current_time = get_clock()->now();

double desired_throttle = actuation_cmd_ptr_->actuation.accel_cmd + accel_pedal_offset_;
Expand Down

0 comments on commit 1504d9f

Please sign in to comment.