feat(static_centerline_optimizer): add a new package

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

planning/static_centerline_optimizer/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.5)
+project(static_centerline_optimizer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+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
+)
+
+ament_auto_add_executable(main
+  src/main.cpp
+  src/static_centerline_optimizer_node.cpp
+  src/optimization_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/optimize_centerline.sh
+  scripts/app.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/static_centerline_optimizer/README.md

@@ -0,0 +1,52 @@
+# Static Centerline Optimizer
+
+## Purpose
+
+This package statically calcualtes the centerline satisfying footprints inside the drivable area.
+
+On narrow-road driving, the default centerline, which is the middle line between lanelet's right and left bounds, often causes footprints outside the drivable area.
+With the static centerline optimization, we have the following advantages.
+
+- We can see the optimized centerline shape in advance.
+- We do not have to calculate a heavy and sometimes unstable path optimization since the footprints are already inside the drivable area.
+
+## Usecases
+
+There are two interfaces to communicate with the centerline optimizer.
+
+### Vector Map Builder Interface
+
+Run the autoware server with the following command by designating `vehicle_model` as lexus for example.
+
+```sh
+ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:=lexus
+```
+
+Run http server with the following command.
+The port number is 5000 by default.
+
+```sh
+ros2 run static_centerline_optimizer app.py
+```
+
+### Command Line Interface
+
+The optimized centerline is generated from the command line interface.
+
+```sh
+# ros2 run static_centerline_optimizer optimize_path.sh <osm-map-path> <start-lanelet-id> <end-lanelet-id> <vehicle-model>
+$ ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml lanelet2_input_file_name:="$HOME"/AutonomousDrivingScenarios/map/kashiwanoha/lanelet2_map.osm start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus run_backgrond:=false
+```
+
+image
+
+The yellow footprints are the original ones in the map file.
+The red footprints are the optimized ones.
+You can see that the red footprints are inside the lane although the yellow ones are outside.
+
+The output map with the optimized centerline locates `/tmp/lanelet2_map.osm`
+If you want to change the output map path, you can remap the path
+
+```sh
+ros2 run static_centerline_optimizer optimize_centerline.sh <map-path> <start-lanelet-id> <end-lanelet-id> --ros-args --remap output:=<output-map-path>
+```

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/optimization_node.hpp

@@ -0,0 +1,106 @@
+// 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__OPTIMIZATION_NODE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__OPTIMIZATION_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 StaticCenterlineOptimizer : 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::Publisher<Trajectory>::SharedPtr traj_pub_;
+  rclcpp::Subscription<Path>::SharedPtr path_sub_;
+
+  // functions
+  void resetPlanning();
+  void resetPrevOptimization();
+
+  // void insertZeroVelocityOutsideDrivableArea(
+  //   std::vector<TrajectoryPoint> & traj_points,
+  //   const CVMaps & cv_maps);
+
+public:
+  explicit StaticCenterlineOptimizer(const rclcpp::NodeOptions & node_options);
+
+  // subscriber callback functions
+  std::vector<TrajectoryPoint> pathCallback(const Path::SharedPtr);
+};
+}  // namespace static_centerline_optimizer
+
+#endif  // STATIC_CENTERLINE_OPTIMIZER__OPTIMIZATION_NODE_HPP_

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -0,0 +1,94 @@
+// 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;
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
+
+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_name);
+  void on_load_map(
+    const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
+
+  // plan route
+  void on_plan_route(
+    const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
+  void plan_route(const int start_lanelet_id, const int end_lanelet_id);
+
+  // plan path
+  void on_plan_path(
+    const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
+  PlanPathResult plan_path(const int start_lanelet_id);
+  void evaluate();
+  MarkerArray createFootprintMarker(
+    const LinearRing2d & footprint_poly, const std::array<double, 3> & marker_color,
+    const size_t idx);
+
+  void save_map(const std::string & lanelet2_output_file_name);
+
+  HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
+  std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
+  std::shared_ptr<lanelet::ConstLanelets> lanelets_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};
+
+  // 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,42 @@
+// 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 visualization_msgs::msg::MarkerArray;
+}  // namespace static_centerline_optimizer
+
+#endif  // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_