feat(static_centerline_optimizer): generate centerline from ego's tra…

…jectory in bag (#6788)

* implement bag_ego_trajectory_based_centerline

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>

planning/route_handler/include/route_handler/route_handler.hpp

@@ -354,6 +354,7 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
   bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getClosestLanelets(const geometry_msgs::msg::Pose & target_pose) const;
 
 private:
   // MUST

planning/route_handler/src/route_handler.cpp

@@ -2286,4 +2286,11 @@ bool RouteHandler::findDrivableLanePath(
   return drivable_lane_path_found;
 }
 
+lanelet::ConstLanelets RouteHandler::getClosestLanelets(
+  const geometry_msgs::msg::Pose & target_pose) const
+{
+  lanelet::ConstLanelets target_lanelets;
+  lanelet::utils::query::getCurrentLanelets(road_lanelets_, target_pose, &target_lanelets);
+  return target_lanelets;
+}
 }  // namespace route_handler

planning/static_centerline_optimizer/CMakeLists.txt

@@ -22,6 +22,8 @@ autoware_package()
 ament_auto_add_executable(main
   src/main.cpp
   src/static_centerline_optimizer_node.cpp
+  src/centerline_source/optimization_trajectory_based_centerline.cpp
+  src/centerline_source/bag_ego_trajectory_based_centerline.cpp
   src/utils.cpp
 )
 

planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml

@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
-    centerline_source: "optimization_trajectory_base" # select from "optimization_trajectory_base" and "bag_ego_trajectory_base"
     marker_color: ["FF0000", "00FF00", "0000FF"]
     marker_color_dist_thresh : [0.1, 0.2, 0.3]
+    output_trajectory_interval: 1.0

planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp

@@ -0,0 +1,29 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "static_centerline_optimizer/type_alias.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace static_centerline_optimizer
+{
+std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node);
+}  // namespace static_centerline_optimizer
+#endif  // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_

planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp

@@ -0,0 +1,45 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
+#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
+
+#include "rclcpp/rclcpp.hpp"
+#include "static_centerline_optimizer/type_alias.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace static_centerline_optimizer
+{
+class OptimizationTrajectoryBasedCenterline
+{
+public:
+  OptimizationTrajectoryBasedCenterline() = default;
+  explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node);
+  std::vector<TrajectoryPoint> generate_centerline_with_optimization(
+    rclcpp::Node & node, const RouteHandler & route_handler,
+    const std::vector<lanelet::Id> & route_lane_ids);
+
+private:
+  std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path) const;
+
+  rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
+  rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
+};
+}  // namespace static_centerline_optimizer
+// clang-format off
+#endif  // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_  // NOLINT
+// clang-format on

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -16,6 +16,7 @@
 #define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp"
 #include "static_centerline_optimizer/srv/load_map.hpp"
 #include "static_centerline_optimizer/srv/plan_path.hpp"
 #include "static_centerline_optimizer/srv/plan_route.hpp"
@@ -25,6 +26,7 @@
 #include <geography_utils/lanelet2_projector.hpp>
 
 #include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float32.hpp"
 #include "std_msgs/msg/int32.hpp"
 
 #include <memory>
@@ -37,18 +39,19 @@ using static_centerline_optimizer::srv::LoadMap;
 using static_centerline_optimizer::srv::PlanPath;
 using static_centerline_optimizer::srv::PlanRoute;
 
+struct CenterlineWithRoute
+{
+  std::vector<TrajectoryPoint> centerline{};
+  std::vector<lanelet::Id> route_lane_ids{};
+};
+
 class StaticCenterlineOptimizerNode : public rclcpp::Node
 {
 public:
   explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options);
   void run();
 
 private:
