feat(obstacle_velocity_limiter): move to motion_velocity_planner (aut…

…owarefoundation#7439)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

.github/CODEOWNERS

@@ -199,13 +199,13 @@ planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi
 planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
+planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp
 planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp
 planning/autoware_obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
 planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
-planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp
 planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp
 planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp
 planning/autoware_route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -3,6 +3,7 @@
   <arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
 
   <arg name="launch_out_of_lane_module" default="true"/>
+  <arg name="launch_obstacle_velocity_limiter_module" default="true"/>
   <!-- <arg name="launch_dynamic_obstacle_stop_module" default="true"/> -->
   <arg name="launch_module_list_end" default="&quot;&quot;]"/>
 
@@ -13,6 +14,11 @@
     value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::OutOfLaneModule, '&quot;)"
     if="$(var launch_out_of_lane_module)"
   />
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleVelocityLimiterModule, '&quot;)"
+    if="$(var launch_obstacle_velocity_limiter_module)"
+  />
   <!-- <let -->
   <!--   name="motion_velocity_planner_launch_modules" -->
   <!--   value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'motion_velocity_planner::DynamicObstacleStopModule, '&quot;)" -->
@@ -135,43 +141,21 @@
         <param from="$(var motion_velocity_planner_velocity_smoother_type_param_path)"/>
         <param from="$(var motion_velocity_planner_param_path)"/>
         <param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
+        <param from="$(var motion_velocity_planner_obstacle_velocity_limiter_param_path)"/>
         <!-- <param from="$(var motion_velocity_planner_template_param_path)"/> -->
         <!-- composable node config -->
         <extra_arg name="use_intra_process_comms" value="false"/>
       </composable_node>
     </load_composable_node>
   </group>
-  <!-- velocity limiter TODO(Maxime): move this node to the motion_velocity_planner -->
-  <group>
-    <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-      <composable_node pkg="obstacle_velocity_limiter" plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode" name="obstacle_velocity_limiter" namespace="">
-        <!-- topic remap -->
-        <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
-        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-        <remap from="~/input/dynamic_obstacles" to="/perception/object_recognition/objects"/>
-        <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
-        <remap from="~/input/obstacle_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
-        <remap from="~/input/map" to="/map/vector_map"/>
-        <remap from="~/output/trajectory" to="obstacle_velocity_limiter/trajectory"/>
-        <remap from="~/output/debug_markers" to="debug_markers"/>
-        <!-- params -->
-        <param from="$(var common_param_path)"/>
-        <param from="$(var vehicle_param_file)"/>
-        <param from="$(var nearest_search_param_path)"/>
-        <param from="$(var obstacle_velocity_limiter_param_path)"/>
-        <!-- composable node config -->
-        <extra_arg name="use_intra_process_comms" value="false"/>
-      </composable_node>
-    </load_composable_node>
-  </group>
 
   <!-- obstacle stop, adaptive cruise -->
   <group>
     <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner'&quot;)">
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
         <composable_node pkg="autoware_obstacle_cruise_planner" plugin="autoware::motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
           <!-- topic remap -->
-          <remap from="~/input/trajectory" to="obstacle_velocity_limiter/trajectory"/>
+          <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
           <remap from="~/input/acceleration" to="/localization/acceleration"/>
           <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
@@ -194,7 +178,7 @@
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
         <composable_node pkg="obstacle_stop_planner" plugin="motion_planning::ObstacleStopPlannerNode" name="obstacle_stop_planner" namespace="">
           <!-- topic remap -->
-          <remap from="~/input/trajectory" to="obstacle_velocity_limiter/trajectory"/>
+          <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
           <remap from="~/input/acceleration" to="/localization/acceleration"/>
           <remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
@@ -219,7 +203,7 @@
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
         <composable_node pkg="topic_tools" plugin="topic_tools::RelayNode" name="obstacle_stop_relay" namespace="">
           <!-- params -->
-          <param name="input_topic" value="obstacle_velocity_limiter/trajectory"/>
+          <param name="input_topic" value="motion_velocity_planner/trajectory"/>
           <param name="output_topic" value="$(var interface_output_topic)"/>
           <param name="type" value="autoware_planning_msgs/msg/Trajectory"/>
           <!-- composable node config -->

planning/.pages

@@ -50,7 +50,6 @@ nav:
       - 'About Obstacle Cruise': planning/autoware_obstacle_cruise_planner
       - 'How to Debug': planning/autoware_obstacle_cruise_planner/docs/debug
     - 'Obstacle Stop Planner': planning/obstacle_stop_planner
-    - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter
     - 'Path Smoother':
       - 'About Path Smoother': planning/autoware_path_smoother
       - 'Elastic Band': planning/autoware_path_smoother/docs/eb
