Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(default_ad_api_helpers): use polling subscriber #7416

Closed
wants to merge 5 commits into from
Closed

feat(default_ad_api_helpers): use polling subscriber #7416

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
3b2b9a8
replace subscriber
zusizusi Jun 10, 2024
3586a0d
replace subscriber
zusizusi Jun 10, 2024
54c7ecb
replace subscriber
zusizusi Jun 10, 2024
8c9fe9c
add previous_timer_time_ variable
zusizusi Jun 10, 2024
db6da56
delete unused functions
zusizusi Jun 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions system/default_ad_api_helpers/ad_api_adaptors/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<depend>map_height_fitter</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_autoware_utils</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
68 changes: 38 additions & 30 deletions system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,14 +24,8 @@
{
using std::placeholders::_1;

sub_fixed_goal_ = create_subscription<PoseStamped>(
"~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1));
sub_rough_goal_ = create_subscription<PoseStamped>(
"~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1));
sub_reroute_ = create_subscription<PoseStamped>(
"~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1));
sub_waypoint_ = create_subscription<PoseStamped>(
"~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1));

const auto adaptor = component_interface_utils::NodeAdaptor(this);
adaptor.init_cli(cli_reroute_);
Expand All @@ -50,6 +44,39 @@

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();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change reduces waypoint throughput. Only one waypoint can be added per timer period. Also, information about the order of arrival of waypoints and goals is lost. Waypoints and goal must be processed in the order in which they arrive.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for taking the time to review my PR.
I understand your concerns regarding the usage of take().
Therefore, I will close this PR.

if (
fixed_goal_msg && rough_goal_msg &&
rclcpp::Time(fixed_goal_msg->header.stamp) > previous_timer_time_ &&

Check warning on line 53 in system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional 

RoutingAdaptor::on_timer has 1 complex conditionals with 3 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
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;

Check warning on line 79 in system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

RoutingAdaptor::on_timer has a cyclomatic complexity of 18, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 79 in system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

RoutingAdaptor::on_timer has 3 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
// 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))
Expand All @@ -73,32 +100,13 @@
}
}

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)
Expand Down
17 changes: 11 additions & 6 deletions system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <autoware_ad_api_specs/routing.hpp>
#include <component_interface_utils/rclcpp.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

Expand All @@ -41,21 +42,25 @@ class RoutingAdaptor : public rclcpp::Node
component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
component_interface_utils::Subscription<RouteState>::SharedPtr sub_state_;
rclcpp::Subscription<PoseStamped>::SharedPtr sub_fixed_goal_;
rclcpp::Subscription<PoseStamped>::SharedPtr sub_rough_goal_;
rclcpp::Subscription<PoseStamped>::SharedPtr sub_waypoint_;
rclcpp::Subscription<PoseStamped>::SharedPtr sub_reroute_;
// Subscriber without callback
tier4_autoware_utils::InterProcessPollingSubscriber<PoseStamped> sub_fixed_goal_{
this, "~/input/fixed_goal"};
tier4_autoware_utils::InterProcessPollingSubscriber<PoseStamped> sub_rough_goal_{
this, "~/input/rough_goal"};
tier4_autoware_utils::InterProcessPollingSubscriber<PoseStamped> 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;
SetRoutePoints::Service::Request::SharedPtr route_;
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);
};

Expand Down
Loading