feat: autoware_motion_velocity_planner_node to core

planning/motion_velocity_planner/autoware_motion_velocity_planner/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_velocity_planner_node)
+
+find_package(autoware_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "srv/LoadPlugin.srv"
+  "srv/UnloadPlugin.srv"
+  DEPENDENCIES
+)
+
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME}_lib SHARED
+  DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_lib
+  PLUGIN "autoware::motion_velocity_planner::MotionVelocityPlannerNode"
+  EXECUTABLE ${PROJECT_NAME}_exe
+)
+
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(${PROJECT_NAME}_lib
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}")
+endif()
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/src/test_node_interface.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    gtest_main
+    ${PROJECT_NAME}_lib
+  )
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

planning/motion_velocity_planner/autoware_motion_velocity_planner/README.md

@@ -0,0 +1,57 @@
+# Motion Velocity Planner
+
+## Overview
+
+`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle.
+It loads modules as plugins. Please refer to the links listed below for detail on each module.
+
+![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg)
+
+- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md)
+
+Each module calculates stop and slow down points to be inserted in the ego trajectory.
+These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory.
+This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhang, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/).
+
+![set_stop_velocity](./docs/set_stop_velocity.drawio.svg)
+
+## Input topics
+
+| Name                           | Type                                                  | Description                   |
+| ------------------------------ | ----------------------------------------------------- | ----------------------------- |
+| `~/input/trajectory`           | autoware_planning_msgs::msg::Trajectory               | input trajectory              |
+| `~/input/vector_map`           | autoware_map_msgs::msg::LaneletMapBin                 | vector map                    |
+| `~/input/vehicle_odometry`     | nav_msgs::msg::Odometry                               | vehicle position and velocity |
+| `~/input/accel`                | geometry_msgs::msg::AccelWithCovarianceStamped        | vehicle acceleration          |
+| `~/input/dynamic_objects`      | autoware_perception_msgs::msg::PredictedObjects       | dynamic objects               |
+| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2                         | obstacle pointcloud           |
+| `~/input/traffic_signals`      | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states          |
+| `~/input/occupancy_grid`       | nav_msgs::msg::OccupancyGrid                          | occupancy grid                |
+
+## Output topics
+
+| Name                                      | Type                                           | Description                                        |
+| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- |
+| `~/output/trajectory`                     | autoware_planning_msgs::msg::Trajectory        | Ego trajectory with updated velocity profile       |
+| `~/output/planning_factors/<MODULE_NAME>` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile |
+
+## Services
+
+| Name                      | Type                                                     | Description                  |
+| ------------------------- | -------------------------------------------------------- | ---------------------------- |
+| `~/service/load_plugin`   | autoware_motion_velocity_planner_node::srv::LoadPlugin   | To request loading a plugin  |
+| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin |
+
+## Node parameters
+
+| Parameter        | Type             | Description            |
+| ---------------- | ---------------- | ---------------------- |
+| `launch_modules` | vector\<string\> | module names to launch |
+
+In addition, the following parameters should be provided to the node:
+
+- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml);
+- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml);
+- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml);
+- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/#parameters)
+- Parameters of each plugin that will be loaded.

planning/motion_velocity_planner/autoware_motion_velocity_planner/config/motion_velocity_planner.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    smooth_velocity_before_planning: true  # [-] if true, smooth the velocity profile of the input trajectory before planning
+
+    trajectory_polygon_collision_check:
+      decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
+      goal_extended_trajectory_length: 6.0
+
+      # consider the current ego pose (it is not the nearest pose on the reference trajectory)
+      # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
+      # The both errors decrease with constant rates against the time.
+      consider_current_pose:
+        enable_to_consider_current_pose: true
+        time_to_convergence: 1.5 #[s]