feat: add autoware_static_centerline_generator

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

planning/autoware_static_centerline_generator/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_static_centerline_generator)
+
+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)
+
+rosidl_generate_interfaces(
+  autoware_static_centerline_generator
+  "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_generator_node.cpp
+  src/centerline_source/optimization_trajectory_based_centerline.cpp
+  src/centerline_source/bag_ego_trajectory_based_centerline.cpp
+  src/utils.cpp
+)
+
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(main
+    autoware_static_centerline_generator "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp")
+    target_link_libraries(main "${cpp_typesupport_target}")
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+    rviz
+)
+
+target_include_directories(main PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+)
+
+if(BUILD_TESTING)
+  add_launch_test(
+    test/test_static_centerline_generator.test.py
+    TIMEOUT "30"
+  )
+  install(DIRECTORY
+    test/data/
+    DESTINATION share/${PROJECT_NAME}/test/data/
+  )
+endif()
+
+install(PROGRAMS
+  scripts/app.py
+  scripts/centerline_updater_helper.py
+  scripts/show_lanelet2_map_diff.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/autoware_static_centerline_generator/README.md

@@ -0,0 +1,83 @@
+# Static Centerline Generator
+
+## 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 autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/).
+
+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 autoware_static_centerline_generator 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 autoware_static_centerline_generator static_centerline_generator.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/autoware_static_centerline_generator/config/common.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    # constraints param for normal driving
+    max_vel: 11.1           # max velocity limit [m/s]
+
+    normal:
+      min_acc: -1.0         # min deceleration [m/ss]
+      max_acc: 1.0          # max acceleration [m/ss]
+      min_jerk: -1.0        # min jerk [m/sss]
+      max_jerk: 1.0         # max jerk [m/sss]
+
+    # constraints to be observed
+    limit:
+      min_acc: -2.5         # min deceleration limit [m/ss]
+      max_acc: 1.0          # max acceleration limit [m/ss]
+      min_jerk: -1.5        # min jerk limit [m/sss]
+      max_jerk: 1.5         # max jerk limit [m/sss]

planning/autoware_static_centerline_generator/config/nearest_search.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    # ego
+    ego_nearest_dist_threshold: 3.0 # [m]
+    ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]

planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    marker_color: ["FF0000", "FF5A00", "FFFF00"]
+    marker_color_dist_thresh : [0.1, 0.2, 0.3]
+    output_trajectory_interval: 1.0
+
+    validation:
+      dist_threshold_to_road_border: 0.0
+      max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease.

planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    wheel_radius: 0.39
+    wheel_width: 0.42
+    wheel_base: 2.74 # between front wheel center and rear wheel center
+    wheel_tread: 1.63 # between left wheel center and right wheel center
+    front_overhang: 1.0 # between front wheel center and vehicle front
+    rear_overhang: 1.03 # between rear wheel center and vehicle rear
+    left_overhang: 0.1 # between left wheel center and vehicle left
+    right_overhang: 0.1 # between right wheel center and vehicle right
+    vehicle_height: 2.5
+    max_steer_angle: 0.70 # [rad]

planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml

@@ -0,0 +1,12 @@
+<launch>
+  <!-- mandatory arguments for planning-->
+  <arg name="vehicle_model"/>
+
+  <include file="$(find-pkg-share autoware_static_centerline_generator)/launch/static_centerline_generator.launch.xml">
+    <arg name="vehicle_model" value="$(var vehicle_model)"/>
+    <arg name="run_background" value="true"/>
+  </include>
+
+  <!-- local server to connect path optimizer and cloud software -->
+  <node pkg="autoware_static_centerline_generator" exec="app.py" name="static_centerline_generator_http_server" output="screen"/>
+</launch>

planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml

@@ -0,0 +1,91 @@
+<launch>
+  <!-- mandatory arguments for planning-->
+  <arg name="vehicle_model"/>
+
+  <!-- flag -->
+  <arg name="mode" default="AUTO" description="select from AUTO, GUI, and VMB"/>
+  <arg name="rviz" default="true"/>
+  <arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
+
+  <!-- mandatory arguments when mode is AUTO -->
+  <arg name="lanelet2_input_file_path" default=""/>
+  <arg name="lanelet2_output_file_path" default="/tmp/autoware_static_centerline_generator/lanelet2_map.osm"/>
+  <arg name="start_lanelet_id" default=""/>
+  <arg name="end_lanelet_id" default=""/>
+
+  <!-- mandatory arguments when mode is GUI -->
+  <arg name="bag_filename" default="bag.db3"/>
+
+  <!-- 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 autoware_map_loader)/config/lanelet2_map_loader.param.yaml"/>
+  <arg
+    name="behavior_path_planner_param"
+    default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml"
+  />
+  <arg
+    name="behavior_velocity_planner_param"
+    default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"
+  />
+  <arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
+  <arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.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 autoware_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="autoware_map_tf_generator" exec="autoware_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="autoware_lanelet2_map_visualizer" 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="autoware_static_centerline_generator" exec="main" name="static_centerline_generator">
+    <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
+    <remap from="input_centerline" to="~/input_centerline"/>
+
+    <param name="mode" value="$(var mode)"/>
+    <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 behavior_path_planner_param)"/>
+    <param from="$(var behavior_velocity_planner_param)"/>
+    <param from="$(var path_smoother_param)"/>
+    <param from="$(var path_optimizer_param)"/>
+    <param from="$(var mission_planner_param)"/>
+    <param name="check_footprint_inside_lanes" value="false"/>
+    <!-- override the mission_planner's parameter -->
+    <!-- node param -->
+    <param from="$(find-pkg-share autoware_static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
+    <param name="centerline_source" value="$(var centerline_source)"/>
+    <param name="bag_filename" value="$(var bag_filename)"/>
+  </node>
+
+  <!-- GUI to select the range of centerline -->
+  <group if="$(eval &quot;'$(var mode)'=='GUI'&quot;)">
+    <node pkg="autoware_static_centerline_generator" exec="centerline_updater_helper.py" name="centerline_updater_helper"/>
+  </group>
+
+  <!-- rviz -->
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
+</launch>

planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg

@@ -0,0 +1,2 @@
+int64 lane_id
+geometry_msgs/Point[] points

planning/autoware_static_centerline_generator/package.xml

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_static_centerline_generator</name>
+  <version>0.40.0</version>
+  <description>The autoware_static_centerline_generator package</description>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+  <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Takayuki Murooka</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>autoware_behavior_path_planner_common</depend>
+  <depend>autoware_geography_utils</depend>
+  <depend>autoware_global_parameter_loader</depend>
+  <depend>autoware_interpolation</depend>
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_lanelet2_map_visualizer</depend>
+  <depend>autoware_map_loader</depend>
+  <depend>autoware_map_msgs</depend>
+  <depend>autoware_map_projection_loader</depend>
+  <depend>autoware_map_tf_generator</depend>
+  <depend>autoware_mission_planner</depend>
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_osqp_interface</depend>
+  <depend>autoware_path_optimizer</depend>
+  <depend>autoware_path_smoother</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <exec_depend>python3-flask-cors</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_behavior_path_planner</test_depend>
+  <test_depend>autoware_behavior_velocity_planner</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>