diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index b070131f1d567..90ef9e45c5439 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -18,6 +18,7 @@ map_height_fitter rclcpp rclcpp_components + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp index 18421c976cf41..a2a4cb68c8278 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp @@ -24,14 +24,8 @@ RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options) { using std::placeholders::_1; - sub_fixed_goal_ = create_subscription( - "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1)); - sub_rough_goal_ = create_subscription( - "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1)); sub_reroute_ = create_subscription( "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1)); - sub_waypoint_ = create_subscription( - "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1)); const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_cli(cli_reroute_); @@ -50,6 +44,39 @@ RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options) void RoutingAdaptor::on_timer() { + const rclcpp::Time current_time = this->get_clock()->now(); + auto fixed_goal_msg = sub_fixed_goal_.takeData(); + auto rough_goal_msg = sub_rough_goal_.takeData(); + auto waypoint_msg = sub_waypoint_.takeData(); + if ( + fixed_goal_msg && rough_goal_msg && + rclcpp::Time(fixed_goal_msg->header.stamp) > previous_timer_time_ && + rclcpp::Time(rough_goal_msg->header.stamp) > previous_timer_time_) { + if (rclcpp::Time(fixed_goal_msg->header.stamp) > rclcpp::Time(rough_goal_msg->header.stamp)) { + request_timing_control_ = 1; + set_route_from_goal(fixed_goal_msg, false); + } else { + request_timing_control_ = 1; + set_route_from_goal(rough_goal_msg, true); + } + } else if (fixed_goal_msg && rclcpp::Time(fixed_goal_msg->header.stamp) > previous_timer_time_) { + request_timing_control_ = 1; + set_route_from_goal(fixed_goal_msg, false); + } else if (rough_goal_msg && rclcpp::Time(rough_goal_msg->header.stamp) > previous_timer_time_) { + request_timing_control_ = 1; + set_route_from_goal(rough_goal_msg, true); + } + if (waypoint_msg && rclcpp::Time(waypoint_msg->header.stamp) > previous_timer_time_) { + if (route_->header.frame_id != waypoint_msg->header.frame_id) { + RCLCPP_ERROR_STREAM(get_logger(), "The waypoint frame does not match the goal."); + previous_timer_time_ = current_time; + return; + } + request_timing_control_ = 1; + route_->waypoints.push_back(waypoint_msg->pose); + } + previous_timer_time_ = current_time; + // Wait a moment to combine consecutive goals and checkpoints into a single request. // This value is rate dependent and set the wait time for merging. constexpr int delay_count = 3; // 0.4 seconds (rate * (value - 1)) @@ -73,32 +100,13 @@ void RoutingAdaptor::on_timer() } } -void RoutingAdaptor::on_fixed_goal(const PoseStamped::ConstSharedPtr pose) +void RoutingAdaptor::set_route_from_goal( + const PoseStamped::ConstSharedPtr msg, const bool allow_goal_modification) { - request_timing_control_ = 1; - route_->header = pose->header; - route_->goal = pose->pose; + route_->header = msg->header; + route_->goal = msg->pose; route_->waypoints.clear(); - route_->option.allow_goal_modification = false; -} - -void RoutingAdaptor::on_rough_goal(const PoseStamped::ConstSharedPtr pose) -{ - request_timing_control_ = 1; - route_->header = pose->header; - route_->goal = pose->pose; - route_->waypoints.clear(); - route_->option.allow_goal_modification = true; -} - -void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose) -{ - if (route_->header.frame_id != pose->header.frame_id) { - RCLCPP_ERROR_STREAM(get_logger(), "The waypoint frame does not match the goal."); - return; - } - request_timing_control_ = 1; - route_->waypoints.push_back(pose->pose); + route_->option.allow_goal_modification = allow_goal_modification; } void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp index 877d705825733..91c62062a1bf0 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -41,11 +42,16 @@ class RoutingAdaptor : public rclcpp::Node component_interface_utils::Client::SharedPtr cli_route_; component_interface_utils::Client::SharedPtr cli_clear_; component_interface_utils::Subscription::SharedPtr sub_state_; - rclcpp::Subscription::SharedPtr sub_fixed_goal_; - rclcpp::Subscription::SharedPtr sub_rough_goal_; - rclcpp::Subscription::SharedPtr sub_waypoint_; rclcpp::Subscription::SharedPtr sub_reroute_; + // Subscriber without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_fixed_goal_{ + this, "~/input/fixed_goal"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_rough_goal_{ + this, "~/input/rough_goal"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_waypoint_{ + this, "~/input/waypoint"}; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time previous_timer_time_ = rclcpp::Time(0); bool calling_service_ = false; int request_timing_control_ = 0; @@ -53,9 +59,8 @@ class RoutingAdaptor : public rclcpp::Node RouteState::Message::_state_type state_; void on_timer(); - void on_fixed_goal(const PoseStamped::ConstSharedPtr pose); - void on_rough_goal(const PoseStamped::ConstSharedPtr pose); - void on_waypoint(const PoseStamped::ConstSharedPtr pose); + void set_route_from_goal( + const PoseStamped::ConstSharedPtr pose, const bool allow_goal_modification); void on_reroute(const PoseStamped::ConstSharedPtr pose); };