diff --git a/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp index f3371a372b800..4702ef17e49bf 100644 --- a/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp +++ b/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ #define AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" + #include #include @@ -41,16 +43,19 @@ class ShiftDecider : public rclcpp::Node void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_cmd_{this, "input/control_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_autoware_state_{this, "input/state"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_gear_{this, "input/current_gear"}; rclcpp::TimerBase::SharedPtr timer_; - autoware_control_msgs::msg::Control::SharedPtr control_cmd_; - autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_; + autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; autoware_vehicle_msgs::msg::GearCommand shift_cmd_; - autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr current_gear_ptr_; uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; bool park_on_goal_; diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml index 3091691783e34..40708c2c73cf1 100644 --- a/control/autoware_shift_decider/package.xml +++ b/control/autoware_shift_decider/package.xml @@ -18,6 +18,7 @@ autoware_vehicle_msgs rclcpp rclcpp_components + tier4_autoware_utils ament_cmake_cppcheck ament_cmake_cpplint diff --git a/control/autoware_shift_decider/src/autoware_shift_decider.cpp b/control/autoware_shift_decider/src/autoware_shift_decider.cpp index 4df45d6ccdb26..91345376c2681 100644 --- a/control/autoware_shift_decider/src/autoware_shift_decider.cpp +++ b/control/autoware_shift_decider/src/autoware_shift_decider.cpp @@ -18,8 +18,6 @@ #include #include -#include -#include namespace autoware::shift_decider { @@ -37,33 +35,15 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) pub_shift_cmd_ = create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( - "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( - "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( - "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); initTimer(0.1); } -void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg) -{ - control_cmd_ = msg; -} - -void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg) -{ - autoware_state_ = msg; -} - -void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) -{ - current_gear_ptr_ = msg; -} - void ShiftDecider::onTimer() { + control_cmd_ = sub_control_cmd_.takeData(); + autoware_state_ = sub_autoware_state_.takeData(); + current_gear_ptr_ = sub_current_gear_.takeData(); if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { return; }