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

refactor(static_centerline_optimizer): clean up the code #6794

Merged
merged 2 commits into from
Apr 12, 2024
Merged

refactor(static_centerline_optimizer): clean up the code #6794

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bc29a15
refactor(static_centerline_optimizer): clean up the code
takayuki5168 Apr 11, 2024
56af7eb
update
takayuki5168 Apr 11, 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
2 changes: 0 additions & 2 deletions ...ude/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,6 @@
#include "rclcpp/rclcpp.hpp"
#include "static_centerline_optimizer/type_alias.hpp"

#include <memory>
#include <string>
#include <vector>

namespace static_centerline_optimizer
Expand Down
2 changes: 0 additions & 2 deletions ...tatic_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,6 @@
#include "rclcpp/rclcpp.hpp"
#include "static_centerline_optimizer/type_alias.hpp"

#include <memory>
#include <string>
#include <vector>

namespace static_centerline_optimizer
Expand Down
4 changes: 2 additions & 2 deletions ...erline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

#include <memory>
#include <string>
#include <utility>
#include <vector>

namespace static_centerline_optimizer
Expand Down Expand Up @@ -81,8 +82,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
std::unique_ptr<lanelet::Projector> map_projector_{nullptr};

int traj_start_index_{0};
int traj_end_index_{0};
std::pair<int, int> traj_range_indices_{0, 0};
std::optional<CenterlineWithRoute> centerline_with_route_{std::nullopt};

