feat: add 'obstacle_velocity_limiter' package (#1579)

* Initial commit with barebone SafeVelocityAdjustorNode Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add debug topics, launch file, and config file Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix debug markers Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix dynamic parameters Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add proper collision detection and debug footprint Implements Proposal 1. Calculation of the adjusted velocity still needs to be improved Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add script to compare the original and adjusted velocity profiles Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix calculation of distance to obstacle Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add test for calculation collision distance Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add launch file to test the safe_velocity_adjustor with a bag Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Cleanup code and add tests for forwardSimulatedVector Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Simplify collision detection by not using a footprint polygon Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add filtering of the dynamic objects from the pointcloud Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * [DEBUG] Print runtimes of expensive functions Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add trajectory downsampling to boost performance + improve debug markers Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Modify velocity only from ego pose + distance parameter Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add 1st Eigen version of distanceToClosestCollision + benchmark Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Switch to using contours from occupancy grid for collision checking Filtering of dynamic objects is not great Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add buffer around dynamic obstacles to avoid false obstacle detection Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter to limit the adjusted velocity Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Use vehicle_info_util to get vehicle footprint Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Calculate accurate distance to collision + add tests Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter for the min velocity where a dynamic obstacle is ignored Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add README and some pictures to explain the node inner workings Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update scenario_planning.launch.xml to run the new node Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix format of launch files Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update launcher and rviz config used for debuging with bag Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Cleanup debug publishing Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Complete tests of collision_distance.hpp Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add docstring + Small code cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Improve test of occupancy_grid_utils Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix bug when setting parameter callback before getting vehicle parameters Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Rename safe_velocity_adjustor to apparent_safe_velocity_limiter Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Move declarations to cpp file (apparent_safe_velocity_limiter_node) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Move declarations to cpp file (occupancy_grid_utils) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Move declarations to cpp file (collision_distance) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add exec of trajectory_visualizer.py in launch files Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Mask trajectory footprint from the occupancy grid (might be expensive) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Filter out the occupancy grid that is outside the envelope polygon Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add improved PolygonIterator using scan line algorithm Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Use autoware_cmake for dependencies Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Improve performances of PolygonIterator Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Minor cleanup of PolygonIterator Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Use improved iterator + add benchmark (max/avg/med) to node Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Minor code cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Switch from set to vector/list in PolygonIterator Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Remove PolygonIterator and use implementation from grid_map_utils Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter to limit deceleration when adjusting the velocity Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Code cleanup, move type decl and debug functions to separate files Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add support for collision detection using pointcloud Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Code cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Speedup pointcloud filtering (still ~100ms on bags) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Improve envelope calculation and use separate node for pcd downsampling Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add ProjectionParameters to prepare for the bicycle model projection Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add bicycle projection with various steering offsets Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update docstring Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Major refactoring, calculate envelope from footprints Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add extraction of static obstacles from lanelet map Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Remove stopwatch Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add arc distance calculation when using bicycle projection Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix multi geometry definitions in tier4_autoware_utils/boost_geometry Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Improve geometry operations to take advantage of Eigen Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Switch to min/max offset and simplify footprint calculations Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix unit tests (unset params.heading) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add option to filter obstacles using the safety envelope Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix bug with distance calculation and improve debug markers Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update README Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter to set map obstacles by linestring id (for debug) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Move param structures to dedicated file and add PreprocessingParameters Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter to calculate steering angle of trajectory points Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Cleanup footprint generation Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix bug with debug marker ids Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix bug where the VelocityParameters were not constructed Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update obstacles extraction Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Minor code cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Switch to collision detection using rtree Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add publishing of the runtime (in microseconds) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add option to ignore obstacles on the trajectory Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add max length and max duration parameters Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Restructure Obstacles structure to separate lines and points for speedup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Convert obstacle linestrings to segments when used in the rtree Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add parameter for extra distance when filtering the ego path Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix issues caused by rebase Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Minor code cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update to run with looping bag replay Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Add debug markers for obstacle masks and only publish when subscribed Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update README Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix humble build issue with PCL library Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update obstacle extraction from lanelet map (no longer based on route) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Optimize use of rtree + use naive collision checking with few obstacles Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Remove debug code and update default parameters Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Do not wait for self pose Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Rename to obstacle_velocity_limiter Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * More minor cleanup Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update READEME.md Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update README to have the purpose written before the illustration Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update copyright notice: Tier IV -> TIER IV Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Remove use_sim_time param from node launch file Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Update launch files to run in the motion_planner + add launch config Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
autowarefoundation · Nov 2, 2022 · 2997983 · 2997983
1 parent 1862ee7
commit 2997983
Show file tree

Hide file tree

Showing 42 changed files with 3,560 additions and 7 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp
@@ -35,8 +35,8 @@ using LineString2d = boost::geometry::model::linestring<Point2d>;
 using LinearRing2d = boost::geometry::model::ring<Point2d>;
 using Polygon2d = boost::geometry::model::polygon<Point2d>;
 using MultiPoint2d = boost::geometry::model::multi_point<Point2d>;
-using MultiLineString2d = boost::geometry::model::multi_linestring<Point2d>;
-using MultiPolygon2d = boost::geometry::model::multi_polygon<Point2d>;
+using MultiLineString2d = boost::geometry::model::multi_linestring<LineString2d>;
+using MultiPolygon2d = boost::geometry::model::multi_polygon<Polygon2d>;
 
 // 3D
 struct Point3d;
@@ -46,8 +46,8 @@ using LineString3d = boost::geometry::model::linestring<Point3d>;
 using LinearRing3d = boost::geometry::model::ring<Point3d>;
 using Polygon3d = boost::geometry::model::polygon<Point3d>;
 using MultiPoint3d = boost::geometry::model::multi_point<Point3d>;
-using MultiLineString3d = boost::geometry::model::multi_linestring<Point3d>;
-using MultiPolygon3d = boost::geometry::model::multi_polygon<Point3d>;
+using MultiLineString3d = boost::geometry::model::multi_linestring<LineString3d>;
+using MultiPolygon3d = boost::geometry::model::multi_polygon<Polygon3d>;
 
 struct Point2d : public Eigen::Vector2d
 {

diff --git a/...ne_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/...ne_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml
@@ -0,0 +1,33 @@
+/**:
+  ros__parameters:
+    min_ttc: 1.0  # [s] minimum time without collision when doing projection at each trajectory point
+    distance_buffer: 0.0  # [m] extra distance to add to a projection (in addition to the vehicle overhang)
+    min_adjusted_velocity: 2.5  # [m/s] minimum velocity that the module can set
+    max_deceleration: 2.0  # [m/s²] maximum deceleration caused by the adjusted velocity
+
+    trajectory_preprocessing:
+      start_distance: 0.0  # [m] distance ahead of ego from which to start modifying the trajectory
+      max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted
+      max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted
+      downsample_factor: 10  # factor by which to downsample the input trajectory for calculation
+      calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading
+
+    simulation:
+      model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle"
+      distance_method: exact  # distance calculation method. Either "exact" or "approximation".
+      # parameters used only with the bicycle model
+      steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection
+      nb_points: 5  # number of points representing the curved projections
+
+    obstacles:
+      dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'.
+      ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored
+      ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored
+      occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles
+      dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection
+      dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module
+      static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles
+        - guard_rail
+      filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles
+      rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection
+      rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection
diff --git a/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -87,6 +87,40 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # obstacle velocity limiter
+    obstacle_velocity_limiter_param_path = os.path.join(
+        LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
+        "scenario_planning",
+        "lane_driving",
+        "motion_planning",
+        "obstacle_velocity_limiter",
+        "obstacle_velocity_limiter.param.yaml",
+    )
+    with open(obstacle_velocity_limiter_param_path, "r") as f:
+        obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    obstacle_velocity_limiter_component = ComposableNode(
+        package="obstacle_velocity_limiter",
+        plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode",
+        name="obstacle_velocity_limiter",
+        namespace="",
+        remappings=[
+            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"),
+            ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"),
+            ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
+            ("~/input/map", "/map/vector_map"),
+            ("~/output/debug_markers", "debug_markers"),
+            ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"),
+        ],
+        parameters=[
+            obstacle_velocity_limiter_param,
+            vehicle_info_param,
+            {"obstacles.dynamic_source": "static_only"},
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # surround obstacle checker
     surround_obstacle_checker_param_path = os.path.join(
         LaunchConfiguration("tier4_planning_launch_param_path").perform(context),
@@ -167,7 +201,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
         ],
         parameters=[
             nearest_search_param,
@@ -197,7 +231,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner",
         namespace="",
         remappings=[
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
+            ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/acceleration", "/localization/acceleration"),
             ("~/input/objects", "/perception/object_recognition/objects"),
@@ -220,7 +254,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_cruise_planner_relay",
         namespace="",
         parameters=[
-            {"input_topic": "obstacle_avoidance_planner/trajectory"},
+            {"input_topic": "obstacle_velocity_limiter/trajectory"},
             {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"},
             {"type": "autoware_auto_planning_msgs/msg/Trajectory"},
         ],
@@ -234,6 +268,7 @@ def launch_setup(context, *args, **kwargs):
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
             obstacle_avoidance_planner_component,
+            obstacle_velocity_limiter_component,
         ],
     )
 

diff --git a/planning/obstacle_velocity_limiter/CMakeLists.txt b/planning/obstacle_velocity_limiter/CMakeLists.txt
@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_velocity_limiter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PCL REQUIRED COMPONENTS common)
+find_package(pcl_conversions REQUIRED)
+
+ament_auto_add_library(obstacle_velocity_limiter_node SHARED
+  DIRECTORY src
+)
+
+ament_target_dependencies(obstacle_velocity_limiter_node PCL)
+target_include_directories(obstacle_velocity_limiter_node
+  SYSTEM PUBLIC
+    "${PCL_INCLUDE_DIRS}"
+)
+
+# Disable warnings due to external dependencies
+get_target_property(lanelet2_core_INCLUDE_DIR
+  lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES
+)
+
+rclcpp_components_register_node(obstacle_velocity_limiter_node
+  PLUGIN "obstacle_velocity_limiter::ObstacleVelocityLimiterNode"
+  EXECUTABLE obstacle_velocity_limiter
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_forward_projection.cpp
+    test/test_obstacles.cpp
+    test/test_collision_distance.cpp
+    test/test_occupancy_grid_utils.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    obstacle_velocity_limiter_node
+  )
+endif()
+
+add_executable(collision_benchmark
+  benchmarks/collision_checker_benchmark.cpp
+)
+target_link_libraries(collision_benchmark
+  obstacle_velocity_limiter_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+    script
+)