From b22580d599ed04f57d04a77ca7f7f636f0dbc2f1 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 19 Jun 2024 15:45:26 +0900 Subject: [PATCH 1/2] feat(default_ad_api): use polling subscription Signed-off-by: Takagi, Isamu --- .../rclcpp/create_interface.hpp | 22 ++++-- .../rclcpp/topic_subscription.hpp | 36 ++++++++++ .../src/compatibility/autoware_state.cpp | 28 ++++---- .../src/compatibility/autoware_state.hpp | 3 - system/default_ad_api/src/planning.cpp | 10 ++- system/default_ad_api/src/planning.hpp | 1 - system/default_ad_api/src/vehicle.cpp | 69 +++++-------------- system/default_ad_api/src/vehicle.hpp | 12 ---- 8 files changed, 86 insertions(+), 95 deletions(-) diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp index d5af211b015bf..395b57c19d5ae 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp @@ -65,11 +65,23 @@ template typename Subscription::SharedPtr create_subscription_impl( NodeT * node, CallbackT && callback) { - // This function is a wrapper for the following. - // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238 - auto subscription = node->template create_subscription( - SpecT::name, get_qos(), std::forward(callback)); - return Subscription::make_shared(subscription); + if constexpr (!std::is_null_pointer_v) { + // This function is a wrapper for the following. + // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238 + auto subscription = node->template create_subscription( + SpecT::name, get_qos(), std::forward(callback)); + return Subscription::make_shared(subscription); + } else { + // If the callback is nullptr, create a subscription for polling. + // https://github.com/autowarefoundation/autoware.universe/tree/main/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp + auto group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = group; + + auto subscription = node->template create_subscription( + SpecT::name, get_qos(), [](const typename SpecT::Message) {}, options); + return Subscription::make_shared(subscription); + } } } // namespace component_interface_utils diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp index 195a04c370964..390828d89b5f4 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp @@ -17,6 +17,8 @@ #include +#include + namespace component_interface_utils { @@ -35,6 +37,40 @@ class Subscription subscription_ = subscription; // to keep the reference count } + typename SpecType::Message::ConstSharedPtr takeLatestData() + { + rclcpp::MessageInfo info; + auto data = std::make_shared(); + bool flag = false; + for (size_t i = 0; i < subscription_->get_actual_qos().depth(); ++i) { + if (!subscription_->take(*data, info)) { + break; + } + flag = true; // Whether there is at least one data. + } + return flag ? data : nullptr; + } + + bool updateWithLatestData(typename SpecType::Message::ConstSharedPtr & ptr) + { + const auto msg = takeLatestData(); + if (!msg) { + return false; + } + ptr = msg; + return true; + } + + bool updateWithLatestData(typename SpecType::Message & ref) + { + const auto msg = takeLatestData(); + if (!msg) { + return false; + } + ref = *msg; + return true; + } + private: RCLCPP_DISABLE_COPY(Subscription) typename WrapType::SharedPtr subscription_; diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 1aa49383aa48f..71261229ed9b7 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -42,9 +42,13 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2)); const auto adaptor = component_interface_utils::NodeAdaptor(this); - adaptor.init_sub(sub_localization_, this, &AutowareStateNode::on_localization); - adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); - adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); + adaptor.init_sub(sub_localization_, nullptr); + adaptor.init_sub(sub_routing_, nullptr); + adaptor.init_sub(sub_operation_mode_, nullptr); + + RCLCPP_INFO_STREAM(get_logger(), sub_localization_); + RCLCPP_INFO_STREAM(get_logger(), sub_routing_); + RCLCPP_INFO_STREAM(get_logger(), sub_operation_mode_); const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); @@ -56,19 +60,6 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) operation_mode_state_.mode = OperationModeState::UNKNOWN; } -void AutowareStateNode::on_localization(const LocalizationState::ConstSharedPtr msg) -{ - localization_state_ = *msg; -} -void AutowareStateNode::on_routing(const RoutingState::ConstSharedPtr msg) -{ - routing_state_ = *msg; -} -void AutowareStateNode::on_operation_mode(const OperationModeState::ConstSharedPtr msg) -{ - operation_mode_state_ = *msg; -} - void AutowareStateNode::on_shutdown( const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr res) { @@ -115,6 +106,11 @@ void AutowareStateNode::on_timer() return AutowareState::PLANNING; }; + // Update each state + sub_localization_->updateWithLatestData(localization_state_); + sub_routing_->updateWithLatestData(routing_state_); + sub_operation_mode_->updateWithLatestData(operation_mode_state_); + // Update launch state. if (launch_state_ == LaunchState::Initializing) { bool is_initialized = true; diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index eff74728efcb0..4d1b46aa7d305 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -61,9 +61,6 @@ class AutowareStateNode : public rclcpp::Node OperationModeState operation_mode_state_; void on_timer(); - void on_localization(const LocalizationState::ConstSharedPtr msg); - void on_routing(const RoutingState::ConstSharedPtr msg); - void on_operation_mode(const OperationModeState::ConstSharedPtr msg); void on_shutdown(const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr); }; diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 6fd27c4963f66..008a8d83d4dbd 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -102,17 +102,12 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning adaptor.init_pub(pub_velocity_factors_); adaptor.init_pub(pub_steering_factors_); adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state); - adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory); + adaptor.init_sub(sub_trajectory_, nullptr); const auto rate = rclcpp::Rate(5); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } -void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) -{ - trajectory_ = msg; -} - void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) { kinematic_state_ = msg; @@ -129,6 +124,9 @@ void PlanningNode::on_timer() auto velocity = merge_factors(now(), velocity_factors_); auto steering = merge_factors(now(), steering_factors_); + // Take latest trajectory. + sub_trajectory_->updateWithLatestData(trajectory_); + // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { for (auto & factor : velocity.factors) { diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp index 7e754efc4855f..3d9742914f301 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -51,7 +51,6 @@ class PlanningNode : public rclcpp::Node using VehicleStopChecker = autoware_motion_utils::VehicleStopCheckerBase; using Trajectory = planning_interface::Trajectory::Message; using KinematicState = localization_interface::KinematicState::Message; - void on_trajectory(const Trajectory::ConstSharedPtr msg); void on_kinematic_state(const KinematicState::ConstSharedPtr msg); void on_timer(); diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 47932f969c172..39d32b901bff9 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -65,14 +65,14 @@ VehicleNode::VehicleNode(const rclcpp::NodeOptions & options) : Node("vehicle", group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.init_pub(pub_kinematics_); adaptor.init_pub(pub_status_); - adaptor.init_sub(sub_kinematic_state_, this, &VehicleNode::kinematic_state); - adaptor.init_sub(sub_acceleration_, this, &VehicleNode::acceleration_status); - adaptor.init_sub(sub_steering_, this, &VehicleNode::steering_status); - adaptor.init_sub(sub_gear_state_, this, &VehicleNode::gear_status); - adaptor.init_sub(sub_turn_indicator_, this, &VehicleNode::turn_indicator_status); - adaptor.init_sub(sub_map_projector_info_, this, &VehicleNode::map_projector_info); - adaptor.init_sub(sub_hazard_light_, this, &VehicleNode::hazard_light_status); - adaptor.init_sub(sub_energy_level_, this, &VehicleNode::energy_status); + adaptor.init_sub(sub_kinematic_state_, nullptr); + adaptor.init_sub(sub_acceleration_, nullptr); + adaptor.init_sub(sub_steering_, nullptr); + adaptor.init_sub(sub_gear_state_, nullptr); + adaptor.init_sub(sub_turn_indicator_, nullptr); + adaptor.init_sub(sub_map_projector_info_, nullptr); + adaptor.init_sub(sub_hazard_light_, nullptr); + adaptor.init_sub(sub_energy_level_, nullptr); const auto rate = rclcpp::Rate(10); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); @@ -88,50 +88,6 @@ uint8_t VehicleNode::mapping( } } -void VehicleNode::kinematic_state( - const localization_interface::KinematicState::Message::ConstSharedPtr msg_ptr) -{ - kinematic_state_msgs_ = msg_ptr; -} - -void VehicleNode::acceleration_status( - const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr) -{ - acceleration_msgs_ = msg_ptr; -} - -void VehicleNode::steering_status( - const vehicle_interface::SteeringStatus::Message::ConstSharedPtr msg_ptr) -{ - steering_status_msgs_ = msg_ptr; -} - -void VehicleNode::gear_status(const GearReport::ConstSharedPtr msg_ptr) -{ - gear_status_msgs_ = msg_ptr; -} - -void VehicleNode::turn_indicator_status(const TurnIndicatorsReport::ConstSharedPtr msg_ptr) -{ - turn_indicator_status_msgs_ = msg_ptr; -} - -void VehicleNode::hazard_light_status(const HazardLightsReport::ConstSharedPtr msg_ptr) -{ - hazard_light_status_msgs_ = msg_ptr; -} - -void VehicleNode::energy_status( - const vehicle_interface::EnergyStatus::Message::ConstSharedPtr msg_ptr) -{ - energy_status_msgs_ = msg_ptr; -} - -void VehicleNode::map_projector_info(const MapProjectorInfo::ConstSharedPtr msg_ptr) -{ - map_projector_info_ = msg_ptr; -} - void VehicleNode::publish_kinematics() { if (!kinematic_state_msgs_ || !acceleration_msgs_ || !map_projector_info_) return; @@ -183,6 +139,15 @@ void VehicleNode::publish_status() void VehicleNode::on_timer() { + sub_kinematic_state_->updateWithLatestData(kinematic_state_msgs_); + sub_acceleration_->updateWithLatestData(acceleration_msgs_); + sub_steering_->updateWithLatestData(steering_status_msgs_); + sub_gear_state_->updateWithLatestData(gear_status_msgs_); + sub_turn_indicator_->updateWithLatestData(turn_indicator_status_msgs_); + sub_hazard_light_->updateWithLatestData(hazard_light_status_msgs_); + sub_energy_level_->updateWithLatestData(energy_status_msgs_); + sub_map_projector_info_->updateWithLatestData(map_projector_info_); + publish_kinematics(); publish_status(); } diff --git a/system/default_ad_api/src/vehicle.hpp b/system/default_ad_api/src/vehicle.hpp index 56012152a132b..66da15c81a559 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/default_ad_api/src/vehicle.hpp @@ -61,18 +61,6 @@ class VehicleNode : public rclcpp::Node vehicle_interface::EnergyStatus::Message::ConstSharedPtr energy_status_msgs_; map_interface::MapProjectorInfo::Message::ConstSharedPtr map_projector_info_; - void kinematic_state( - const localization_interface::KinematicState::Message::ConstSharedPtr msg_ptr); - void acceleration_status( - const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr); - void steering_status(const vehicle_interface::SteeringStatus::Message::ConstSharedPtr msg_ptr); - void gear_status(const vehicle_interface::GearStatus::Message::ConstSharedPtr msg_ptr); - void turn_indicator_status( - const vehicle_interface::TurnIndicatorStatus::Message::ConstSharedPtr msg_ptr); - void map_projector_info(const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg_ptr); - void hazard_light_status( - const vehicle_interface::HazardLightStatus::Message::ConstSharedPtr msg_ptr); - void energy_status(const vehicle_interface::EnergyStatus::Message::ConstSharedPtr msg_ptr); uint8_t mapping( std::unordered_map hash_map, uint8_t input, uint8_t default_value); void publish_kinematics(); From 3b72f5926efbb33d36ceca367c1adecfd9d83333 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 24 Jun 2024 14:57:12 +0900 Subject: [PATCH 2/2] remove debug code Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/compatibility/autoware_state.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 71261229ed9b7..5eed559c729ba 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -46,10 +46,6 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) adaptor.init_sub(sub_routing_, nullptr); adaptor.init_sub(sub_operation_mode_, nullptr); - RCLCPP_INFO_STREAM(get_logger(), sub_localization_); - RCLCPP_INFO_STREAM(get_logger(), sub_routing_); - RCLCPP_INFO_STREAM(get_logger(), sub_operation_mode_); - const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });