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(autoware_lane_departure_checker): use polling subscriber #7358

Merged
merged 4 commits into from
Jun 10, 2024
Merged

feat(autoware_lane_departure_checker): use polling subscriber #7358

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
04dcc78
add prefix autoware_ to lane_departure_checker package
kyoichi-sugahara Jun 6, 2024
9c2d0d1
revert variable name
kyoichi-sugahara Jun 6, 2024
961a4a6
style(pre-commit): autofix
pre-commit-ci[bot] Jun 7, 2024
3041876
use polling subscriber
kyoichi-sugahara Jun 7, 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
16 changes: 11 additions & 5 deletions ...departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_

#include "autoware_lane_departure_checker/lane_departure_checker.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -66,11 +67,16 @@ class LaneDepartureCheckerNode : public rclcpp::Node

private:
// Subscriber
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_lanelet_map_bin_;
rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_trajectory_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_trajectory_;
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odom_{
this, "~/input/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<LaneletMapBin> sub_lanelet_map_bin_{
this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()};
tier4_autoware_utils::InterProcessPollingSubscriber<LaneletRoute> sub_route_{
this, "~/input/route"};
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_trajectory_{
this, "~/input/reference_trajectory"};
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_trajectory_{
this, "~/input/predicted_trajectory"};

// Data Buffer
nav_msgs::msg::Odometry::ConstSharedPtr current_odom_;
Expand Down
62 changes: 16 additions & 46 deletions ...ol/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 4.29 to 5.21, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.

Check notice on line 1 in control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession 

The ratio of primitive types in function arguments increases from 35.48% to 42.31%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -169,22 +169,6 @@
lane_departure_checker_ = std::make_unique<LaneDepartureChecker>();
lane_departure_checker_->setParam(param_, vehicle_info);

// Subscriber
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
"~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1));
sub_lanelet_map_bin_ = this->create_subscription<LaneletMapBin>(
"~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(),
std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1));
sub_route_ = this->create_subscription<LaneletRoute>(
"~/input/route", rclcpp::QoS{1}.transient_local(),
std::bind(&LaneDepartureCheckerNode::onRoute, this, _1));
sub_reference_trajectory_ = this->create_subscription<Trajectory>(
"~/input/reference_trajectory", 1,
std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1));
sub_predicted_trajectory_ = this->create_subscription<Trajectory>(
"~/input/predicted_trajectory", 1,
std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1));

// Publisher
// Nothing

Expand All @@ -201,36 +185,6 @@
this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this));
}

void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
current_odom_ = msg;
}

void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg)
{
lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_);

// get all shoulder lanes
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
}

void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
{
route_ = msg;
}

void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg)
{
reference_trajectory_ = msg;
}

void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg)
{
predicted_trajectory_ = msg;
}

bool LaneDepartureCheckerNode::isDataReady()
{
if (!current_odom_) {
Expand Down Expand Up @@ -300,6 +254,22 @@
tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
stop_watch.tic("Total");

current_odom_ = sub_odom_.takeData();
route_ = sub_route_.takeData();
reference_trajectory_ = sub_reference_trajectory_.takeData();
predicted_trajectory_ = sub_predicted_trajectory_.takeData();

const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData();
if (lanelet_map_bin_msg) {
lanelet_map_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(
*lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_);

// get all shoulder lanes
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_);
shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
}

Check warning on line 272 in control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

LaneDepartureCheckerNode::onTimer increases in cyclomatic complexity from 10 to 11, 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.
if (!isDataReady()) {
return;
}
Expand Down
Loading