@@ -63,9 +62,10 @@ nav:
       - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker
       - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja
     - 'Motion Velocity Planner':
-      - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/
+      - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node/
       - 'Available Modules':
-        - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/
+        - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_planner_out_of_lane_module/
+        - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/
     - 'Velocity Smoother':
       - 'About Velocity Smoother': planning/autoware_velocity_smoother
       - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_motion_velocity_obstacle_velocity_limiter_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+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}
+    ${PROJECT_NAME}
+  )
+endif()
+
+add_executable(collision_benchmark
+  benchmarks/collision_checker_benchmark.cpp
+)
+target_link_libraries(collision_benchmark
+  ${PROJECT_NAME}
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+)

planning/obstacle_velocity_limiter/README.md → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/README.md

@@ -142,27 +142,6 @@ Parameters `trajectory_preprocessing.max_length` and `trajectory_preprocessing.m
 To reduce computation cost at the cost of precision, the trajectory can be downsampled using parameter `trajectory_preprocessing.downsample_factor`.
 For example a value of `1` means all trajectory points will be evaluated while a value of `10` means only 1/10th of the points will be evaluated.
 
-## Inputs / Outputs
-
-### Inputs
-
-| Name                          | Type                                        | Description                                        |
-| ----------------------------- | ------------------------------------------- | -------------------------------------------------- |
-| `~/input/trajectory`          | `autoware_planning_msgs/Trajectory`         | Reference trajectory                               |
-| `~/input/occupancy_grid`      | `nav_msgs/OccupancyGrid`                    | Occupancy grid with obstacle information           |
-| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2`                   | Pointcloud containing only obstacle points         |
-| `~/input/dynamic_obstacles`   | `autoware_perception_msgs/PredictedObjects` | Dynamic objects                                    |
-| `~/input/odometry`            | `nav_msgs/Odometry`                         | Odometry used to retrieve the current ego velocity |
-| `~/input/map`                 | `autoware_map_msgs/LaneletMapBin`           | Vector map used to retrieve static obstacles       |
-
-### Outputs
-
-| Name                            | Type                                | Description                                              |
-| ------------------------------- | ----------------------------------- | -------------------------------------------------------- |
-| `~/output/trajectory`           | `autoware_planning_msgs/Trajectory` | Trajectory with adjusted velocities                      |
-| `~/output/debug_markers`        | `visualization_msgs/MarkerArray`    | Debug markers (envelopes, obstacle polygons)             |
-| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64`          | Time taken to calculate the trajectory (in microseconds) |
-
 ## Parameters
 
 | Name                                                | Type        | Description                                                                                                                             |

planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp

@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "obstacle_velocity_limiter/obstacles.hpp"
-#include "obstacle_velocity_limiter/types.hpp"
+#include "../src/obstacles.hpp"
+#include "../src/types.hpp"
 
 #include <chrono>
 #include <random>
 
-using obstacle_velocity_limiter::CollisionChecker;
-using obstacle_velocity_limiter::linestring_t;
-using obstacle_velocity_limiter::Obstacles;
-using obstacle_velocity_limiter::point_t;
-using obstacle_velocity_limiter::polygon_t;
+using autoware::motion_velocity_planner::obstacle_velocity_limiter::CollisionChecker;
+using autoware::motion_velocity_planner::obstacle_velocity_limiter::linestring_t;
+using autoware::motion_velocity_planner::obstacle_velocity_limiter::Obstacles;
+using autoware::motion_velocity_planner::obstacle_velocity_limiter::point_t;
+using autoware::motion_velocity_planner::obstacle_velocity_limiter::polygon_t;
 
 point_t random_point()
 {

planning/obstacle_velocity_limiter/package.xml → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>obstacle_velocity_limiter</name>
+  <name>autoware_motion_velocity_obstacle_velocity_limiter_module</name>
   <version>0.1.0</version>
   <description>Package to adjust velocities of a trajectory in order for the ride to feel safe</description>
 
@@ -11,6 +11,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_motion_velocity_planner_common</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_planning_test_manager</depend>
@@ -25,8 +26,8 @@
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pcl_ros</depend>
+  <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/plugins.xml

@@ -0,0 +1,3 @@
+<library path="autoware_motion_velocity_obstacle_velocity_limiter_module">
+  <class type="autoware::motion_velocity_planner::ObstacleVelocityLimiterModule" base_class_type="autoware::motion_velocity_planner::PluginModuleInterface"/>
+</library>

planning/obstacle_velocity_limiter/src/debug.cpp → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.cpp

@@ -12,16 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "obstacle_velocity_limiter/debug.hpp"
+#include "debug.hpp"
 
-#include "obstacle_velocity_limiter/parameters.hpp"
-#include "obstacle_velocity_limiter/types.hpp"
+#include "parameters.hpp"
+#include "types.hpp"
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
-namespace obstacle_velocity_limiter
+#include <algorithm>
+
+namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
-visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const Float z)
+visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const double z)
 {
   visualization_msgs::msg::Marker marker;
   marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
@@ -38,7 +40,7 @@ visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, co
   return marker;
 }
 
-visualization_msgs::msg::Marker makePolygonMarker(const polygon_t & polygon, const Float z)
+visualization_msgs::msg::Marker makePolygonMarker(const polygon_t & polygon, const double z)
 {
   visualization_msgs::msg::Marker marker;
   marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
@@ -60,7 +62,7 @@ visualization_msgs::msg::MarkerArray makeDebugMarkers(
   const std::vector<multi_linestring_t> & adjusted_projections,
   const std::vector<polygon_t> & original_footprints,
   const std::vector<polygon_t> & adjusted_footprints, const ObstacleMasks & obstacle_masks,
-  const Float marker_z)
+  const double marker_z)
 {
   visualization_msgs::msg::MarkerArray debug_markers;
   auto original_projections_id_offset = 0;
@@ -182,4 +184,4 @@ visualization_msgs::msg::MarkerArray makeDebugMarkers(
   return debug_markers;
 }
 
-}  // namespace obstacle_velocity_limiter
+}  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter

planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/debug.hpp

@@ -12,25 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_
-#define OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_
+#ifndef DEBUG_HPP_
+#define DEBUG_HPP_
 
-#include "obstacle_velocity_limiter/obstacles.hpp"
-#include "obstacle_velocity_limiter/types.hpp"
+#include "obstacles.hpp"
+#include "types.hpp"
 
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
 #include <vector>
 
-namespace obstacle_velocity_limiter
+namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
 /// @brief make the visualization Marker of the given linestring
 /// @param[in] ls linestring to turn into a marker
 /// @param[in] z z-value to use in the marker
 /// @return marker representing the linestring
-visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const Float z);
+visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const double z);
 
 /// @brief make debug marker array
 /// @param[in] obstacles obstacles
@@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray makeDebugMarkers(
   const std::vector<multi_linestring_t> & adjusted_projections,
   const std::vector<polygon_t> & original_footprints,
   const std::vector<polygon_t> & adjusted_footprints, const ObstacleMasks & obstacle_masks,
-  const Float marker_z);
+  const double marker_z);
 
-}  // namespace obstacle_velocity_limiter
-#endif  // OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_
+}  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
+#endif  // DEBUG_HPP_

planning/obstacle_velocity_limiter/src/distance.cpp → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/distance.cpp

@@ -12,9 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "obstacle_velocity_limiter/obstacles.hpp"
-
-#include <obstacle_velocity_limiter/distance.hpp>
+#include "distance.hpp"
 
 #include <boost/geometry.hpp>
 #include <boost/geometry/algorithms/detail/distance/interface.hpp>
@@ -24,7 +22,7 @@
 #include <algorithm>
 #include <limits>
 
-namespace obstacle_velocity_limiter
+namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
 namespace bg = boost::geometry;
 
@@ -70,4 +68,4 @@ double arcDistance(const point_t & origin, const double heading, const point_t &
     std::acos((2 * squared_radius - (origin - target).squaredNorm()) / (2 * squared_radius));
   return std::sqrt(squared_radius) * angle;
 }
-}  // namespace obstacle_velocity_limiter
+}  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter

planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp → planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/distance.hpp

@@ -12,19 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_
-#define OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_
+#ifndef DISTANCE_HPP_
+#define DISTANCE_HPP_
 
-#include "obstacle_velocity_limiter/obstacles.hpp"
-#include "obstacle_velocity_limiter/parameters.hpp"
-#include "obstacle_velocity_limiter/types.hpp"
+#include "obstacles.hpp"
+#include "parameters.hpp"
+#include "types.hpp"
 
 #include <geometry_msgs/msg/vector3.hpp>
 
 #include <optional>
 #include <vector>
 
-namespace obstacle_velocity_limiter
+namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
 /// @brief calculate the closest distance to a collision
 /// @param [in] projection forward projection line
@@ -43,6 +43,6 @@ std::optional<double> distanceToClosestCollision(
 /// @return distance from origin to target along a circle
 double arcDistance(const point_t & origin, const double heading, const point_t & target);
 
-}  // namespace obstacle_velocity_limiter
+}  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
-#endif  // OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_
+#endif  // DISTANCE_HPP_