diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..20f4b3aa8f4d3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_dynamic_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..8aa3415c3f329 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -0,0 +1,85 @@ +## Dynamic Obstacle Stop + +### Role + +`dynamic_obstacle_stop` is a module that stops the ego vehicle from entering the _immediate_ path of a dynamic object. + +The _immediate_ path of an object is the area that the object would traverse during a given time horizon, assuming constant velocity and heading. + +![rviz example](docs/dynamic_obstacle_stop_rviz.png) + +### Activation Timing + +This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the behavior planning launch file. + +### Inner-workings / Algorithms + +The module insert a stop point where the ego path collides with the immediate path of an object. +The overall module flow can be summarized with the following 4 steps. + +1. Filter dynamic objects. +2. Calculate immediate path rectangles of the dynamic objects. +3. Find earliest collision where ego collides with an immediate path rectangle. +4. Insert stop point before the collision. + +In addition to these 4 steps, 2 mechanisms are in place to make the stop point of this module more stable: an hysteresis and a decision duration buffer. + +The `hysteresis` parameter is used when a stop point was already being inserted in the previous iteration +and it increases the range where dynamic objects are considered close enough to the ego path to be used by the module. + +The `decision_duration_buffer` parameter defines the duration when the module will keep inserted the previous stop point, even after no collisions were found. + +#### Filter dynamic objects + +![filtering example](docs/DynamicObstacleStop-Filtering.drawio.svg) + +An object is considered by the module only if it meets all of the following conditions: + +- it is a vehicle (pedestrians are ignored); +- it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; +- it is not too close to the current position of the ego vehicle; +- it is close to the ego path. + +For the last condition, +the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). +In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. + +#### Calculate immediate path rectangles + +![Immediate paths example](docs/DynamicObstacleStop-ImmediatePaths.drawio.svg) + +For each considered object, a rectangle is created representing its _immediate_ path. +The rectangle has the width of the object plus the `extra_object_width` parameter +and its length is the current speed of the object multiplied by the `time_horizon`. + +#### Find earliest collision + +![Earliest collision example](docs/DynamicObstacleStop-Collision.drawio.svg) + +We build the ego path footprints as the set of ego footprint polygons projected on each path point. +We then calculate the intersections between these ego path footprints and the previously calculated immediate path rectangles. +An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the path point is larger than $\frac{3 \pi}{4}$. + +The collision point with the lowest arc length when projected on the ego path will be used to calculate the final stop point. + +#### Insert stop point + +![Stop point example](docs/DynamicObstacleStop-StopPoint.drawio.svg) + +Before inserting a stop point, we calculate the range of path arc lengths where it can be inserted. +The minimum is calculated to satisfy the acceleration and jerk constraints of the vehicle. +If a stop point was inserted in the previous iteration of the module, its arc length is used as the maximum. +Finally, +the stop point arc length is calculated to be the arc length of the previously found collision point minus the `stop_distance_buffer` and the ego vehicle longitudinal offset, clamped between the minimum and maximum values. + +### Module Parameters + +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml new file mode 100644 index 0000000000000..14483093e8bb3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object + extra_object_width: 1.0 # [m] extra width around detected objects + minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored + stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point + time_horizon: 5.0 # [s] time horizon used for collision checks + hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection + decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled + minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg new file mode 100644 index 0000000000000..77beb27c20db8 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg @@ -0,0 +1,236 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + + + + + + + +
+
+
+
abs(angle) < ¾π
+
collisions are ignored
+
+
+
+ + abs(angle) < ¾π... + + + + + + + +
+
+
+ earliest collision point +
+
+
+ + earliest collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg new file mode 100644 index 0000000000000..08638931b599f --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + +
+
+
+ NPC 2 +
+
+
+ + NPC 2 + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + +
+
+
+
NPC 4
+
+
+
+ + NPC 4 + + + + + + + + +
+
+
+ NPC 2 is too far from the path and is ignored +
+
+
+ + NPC 2 is too far from the path and is ig... + + + + + + + + + +
+
+
+ NPC 4 is too close to ego and is ignored +
+
+
+ + NPC 4 is too close to ego and is ignored + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg new file mode 100644 index 0000000000000..3f07c95c7399d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg @@ -0,0 +1,103 @@ + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + +
+
+
+
object width +
+
extra_object_width
+
+
+
+ + object width +... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg new file mode 100644 index 0000000000000..0b0953cbea265 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg @@ -0,0 +1,98 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + +
+
+
+ ego longitudinal offset + extra_distance_buffer +
+
+
+ + ego longitudinal offset + extra_distance_buffer + + + + + + + +
+
+
+ stop point +
+
+
+ + stop point + + + + + + +
+
+
+ collision point +
+
+
+ + collision... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png new file mode 100644 index 0000000000000..169cb40875d2d Binary files /dev/null and b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png differ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..7e9599c49bc2d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -0,0 +1,36 @@ + + + + behavior_velocity_dynamic_obstacle_stop_module + 0.1.0 + dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object + + Maxime Clement + Mamoru Sobue + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..2f5662c7998ac --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp new file mode 100644 index 0000000000000..a06580fbbaac5 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "collision.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data) +{ + auto earliest_idx = ego_data.path_footprints.size(); + std::vector path_points; + for (const auto & p : ego_data.path.points) path_points.push_back(p.point); + auto earliest_arc_length = motion_utils::calcArcLength(path_points); + std::optional earliest_collision_point; + debug_data.collisions.clear(); + std::vector rough_collisions; + for (auto obstacle_idx = 0UL; obstacle_idx < objects.size(); ++obstacle_idx) { + rough_collisions.clear(); + const auto & obstacle_pose = objects[obstacle_idx].kinematics.initial_pose_with_covariance.pose; + const auto & obstacle_footprint = obstacle_forward_footprints[obstacle_idx]; + ego_data.rtree.query( + boost::geometry::index::intersects(obstacle_footprint), std::back_inserter(rough_collisions)); + for (const auto & rough_collision : rough_collisions) { + const auto path_idx = rough_collision.second; + const auto & ego_footprint = ego_data.path_footprints[path_idx]; + const auto & ego_pose = ego_data.path.points[ego_data.first_path_idx + path_idx].point.pose; + const auto angle_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(ego_pose.orientation) - tf2::getYaw(obstacle_pose.orientation)); + if (path_idx <= earliest_idx && std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { + tier4_autoware_utils::MultiPoint2d collision_points; + boost::geometry::intersection( + obstacle_footprint.outer(), ego_footprint.outer(), collision_points); + earliest_idx = path_idx; + for (const auto & coll_p : collision_points) { + auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); + const auto arc_length = + motion_utils::calcSignedArcLength(path_points, ego_data.first_path_idx, p); + if (arc_length < earliest_arc_length) { + earliest_arc_length = arc_length; + debug_data.collisions = {obstacle_footprint, ego_footprint}; + earliest_collision_point = p; + } + } + } + } + } + return earliest_collision_point; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp new file mode 100644 index 0000000000000..ccc3fa94df603 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COLLISION_HPP_ +#define COLLISION_HPP_ + +#include "types.hpp" + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief find the earliest collision along the ego path and an obstacle footprint +/// @param [in] ego_data ego data including its path and footprint +/// @param [in] objects obstacles +/// @param [in] obstacle_forward_footprints obstacle footprints +/// @param [in] debug_data debug data +/// @return the point of earliest collision along the ego path +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp new file mode 100644 index 0000000000000..cc5dafa218654 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "debug.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ + +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = ns; + for (marker.id = static_cast(from); marker.id < static_cast(to); ++marker.id) + markers.push_back(marker); + return markers; +} + +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "dynamic_obstacles"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 1.0); + marker.text = "dynamic obstacle"; + for (const auto & obstacle : obstacles) { + marker.pose = obstacle.kinematics.initial_pose_with_covariance.pose; + markers.push_back(marker); + ++marker.id; + } + return markers; +} + +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + for (const auto & footprint : footprints) { + marker.points.clear(); + for (const auto & p : footprint.outer()) { + marker.points.emplace_back(); + marker.points.back().x = p.x(); + marker.points.back().y = p.y(); + marker.points.back().z = z; + } + markers.push_back(marker); + ++marker.id; + } + return markers; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp new file mode 100644 index 0000000000000..02f550d314d39 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns); +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles); +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); +std::vector make_collision_markers( + const tier4_autoware_utils::MultiPoint2d & collisions, const double z); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug + +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp new file mode 100644 index 0000000000000..6a0c61963ac81 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -0,0 +1,84 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "footprint.hpp" + +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis) +{ + tier4_autoware_utils::MultiPolygon2d forward_footprints; + for (const auto & obstacle : obstacles) + forward_footprints.push_back(project_to_pose( + make_forward_footprint(obstacle, params, hysteresis), + obstacle.kinematics.initial_pose_with_covariance.pose)); + return forward_footprints; +} + +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis) +{ + const auto & shape = obstacle.shape.dimensions; + const auto longitudinal_offset = + shape.x / 2.0 + + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; + const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; + return tier4_autoware_utils::Polygon2d{ + {{longitudinal_offset, -lateral_offset}, + {longitudinal_offset, lateral_offset}, + {shape.x / 2.0, lateral_offset}, + {shape.x / 2.0, -lateral_offset}, + {longitudinal_offset, -lateral_offset}}, + {}}; +} + +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + tier4_autoware_utils::Polygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) +{ + for (const auto & p : ego_data.path.points) + ego_data.path_footprints.push_back(tier4_autoware_utils::toFootprint( + p.point.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + for (auto i = 0UL; i < ego_data.path_footprints.size(); ++i) { + const auto box = + boost::geometry::return_envelope(ego_data.path_footprints[i]); + ego_data.rtree.insert(std::make_pair(box, i)); + } +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp new file mode 100644 index 0000000000000..c28e89ac6c9f6 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +/// @brief create the footprint of the given obstacles and their projection over a fixed time +/// horizon +/// @param [in] obstacles obstacles +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprints +/// @return forward footprint of the obstacle +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis); +/// @brief create the footprint of the given obstacle and its projection over a fixed time horizon +/// @param [in] obstacle obstacle +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprint +/// @return forward footprint of the obstacle +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief create the rtree indexing the ego footprint along the path +/// @param [inout] ego_data ego data with its path and the rtree to populate +/// @param [in] params parameters +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp new file mode 100644 index 0000000000000..226bd3b19a415 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -0,0 +1,68 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include "scene_dynamic_obstacle_stop.hpp" + +#include + +namespace behavior_velocity_planner +{ +DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + + pp.extra_object_width = node.declare_parameter(ns + ".extra_object_width"); + pp.minimum_object_velocity = node.declare_parameter(ns + ".minimum_object_velocity"); + pp.stop_distance_buffer = node.declare_parameter(ns + ".stop_distance_buffer"); + pp.time_horizon = node.declare_parameter(ns + ".time_horizon"); + pp.hysteresis = node.declare_parameter(ns + ".hysteresis"); + pp.decision_duration_buffer = node.declare_parameter(ns + ".decision_duration_buffer"); + pp.minimum_object_distance_from_ego_path = + node.declare_parameter(ns + ".minimum_object_distance_from_ego_path"); + + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.ego_lateral_offset = + std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); + pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; +} + +void DynamicObstacleStopModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) return; + // general + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); + } +} + +std::function &)> +DynamicObstacleStopModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; + }; +} +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DynamicObstacleStopModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp new file mode 100644 index 0000000000000..e461cc9c16445 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_dynamic_obstacle_stop.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface +{ +public: + explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "dynamic_obstacle_stop"; } + +private: + using PlannerParam = dynamic_obstacle_stop::PlannerParam; + + PlannerParam planner_param_; + int64_t module_id_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class DynamicObstacleStopModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp new file mode 100644 index 0000000000000..0e11223f54158 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -0,0 +1,74 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "object_filtering.hpp" + +#include "types.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis) +{ + std::vector filtered_objects; + const auto is_vehicle = [](const auto & o) { + return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + }) != o.classification.end(); + }; + std::vector path_points; + for (const auto & p : ego_data.path.points) path_points.push_back(p.point); + const auto is_in_range = [&](const auto & o) { + const auto distance = std::abs(motion_utils::calcLateralOffset( + path_points, o.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset + + o.shape.dimensions.y / 2.0 + hysteresis; + }; + const auto is_not_too_close = [&](const auto & o) { + const auto obj_arc_length = motion_utils::calcSignedArcLength( + path_points, ego_data.pose.position, o.kinematics.initial_pose_with_covariance.pose.position); + return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; + }; + for (const auto & object : objects.objects) + if ( + is_vehicle(object) && + object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity && + is_in_range(object) && is_not_too_close(object)) + filtered_objects.push_back(object); + return filtered_objects; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp new file mode 100644 index 0000000000000..22857f6db1bda --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBJECT_FILTERING_HPP_ +#define OBJECT_FILTERING_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp new file mode 100644 index 0000000000000..5deac35403550 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_dynamic_obstacle_stop.hpp" + +#include "collision.hpp" +#include "debug.hpp" +#include "footprint.hpp" +#include "object_filtering.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +DynamicObstacleStopModule::DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) +{ + prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + velocity_factor_.init(VelocityFactor::UNKNOWN); +} + +bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_.reset_data(); + *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); + if (!path || path->points.size() < 2) return true; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + + stopwatch.tic("preprocessing"); + EgoData ego_data; + ego_data.pose = planner_data_->current_odometry->pose; + ego_data.path.points = path->points; + motion_utils::removeOverlapPoints(ego_data.path.points); + ego_data.first_path_idx = + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( + ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + + make_ego_footprint_rtree(ego_data, params_); + const auto has_decided_to_stop = + (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + if (!has_decided_to_stop) current_stop_pose_.reset(); + double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; + const auto dynamic_obstacles = + filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); + + const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); + + stopwatch.tic("footprints"); + const auto obstacle_forward_footprints = + make_forward_footprints(dynamic_obstacles, params_, hysteresis); + const auto footprints_duration_us = stopwatch.toc("footprints"); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + stopwatch.tic("collisions"); + const auto collision = + find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); + const auto collisions_duration_us = stopwatch.toc("collisions"); + if (collision) { + const auto arc_length_diff = + motion_utils::calcSignedArcLength(ego_data.path.points, *collision, ego_data.pose.position); + const auto can_stop_before_limit = arc_length_diff < min_stop_distance - + params_.ego_longitudinal_offset - + params_.stop_distance_buffer; + const auto stop_pose = can_stop_before_limit + ? motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, *collision, + -params_.stop_distance_buffer - params_.ego_longitudinal_offset) + : motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); + if (stop_pose) { + const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( + path->points, stop_pose->position, + current_stop_pose_->position) > 0.0; + if (use_new_stop_pose) current_stop_pose_ = *stop_pose; + prev_stop_decision_time_ = clock_->now(); + } + } + + if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points); + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " + "%2.2fus\n\tcollisions = %2.2fus\n", + total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); + debug_data_.ego_footprints = ego_data.path_footprints; + debug_data_.obstacle_footprints = obstacle_forward_footprints; + return true; +} + +MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() +{ + constexpr auto z = 0.0; + MarkerArray debug_marker_array; + // dynamic obstacles footprints + const auto obstacle_footprint_markers = + debug::make_polygon_markers(debug_data_.obstacle_footprints, "dynamic_obstacles_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), obstacle_footprint_markers.begin(), + obstacle_footprint_markers.end()); + const auto delete_footprint_markers = debug::make_delete_markers( + obstacle_footprint_markers.size(), debug_data_.prev_dynamic_obstacles_nb, + "dynamic_obstacles_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_footprint_markers.begin(), + delete_footprint_markers.end()); + // ego path footprints + const auto ego_footprint_markers = + debug::make_polygon_markers(debug_data_.ego_footprints, "ego_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), ego_footprint_markers.begin(), ego_footprint_markers.end()); + const auto delete_ego_footprint_markers = debug::make_delete_markers( + ego_footprint_markers.size(), debug_data_.prev_ego_footprints_nb, "ego_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_ego_footprint_markers.begin(), + delete_ego_footprint_markers.end()); + // collisions + auto collision_markers = debug::make_polygon_markers(debug_data_.collisions, "collisions", z); + for (auto & m : collision_markers) m.color.r = 1.0; + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), collision_markers.begin(), collision_markers.end()); + const auto delete_collision_markers = debug::make_delete_markers( + collision_markers.size(), debug_data_.prev_collisions_nb, "collisions"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_collision_markers.begin(), + delete_collision_markers.end()); + + debug_data_.prev_dynamic_obstacles_nb = obstacle_footprint_markers.size(); + debug_data_.prev_collisions_nb = collision_markers.size(); + debug_data_.prev_ego_footprints_nb = ego_footprint_markers.size(); + return debug_marker_array; +} + +motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls{}; + if (current_stop_pose_) { + motion_utils::VirtualWall virtual_wall; + virtual_wall.text = "dynamic_obstacle_stop"; + virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; + virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.pose = *current_stop_pose_; + virtual_walls.push_back(virtual_wall); + } + return virtual_walls; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp new file mode 100644 index 0000000000000..c7bca149890a0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ +#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ + +#include "types.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +class DynamicObstacleStopModule : public SceneModuleInterface +{ +public: + DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); + + /// @brief insert stop or slow down points to prevent dangerously entering another lane + /// @param [inout] path the path to update + /// @param [inout] stop_reason reason for stopping + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + motion_utils::VirtualWalls createVirtualWalls() override; + +private: + PlannerParam params_; + rclcpp::Time prev_stop_decision_time_; + std::optional current_stop_pose_; + +protected: + int64_t module_id_{}; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..74fd5ca818fb0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +typedef std::pair BoxIndexPair; +typedef boost::geometry::index::rtree> Rtree; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + double extra_object_width; + double minimum_object_velocity; + double stop_distance_buffer; + double time_horizon; + double hysteresis; + double decision_duration_buffer; + double ego_longitudinal_offset; + double ego_lateral_offset; + double minimum_object_distance_from_ego_path; +}; + +struct EgoData +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + size_t first_path_idx{}; + double longitudinal_offset_to_first_path_idx; // [m] + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::MultiPolygon2d path_footprints; + Rtree rtree; +}; + +/// @brief debug data +struct DebugData +{ + tier4_autoware_utils::MultiPolygon2d obstacle_footprints{}; + size_t prev_dynamic_obstacles_nb{}; + tier4_autoware_utils::MultiPolygon2d collisions{}; + size_t prev_collisions_nb{}; + tier4_autoware_utils::MultiPolygon2d ego_footprints{}; + size_t prev_ego_footprints_nb{}; + std::optional stop_pose{}; + void reset_data() + { + obstacle_footprints.clear(); + collisions.clear(); + ego_footprints.clear(); + stop_pose.reset(); + } +}; + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index e8377f841aa77..6fd666ea67a4e 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -24,3 +24,4 @@ # behavior_velocity_planner::SpeedBumpModulePlugin - behavior_velocity_planner::OutOfLaneModulePlugin - behavior_velocity_planner::NoDrivableLaneModulePlugin + - behavior_velocity_planner::DynamicObstacleStopModulePlugin diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index b517b77b328c7..8b0e215bc550a 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -20,6 +20,7 @@ + @@ -41,6 +42,7 @@ + @@ -69,6 +71,7 @@ +