-  struct CenterlineWithRoute
-  {
-    std::vector<TrajectoryPoint> centerline{};
-    std::vector<lanelet::Id> route_lane_ids{};
-  };
   // load map
   void load_map(const std::string & lanelet2_input_file_path);
   void on_load_map(
@@ -60,13 +63,12 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
   void on_plan_route(
     const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
 
-  // plan path
+  // plan centerline
   CenterlineWithRoute generate_centerline_with_route();
-  std::vector<TrajectoryPoint> plan_path(const std::vector<lanelet::Id> & route_lane_ids);
-  std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path) const;
   void on_plan_path(
     const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
 
+  void update_centerline_range(const int traj_start_index, const int traj_end_index);
   void evaluate(
     const std::vector<lanelet::Id> & route_lane_ids,
     const std::vector<TrajectoryPoint> & optimized_traj_points);
@@ -88,19 +90,19 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
     BagEgoTrajectoryBase,
   };
   CenterlineSource centerline_source_;
+  OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_;
 
   // publisher
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_{nullptr};
-  rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
-  rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_unsafe_footprints_{nullptr};
-  rclcpp::Publisher<Trajectory>::SharedPtr pub_whole_optimized_centerline_{nullptr};
-  rclcpp::Publisher<Trajectory>::SharedPtr pub_optimized_centerline_{nullptr};
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_whole_centerline_{nullptr};
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_centerline_{nullptr};
 
   // subscriber
   rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_traj_start_index_;
   rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_traj_end_index_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_save_map_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_traj_resample_interval_;
 
   // service
   rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;

planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp

@@ -28,6 +28,9 @@ namespace static_centerline_optimizer
 {
 namespace utils
 {
+lanelet::ConstLanelets get_lanelets_from_ids(
+  const RouteHandler & route_handler, const std::vector<lanelet::Id> & lane_ids);
+
 geometry_msgs::msg::Pose get_center_pose(
   const RouteHandler & route_handler, const size_t lanelet_id);
 

planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml

@@ -5,6 +5,9 @@
   <!-- 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="bag_filename" default="bag.db3"/>
 
   <!-- mandatory arguments when run_background is true -->
   <arg name="lanelet2_input_file_path" default=""/>
@@ -78,6 +81,8 @@
     <param from="$(var mission_planner_param)"/>
     <!-- node param -->
     <param from="$(find-pkg-share static_centerline_optimizer)/config/static_centerline_optimizer.param.yaml"/>
+    <param name="centerline_source" value="$(var centerline_source)"/>
+    <param name="bag_filename" value="$(var bag_filename)"/>
   </node>
 
   <!-- rviz -->

planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix static_centerline_optimizer --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus

planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus

planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp

@@ -0,0 +1,82 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp"
+
+#include "rclcpp/serialization.hpp"
+#include "rosbag2_cpp/reader.hpp"
+#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace static_centerline_optimizer
+{
+std::vector<TrajectoryPoint> generate_centerline_with_bag(rclcpp::Node & node)
+{
+  const auto bag_filename = node.declare_parameter<std::string>("bag_filename");
+
+  rosbag2_cpp::Reader bag_reader;
+  bag_reader.open(bag_filename);
+
+  rclcpp::Serialization<nav_msgs::msg::Odometry> bag_serialization;
+  std::vector<TrajectoryPoint> centerline_traj_points;
+  while (bag_reader.has_next()) {
+    const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next();
+
+    if (msg->topic_name != "/localization/kinematic_state") {
+      continue;
+    }
+
+    rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
+    const auto ros_msg = std::make_shared<nav_msgs::msg::Odometry>();
+
+    bag_serialization.deserialize_message(&serialized_msg, ros_msg.get());
+
+    if (!centerline_traj_points.empty()) {
+      constexpr double epsilon = 1e-1;
+      if (
+        std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) <
+          epsilon &&
+        std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) <
+          epsilon) {
+        continue;
+      }
+    }
+    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);
+  }
+
+  RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag.");
+
+  // calculate rough orientation of centerline trajectory points
+  for (size_t i = 0; i < centerline_traj_points.size(); ++i) {
+    if (i == centerline_traj_points.size() - 1) {
+      if (i != 0) {
+        centerline_traj_points.at(i).pose.orientation =
+          centerline_traj_points.at(i - 1).pose.orientation;
+      }
+    } else {
+      const double yaw_angle = tier4_autoware_utils::calcAzimuthAngle(
+        centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position);
+      centerline_traj_points.at(i).pose.orientation =
+        tier4_autoware_utils::createQuaternionFromYaw(yaw_angle);
+    }
+  }
+
+  return centerline_traj_points;
+}
+}  // namespace static_centerline_optimizer