enum class CenterlineSource {
Expand Down
2 changes: 2 additions & 0 deletions planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ namespace static_centerline_optimizer
{
namespace utils
{
rclcpp::QoS create_transient_local_qos();

lanelet::ConstLanelets get_lanelets_from_ids(
const RouteHandler & route_handler, const std::vector<lanelet::Id> & lane_ids);

Expand Down
3 changes: 1 addition & 2 deletions planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,7 @@
<!-- flag -->
<arg name="run_background" default="false"/>
<arg name="rviz" default="true"/>
<!-- <arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/> -->
<arg name="centerline_source" default="bag_ego_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
<arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
<arg name="bag_filename" default="bag.db3"/>

<!-- mandatory arguments when run_background is true -->
Expand Down
4 changes: 2 additions & 2 deletions ...static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,11 @@ std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node)
{
const auto bag_filename = node.declare_parameter<std::string>("bag_filename");

// open rosbag
rosbag2_cpp::Reader bag_reader;
bag_reader.open(bag_filename);

// extract 2D position of ego's trajectory from rosbag
rclcpp::Serialization<nav_msgs::msg::Odometry> bag_serialization;
std::vector<TrajectoryPoint> centerline_traj_points;
while (bag_reader.has_next()) {
Expand All @@ -55,8 +57,6 @@ std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node)
}
TrajectoryPoint centerline_traj_point;
centerline_traj_point.pose.position = ros_msg->pose.pose.position;
// std::cerr << centerline_traj_point.pose.position.x << " "
// << centerline_traj_point.pose.position.y << std::endl;
centerline_traj_points.push_back(centerline_traj_point);
}

Expand Down
27 changes: 9 additions & 18 deletions ...c_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "path_smoother/elastic_band_smoother.hpp"
#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp"
#include "static_centerline_optimizer/utils.hpp"
#include "tier4_autoware_utils/ros/parameter.hpp"

namespace static_centerline_optimizer
{
Expand All @@ -30,10 +31,6 @@ rclcpp::NodeOptions create_node_options()
return rclcpp::NodeOptions{};
}

rclcpp::QoS create_transient_local_qos()
{
return rclcpp::QoS{1}.transient_local();
}
Path convert_to_path(const PathWithLaneId & path_with_lane_id)
{
Path path;
Expand Down Expand Up @@ -66,8 +63,9 @@ Trajectory convert_to_trajectory(const Path & path)
OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node)
{
pub_raw_path_with_lane_id_ =
node.create_publisher<PathWithLaneId>("input_centerline", create_transient_local_qos());
pub_raw_path_ = node.create_publisher<Path>("debug/raw_centerline", create_transient_local_qos());
node.create_publisher<PathWithLaneId>("input_centerline", utils::create_transient_local_qos());
pub_raw_path_ =
node.create_publisher<Path>("debug/raw_centerline", utils::create_transient_local_qos());
}

std::vector<TrajectoryPoint>
Expand All @@ -82,20 +80,13 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization(

// get ego nearest search parameters and resample interval in behavior_path_planner
const double ego_nearest_dist_threshold =
node.has_parameter("ego_nearest_dist_threshold")
? node.get_parameter("ego_nearest_dist_threshold").as_double()
: node.declare_parameter<double>("ego_nearest_dist_threshold");
tier4_autoware_utils::getOrDeclareParameter<double>(node, "ego_nearest_dist_threshold");
const double ego_nearest_yaw_threshold =
node.has_parameter("ego_nearest_yaw_threshold")
? node.get_parameter("ego_nearest_yaw_threshold").as_double()
: node.declare_parameter<double>("ego_nearest_yaw_threshold");
const double behavior_path_interval = node.has_parameter("output_path_interval")
? node.get_parameter("output_path_interval").as_double()
: node.declare_parameter<double>("output_path_interval");
tier4_autoware_utils::getOrDeclareParameter<double>(node, "ego_nearest_yaw_threshold");
const double behavior_path_interval =
tier4_autoware_utils::getOrDeclareParameter<double>(node, "output_path_interval");
const double behavior_vel_interval =
node.has_parameter("behavior_output_path_interval")
? node.get_parameter("behavior_output_path_interval").as_double()
: node.declare_parameter<double>("behavior_output_path_interval");
tier4_autoware_utils::getOrDeclareParameter<double>(node, "behavior_output_path_interval");

// extract path with lane id from lanelets
const auto raw_path_with_lane_id = [&]() {
Expand Down
89 changes: 34 additions & 55 deletions planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,19 +57,6 @@
{
namespace
{
[[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route(
const RouteHandler & route_handler, const LaneletRoute & route)
{
lanelet::ConstLanelets lanelets;
for (const auto & segment : route.segments) {
const auto & target_lanelet_id = segment.preferred_primitive.id;
const auto target_lanelet = route_handler.getLaneletsFromId(target_lanelet_id);
lanelets.push_back(target_lanelet);
}

return lanelets;
}

std::vector<lanelet::Id> get_lane_ids_from_route(const LaneletRoute & route)
{
std::vector<lanelet::Id> lane_ids;
Expand All @@ -81,12 +68,7 @@
return lane_ids;
}

rclcpp::QoS create_transient_local_qos()
{
return rclcpp::QoS{1}.transient_local();
}

lanelet::BasicPoint2d convertToLaneletPoint(const geometry_msgs::msg::Point & geom_point)
lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point)
{
lanelet::BasicPoint2d point(geom_point.x, geom_point.y);
return point;
Expand Down Expand Up @@ -131,7 +113,7 @@
return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0);
}

std::array<double, 3> convertHexStringToDecimal(const std::string & hex_str_color)
std::array<double, 3> convert_hex_string_to_decimal(const std::string & hex_str_color)
{
unsigned int hex_int_color;
std::istringstream iss(hex_str_color);
Expand Down Expand Up @@ -165,15 +147,15 @@
return unconnected_lane_ids;
}

std_msgs::msg::Header createHeader(const rclcpp::Time & now)
std_msgs::msg::Header create_header(const rclcpp::Time & now)
{
std_msgs::msg::Header header;
header.frame_id = "map";
header.stamp = now;
return header;
}

std::vector<TrajectoryPoint> resampleTrajectoryPoints(
std::vector<TrajectoryPoint> resample_trajectory_points(
const std::vector<TrajectoryPoint> & input_traj_points, const double resample_interval)
{
// resample and calculate trajectory points' orientation
Expand All @@ -188,39 +170,38 @@
: Node("static_centerline_optimizer", node_options)
{
// publishers
pub_map_bin_ = create_publisher<HADMapBin>("lanelet2_map_topic", create_transient_local_qos());
pub_map_bin_ =
create_publisher<HADMapBin>("lanelet2_map_topic", utils::create_transient_local_qos());
pub_whole_centerline_ =
create_publisher<Trajectory>("output_whole_centerline", create_transient_local_qos());
pub_centerline_ = create_publisher<Trajectory>("output_centerline", create_transient_local_qos());
create_publisher<Trajectory>("output_whole_centerline", utils::create_transient_local_qos());
pub_centerline_ =
create_publisher<Trajectory>("output_centerline", utils::create_transient_local_qos());

// debug publishers
pub_debug_unsafe_footprints_ =
create_publisher<MarkerArray>("debug/unsafe_footprints", create_transient_local_qos());
create_publisher<MarkerArray>("debug/unsafe_footprints", utils::create_transient_local_qos());

// subscribers
sub_traj_start_index_ = create_subscription<std_msgs::msg::Int32>(
"/centerline_updater_helper/traj_start_index", rclcpp::QoS{1},
[this](const std_msgs::msg::Int32 & msg) {
if (msg.data <= traj_end_index_ + 1) {
update_centerline_range(msg.data, traj_end_index_);
}
update_centerline_range(msg.data, traj_range_indices_.second);
});
sub_traj_end_index_ = create_subscription<std_msgs::msg::Int32>(
"/centerline_updater_helper/traj_end_index", rclcpp::QoS{1},
[this](const std_msgs::msg::Int32 & msg) {
if (traj_start_index_ <= msg.data + 1) {
update_centerline_range(traj_start_index_, msg.data);
}
update_centerline_range(traj_range_indices_.first, msg.data);
});
sub_save_map_ = create_subscription<std_msgs::msg::Bool>(
"/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) {
const auto lanelet2_output_file_path =
tier4_autoware_utils::getOrDeclareParameter<std::string>(
*this, "lanelet2_output_file_path");
if (!centerline_with_route_ || msg.data) {
const auto & c = *centerline_with_route_;
const auto selected_centerline = std::vector<TrajectoryPoint>(
c.centerline.begin() + traj_start_index_, c.centerline.begin() + traj_end_index_ + 1);
c.centerline.begin() + traj_range_indices_.first,
c.centerline.begin() + traj_range_indices_.second + 1);

Check notice on line 204 in planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode decreases from 73 to 72 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
save_map(
lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids});
}
Expand Down Expand Up @@ -271,19 +252,19 @@
void StaticCenterlineOptimizerNode::update_centerline_range(
const int traj_start_index, const int traj_end_index)
{
if (!centerline_with_route_) {
if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) {
return;
}

traj_start_index_ = traj_start_index;
traj_end_index_ = traj_end_index;
traj_range_indices_ = std::make_pair(traj_start_index, traj_end_index);

const auto & centerline = centerline_with_route_->centerline;
const auto selected_centerline = std::vector<TrajectoryPoint>(
centerline.begin() + traj_start_index_, centerline.begin() + traj_end_index_ + 1);
centerline.begin() + traj_range_indices_.first,
centerline.begin() + traj_range_indices_.second + 1);

pub_centerline_->publish(
motion_utils::convertToTrajectory(selected_centerline, createHeader(this->now())));
motion_utils::convertToTrajectory(selected_centerline, create_header(this->now())));
}

void StaticCenterlineOptimizerNode::run()
Expand All @@ -296,8 +277,7 @@
// process
load_map(lanelet2_input_file_path);
const auto centerline_with_route = generate_centerline_with_route();
traj_start_index_ = 0;
traj_end_index_ = centerline_with_route.centerline.size() - 1;
traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1);
save_map(lanelet2_output_file_path, centerline_with_route);

centerline_with_route_ = centerline_with_route;
Expand All @@ -310,6 +290,7 @@
return CenterlineWithRoute{};
}

// generate centerline with route
auto centerline_with_route = [&]() {
if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) {
const lanelet::Id start_lanelet_id = declare_parameter<int64_t>("start_lanelet_id");
Expand Down Expand Up @@ -339,15 +320,16 @@
"The centerline source is not supported in static_centerline_optimizer.");
}();

// resample
const double output_trajectory_interval = declare_parameter<double>("output_trajectory_interval");
centerline_with_route.centerline =
resampleTrajectoryPoints(centerline_with_route.centerline, output_trajectory_interval);
resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval);

pub_whole_centerline_->publish(
motion_utils::convertToTrajectory(centerline_with_route.centerline, createHeader(this->now())));
pub_whole_centerline_->publish(motion_utils::convertToTrajectory(
centerline_with_route.centerline, create_header(this->now())));

pub_centerline_->publish(
motion_utils::convertToTrajectory(centerline_with_route.centerline, createHeader(this->now())));
pub_centerline_->publish(motion_utils::convertToTrajectory(
centerline_with_route.centerline, create_header(this->now())));

return centerline_with_route;
}
Expand Down Expand Up @@ -537,8 +519,8 @@
std::vector<geometry_msgs::msg::Point> current_lanelet_points;

