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);
};