diff --git a/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/create_interface.hpp b/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/create_interface.hpp index 66f85613cb11b..b2b4983bcef28 100644 --- a/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/create_interface.hpp +++ b/common/autoware_component_interface_utils/include/autoware/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 autoware::component_interface_utils diff --git a/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/topic_subscription.hpp b/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/topic_subscription.hpp index bd092c30b9866..e9a7d3594b828 100644 --- a/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/topic_subscription.hpp +++ b/common/autoware_component_interface_utils/include/autoware/component_interface_utils/rclcpp/topic_subscription.hpp @@ -17,6 +17,8 @@ #include +#include + namespace autoware::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/autoware_default_adapi/src/compatibility/autoware_state.cpp b/system/autoware_default_adapi/src/compatibility/autoware_state.cpp index e11a4532d96b7..5323034b2c284 100644 --- a/system/autoware_default_adapi/src/compatibility/autoware_state.cpp +++ b/system/autoware_default_adapi/src/compatibility/autoware_state.cpp @@ -42,9 +42,9 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2)); const auto adaptor = autoware::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); const auto rate = rclcpp::Rate(declare_parameter("update_rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); @@ -56,19 +56,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 +102,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/autoware_default_adapi/src/compatibility/autoware_state.hpp b/system/autoware_default_adapi/src/compatibility/autoware_state.hpp index c292f2f62364c..52a23c049ea22 100644 --- a/system/autoware_default_adapi/src/compatibility/autoware_state.hpp +++ b/system/autoware_default_adapi/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/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index d89ef6c221666..01840e756cfb2 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -115,17 +115,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; @@ -142,6 +137,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/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 3a9b99e33a997..ae509129389bf 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -51,7 +51,6 @@ class PlanningNode : public rclcpp::Node using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase; using Trajectory = autoware::component_interface_specs::planning::Trajectory::Message; using KinematicState = autoware::component_interface_specs::localization::KinematicState::Message; - void on_trajectory(const Trajectory::ConstSharedPtr msg); void on_kinematic_state(const KinematicState::ConstSharedPtr msg); void on_timer(); diff --git a/system/autoware_default_adapi/src/vehicle.cpp b/system/autoware_default_adapi/src/vehicle.cpp index fc35efff523c2..a0df7f6873e63 100644 --- a/system/autoware_default_adapi/src/vehicle.cpp +++ b/system/autoware_default_adapi/src/vehicle.cpp @@ -67,14 +67,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(); }); @@ -90,53 +90,6 @@ uint8_t VehicleNode::mapping( } } -void VehicleNode::kinematic_state( - const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr - msg_ptr) -{ - kinematic_state_msgs_ = msg_ptr; -} - -void VehicleNode::acceleration_status( - const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr - msg_ptr) -{ - acceleration_msgs_ = msg_ptr; -} - -void VehicleNode::steering_status( - const autoware::component_interface_specs::vehicle::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 autoware::component_interface_specs::vehicle::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; @@ -190,6 +143,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/autoware_default_adapi/src/vehicle.hpp b/system/autoware_default_adapi/src/vehicle.hpp index 9bb83cb1613e9..839f68f2500bf 100644 --- a/system/autoware_default_adapi/src/vehicle.hpp +++ b/system/autoware_default_adapi/src/vehicle.hpp @@ -69,30 +69,6 @@ class VehicleNode : public rclcpp::Node autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr map_projector_info_; - void kinematic_state( - const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr - msg_ptr); - void acceleration_status( - const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr - msg_ptr); - void steering_status( - const autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr - msg_ptr); - void gear_status( - const autoware::component_interface_specs::vehicle::GearStatus::Message::ConstSharedPtr - msg_ptr); - void turn_indicator_status( - const autoware::component_interface_specs::vehicle::TurnIndicatorStatus::Message::ConstSharedPtr - msg_ptr); - void map_projector_info( - const autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr - msg_ptr); - void hazard_light_status( - const autoware::component_interface_specs::vehicle::HazardLightStatus::Message::ConstSharedPtr - msg_ptr); - void energy_status( - const autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr - msg_ptr); uint8_t mapping( std::unordered_map hash_map, uint8_t input, uint8_t default_value); void publish_kinematics();