// check if target point is inside the lanelet
while (
lanelet::geometry::inside(lanelet, convertToLaneletPoint(target_traj_point->pose.position))) {
while (lanelet::geometry::inside(
lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) {
// memorize points inside the lanelet
current_lanelet_points.push_back(target_traj_point->pose.position);
target_traj_point++;
Expand Down Expand Up @@ -571,18 +553,15 @@
const std::vector<TrajectoryPoint> & optimized_traj_points)
{
const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids);
const auto dist_thresh_vec =
has_parameter("marker_color_dist_thresh")
? get_parameter("marker_color_dist_thresh").as_double_array()
: declare_parameter<std::vector<double>>("marker_color_dist_thresh");
const auto marker_color_vec = has_parameter("marker_color")
? get_parameter("marker_color").as_string_array()
: declare_parameter<std::vector<std::string>>("marker_color");
const auto dist_thresh_vec = tier4_autoware_utils::getOrDeclareParameter<std::vector<double>>(
*this, "marker_color_dist_thresh");
const auto marker_color_vec =
tier4_autoware_utils::getOrDeclareParameter<std::vector<std::string>>(*this, "marker_color");
const auto get_marker_color = [&](const double dist) -> boost::optional<std::array<double, 3>> {
for (size_t i = 0; i < dist_thresh_vec.size(); ++i) {
const double dist_thresh = dist_thresh_vec.at(i);
if (dist < dist_thresh) {
return convertHexStringToDecimal(marker_color_vec.at(i));
return convert_hex_string_to_decimal(marker_color_vec.at(i));

Check notice on line 564 in planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

StaticCenterlineOptimizerNode::evaluate decreases in cyclomatic complexity from 11 to 9, 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.
}
}
return boost::none;
Expand Down
5 changes: 5 additions & 0 deletions planning/static_centerline_optimizer/src/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,11 @@ lanelet::Point3d createPoint3d(const double x, const double y, const double z =

namespace utils
{
rclcpp::QoS create_transient_local_qos()
{
return rclcpp::QoS{1}.transient_local();
}

lanelet::ConstLanelets get_lanelets_from_ids(
const RouteHandler & route_handler, const std::vector<lanelet::Id> & lane_ids)
{
Expand Down
Loading