Skip to content

Commit 2b4be1d

Browse files
maxime-clemkarishma1911
maxime-clem
authored and
karishma1911
committed
feat(motion_velocity_planner): add new motion velocity planning (autowarefoundation#7064)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent b158aa9 commit 2b4be1d

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

51 files changed

+4814
-10
lines changed

‎common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
2020
#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
2121
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
22-
#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
2322
#include <geometry_msgs/msg/pose.hpp>
2423

2524
#include <string>
@@ -40,10 +39,10 @@ class VelocityFactorInterface
4039
void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
4140
void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
4241

42+
template <class PointType>
4343
void set(
44-
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
45-
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
46-
const std::string & detail = "");
44+
const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
45+
const VelocityFactorStatus status, const std::string & detail = "");
4746

4847
private:
4948
VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};

‎common/motion_utils/src/factor/velocity_factor_interface.cpp

+17-3
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,16 @@
1515
#include <motion_utils/factor/velocity_factor_interface.hpp>
1616
#include <motion_utils/trajectory/trajectory.hpp>
1717

18+
#include <autoware_auto_planning_msgs/msg/path_point.hpp>
19+
#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
20+
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
21+
1822
namespace motion_utils
1923
{
24+
template <class PointType>
2025
void VelocityFactorInterface::set(
21-
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
22-
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
23-
const std::string & detail)
26+
const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
27+
const VelocityFactorStatus status, const std::string & detail)
2428
{
2529
const auto & curr_point = curr_pose.position;
2630
const auto & stop_point = stop_pose.position;
@@ -32,4 +36,14 @@ void VelocityFactorInterface::set(
3236
velocity_factor_.detail = detail;
3337
}
3438

39+
template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPointWithLaneId>(
40+
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> &, const Pose &,
41+
const Pose &, const VelocityFactorStatus, const std::string &);
42+
template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPoint>(
43+
const std::vector<autoware_auto_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
44+
const VelocityFactorStatus, const std::string &);
45+
template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::TrajectoryPoint>(
46+
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> &, const Pose &,
47+
const Pose &, const VelocityFactorStatus, const std::string &);
48+
3549
} // namespace motion_utils

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

+52-2
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,24 @@
22
<arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
33
<arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
44

5+
<arg name="launch_motion_out_of_lane_module" default="true"/>
6+
<arg name="launch_dynamic_obstacle_stop_module" default="true"/>
7+
<arg name="launch_module_list_end" default="&quot;&quot;]"/>
8+
9+
<!-- assemble launch config for motion velocity planner -->
10+
<arg name="motion_velocity_planner_launch_modules" default="["/>
11+
<let
12+
name="motion_velocity_planner_launch_modules"
13+
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::OutOfLaneModule, '&quot;)"
14+
if="$(var launch_motion_out_of_lane_module)"
15+
/>
16+
<let
17+
name="motion_velocity_planner_launch_modules"
18+
value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'motion_velocity_planner::DynamicObstacleStopModule, '&quot;)"
19+
if="$(var launch_dynamic_obstacle_stop_module)"
20+
/>
21+
<let name="motion_velocity_planner_launch_modules" value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
22+
523
<node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="" output="screen">
624
<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
725
</node_container>
@@ -91,12 +109,44 @@
91109
</group>
92110
</group>
93111

94-
<!-- velocity limiter -->
112+
<!-- plan slowdown or stops on the final trajectory -->
95113
<group>
96114
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
97-
<composable_node pkg="obstacle_velocity_limiter" plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode" name="obstacle_velocity_limiter" namespace="">
115+
<composable_node pkg="autoware_motion_velocity_planner_node" plugin="autoware::motion_velocity_planner::MotionVelocityPlannerNode" name="motion_velocity_planner" namespace="">
98116
<!-- topic remap -->
99117
<remap from="~/input/trajectory" to="obstacle_avoidance_planner/trajectory"/>
118+
<remap from="~/input/vector_map" to="/map/vector_map"/>
119+
<remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
120+
<remap from="~/input/accel" to="/localization/acceleration"/>
121+
<remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
122+
<remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
123+
<remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
124+
<remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
125+
<remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
126+
<remap from="~/output/trajectory" to="motion_velocity_planner/trajectory"/>
127+
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
128+
<remap from="~/output/velocity_factors" to="/planning/velocity_factors/motion_velocity_planner"/>
129+
<!-- params -->
130+
<param name="launch_modules" value="$(var motion_velocity_planner_launch_modules)"/>
131+
<param from="$(var common_param_path)"/>
132+
<param from="$(var vehicle_param_file)"/>
133+
<param from="$(var nearest_search_param_path)"/>
134+
<param from="$(var motion_velocity_smoother_param_path)"/>
135+
<param from="$(var motion_velocity_smoother_type_param_path)"/>
136+
<param from="$(var motion_velocity_planner_param_path)"/>
137+
<param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
138+
<!-- <param from="$(var motion_velocity_planner_template_param_path)"/> -->
139+
<!-- composable node config -->
140+
<extra_arg name="use_intra_process_comms" value="false"/>
141+
</composable_node>
142+
</load_composable_node>
143+
</group>
144+
<!-- velocity limiter TODO(Maxime): move this node to the motion_velocity_planner -->
145+
<group>
146+
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
147+
<composable_node pkg="obstacle_velocity_limiter" plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode" name="obstacle_velocity_limiter" namespace="">
148+
<!-- topic remap -->
149+
<remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
100150
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
101151
<remap from="~/input/dynamic_obstacles" to="/perception/object_recognition/objects"/>
102152
<remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>

‎planning/.pages

