From 013a37c177369909c11eb98133eb5ac8a3fd1533 Mon Sep 17 00:00:00 2001 From: Sho Iwasawa Date: Thu, 4 Jul 2024 14:23:51 +0900 Subject: [PATCH 1/3] Revert "chore(pacmod_interface): separate pacmod callback (#68)" This reverts commit b779f8496f7f5c92ddf2dcf321897fe8602f0414. Signed-off-by: Sho Iwasawa --- .../pacmod_interface/pacmod_interface.hpp | 53 +++++---- .../src/pacmod_interface/pacmod_interface.cpp | 110 +++++++----------- 2 files changed, 69 insertions(+), 94 deletions(-) diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index e13553d..89b6df8 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -45,6 +45,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -62,6 +66,12 @@ 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::SharedPtr control_cmd_sub_; @@ -76,13 +86,16 @@ class PacmodInterface : public rclcpp::Node // From Pacmod rclcpp::Subscription::SharedPtr rear_door_rpt_sub_; - rclcpp::Subscription::SharedPtr steer_wheel_rpt_sub_; - rclcpp::Subscription::SharedPtr wheel_speed_rpt_sub_; - rclcpp::Subscription::SharedPtr accel_rpt_sub_; - rclcpp::Subscription::SharedPtr brake_rpt_sub_; - rclcpp::Subscription::SharedPtr shift_rpt_sub_; - rclcpp::Subscription::SharedPtr turn_rpt_sub_; - rclcpp::Subscription::SharedPtr global_rpt_sub_; + std::unique_ptr> + steer_wheel_rpt_sub_; + std::unique_ptr> + wheel_speed_rpt_sub_; + std::unique_ptr> accel_rpt_sub_; + std::unique_ptr> brake_rpt_sub_; + std::unique_ptr> shift_rpt_sub_; + std::unique_ptr> turn_rpt_sub_; + std::unique_ptr> global_rpt_sub_; + std::unique_ptr> pacmod_feedbacks_sync_; /* publishers */ // To Pacmod @@ -113,14 +126,6 @@ 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; @@ -198,19 +203,17 @@ class PacmodInterface : public rclcpp::Node const autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_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); + 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); /* functions */ void publishCommands(); - void publishVehicleStatus(); double calculateVehicleVelocity( const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt, const pacmod3_msgs::msg::SystemRptInt & shift_rpt); diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 2d71b2a..49c089d 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -95,20 +95,32 @@ PacmodInterface::PacmodInterface() rear_door_rpt_sub_ = create_subscription( "/pacmod/rear_pass_door_rpt", 1, std::bind(&PacmodInterface::callbackRearDoor, this, _1)); - steer_wheel_rpt_sub_ = create_subscription( - "/pacmod/steering_rpt", 1, std::bind(&PacmodInterface::callbackSteerWheelRpt, this, _1)); - wheel_speed_rpt_sub_ = create_subscription( - "/pacmod/wheel_speed_rpt", 1, std::bind(&PacmodInterface::callbackWheelSpeedRpt, this, _1)); - accel_rpt_sub_ = create_subscription( - "/pacmod/accel_rpt", 1, std::bind(&PacmodInterface::callbackAccelRpt, this, _1)); - brake_rpt_sub_ = create_subscription( - "/pacmod/brake_rpt", 1, std::bind(&PacmodInterface::callbackBrakeRpt, this, _1)); - shift_rpt_sub_ = create_subscription( - "/pacmod/shift_rpt", 1, std::bind(&PacmodInterface::callbackGearRpt, this, _1)); - turn_rpt_sub_ = create_subscription( - "/pacmod/turn_rpt", 1, std::bind(&PacmodInterface::callbackTurnRpt, this, _1)); - global_rpt_sub_ = create_subscription( - "/pacmod/global_rpt", 1, std::bind(&PacmodInterface::callbackGlobalRpt, this, _1)); + steer_wheel_rpt_sub_ = + std::make_unique>( + this, "/pacmod/steering_rpt"); + wheel_speed_rpt_sub_ = + std::make_unique>( + this, "/pacmod/wheel_speed_rpt"); + accel_rpt_sub_ = std::make_unique>( + this, "/pacmod/accel_rpt"); + brake_rpt_sub_ = std::make_unique>( + this, "/pacmod/brake_rpt"); + shift_rpt_sub_ = std::make_unique>( + this, "/pacmod/shift_rpt"); + turn_rpt_sub_ = std::make_unique>( + this, "/pacmod/turn_rpt"); + global_rpt_sub_ = std::make_unique>( + this, "/pacmod/global_rpt"); + + pacmod_feedbacks_sync_ = + std::make_unique>( + 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)); /* publisher */ // To pacmod @@ -227,57 +239,24 @@ void PacmodInterface::callbackRearDoor( door_status_pub_->publish(toAutowareDoorStatusMsg(*rear_door_rpt)); } -void PacmodInterface::callbackSteerWheelRpt( - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_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) { - is_steer_wheel_rpt_received_ = true; + is_pacmod_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; -} - -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; + gear_cmd_rpt_ptr_ = shift_rpt; global_rpt_ptr_ = global_rpt; -} + turn_rpt_ptr_ = turn_rpt; -void PacmodInterface::publishVehicleStatus() -{ is_pacmod_enabled_ = steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled; RCLCPP_DEBUG( @@ -312,8 +291,8 @@ void PacmodInterface::publishVehicleStatus() autoware_vehicle_msgs::msg::ControlModeReport control_mode_msg; control_mode_msg.stamp = header.stamp; - if (global_rpt_ptr_->enabled && is_pacmod_enabled_) { - control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + if (global_rpt->enabled && is_pacmod_enabled_) { + control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;; } else { control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; } @@ -363,23 +342,18 @@ void PacmodInterface::publishVehicleStatus() { autoware_vehicle_msgs::msg::TurnIndicatorsReport turn_msg; turn_msg.stamp = header.stamp; - turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt_ptr_); + turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt); turn_indicators_status_pub_->publish(turn_msg); autoware_vehicle_msgs::msg::HazardLightsReport hazard_msg; hazard_msg.stamp = header.stamp; - hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt_ptr_); + hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt); 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( @@ -389,8 +363,6 @@ 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_; From a19cee3ce94bcd37eaae1f376fc6a55347e48d26 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Jul 2024 05:37:29 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- pacmod_interface/src/pacmod_interface/pacmod_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 49c089d..6d264cc 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -292,7 +292,8 @@ void PacmodInterface::callbackPacmodRpt( control_mode_msg.stamp = header.stamp; if (global_rpt->enabled && is_pacmod_enabled_) { - control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;; + control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + ; } else { control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; } From ed234d483fd856919204871f4d54c32610c1f202 Mon Sep 17 00:00:00 2001 From: Sho Iwasawa Date: Thu, 4 Jul 2024 14:55:51 +0900 Subject: [PATCH 3/3] fix type Signed-off-by: Sho Iwasawa --- pacmod_interface/src/pacmod_interface/pacmod_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 6d264cc..5176b47 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -293,7 +293,6 @@ void PacmodInterface::callbackPacmodRpt( if (global_rpt->enabled && is_pacmod_enabled_) { control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; - ; } else { control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; }