From 957e55346df22260c30d6edeb655679d2035fed3 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 12 Apr 2024 10:10:46 +0900 Subject: [PATCH] refactor(static_centerline_optimizer): clean up the code (#6794) * refactor(static_centerline_optimizer): clean up the code Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../bag_ego_trajectory_based_centerline.hpp | 2 - ...timization_trajectory_based_centerline.hpp | 2 - .../static_centerline_optimizer_node.hpp | 4 +- .../static_centerline_optimizer/utils.hpp | 2 + .../static_centerline_optimizer.launch.xml | 3 +- .../bag_ego_trajectory_based_centerline.cpp | 4 +- ...timization_trajectory_based_centerline.cpp | 27 ++---- .../src/static_centerline_optimizer_node.cpp | 89 +++++++------------ .../static_centerline_optimizer/src/utils.cpp | 5 ++ 9 files changed, 55 insertions(+), 83 deletions(-) diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp index db954b6ccbd47..b29f222b068cc 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -18,8 +18,6 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_optimizer/type_alias.hpp" -#include -#include #include namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp index 6e52ff98d78da..8c7c1e69e9ce6 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp @@ -18,8 +18,6 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_optimizer/type_alias.hpp" -#include -#include #include namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index 242a76ae0d94f..5d9530a469012 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp @@ -31,6 +31,7 @@ #include #include +#include #include namespace static_centerline_optimizer @@ -81,8 +82,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_{nullptr}; - int traj_start_index_{0}; - int traj_end_index_{0}; + std::pair traj_range_indices_{0, 0}; std::optional centerline_with_route_{std::nullopt}; enum class CenterlineSource { diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp index 52dcd171e8658..04f2268aa972f 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp @@ -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 & lane_ids); diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index 10718150d1751..ed137367f3ba4 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -5,8 +5,7 @@ - - + diff --git a/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 150218b498ae7..571e98cdbe1c3 100644 --- a/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -26,9 +26,11 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) { const auto bag_filename = node.declare_parameter("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 bag_serialization; std::vector centerline_traj_points; while (bag_reader.has_next()) { @@ -55,8 +57,6 @@ std::vector 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); } diff --git a/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp index 5bce91be722ca..20b2027f39d54 100644 --- a/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -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 { @@ -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; @@ -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("input_centerline", create_transient_local_qos()); - pub_raw_path_ = node.create_publisher("debug/raw_centerline", create_transient_local_qos()); + node.create_publisher("input_centerline", utils::create_transient_local_qos()); + pub_raw_path_ = + node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); } std::vector @@ -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("ego_nearest_dist_threshold"); + tier4_autoware_utils::getOrDeclareParameter(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("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("output_path_interval"); + tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + const double behavior_path_interval = + tier4_autoware_utils::getOrDeclareParameter(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("behavior_output_path_interval"); + tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 4f729ab5c8c57..073d30f5b5a39 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -57,19 +57,6 @@ namespace static_centerline_optimizer { 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 get_lane_ids_from_route(const LaneletRoute & route) { std::vector lane_ids; @@ -81,12 +68,7 @@ std::vector get_lane_ids_from_route(const LaneletRoute & route) 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; @@ -131,7 +113,7 @@ geometry_msgs::msg::Pose get_text_pose( return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } -std::array convertHexStringToDecimal(const std::string & hex_str_color) +std::array convert_hex_string_to_decimal(const std::string & hex_str_color) { unsigned int hex_int_color; std::istringstream iss(hex_str_color); @@ -165,7 +147,7 @@ std::vector check_lanelet_connection( 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"; @@ -173,7 +155,7 @@ std_msgs::msg::Header createHeader(const rclcpp::Time & now) return header; } -std::vector resampleTrajectoryPoints( +std::vector resample_trajectory_points( const std::vector & input_traj_points, const double resample_interval) { // resample and calculate trajectory points' orientation @@ -188,29 +170,27 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( : Node("static_centerline_optimizer", node_options) { // publishers - pub_map_bin_ = create_publisher("lanelet2_map_topic", create_transient_local_qos()); + pub_map_bin_ = + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = - create_publisher("output_whole_centerline", create_transient_local_qos()); - pub_centerline_ = create_publisher("output_centerline", create_transient_local_qos()); + create_publisher("output_whole_centerline", utils::create_transient_local_qos()); + pub_centerline_ = + create_publisher("output_centerline", utils::create_transient_local_qos()); // debug publishers pub_debug_unsafe_footprints_ = - create_publisher("debug/unsafe_footprints", create_transient_local_qos()); + create_publisher("debug/unsafe_footprints", utils::create_transient_local_qos()); // subscribers sub_traj_start_index_ = create_subscription( "/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( "/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( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { @@ -220,7 +200,8 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; const auto selected_centerline = std::vector( - 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); save_map( lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids}); } @@ -271,19 +252,19 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( 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( - 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() @@ -296,8 +277,7 @@ void StaticCenterlineOptimizerNode::run() // 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; @@ -310,6 +290,7 @@ CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_rout return CenterlineWithRoute{}; } + // generate centerline with route auto centerline_with_route = [&]() { if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); @@ -339,15 +320,16 @@ CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_rout "The centerline source is not supported in static_centerline_optimizer."); }(); + // resample const double output_trajectory_interval = declare_parameter("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; } @@ -537,8 +519,8 @@ void StaticCenterlineOptimizerNode::on_plan_path( std::vector 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++; @@ -571,18 +553,15 @@ void StaticCenterlineOptimizerNode::evaluate( const std::vector & 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>("marker_color_dist_thresh"); - const auto marker_color_vec = has_parameter("marker_color") - ? get_parameter("marker_color").as_string_array() - : declare_parameter>("marker_color"); + const auto dist_thresh_vec = tier4_autoware_utils::getOrDeclareParameter>( + *this, "marker_color_dist_thresh"); + const auto marker_color_vec = + tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { 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)); } } return boost::none; diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 3cc7ed5ca1fda..9448677d1bbae 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -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 & lane_ids) {