Feat/save centerline from bag

planning/route_handler/include/route_handler/route_handler.hpp

@@ -353,6 +353,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

@@ -2272,4 +2272,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

@@ -28,23 +28,45 @@ ament_auto_add_executable(main
   src/utils.cpp
 )
 
+ament_auto_add_executable(main2
+  src/main2.cpp
+  src/bag_centerline_generator_node.cpp
+  src/successive_trajectory_optimizer_node.cpp
+  src/utils.cpp
+)
+
 target_include_directories(main
   SYSTEM PUBLIC
     ${OpenCV_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIR}
 )
 
+target_include_directories(main2
+  SYSTEM PUBLIC
+    ${OpenCV_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+)
+
 target_link_libraries(main
   ${OpenCV_LIBRARIES}
 )
 
+target_link_libraries(main2
+  ${OpenCV_LIBRARIES}
+)
+
 if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
     rosidl_target_interfaces(main
     static_centerline_optimizer "rosidl_typesupport_cpp")
+
+    rosidl_target_interfaces(main2
+    static_centerline_optimizer "rosidl_typesupport_cpp")
 else()
     rosidl_get_typesupport_target(
             cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp")
     target_link_libraries(main "${cpp_typesupport_target}")
+
+    target_link_libraries(main2 "${cpp_typesupport_target}")
 endif()
 
 ament_auto_package(

planning/static_centerline_optimizer/include/static_centerline_optimizer/bag_centerline_generator_node.hpp

@@ -0,0 +1,91 @@
+// Copyright 2022 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__BAG_CENTERLINE_GENERATOR_NODE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__BAG_CENTERLINE_GENERATOR_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/serialization.hpp"
+#include "rosbag2_cpp/reader.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"
+#include "static_centerline_optimizer/type_alias.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace static_centerline_optimizer
+{
+using static_centerline_optimizer::srv::LoadMap;
+using static_centerline_optimizer::srv::PlanPath;
+using static_centerline_optimizer::srv::PlanRoute;
+
+class BagCenterlineGeneratorNode : public rclcpp::Node
+{
+public:
+  explicit BagCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options);
+  void run();
+
+private:
+  std::vector<TrajectoryPoint> generateCenterlineFromBag();
+
+  // load map
+  void load_map(const std::string & lanelet2_input_file_path);
+  void on_load_map(
+    const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
+
+  // plan route
+  std::vector<lanelet::Id> plan_route(
+    const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id);
+  void on_plan_route(
+    const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
+
+  // plan path
+  std::vector<TrajectoryPoint> plan_path(const std::vector<lanelet::Id> & route_lane_ids);
+  void on_plan_path(
+    const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
+
+  void evaluate(
+    const std::vector<lanelet::Id> & route_lane_ids,
+    const std::vector<TrajectoryPoint> & optimized_traj_points);
+  void save_map(
+    const std::string & lanelet2_output_file_path, const std::vector<lanelet::Id> & route_lane_ids,
+    const std::vector<TrajectoryPoint> & optimized_traj_points);
+
+  HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
+  std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
+
+  // 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_optimized_centerline_{nullptr};
+
+  // service
+  rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;
+  rclcpp::Service<PlanRoute>::SharedPtr srv_plan_route_;
+  rclcpp::Service<PlanPath>::SharedPtr srv_plan_path_;
+
+  // callback group for service
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
+  // vehicle info
+  vehicle_info_util::VehicleInfo vehicle_info_;
+};
+}  // namespace static_centerline_optimizer
+#endif  // STATIC_CENTERLINE_OPTIMIZER__BAG_CENTERLINE_GENERATOR_NODE_HPP_

planning/static_centerline_optimizer/launch/bag_centerline_generator.launch.xml

@@ -0,0 +1,72 @@
+<launch>
+  <!-- mandatory arguments for planning-->
+  <arg name="vehicle_model"/>
+
+  <!-- flag -->
+  <arg name="run_background" default="false"/>
+  <arg name="rviz" default="true"/>
+
+  <!-- mandatory arguments when run_background is true -->
+  <arg name="lanelet2_input_file_path" default=""/>
+  <arg name="lanelet2_output_file_path" default="/tmp/lanelet2_map.osm"/>
+  <arg name="start_lanelet_id" default=""/>
+  <arg name="end_lanelet_id" default=""/>
+
+  <!-- topic -->
+  <arg name="lanelet2_map_topic" default="/map/vector_map"/>
+  <arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
+  <!-- param -->
+  <arg name="map_loader_param" default="$(find-pkg-share map_loader)/config/lanelet2_map_loader.param.yaml"/>
+  <arg
+    name="obstacle_avoidance_planner_param"
+    default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml"
+  />
+  <arg name="mission_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
+
+  <!-- Global parameters (for PathFootprint in tier4_planning_rviz_plugin) -->
+  <!-- Do not add "group" in order to propagate global parameters -->
+  <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
+    <arg name="vehicle_model" value="$(var vehicle_model)"/>
+  </include>
+
+  <!-- generate tf from "viewer" to "map" -->
+  <node pkg="map_tf_generator" exec="vector_map_tf_generator" name="vector_map_tf_generator">
+    <remap from="vector_map" to="$(var lanelet2_map_topic)"/>
+
+    <param name="map_frame" value="map"/>
+    <param name="viewer_frame" value="viewer"/>
+  </node>
+
+  <!-- visualize map -->
+  <node pkg="map_loader" exec="lanelet2_map_visualization" name="lanelet2_map_visualization">
+    <remap from="input/lanelet2_map" to="$(var lanelet2_map_topic)"/>
+    <remap from="output/lanelet2_map_marker" to="$(var lanelet2_map_marker_topic)"/>
+  </node>
+
+  <!-- optimize path -->
+  <node pkg="static_centerline_optimizer" exec="main2" name="bag_centerline_generator">
+    <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
+    <remap from="input_centerline" to="~/input_centerline"/>
+    <remap from="output_centerline" to="~/output_centerline"/>
+    <remap from="debug/raw_centerline" to="~/debug/raw_centerline"/>
+    <remap from="debug/unsafe_footprints" to="~/debug/unsafe_footprints"/>
+
+    <param name="run_background" value="$(var run_background)"/>
+    <param name="lanelet2_input_file_path" value="$(var lanelet2_input_file_path)"/>
+    <param name="lanelet2_output_file_path" value="$(var lanelet2_output_file_path)"/>
+    <param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
+    <param name="end_lanelet_id" value="$(var end_lanelet_id)"/>
+    <!-- common param -->
+    <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/common.param.yaml"/>
+    <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml"/>
+    <!-- component param -->
+    <param from="$(var map_loader_param)"/>
+    <param from="$(var obstacle_avoidance_planner_param)"/>
+    <param from="$(var mission_planner_param)"/>
+    <!-- node param -->
+    <param from="$(find-pkg-share static_centerline_optimizer)/config/static_centerline_optimizer.param.yaml"/>
+  </node>
+
+  <!-- rviz -->
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share static_centerline_optimizer)/rviz/static_centerline_optimizer.rviz" if="$(var rviz)"/>
+</launch>

planning/static_centerline_optimizer/package.xml

@@ -24,6 +24,7 @@
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>map_loader</depend>
+  <depend>map_projection_loader</depend>
   <depend>mission_planner</depend>
   <depend>motion_utils</depend>
   <depend>obstacle_avoidance_planner</depend>
@@ -36,6 +37,8 @@
   <depend>vehicle_info_util</depend>
 
   <exec_depend>python3-flask-cors</exec_depend>
+  <exec_depend>python3-mgrs</exec_depend>
+  <exec_depend>python3-xmltodict</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>