refactor(static_centerline_optimizer): clean up the code (autowarefou…

…ndation#6794) * refactor(static_centerline_optimizer): clean up the code Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Interplai · Jun 3, 2024 · e7b247b · e7b247b
1 parent cb9faf3
commit e7b247b
Show file tree

Hide file tree

Showing 9 changed files with 55 additions and 83 deletions.
diff --git a/...ude/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp b/...ude/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 <memory>
-#include <string>
 #include <vector>
 
 namespace static_centerline_optimizer

diff --git a/...tatic_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp b/...tatic_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 <memory>
-#include <string>
 #include <vector>
 
 namespace static_centerline_optimizer

diff --git a/...erline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/...erline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp
@@ -31,6 +31,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace static_centerline_optimizer
@@ -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 {

diff --git 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<lanelet::Id> & 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
@@ -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 -->

diff --git a/...static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/...static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp
@@ -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()) {
@@ -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);
   }
 

diff --git a/...c_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp b/...c_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<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>
@@ -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 = [&]() {

diff --git 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<lanelet::Id> get_lane_ids_from_route(const LaneletRoute & route)
 {
   std::vector<lanelet::Id> lane_ids;
@@ -81,12 +68,7 @@ std::vector<lanelet::Id> 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<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);
@@ -165,15 +147,15 @@ std::vector<lanelet::Id> 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";
   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
@@ -188,29 +170,27 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode(
 : 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) {
@@ -220,7 +200,8 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode(
       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);
         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<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()
@@ -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<int64_t>("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<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;
 }
@@ -537,8 +519,8 @@ void StaticCenterlineOptimizerNode::on_plan_path(
     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++;
@@ -571,18 +553,15 @@ void StaticCenterlineOptimizerNode::evaluate(
   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));
       }
     }
     return boost::none;

diff --git 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<lanelet::Id> & lane_ids)
 {