+4
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,10 @@ nav:
6363
- 'Surround Obstacle Checker':
6464
- 'About Surround Obstacle Checker': planning/surround_obstacle_checker
6565
- 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja
66+
- 'Motion Velocity Planner':
67+
- 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/
68+
- 'Available Modules':
69+
- 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/
6670
- 'Motion Velocity Smoother':
6771
- 'About Motion Velocity Smoother': planning/motion_velocity_smoother
6872
- 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_motion_velocity_out_of_lane_module)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml)
7+
8+
ament_auto_add_library(${PROJECT_NAME} SHARED
9+
DIRECTORY src
10+
)
11+
12+
if(BUILD_TESTING)
13+
find_package(ament_lint_auto REQUIRED)
14+
ament_lint_auto_find_test_dependencies()
15+
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
16+
test/test_filter_predicted_objects.cpp
17+
)
18+
target_link_libraries(test_${PROJECT_NAME}
19+
${PROJECT_NAME}
20+
)
21+
endif()
22+
23+
ament_auto_package(INSTALL_TO_SHARE config)

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md

+165
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
## Out of Lane
2+
3+
### Role
4+
5+
`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects.
6+
7+
### Activation Timing
8+
9+
This module is activated if `launch_out_of_lane` is set to true.
10+
11+
### Inner-workings / Algorithms
12+
13+
The algorithm is made of the following steps.
14+
15+
1. Calculate the ego path footprints (red).
16+
2. Calculate the other lanes (magenta).
17+
3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green).
18+
4. For each overlapping range, decide if a stop or slow down action must be taken.
19+
5. For each action, insert the corresponding stop or slow down point in the path.
20+
21+
![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png)
22+
23+
#### 1. Ego Path Footprints
24+
25+
In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters.
26+
27+
#### 2. Other lanes
28+
29+
In the second step, the set of lanes to consider for overlaps is generated.
30+
This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets.
31+
The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`.
32+
33+
A lanelet is deemed non-relevant if it meets one of the following conditions.
34+
35+
- It is part of the lanelets followed by the ego path.
36+
- It contains the rear point of the ego footprint.
37+
- It follows one of the ego path lanelets.
38+
39+
#### 3. Overlapping ranges
40+
41+
In the third step, overlaps between the ego path footprints and the other lanes are calculated.
42+
For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`.
43+
For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored.
44+
Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored.
45+
Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information:
46+
47+
- overlapped other lane $l$.
48+
- start and end ego path indexes.
49+
- start and end ego path arc lengths.
50+
- start and end overlap points.
51+
52+
#### 4. Decisions
53+
54+
In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects.
55+
The conditions for the decision depend on the value of the `mode` parameter.
56+
57+
Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path).
58+
If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used.
59+
If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used.
60+
61+
<table width="100%">
62+
<tr>
63+
<td>
64+
<img src="./docs/out_of_lane_slow.png" width="550px"/>
65+
</td>
66+
<td>
67+
<img src="./docs/out_of_lane_stop.png" width="550px"/>
68+
</td>
69+
</tr>
70+
</table>
71+
72+
##### Threshold
73+
74+
With the `mode` set to `"threshold"`,
75+
a decision to stop or slow down before a range is made if
76+
an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`.
77+
78+
##### TTC (time to collision)
79+
80+
With the `mode` set to `"ttc"`,
81+
estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated.
82+
This is then used to calculate the time to collision over the period where ego crosses the overlap.
83+
If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made.
84+
85+
##### Intervals
86+
87+
With the `mode` set to `"intervals"`,
88+
the estimated times when ego and the dynamic objects reach the start and end points of
89+
the overlapping range are used to create time intervals.
90+
These intervals can be made shorter or longer using the
91+
`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters.
92+
If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made.
93+
94+
##### Time estimates
95+
96+
###### Ego
97+
98+
To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path
99+
at its current velocity or at half the velocity of the path points, whichever is higher.
100+
101+
###### Dynamic objects
102+
103+
Two methods are used to estimate the time when a dynamic objects with reach some point.
104+
If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter.
105+
Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity.
106+
107+
#### 5. Path update
108+
109+
Finally, for each decision to stop or slow down before an overlapping range,
110+
a point is inserted in the path.
111+
For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$,
112+
a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$.
113+
Such point with no overlap must exist since, by definition of the overlapping range,
114+
we know that there is no overlap at $i-1$.
115+
116+
If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter),
117+
it is skipped.
118+
119+
Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible.
120+
121+
### Module Parameters
122+
123+
| Parameter | Type | Description |
124+
| ----------------------------- | ------ | --------------------------------------------------------------------------------- |
125+
| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc |
126+
| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane |
127+
128+
| Parameter /threshold | Type | Description |
129+
| -------------------- | ------ | ---------------------------------------------------------------- |
130+
| `time_threshold` | double | [s] consider objects that will reach an overlap within this time |
131+
132+
| Parameter /intervals | Type | Description |
133+
| --------------------- | ------ | ------------------------------------------------------- |
134+
| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer |
135+
| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer |
136+
137+
| Parameter /ttc | Type | Description |
138+
| -------------- | ------ | ------------------------------------------------------------------------------------------------------ |
139+
| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap |
140+
141+
| Parameter /objects | Type | Description |
142+
| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
143+
| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value |
144+
| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered |
145+
| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in |
146+
147+
| Parameter /overlap | Type | Description |
148+
| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- |
149+
| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered |
150+
| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) |
151+
152+
| Parameter /action | Type | Description |
153+
| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
154+
| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed |
155+
| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane |
156+
| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap |
157+
| `slowdown.velocity` | double | [m] slow down velocity |
158+
| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap |
159+
160+
| Parameter /ego | Type | Description |
161+
| -------------------- | ------ | ---------------------------------------------------- |
162+
| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint |
163+
| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint |
164+
| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint |
165+
| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint |

0 commit comments

Comments
 (0)