feat(static centerline optimizer): new package (autowarefoundation#2013)

* feat(mission_planner): add another initialization function

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* feat(static_centerline_optimizer): add a new package

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update README.md

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* refactor

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix spell

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* revert mission_planner's change

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix png path

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* remove unnecessary cerr

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix colcon test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update cmake version

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* tmp fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix CI error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix CI error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: kminoda <koji.minoda@tier4.jp>

planning/static_centerline_optimizer/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.14)
+project(static_centerline_optimizer)
+
+find_package(autoware_cmake REQUIRED)
+
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+rosidl_generate_interfaces(
+  static_centerline_optimizer
+  "srv/LoadMap.srv"
+  "srv/PlanRoute.srv"
+  "srv/PlanPath.srv"
+  "msg/PointsWithLaneId.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs
+)
+
+autoware_package()
+
+ament_auto_add_executable(main
+  src/main.cpp
+  src/static_centerline_optimizer_node.cpp
+  src/collision_free_optimizer_node.cpp
+  src/utils.cpp
+)
+
+target_include_directories(main
+  SYSTEM PUBLIC
+    ${OpenCV_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+target_link_libraries(main
+  ${OpenCV_LIBRARIES}
+)
+
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(main
+    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}")
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+    rviz
+)
+
+install(PROGRAMS
+  scripts/app.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/static_centerline_optimizer/README.md

@@ -0,0 +1,83 @@
+# Static Centerline Optimizer
+
+## Purpose
+
+This package statically calculates the centerline satisfying path footprints inside the drivable area.
+
+On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area.
+To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/).
+
+Instead of online path shape optimization, we introduce static centerline optimization.
+With this static centerline optimization, we have following advantages.
+
+- We can see the optimized centerline shape in advance.
+  - With the default autoware, path shape is not determined until the vehicle drives there.
+  - This enables offline path shape evaluation.
+- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area.
+
+## Use cases
+
+There are two interfaces to communicate with the centerline optimizer.
+
+### Vector Map Builder Interface
+
+Note: This function of Vector Map Builder has not been released. Please wait for a while.
+Currently there is no documentation about Vector Map Builder's operation for this function.
+
+The optimized centerline can be generated from Vector Map Builder's operation.
+
+We can run
+
+- path planning server
+- http server to connect path planning server and Vector Map Builder
+
+with the following command by designating `<vehicle_model>`
+
+```sh
+ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:=<vehicle-model>
+```
+
+FYI, port ID of the http server is 4010 by default.
+
+### Command Line Interface
+
+The optimized centerline can be generated from the command line interface by designating
+
+- `<input-osm-path>`
+- `<output-osm-path>` (not mandatory)
+- `<start-lanelet-id>`
+- `<end-lanelet-id>`
+- `<vehicle-model>`
+
+```sh
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_backgrond:=false lanelet2_input_file_path:=<input-osm-path> lanelet2_output_file_path:=<output-osm-path> start_lanelet_id:=<start-lane-id> end_lanelet_id:=<end-lane-id> vehicle_model:=<vehicle-model>
+```
+
+The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`.
+If you want to change the output map path, you can remap the path by designating `<output-osm-path>`.
+
+## Visualization
+
+When launching the path planning server, rviz is launched as well as follows.
+![rviz](./media/rviz.png)
+
+- The yellow footprints are the original ones from the osm map file.
+  - FYI: Footprints are generated based on the centerline and vehicle size.
+- The red footprints are the optimized ones.
+- The gray area is the drivable area.
+- You can see that the red footprints are inside the drivable area although the yellow ones are outside.
+
+### Unsafe footprints
+
+Sometimes the optimized centerline footprints are close to the lanes' boundaries.
+We can check how close they are with `unsafe footprints` marker as follows.
+
+Footprints' color depends on its distance to the boundaries, and text expresses its distance.
+
+![rviz](./media/unsafe_footprints.png)
+
+By default, footprints' color is
+
+- when the distance is less than 0.1 [m] : red
+- when the distance is less than 0.2 [m] : green
+- when the distance is less than 0.3 [m] : blue

planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    marker_color: ["FF0000", "00FF00", "0000FF"]
+    marker_color_dist_thresh : [0.1, 0.2, 0.3]

planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp

@@ -0,0 +1,105 @@
+// 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.
+
+// NOTE: This file was copied from a part of implementation in
+// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
+
+#ifndef STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
+
+#include "motion_utils/motion_utils.hpp"
+#include "obstacle_avoidance_planner/common_structs.hpp"
+#include "obstacle_avoidance_planner/costmap_generator.hpp"
+#include "obstacle_avoidance_planner/eb_path_optimizer.hpp"
+#include "obstacle_avoidance_planner/mpt_optimizer.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "static_centerline_optimizer/type_alias.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include "boost/optional.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace static_centerline_optimizer
+{
+class CollisionFreeOptimizerNode : public rclcpp::Node
+{
+private:
+  rclcpp::Clock logger_ros_clock_;
+  int eb_solved_count_;
+  bool is_driving_forward_{true};
+
+  bool is_publishing_debug_visualization_marker_;
+  bool is_publishing_area_with_objects_;
+  bool is_publishing_object_clearance_map_;
+  bool is_publishing_clearance_map_;
+  bool is_showing_debug_info_;
+  bool is_showing_calculation_time_;
+  bool is_stopping_if_outside_drivable_area_;
+  bool enable_avoidance_;
+  bool enable_pre_smoothing_;
+  bool skip_optimization_;
+  bool reset_prev_optimization_;
+
+  // vehicle circles info for for mpt constraints
+  std::string vehicle_circle_method_;
+  int vehicle_circle_num_for_calculation_;
+  std::vector<double> vehicle_circle_radius_ratios_;
+
+  // params for replan
+  double max_path_shape_change_dist_for_replan_;
+  double max_ego_moving_dist_for_replan_;
+  double max_delta_time_sec_for_replan_;
+
+  // logic
+  std::unique_ptr<CostmapGenerator> costmap_generator_ptr_;
+  std::unique_ptr<EBPathOptimizer> eb_path_optimizer_ptr_;
+  std::unique_ptr<MPTOptimizer> mpt_optimizer_ptr_;
+
+  // params
+  TrajectoryParam traj_param_;
+  EBParam eb_param_;
+  VehicleParam vehicle_param_;
+  MPTParam mpt_param_;
+  int mpt_visualize_sampling_num_;
+
+  // debug
+  mutable DebugData debug_data_;
+
+  geometry_msgs::msg::Pose current_ego_pose_;
+  std::unique_ptr<Trajectories> prev_optimal_trajs_ptr_;
+  std::unique_ptr<std::vector<PathPoint>> prev_path_points_ptr_;
+
+  std::unique_ptr<rclcpp::Time> latest_replanned_time_ptr_;
+
+  // ROS
+  rclcpp::Subscription<Path>::SharedPtr path_sub_;
+
+  // functions
+  void resetPlanning();
+  void resetPrevOptimization();
+
+public:
+  explicit CollisionFreeOptimizerNode(const rclcpp::NodeOptions & node_options);
+
+  // subscriber callback functions
+  Trajectory pathCallback(const Path::ConstSharedPtr);
+};
+}  // namespace static_centerline_optimizer
+
+#endif  // STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -0,0 +1,90 @@
+// 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__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
+
+#include "rclcpp/rclcpp.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 StaticCenterlineOptimizerNode : public rclcpp::Node
+{
+public:
+  enum class PlanPathResult {
+    SUCCESS = 0,
+    ROUTE_IS_NOT_READY = 1,
+  };
+
+  explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options);
+  void run();
+
+private:
+  // 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<unsigned int> plan_route(const int start_lanelet_id, const int end_lanelet_id);
+  void on_plan_route(
+    const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
+
+  // plan path
+  void on_plan_path(
+    const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
+  PlanPathResult plan_path(const std::vector<unsigned int> & route_lane_ids);
+  void evaluate(const std::vector<unsigned int> & route_lane_ids);
+
+  void save_map(
+    const std::string & lanelet2_output_file_path,
+    const std::vector<unsigned int> & route_lane_ids);
+
+  HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
+  std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
+  std::vector<TrajectoryPoint> optimized_traj_points_{};
+
+  // 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__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_

planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp

@@ -0,0 +1,45 @@
+// 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__TYPE_ALIAS_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_
+
+#include "route_handler/route_handler.hpp"
+
+#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
+#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
+#include "autoware_auto_planning_msgs/msg/had_map_route.hpp"
+#include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_point.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+namespace static_centerline_optimizer
+{
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_planning_msgs::msg::HADMapRoute;
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using route_handler::RouteHandler;
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
+using visualization_msgs::msg::MarkerArray;
+}  // namespace static_centerline_optimizer
+
+#endif  // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_