From 800457f01a061ca7f44172fd21daefed9c43fdc6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 18 Dec 2023 17:30:45 +0900 Subject: [PATCH 01/26] Add barebone function to add virtual occluded object Signed-off-by: Maxime CLEMENT --- .../CMakeLists.txt | 6 +- .../package.xml | 5 + .../src/occluded_object.hpp | 195 ++++++++++++++++++ .../src/scene_crosswalk.cpp | 32 ++- .../src/scene_crosswalk.hpp | 3 + 5 files changed, 234 insertions(+), 7 deletions(-) create mode 100644 planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 3217b3828b8d7..69dd1ced695ec 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -6,10 +6,8 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene_crosswalk.cpp - src/util.cpp + DIRECTORY + src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 9eef11e72239e..2c38a9836e0af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -28,9 +28,14 @@ behavior_velocity_planner_common eigen geometry_msgs + grid_map_core + grid_map_ros + grid_map_utils + interpolation lanelet2_extension libboost-dev motion_utils + nav_msgs pcl_conversions pluginlib rclcpp diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp new file mode 100644 index 0000000000000..d7bd342fa94fb --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp @@ -0,0 +1,195 @@ +// 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 OCCLUDED_OBJECT_HPP_ +#define OCCLUDED_OBJECT_HPP_ + +#include "behavior_velocity_crosswalk_module/util.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +std::optional create_occluded_object( + const tier4_autoware_utils::Polygon2d & attention_polygon, + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const autoware_auto_planning_msgs::msg::PathWithLaneId & ego_path, + const geometry_msgs::msg::Point & ego_position) +{ + // TODO(Maxime): param + constexpr auto virtual_object_size = 0.5; + constexpr auto virtual_object_velocity = 1.0; + constexpr auto virtual_object_time_resolution = 0.5; + constexpr auto virtual_object_time_horizon = 5.0; + + std::optional occluded_object{}; + + tier4_autoware_utils::LineString2d path_ls; + for (const auto & p : ego_path.points) + path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); + + tier4_autoware_utils::MultiPoint2d intersections; + tier4_autoware_utils::LineString2d crosswalk_centerline; + for (const auto & p : crosswalk_lanelet.centerline2d().basicLineString()) + crosswalk_centerline.emplace_back(p.x(), p.y()); + boost::geometry::intersection(crosswalk_centerline, path_ls, intersections); + if (intersections.empty()) return {}; + const auto crosswalk_center = intersections.front(); + const auto range_distance = virtual_object_velocity * virtual_object_time_horizon; + const grid_map::Polygon in_range_poly{std::vector({ + {crosswalk_center.x() - range_distance, crosswalk_center.y() + range_distance}, + {crosswalk_center.x() + range_distance, crosswalk_center.y() + range_distance}, + {crosswalk_center.x() + range_distance, crosswalk_center.y() - range_distance}, + {crosswalk_center.x() - range_distance, crosswalk_center.y() - range_distance}, + })}; + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + // ignore the attention polygon + grid_map::Polygon attention_poly; + boost::geometry::for_each_point( + attention_polygon, [&](const auto & p) { attention_poly.addVertex(p); }); + for (auto iter = grid_map_utils::PolygonIterator(grid_map, attention_poly); !iter.isPastEnd(); + ++iter) + grid_map.at("layer", *iter) = 0.0; + + // find occluded point closest to the crosswalk center + const auto crosswalk_center_arc_length = + lanelet::geometry::toArcCoordinates(crosswalk_lanelet.centerline2d(), crosswalk_center).length; + std::optional closest_arc_coordinates; + double closest_arc_length_distance = std::numeric_limits::max(); + geometry_msgs::msg::Point p; + grid_map::Position closest_position; + const auto min_nb_of_cells = std::ceil(virtual_object_size / grid_map.getResolution()); + for (grid_map_utils::PolygonIterator iter(grid_map, in_range_poly); !iter.isPastEnd(); ++iter) { + const auto idx = *iter; + // TODO(Maxime): move to function + const auto is_occluded = [&]() { + grid_map::Index idx_offset; + for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { + for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { + const auto index = idx + idx_offset; + if ((index < grid_map.getSize()).all()) { + const auto cell_value = grid_map.at("layer", index); + // TODO(Maxime): magic number -> params + if (cell_value < 47 || cell_value > 53) return false; + } + } + } + return true; + }(); + if (is_occluded) { + grid_map::Position position; + grid_map.getPosition(idx, position); + // this projection can be outside of the centerline + const auto projection = + lanelet::geometry::project(crosswalk_lanelet.centerline2d(), position); + // this arc coordinate is clamped between 0 and the length of the centerline + auto arc_coordinates = + lanelet::geometry::toArcCoordinates(crosswalk_lanelet.centerline2d(), projection); + arc_coordinates.distance = lanelet::geometry::distance2d(projection, position); + if (arc_coordinates.length <= 0.0) { + arc_coordinates.length = + -lanelet::geometry::distance2d(crosswalk_lanelet.centerline2d().front(), projection); + } else if ( + arc_coordinates.length >= lanelet::geometry::length(crosswalk_lanelet.centerline2d())) { + arc_coordinates.length = + lanelet::geometry::length(crosswalk_lanelet.centerline2d()) + + lanelet::geometry::distance2d(crosswalk_lanelet.centerline2d().back(), projection); + } + const auto dist = std::abs(arc_coordinates.length - crosswalk_center_arc_length); + p.x = position.x(); + p.y = position.y(); + const auto is_after_ego = + motion_utils::calcSignedArcLength(ego_path.points, ego_position, p) > 0.0; + if (is_after_ego && dist < closest_arc_length_distance) { + closest_arc_length_distance = dist; + closest_arc_coordinates = arc_coordinates; + closest_position = position; + } + } + } + // TODO(Maxime): REMOVE + { + tier4_autoware_utils::Polygon2d in_range_polygon; + for (const auto & p : in_range_poly.getVertices()) + in_range_polygon.outer().emplace_back(p.x(), p.y()); + boost::geometry::correct(in_range_polygon); + } + if (closest_arc_coordinates) { + const auto inverse_projected_point = lanelet::geometry::fromArcCoordinates( + crosswalk_lanelet.centerline2d(), *closest_arc_coordinates); + closest_arc_coordinates->distance = 0.0; // assume a straight path to the crosswalk + const auto initial_position = lanelet::geometry::fromArcCoordinates( + crosswalk_lanelet.centerline2d(), *closest_arc_coordinates); + occluded_object.emplace(); + occluded_object->kinematics.initial_pose_with_covariance.pose.position.x = initial_position.x(); + occluded_object->kinematics.initial_pose_with_covariance.pose.position.y = initial_position.y(); + occluded_object->kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; + occluded_object->shape.dimensions.x = virtual_object_size; + occluded_object->shape.dimensions.y = virtual_object_size; + occluded_object->classification.emplace_back(); + occluded_object->classification.back().probability = 1.0; + occluded_object->classification.back().label = + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + + occluded_object->kinematics.predicted_paths.emplace_back(); + occluded_object->kinematics.predicted_paths.back().confidence = 1.0; + occluded_object->kinematics.predicted_paths.back().time_step = + rclcpp::Duration::from_seconds(virtual_object_time_resolution); + geometry_msgs::msg::Point crosswalk_center_position; + crosswalk_center_position.x = crosswalk_center.x(); + crosswalk_center_position.y = crosswalk_center.y(); + const auto dist_to_target = tier4_autoware_utils::calcDistance2d( + occluded_object->kinematics.initial_pose_with_covariance.pose.position, + crosswalk_center_position); + geometry_msgs::msg::Pose pose; + for (auto s = virtual_object_time_resolution * virtual_object_velocity; + s <= virtual_object_time_horizon * virtual_object_velocity; + s += virtual_object_time_resolution * virtual_object_velocity) { + pose.position.x = interpolation::lerp( + occluded_object->kinematics.initial_pose_with_covariance.pose.position.x, + crosswalk_center_position.x, s / dist_to_target); + pose.position.y = interpolation::lerp( + occluded_object->kinematics.initial_pose_with_covariance.pose.position.y, + crosswalk_center_position.y, s / dist_to_target); + occluded_object->kinematics.predicted_paths.back().path.push_back(pose); + } + } + return occluded_object; +} +} // namespace behavior_velocity_planner + +#endif // OCCLUDED_OBJECT_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cee6975df3155..34c497c9a8dde 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-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. @@ -14,6 +14,8 @@ #include "scene_crosswalk.hpp" +#include "occluded_object.hpp" + #include #include #include @@ -666,8 +668,6 @@ std::optional CrosswalkModule::getCollisionPoint( { stop_watch_.tic(__func__); - std::vector ret{}; - const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -1051,6 +1051,32 @@ void CrosswalkModule::updateObjectState( object.kinematics.initial_pose_with_covariance.pose, object.shape, getLabelColor(collision_state)); } + + constexpr auto use_occluded_pedestrian = true; // TODO(Maxime): param + constexpr auto time_buffer = 1.0; // TODO(Maxime): param + // Update the virtual object + if (use_occluded_pedestrian) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto occluded_object = create_occluded_object( + attention_area, crosswalk_, *planner_data_->occupancy_grid, sparse_resample_path, ego_pos); + if (occluded_object) { + if (!current_occlusion_start_time_) current_occlusion_start_time_ = clock_->now(); + if ( + current_occlusion_start_time_ && + (clock_->now() - *current_occlusion_start_time_).seconds() >= time_buffer) { + const auto collision_point = getCollisionPoint( + sparse_resample_path, *occluded_object, crosswalk_attention_range, attention_area); + object_info_manager_.update( + "virtual", occluded_object->kinematics.initial_pose_with_covariance.pose.position, + occluded_object->kinematics.initial_twist_with_covariance.twist.linear.x, clock_->now(), + is_ego_yielding, has_traffic_light, collision_point, + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, planner_param_, + crosswalk_.polygon2d().basicPolygon()); + } + } else { + current_occlusion_start_time_.reset(); + } + } object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d1196bb7b4a9a..6de929467c1f9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -430,6 +430,9 @@ class CrosswalkModule : public SceneModuleInterface // whether use regulatory element const bool use_regulatory_element_; + + // occluded space time buffer + std::optional current_occlusion_start_time_; }; } // namespace behavior_velocity_planner From 0f8b6f6520ebc6cb59ffd197dc031023dcfd858f Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 16 Jan 2024 11:25:49 +0900 Subject: [PATCH 02/26] Switching to a simpler slowdown based approach Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.hpp | 81 ++++++++ .../src/occluded_object.hpp | 195 ------------------ .../src/scene_crosswalk.cpp | 54 ++--- .../src/scene_crosswalk.hpp | 6 +- 4 files changed, 107 insertions(+), 229 deletions(-) create mode 100644 planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp delete mode 100644 planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp new file mode 100644 index 0000000000000..ee9a8edf4ad14 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -0,0 +1,81 @@ +// 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 OCCLUDED_CROSSWALK_HPP_ +#define OCCLUDED_CROSSWALK_HPP_ + +#include "behavior_velocity_crosswalk_module/util.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +bool is_crosswalk_occluded( + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid) +{ + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + + constexpr double min_size = 0.5; // TODO(Maxime): param + const auto min_nb_of_cells = std::ceil(min_size / grid_map.getResolution()); + grid_map::Polygon incoming_area_poly; + for (const auto & p : crosswalk_lanelet.polygon2d().basicPolygon()) + incoming_area_poly.addVertex(grid_map::Position(p.x(), p.y())); + for (grid_map_utils::PolygonIterator iter(grid_map, incoming_area_poly); !iter.isPastEnd(); + ++iter) { + const auto idx = *iter; + // TODO(Maxime): move to function + const auto is_occluded = [&]() { + grid_map::Index idx_offset; + for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { + for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { + const auto index = idx + idx_offset; + if ((index < grid_map.getSize()).all()) { + const auto cell_value = grid_map.at("layer", index); + // TODO(Maxime): magic number -> params + if (cell_value < 47 || cell_value > 53) return false; + } + } + } + return true; + }(); + if (is_occluded) return true; + } + return false; +} +} // namespace behavior_velocity_planner + +#endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp deleted file mode 100644 index d7bd342fa94fb..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_object.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// 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 OCCLUDED_OBJECT_HPP_ -#define OCCLUDED_OBJECT_HPP_ - -#include "behavior_velocity_crosswalk_module/util.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace behavior_velocity_planner -{ -std::optional create_occluded_object( - const tier4_autoware_utils::Polygon2d & attention_polygon, - const lanelet::ConstLanelet & crosswalk_lanelet, - const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const autoware_auto_planning_msgs::msg::PathWithLaneId & ego_path, - const geometry_msgs::msg::Point & ego_position) -{ - // TODO(Maxime): param - constexpr auto virtual_object_size = 0.5; - constexpr auto virtual_object_velocity = 1.0; - constexpr auto virtual_object_time_resolution = 0.5; - constexpr auto virtual_object_time_horizon = 5.0; - - std::optional occluded_object{}; - - tier4_autoware_utils::LineString2d path_ls; - for (const auto & p : ego_path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - - tier4_autoware_utils::MultiPoint2d intersections; - tier4_autoware_utils::LineString2d crosswalk_centerline; - for (const auto & p : crosswalk_lanelet.centerline2d().basicLineString()) - crosswalk_centerline.emplace_back(p.x(), p.y()); - boost::geometry::intersection(crosswalk_centerline, path_ls, intersections); - if (intersections.empty()) return {}; - const auto crosswalk_center = intersections.front(); - const auto range_distance = virtual_object_velocity * virtual_object_time_horizon; - const grid_map::Polygon in_range_poly{std::vector({ - {crosswalk_center.x() - range_distance, crosswalk_center.y() + range_distance}, - {crosswalk_center.x() + range_distance, crosswalk_center.y() + range_distance}, - {crosswalk_center.x() + range_distance, crosswalk_center.y() - range_distance}, - {crosswalk_center.x() - range_distance, crosswalk_center.y() - range_distance}, - })}; - grid_map::GridMap grid_map; - grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); - // ignore the attention polygon - grid_map::Polygon attention_poly; - boost::geometry::for_each_point( - attention_polygon, [&](const auto & p) { attention_poly.addVertex(p); }); - for (auto iter = grid_map_utils::PolygonIterator(grid_map, attention_poly); !iter.isPastEnd(); - ++iter) - grid_map.at("layer", *iter) = 0.0; - - // find occluded point closest to the crosswalk center - const auto crosswalk_center_arc_length = - lanelet::geometry::toArcCoordinates(crosswalk_lanelet.centerline2d(), crosswalk_center).length; - std::optional closest_arc_coordinates; - double closest_arc_length_distance = std::numeric_limits::max(); - geometry_msgs::msg::Point p; - grid_map::Position closest_position; - const auto min_nb_of_cells = std::ceil(virtual_object_size / grid_map.getResolution()); - for (grid_map_utils::PolygonIterator iter(grid_map, in_range_poly); !iter.isPastEnd(); ++iter) { - const auto idx = *iter; - // TODO(Maxime): move to function - const auto is_occluded = [&]() { - grid_map::Index idx_offset; - for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { - for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { - const auto index = idx + idx_offset; - if ((index < grid_map.getSize()).all()) { - const auto cell_value = grid_map.at("layer", index); - // TODO(Maxime): magic number -> params - if (cell_value < 47 || cell_value > 53) return false; - } - } - } - return true; - }(); - if (is_occluded) { - grid_map::Position position; - grid_map.getPosition(idx, position); - // this projection can be outside of the centerline - const auto projection = - lanelet::geometry::project(crosswalk_lanelet.centerline2d(), position); - // this arc coordinate is clamped between 0 and the length of the centerline - auto arc_coordinates = - lanelet::geometry::toArcCoordinates(crosswalk_lanelet.centerline2d(), projection); - arc_coordinates.distance = lanelet::geometry::distance2d(projection, position); - if (arc_coordinates.length <= 0.0) { - arc_coordinates.length = - -lanelet::geometry::distance2d(crosswalk_lanelet.centerline2d().front(), projection); - } else if ( - arc_coordinates.length >= lanelet::geometry::length(crosswalk_lanelet.centerline2d())) { - arc_coordinates.length = - lanelet::geometry::length(crosswalk_lanelet.centerline2d()) + - lanelet::geometry::distance2d(crosswalk_lanelet.centerline2d().back(), projection); - } - const auto dist = std::abs(arc_coordinates.length - crosswalk_center_arc_length); - p.x = position.x(); - p.y = position.y(); - const auto is_after_ego = - motion_utils::calcSignedArcLength(ego_path.points, ego_position, p) > 0.0; - if (is_after_ego && dist < closest_arc_length_distance) { - closest_arc_length_distance = dist; - closest_arc_coordinates = arc_coordinates; - closest_position = position; - } - } - } - // TODO(Maxime): REMOVE - { - tier4_autoware_utils::Polygon2d in_range_polygon; - for (const auto & p : in_range_poly.getVertices()) - in_range_polygon.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(in_range_polygon); - } - if (closest_arc_coordinates) { - const auto inverse_projected_point = lanelet::geometry::fromArcCoordinates( - crosswalk_lanelet.centerline2d(), *closest_arc_coordinates); - closest_arc_coordinates->distance = 0.0; // assume a straight path to the crosswalk - const auto initial_position = lanelet::geometry::fromArcCoordinates( - crosswalk_lanelet.centerline2d(), *closest_arc_coordinates); - occluded_object.emplace(); - occluded_object->kinematics.initial_pose_with_covariance.pose.position.x = initial_position.x(); - occluded_object->kinematics.initial_pose_with_covariance.pose.position.y = initial_position.y(); - occluded_object->kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; - occluded_object->shape.dimensions.x = virtual_object_size; - occluded_object->shape.dimensions.y = virtual_object_size; - occluded_object->classification.emplace_back(); - occluded_object->classification.back().probability = 1.0; - occluded_object->classification.back().label = - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - - occluded_object->kinematics.predicted_paths.emplace_back(); - occluded_object->kinematics.predicted_paths.back().confidence = 1.0; - occluded_object->kinematics.predicted_paths.back().time_step = - rclcpp::Duration::from_seconds(virtual_object_time_resolution); - geometry_msgs::msg::Point crosswalk_center_position; - crosswalk_center_position.x = crosswalk_center.x(); - crosswalk_center_position.y = crosswalk_center.y(); - const auto dist_to_target = tier4_autoware_utils::calcDistance2d( - occluded_object->kinematics.initial_pose_with_covariance.pose.position, - crosswalk_center_position); - geometry_msgs::msg::Pose pose; - for (auto s = virtual_object_time_resolution * virtual_object_velocity; - s <= virtual_object_time_horizon * virtual_object_velocity; - s += virtual_object_time_resolution * virtual_object_velocity) { - pose.position.x = interpolation::lerp( - occluded_object->kinematics.initial_pose_with_covariance.pose.position.x, - crosswalk_center_position.x, s / dist_to_target); - pose.position.y = interpolation::lerp( - occluded_object->kinematics.initial_pose_with_covariance.pose.position.y, - crosswalk_center_position.y, s / dist_to_target); - occluded_object->kinematics.predicted_paths.back().path.push_back(pose); - } - } - return occluded_object; -} -} // namespace behavior_velocity_planner - -#endif // OCCLUDED_OBJECT_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 34c497c9a8dde..65f0eba49bc25 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -14,7 +14,7 @@ #include "scene_crosswalk.hpp" -#include "occluded_object.hpp" +#include "occluded_crosswalk.hpp" #include #include @@ -230,7 +230,25 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Apply safety slow down speed if defined in Lanelet2 map if (crosswalk_.hasAttribute("safety_slow_down_speed")) { - applySafetySlowDownSpeed(*path, path_intersects); + applySafetySlowDownSpeed( + *path, path_intersects, + static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); + } + // TODO(Maxime): param + constexpr auto use_occluded_space = true; + constexpr auto occluded_slow_down_speed = 1.0f; + constexpr auto time_buffer = 1.0; + if (use_occluded_space && is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid)) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); + if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) + most_recent_occlusion_time_ = clock_->now(); + } else { + current_initial_occlusion_time_.reset(); + } + if ( + most_recent_occlusion_time_ && + (clock_->now() - *most_recent_occlusion_time_).seconds() <= time_buffer) { + applySafetySlowDownSpeed(*path, path_intersects, occluded_slow_down_speed); } recordTime(2); @@ -786,7 +804,8 @@ CollisionPoint CrosswalkModule::createCollisionPoint( } void CrosswalkModule::applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects) + PathWithLaneId & output, const std::vector & path_intersects, + const float safety_slow_down_speed) { if (path_intersects.empty()) { return; @@ -795,10 +814,6 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_path = output; - // Safety slow down speed in [m/s] - const auto & safety_slow_down_speed = - static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()); - if (!passed_safety_slow_point_) { // Safety slow down distance [m] const double safety_slow_down_distance = @@ -1052,31 +1067,6 @@ void CrosswalkModule::updateObjectState( getLabelColor(collision_state)); } - constexpr auto use_occluded_pedestrian = true; // TODO(Maxime): param - constexpr auto time_buffer = 1.0; // TODO(Maxime): param - // Update the virtual object - if (use_occluded_pedestrian) { - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto occluded_object = create_occluded_object( - attention_area, crosswalk_, *planner_data_->occupancy_grid, sparse_resample_path, ego_pos); - if (occluded_object) { - if (!current_occlusion_start_time_) current_occlusion_start_time_ = clock_->now(); - if ( - current_occlusion_start_time_ && - (clock_->now() - *current_occlusion_start_time_).seconds() >= time_buffer) { - const auto collision_point = getCollisionPoint( - sparse_resample_path, *occluded_object, crosswalk_attention_range, attention_area); - object_info_manager_.update( - "virtual", occluded_object->kinematics.initial_pose_with_covariance.pose.position, - occluded_object->kinematics.initial_twist_with_covariance.twist.linear.x, clock_->now(), - is_ego_yielding, has_traffic_light, collision_point, - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, planner_param_, - crosswalk_.polygon2d().basicPolygon()); - } - } else { - current_occlusion_start_time_.reset(); - } - } object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 6de929467c1f9..d05df179c5045 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -309,7 +309,8 @@ class CrosswalkModule : public SceneModuleInterface private: // main functions void applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects); + PathWithLaneId & output, const std::vector & path_intersects, + const float speed); std::optional> getStopPointWithMargin( const PathWithLaneId & ego_path, @@ -432,7 +433,8 @@ class CrosswalkModule : public SceneModuleInterface const bool use_regulatory_element_; // occluded space time buffer - std::optional current_occlusion_start_time_; + std::optional current_initial_occlusion_time_; + std::optional most_recent_occlusion_time_; }; } // namespace behavior_velocity_planner From 397196cdd8e9faafc52977fa35c6540968a9b3d0 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 16 Jan 2024 17:31:03 +0900 Subject: [PATCH 03/26] add detection areas at the front&back of the crosswalk Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.hpp | 122 +++++++++++++----- .../src/scene_crosswalk.cpp | 4 +- 2 files changed, 90 insertions(+), 36 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index ee9a8edf4ad14..025a80d88b8c7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -20,59 +20,111 @@ #include #include #include -#include -#include -#include -#include -#include #include #include -#include -#include - -#include #include -#include -#include -#include #include +// for writing the svg file +#include +#include +// for the geometry types +#include +// for the svg mapper +#include +#include + namespace behavior_velocity_planner { +/// @brief check if the gridmap is occluded at the given index +bool is_occluded( + const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx) +{ + grid_map::Index idx_offset; + for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { + for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { + const auto index = idx + idx_offset; + if ((index < grid_map.getSize()).all()) { + const auto cell_value = grid_map.at("layer", index); + // TODO(Maxime): magic number -> params + if (cell_value < 47 || cell_value > 53) return false; + } + } + } + return true; +} + +lanelet::BasicPoint2d interpolate_point( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance) +{ + const auto direction_vector = (p2 - p1).normalized(); + return p2 + extra_distance * direction_vector; +} + +/// @brief check if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, - const nav_msgs::msg::OccupancyGrid & occupancy_grid) + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const geometry_msgs::msg::Point & path_intersection) { + // Declare a stream and an SVG mapper + std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH + boost::geometry::svg_mapper mapper(svg, 400, 400); grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); constexpr double min_size = 0.5; // TODO(Maxime): param + constexpr double range = 8.0; // TODO(Maxime): param, should include the ego width ? const auto min_nb_of_cells = std::ceil(min_size / grid_map.getResolution()); - grid_map::Polygon incoming_area_poly; - for (const auto & p : crosswalk_lanelet.polygon2d().basicPolygon()) - incoming_area_poly.addVertex(grid_map::Position(p.x(), p.y())); - for (grid_map_utils::PolygonIterator iter(grid_map, incoming_area_poly); !iter.isPastEnd(); - ++iter) { - const auto idx = *iter; - // TODO(Maxime): move to function - const auto is_occluded = [&]() { - grid_map::Index idx_offset; - for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { - for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { - const auto index = idx + idx_offset; - if ((index < grid_map.getSize()).all()) { - const auto cell_value = grid_map.at("layer", index); - // TODO(Maxime): magic number -> params - if (cell_value < 47 || cell_value > 53) return false; - } - } - } - return true; - }(); - if (is_occluded) return true; + std::vector incoming_areas; + // front + { + const auto dist = lanelet::geometry::distance2d( + crosswalk_lanelet.centerline2d().basicLineString().front(), path_inter); + if (dist < range) { + const auto & base_left = crosswalk_lanelet.leftBound2d().front().basicPoint2d(); + const auto & base_right = crosswalk_lanelet.rightBound2d().front().basicPoint2d(); + const auto target_left = interpolate_point( + *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, range - dist); + const auto target_right = interpolate_point( + *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, range - dist); + incoming_areas.push_back({base_left, target_left, target_right, base_right}); + } + } + // back + { + const auto dist = lanelet::geometry::distance2d( + crosswalk_lanelet.centerline2d().basicLineString().back(), path_inter); + if (dist < range) { + const auto & base_left = crosswalk_lanelet.leftBound2d().back().basicPoint2d(); + const auto & base_right = crosswalk_lanelet.rightBound2d().back().basicPoint2d(); + const auto target_left = interpolate_point( + *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, range - dist); + const auto target_right = interpolate_point( + *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, range - dist); + incoming_areas.push_back({base_left, target_left, target_right, base_right}); + } + } + incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); + mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); + mapper.add(path_inter); + mapper.add(incoming_areas[0]); + mapper.add(incoming_areas[1]); + mapper.map( + crosswalk_lanelet.polygon2d().basicPolygon(), + "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); + mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); + mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); + mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); + for (const auto & incoming_area : incoming_areas) { + grid_map::Polygon poly; + for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); + for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) + if (is_occluded(grid_map, min_nb_of_cells, *iter)) return true; } return false; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 65f0eba49bc25..0d29fd4402d1e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -238,7 +238,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto constexpr auto use_occluded_space = true; constexpr auto occluded_slow_down_speed = 1.0f; constexpr auto time_buffer = 1.0; - if (use_occluded_space && is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid)) { + if ( + use_occluded_space && !path_intersects.empty() && + is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid, path_intersects.front())) { if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) most_recent_occlusion_time_ = clock_->now(); From 85179bbe1eedc1382fa07fcebd6ccd249df96159 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 17 Jan 2024 13:34:21 +0900 Subject: [PATCH 04/26] Improve time buffer Signed-off-by: Maxime CLEMENT --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0d29fd4402d1e..12a98499889e3 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -244,8 +244,6 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) most_recent_occlusion_time_ = clock_->now(); - } else { - current_initial_occlusion_time_.reset(); } if ( most_recent_occlusion_time_ && From 0ff684e164e2030369d5601b32389aa807049f6c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 17 Jan 2024 13:34:34 +0900 Subject: [PATCH 05/26] Comment out SVG debug code Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.hpp | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 025a80d88b8c7..93c88eacf48ff 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -28,6 +28,7 @@ #include +/* // for writing the svg file #include #include @@ -36,7 +37,7 @@ // for the svg mapper #include #include - +*/ namespace behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index @@ -71,8 +72,8 @@ bool is_crosswalk_occluded( const geometry_msgs::msg::Point & path_intersection) { // Declare a stream and an SVG mapper - std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH - boost::geometry::svg_mapper mapper(svg, 400, 400); + // std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH + // boost::geometry::svg_mapper mapper(svg, 400, 400); grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); @@ -110,16 +111,16 @@ bool is_crosswalk_occluded( } } incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); - mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); - mapper.add(path_inter); - mapper.add(incoming_areas[0]); - mapper.add(incoming_areas[1]); - mapper.map( - crosswalk_lanelet.polygon2d().basicPolygon(), - "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); - mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); - mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); - mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); + // mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); + // mapper.add(path_inter); + // mapper.add(incoming_areas[0]); + // mapper.add(incoming_areas[1]); + // mapper.map( + // crosswalk_lanelet.polygon2d().basicPolygon(), + // "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); + // mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); + // mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); + // mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); for (const auto & incoming_area : incoming_areas) { grid_map::Polygon poly; for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); From baa4eefc4f160b35c7621328b22eeb811193a688 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 18 Jan 2024 13:07:05 +0900 Subject: [PATCH 06/26] Check that ego did not already pass the crosswalk Signed-off-by: Maxime CLEMENT --- .../src/scene_crosswalk.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 12a98499889e3..062adc6631a64 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -235,20 +235,22 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } // TODO(Maxime): param - constexpr auto use_occluded_space = true; + const auto use_occluded_space = [&]() -> bool { + return !path_intersects.empty() && calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; + }(); constexpr auto occluded_slow_down_speed = 1.0f; constexpr auto time_buffer = 1.0; - if ( - use_occluded_space && !path_intersects.empty() && - is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid, path_intersects.front())) { - if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); - if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) - most_recent_occlusion_time_ = clock_->now(); - } - if ( - most_recent_occlusion_time_ && - (clock_->now() - *most_recent_occlusion_time_).seconds() <= time_buffer) { - applySafetySlowDownSpeed(*path, path_intersects, occluded_slow_down_speed); + if (use_occluded_space) { + if(is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid, path_intersects.front())) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); + if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) + most_recent_occlusion_time_ = clock_->now(); + } + if ( + most_recent_occlusion_time_ && + (clock_->now() - *most_recent_occlusion_time_).seconds() <= time_buffer) { + applySafetySlowDownSpeed(*path, path_intersects, occluded_slow_down_speed); + } } recordTime(2); From 99e688f94e8a893bca5411ec26b4fb9b2bd0eeb9 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 13:35:08 +0900 Subject: [PATCH 07/26] Add ROS params Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 10 ++++++ .../src/manager.cpp | 11 ++++++ .../src/occluded_crosswalk.hpp | 34 +++++++++++-------- .../src/scene_crosswalk.cpp | 18 +++++----- .../src/scene_crosswalk.hpp | 8 +++++ 5 files changed, 59 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index cf3d2c621322b..ebf074a4365e6 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -68,3 +68,13 @@ bicycle: true # [-] whether to look and stop by BICYCLE objects motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + + # param for occlusions + occlusion: + enable: true # if true, ego will slowdown around crosswalks that are occluded + slow_down_velocity: 1.0 # [m/s] + time_buffer: 1.0 # [s] consecutive time with/without an occlusion to add/remove the slowdown + min_size: 0.5 # [m] minimum size of an occlusion (square side size) + detection_range: 8.0 # [m] distance away from the crosswalk-ego path intersection that is checked for occlusion + free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 1fa9247548add..14dcf31dc9a03 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -123,6 +123,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); + + // param for occlusions + cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_slow_down_velocity = + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); + cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); + cp.occlusion_detection_range = + getOrDeclareParameter(node, ns + ".occlusion.detection_range"); + cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 93c88eacf48ff..f4f212232e46f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -42,7 +42,8 @@ namespace behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index bool is_occluded( - const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx) + const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::Index idx_offset; for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { @@ -50,8 +51,10 @@ bool is_occluded( const auto index = idx + idx_offset; if ((index < grid_map.getSize()).all()) { const auto cell_value = grid_map.at("layer", index); - // TODO(Maxime): magic number -> params - if (cell_value < 47 || cell_value > 53) return false; + if ( + cell_value < params.occlusion_free_space_max || + cell_value > params.occlusion_occupied_min) + return false; } } } @@ -69,7 +72,8 @@ lanelet::BasicPoint2d interpolate_point( bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const geometry_msgs::msg::Point & path_intersection) + const geometry_msgs::msg::Point & path_intersection, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { // Declare a stream and an SVG mapper // std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH @@ -78,21 +82,21 @@ bool is_crosswalk_occluded( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); - constexpr double min_size = 0.5; // TODO(Maxime): param - constexpr double range = 8.0; // TODO(Maxime): param, should include the ego width ? - const auto min_nb_of_cells = std::ceil(min_size / grid_map.getResolution()); + const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); std::vector incoming_areas; // front { const auto dist = lanelet::geometry::distance2d( crosswalk_lanelet.centerline2d().basicLineString().front(), path_inter); - if (dist < range) { + if (dist < params.occlusion_detection_range) { const auto & base_left = crosswalk_lanelet.leftBound2d().front().basicPoint2d(); const auto & base_right = crosswalk_lanelet.rightBound2d().front().basicPoint2d(); const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, range - dist); + *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, + params.occlusion_detection_range - dist); const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, range - dist); + *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, + params.occlusion_detection_range - dist); incoming_areas.push_back({base_left, target_left, target_right, base_right}); } } @@ -100,13 +104,15 @@ bool is_crosswalk_occluded( { const auto dist = lanelet::geometry::distance2d( crosswalk_lanelet.centerline2d().basicLineString().back(), path_inter); - if (dist < range) { + if (dist < params.occlusion_detection_range) { const auto & base_left = crosswalk_lanelet.leftBound2d().back().basicPoint2d(); const auto & base_right = crosswalk_lanelet.rightBound2d().back().basicPoint2d(); const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, range - dist); + *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, + params.occlusion_detection_range - dist); const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, range - dist); + *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, + params.occlusion_detection_range - dist); incoming_areas.push_back({base_left, target_left, target_right, base_right}); } } @@ -125,7 +131,7 @@ bool is_crosswalk_occluded( grid_map::Polygon poly; for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) - if (is_occluded(grid_map, min_nb_of_cells, *iter)) return true; + if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; } return false; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 062adc6631a64..fe60e72243b01 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -236,20 +236,22 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } // TODO(Maxime): param const auto use_occluded_space = [&]() -> bool { - return !path_intersects.empty() && calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; + return !path_intersects.empty() && + calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; }(); - constexpr auto occluded_slow_down_speed = 1.0f; - constexpr auto time_buffer = 1.0; if (use_occluded_space) { - if(is_crosswalk_occluded(crosswalk_, *planner_data_->occupancy_grid, path_intersects.front())) { + if (is_crosswalk_occluded( + crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), planner_param_)) { if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); - if ((clock_->now() - *current_initial_occlusion_time_).seconds() >= time_buffer) + if ( + (clock_->now() - *current_initial_occlusion_time_).seconds() >= + planner_param_.occlusion_time_buffer) most_recent_occlusion_time_ = clock_->now(); } if ( - most_recent_occlusion_time_ && - (clock_->now() - *most_recent_occlusion_time_).seconds() <= time_buffer) { - applySafetySlowDownSpeed(*path, path_intersects, occluded_slow_down_speed); + most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= + planner_param_.occlusion_time_buffer) { + applySafetySlowDownSpeed(*path, path_intersects, planner_param_.occlusion_slow_down_velocity); } } recordTime(2); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d05df179c5045..3692f046f524d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -154,6 +154,14 @@ class CrosswalkModule : public SceneModuleInterface bool look_bicycle; bool look_motorcycle; bool look_pedestrian; + // param for occlusions + bool occlusion_enable; + double occlusion_slow_down_velocity; + double occlusion_time_buffer; + double occlusion_min_size; + double occlusion_detection_range; + int occlusion_free_space_max; + int occlusion_occupied_min; }; struct ObjectInfo From aecd6050ddc729947760e5fd17faeccd02c95de2 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 14:19:31 +0900 Subject: [PATCH 08/26] Split hpp/cpp Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 131 ++++++++++++++++++ .../src/occluded_crosswalk.hpp | 108 +-------------- 2 files changed, 138 insertions(+), 101 deletions(-) create mode 100644 planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp new file mode 100644 index 0000000000000..02d4f5be63edc --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -0,0 +1,131 @@ +// Copyright 2024 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 "occluded_crosswalk.hpp" + +#include "behavior_velocity_crosswalk_module/util.hpp" + +#include +#include + +#include + +/* +// for writing the svg file +#include +#include +// for the geometry types +#include +// for the svg mapper +#include +#include +*/ +namespace behavior_velocity_planner +{ +/// @brief check if the gridmap is occluded at the given index +bool is_occluded( + const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +{ + grid_map::Index idx_offset; + for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { + for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { + const auto index = idx + idx_offset; + if ((index < grid_map.getSize()).all()) { + const auto cell_value = grid_map.at("layer", index); + if ( + cell_value < params.occlusion_free_space_max || + cell_value > params.occlusion_occupied_min) + return false; + } + } + } + return true; +} + +lanelet::BasicPoint2d interpolate_point( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance) +{ + const auto direction_vector = (p2 - p1).normalized(); + return p2 + extra_distance * direction_vector; +} + +/// @brief check if the crosswalk is occluded +bool is_crosswalk_occluded( + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const geometry_msgs::msg::Point & path_intersection, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +{ + // Declare a stream and an SVG mapper + // std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH + // boost::geometry::svg_mapper mapper(svg, 400, 400); + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); + + const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); + std::vector incoming_areas; + // front + { + const auto dist = lanelet::geometry::distance2d( + crosswalk_lanelet.centerline2d().basicLineString().front(), path_inter); + if (dist < params.occlusion_detection_range) { + const auto & base_left = crosswalk_lanelet.leftBound2d().front().basicPoint2d(); + const auto & base_right = crosswalk_lanelet.rightBound2d().front().basicPoint2d(); + const auto target_left = interpolate_point( + *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, + params.occlusion_detection_range - dist); + const auto target_right = interpolate_point( + *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, + params.occlusion_detection_range - dist); + incoming_areas.push_back({base_left, target_left, target_right, base_right}); + } + } + // back + { + const auto dist = lanelet::geometry::distance2d( + crosswalk_lanelet.centerline2d().basicLineString().back(), path_inter); + if (dist < params.occlusion_detection_range) { + const auto & base_left = crosswalk_lanelet.leftBound2d().back().basicPoint2d(); + const auto & base_right = crosswalk_lanelet.rightBound2d().back().basicPoint2d(); + const auto target_left = interpolate_point( + *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, + params.occlusion_detection_range - dist); + const auto target_right = interpolate_point( + *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, + params.occlusion_detection_range - dist); + incoming_areas.push_back({base_left, target_left, target_right, base_right}); + } + } + incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); + // mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); + // mapper.add(path_inter); + // mapper.add(incoming_areas[0]); + // mapper.add(incoming_areas[1]); + // mapper.map( + // crosswalk_lanelet.polygon2d().basicPolygon(), + // "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); + // mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); + // mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); + // mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); + for (const auto & incoming_area : incoming_areas) { + grid_map::Polygon poly; + for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); + for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) + if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; + } + return false; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index f4f212232e46f..f5a01be699df0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 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. @@ -15,126 +15,32 @@ #ifndef OCCLUDED_CROSSWALK_HPP_ #define OCCLUDED_CROSSWALK_HPP_ -#include "behavior_velocity_crosswalk_module/util.hpp" +#include "scene_crosswalk.hpp" #include -#include -#include #include #include -#include +#include +#include -#include - -/* -// for writing the svg file -#include -#include -// for the geometry types -#include -// for the svg mapper -#include -#include -*/ namespace behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) -{ - grid_map::Index idx_offset; - for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { - for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { - const auto index = idx + idx_offset; - if ((index < grid_map.getSize()).all()) { - const auto cell_value = grid_map.at("layer", index); - if ( - cell_value < params.occlusion_free_space_max || - cell_value > params.occlusion_occupied_min) - return false; - } - } - } - return true; -} + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); lanelet::BasicPoint2d interpolate_point( - const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance) -{ - const auto direction_vector = (p2 - p1).normalized(); - return p2 + extra_distance * direction_vector; -} + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance); /// @brief check if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) -{ - // Declare a stream and an SVG mapper - // std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH - // boost::geometry::svg_mapper mapper(svg, 400, 400); - grid_map::GridMap grid_map; - grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); - const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); - - const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); - std::vector incoming_areas; - // front - { - const auto dist = lanelet::geometry::distance2d( - crosswalk_lanelet.centerline2d().basicLineString().front(), path_inter); - if (dist < params.occlusion_detection_range) { - const auto & base_left = crosswalk_lanelet.leftBound2d().front().basicPoint2d(); - const auto & base_right = crosswalk_lanelet.rightBound2d().front().basicPoint2d(); - const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, - params.occlusion_detection_range - dist); - const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, - params.occlusion_detection_range - dist); - incoming_areas.push_back({base_left, target_left, target_right, base_right}); - } - } - // back - { - const auto dist = lanelet::geometry::distance2d( - crosswalk_lanelet.centerline2d().basicLineString().back(), path_inter); - if (dist < params.occlusion_detection_range) { - const auto & base_left = crosswalk_lanelet.leftBound2d().back().basicPoint2d(); - const auto & base_right = crosswalk_lanelet.rightBound2d().back().basicPoint2d(); - const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, - params.occlusion_detection_range - dist); - const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, - params.occlusion_detection_range - dist); - incoming_areas.push_back({base_left, target_left, target_right, base_right}); - } - } - incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); - // mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); - // mapper.add(path_inter); - // mapper.add(incoming_areas[0]); - // mapper.add(incoming_areas[1]); - // mapper.map( - // crosswalk_lanelet.polygon2d().basicPolygon(), - // "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); - // mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); - // mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); - // mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); - for (const auto & incoming_area : incoming_areas) { - grid_map::Polygon poly; - for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); - for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) - if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; - } - return false; -} + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); } // namespace behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ From 67f17b990c9d3c679d9ea5358463448baceda495 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 14:28:10 +0900 Subject: [PATCH 09/26] Add docstring Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 2 -- .../src/occluded_crosswalk.hpp | 16 ++++++++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 02d4f5be63edc..cd7b2104d4437 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -33,7 +33,6 @@ */ namespace behavior_velocity_planner { -/// @brief check if the gridmap is occluded at the given index bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) @@ -61,7 +60,6 @@ lanelet::BasicPoint2d interpolate_point( return p2 + extra_distance * direction_vector; } -/// @brief check if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index f5a01be699df0..5ee61b5b1f897 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -28,14 +28,30 @@ namespace behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index +/// @param [in] grid_map input grid map +/// @param [in] min_nb_of_cells minimum number of occluded cells needed to detect an occlusion (as +/// side of a square centered at the index) +/// @param [in] idx target index in the grid map +/// @param [in] params parameters +/// @return true if the index is occluded bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); +/// @brief interpolate a point beyond the end of the given segment +/// @param [in] p1 first point of the segment +/// @param [in] p2 second point of the segment +/// @param [in] extra_distance distance beyond p2 of the interpolated point +/// @return interpolated point beyond p2 lanelet::BasicPoint2d interpolate_point( const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance); /// @brief check if the crosswalk is occluded +/// @param crosswalk_lanelet lanelet of the crosswalk +/// @param occupancy_grid occupancy grid +/// @param path_intersection intersection between the crosswalk and the ego path +/// @param params parameters +/// @return true if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, From 39c5861876b5650c55f21c47c47dead0591d0481 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 14:59:45 +0900 Subject: [PATCH 10/26] Small refactor (point2d,point2d) -> segment2d Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 61 +++++++++---------- .../src/occluded_crosswalk.hpp | 9 ++- 2 files changed, 32 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index cd7b2104d4437..42ea420ae5289 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -54,10 +54,10 @@ bool is_occluded( } lanelet::BasicPoint2d interpolate_point( - const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance) + const lanelet::BasicSegment2d & segment, const double extra_distance) { - const auto direction_vector = (p2 - p1).normalized(); - return p2 + extra_distance * direction_vector; + const auto direction_vector = (segment.second - segment.first).normalized(); + return segment.second + extra_distance * direction_vector; } bool is_crosswalk_occluded( @@ -75,36 +75,31 @@ bool is_crosswalk_occluded( const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); std::vector incoming_areas; - // front - { - const auto dist = lanelet::geometry::distance2d( - crosswalk_lanelet.centerline2d().basicLineString().front(), path_inter); - if (dist < params.occlusion_detection_range) { - const auto & base_left = crosswalk_lanelet.leftBound2d().front().basicPoint2d(); - const auto & base_right = crosswalk_lanelet.rightBound2d().front().basicPoint2d(); - const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicBegin() + 1), base_left, - params.occlusion_detection_range - dist); - const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicBegin() + 1), base_right, - params.occlusion_detection_range - dist); - incoming_areas.push_back({base_left, target_left, target_right, base_right}); - } - } - // back - { - const auto dist = lanelet::geometry::distance2d( - crosswalk_lanelet.centerline2d().basicLineString().back(), path_inter); - if (dist < params.occlusion_detection_range) { - const auto & base_left = crosswalk_lanelet.leftBound2d().back().basicPoint2d(); - const auto & base_right = crosswalk_lanelet.rightBound2d().back().basicPoint2d(); - const auto target_left = interpolate_point( - *(crosswalk_lanelet.leftBound2d().basicEnd() - 2), base_left, - params.occlusion_detection_range - dist); - const auto target_right = interpolate_point( - *(crosswalk_lanelet.rightBound2d().basicEnd() - 2), base_right, - params.occlusion_detection_range - dist); - incoming_areas.push_back({base_left, target_left, target_right, base_right}); + const std::vector> + segment_getters = { + [](const auto & ls) -> lanelet::BasicSegment2d { + return {ls[1].basicPoint2d(), ls[0].basicPoint2d()}; + }, + [](const auto & ls) -> lanelet::BasicSegment2d { + const auto size = ls.size(); + return {ls[size - 2].basicPoint2d(), ls[size - 1].basicPoint2d()}; + }}; + if ( + crosswalk_lanelet.centerline2d().size() > 1 && crosswalk_lanelet.leftBound2d().size() > 1 && + crosswalk_lanelet.rightBound2d().size() > 1) { + for (const auto segment_getter : segment_getters) { + const auto center_segment = segment_getter(crosswalk_lanelet.centerline2d()); + const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d()); + const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d()); + const auto dist = lanelet::geometry::distance2d(center_segment.second, path_inter); + if (dist < params.occlusion_detection_range) { + const auto target_left = + interpolate_point(left_segment, params.occlusion_detection_range - dist); + const auto target_right = + interpolate_point(right_segment, params.occlusion_detection_range - dist); + incoming_areas.push_back( + {left_segment.second, target_left, target_right, right_segment.second}); + } } } incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 5ee61b5b1f897..6a14183f201c3 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -39,12 +39,11 @@ bool is_occluded( const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief interpolate a point beyond the end of the given segment -/// @param [in] p1 first point of the segment -/// @param [in] p2 second point of the segment -/// @param [in] extra_distance distance beyond p2 of the interpolated point -/// @return interpolated point beyond p2 +/// @param [in] segment input segment +/// @param [in] extra_distance desired distance beyond the end of the segment +/// @return interpolated point beyond the end of the segment lanelet::BasicPoint2d interpolate_point( - const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, const double extra_distance); + const lanelet::BasicSegment2d & segment, const double extra_distance); /// @brief check if the crosswalk is occluded /// @param crosswalk_lanelet lanelet of the crosswalk From cfe7113545a84e178ba212993021db1a83374e8d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 16:39:30 +0900 Subject: [PATCH 11/26] Remove svg code Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 42ea420ae5289..b9f5e988cb974 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -21,16 +21,6 @@ #include -/* -// for writing the svg file -#include -#include -// for the geometry types -#include -// for the svg mapper -#include -#include -*/ namespace behavior_velocity_planner { bool is_occluded( @@ -66,9 +56,6 @@ bool is_crosswalk_occluded( const geometry_msgs::msg::Point & path_intersection, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { - // Declare a stream and an SVG mapper - // std::ofstream svg("/home/mclement/Pictures/crosswalk.svg"); // /!\ CHANGE PATH - // boost::geometry::svg_mapper mapper(svg, 400, 400); grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); @@ -103,16 +90,6 @@ bool is_crosswalk_occluded( } } incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); - // mapper.add(crosswalk_lanelet.polygon2d().basicPolygon()); - // mapper.add(path_inter); - // mapper.add(incoming_areas[0]); - // mapper.add(incoming_areas[1]); - // mapper.map( - // crosswalk_lanelet.polygon2d().basicPolygon(), - // "fill-opacity:0.3;fill:grey;stroke:black;stroke-width:1"); - // mapper.map(path_inter, "opacity:0.5;fill:pink;stroke:pink;stroke-width:1", 1); - // mapper.map(incoming_areas[0], "fill-opacity:0.3;fill:green;stroke:none;stroke-width:1"); - // mapper.map(incoming_areas[1], "fill-opacity:0.3;fill:red;stroke:none;stroke-width:1"); for (const auto & incoming_area : incoming_areas) { grid_map::Polygon poly; for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); From 18d726b33a1a238074d3a8785bd6e612548364f8 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 19 Jan 2024 17:13:38 +0900 Subject: [PATCH 12/26] Refactor timers Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 11 ++++++- .../src/occluded_crosswalk.hpp | 12 ++++++++ .../src/scene_crosswalk.cpp | 29 +++++++++---------- 3 files changed, 36 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index b9f5e988cb974..c5e7d267caf08 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -74,7 +74,7 @@ bool is_crosswalk_occluded( if ( crosswalk_lanelet.centerline2d().size() > 1 && crosswalk_lanelet.leftBound2d().size() > 1 && crosswalk_lanelet.rightBound2d().size() > 1) { - for (const auto segment_getter : segment_getters) { + for (const auto & segment_getter : segment_getters) { const auto center_segment = segment_getter(crosswalk_lanelet.centerline2d()); const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d()); const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d()); @@ -98,4 +98,13 @@ bool is_crosswalk_occluded( } return false; } + +void update_occlusion_timers( + std::optional & initial_time, + std::optional & most_recent_slowdown_time, const rclcpp::Time & now, + const double buffer) +{ + if (!initial_time) initial_time = now; + if ((now - *initial_time).seconds() >= buffer) most_recent_slowdown_time = now; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 6a14183f201c3..3ba1fd1b3d532 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -18,6 +18,7 @@ #include "scene_crosswalk.hpp" #include +#include #include #include @@ -25,6 +26,8 @@ #include #include +#include + namespace behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index @@ -56,6 +59,15 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + +/// @brief update timers so that the slowdown activates if the initial time is older than the buffer +/// @param initial_time initial time +/// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer +/// @param buffer [s] time buffer +void update_occlusion_timers( + std::optional & initial_time, + std::optional & most_recent_slowdown_time, const rclcpp::Time & now, + const double buffer); } // namespace behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index fe60e72243b01..701db31f07fd1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -234,25 +234,24 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *path, path_intersects, static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } - // TODO(Maxime): param - const auto use_occluded_space = [&]() -> bool { - return !path_intersects.empty() && - calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; - }(); + // Apply slow down if the crosswalk is occluded + const auto use_occluded_space = + planner_param_.occlusion_enable && !path_intersects.empty() && + calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; if (use_occluded_space) { if (is_crosswalk_occluded( - crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), planner_param_)) { - if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = clock_->now(); - if ( - (clock_->now() - *current_initial_occlusion_time_).seconds() >= - planner_param_.occlusion_time_buffer) - most_recent_occlusion_time_ = clock_->now(); - } - if ( + crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), planner_param_)) + update_occlusion_timers( + current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(), + planner_param_.occlusion_time_buffer); + else + current_initial_occlusion_time_.reset(); + + const auto is_last_occlusion_within_time_buffer = most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= - planner_param_.occlusion_time_buffer) { + planner_param_.occlusion_time_buffer; + if (is_last_occlusion_within_time_buffer) applySafetySlowDownSpeed(*path, path_intersects, planner_param_.occlusion_slow_down_velocity); - } } recordTime(2); From fd7633e5865b8a96642737ec4c636891dab27b2b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 22 Jan 2024 15:19:40 +0900 Subject: [PATCH 13/26] Calculate the detection range based on the distance from the crosswalk Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 2 +- .../src/manager.cpp | 4 +- .../src/occluded_crosswalk.cpp | 19 +++++--- .../src/occluded_crosswalk.hpp | 8 +++- .../src/scene_crosswalk.cpp | 44 +++++++++++-------- .../src/scene_crosswalk.hpp | 2 +- 6 files changed, 49 insertions(+), 30 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index ebf074a4365e6..55b14f5f77eca 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -72,9 +72,9 @@ # param for occlusions occlusion: enable: true # if true, ego will slowdown around crosswalks that are occluded + occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space slow_down_velocity: 1.0 # [m/s] time_buffer: 1.0 # [s] consecutive time with/without an occlusion to add/remove the slowdown min_size: 0.5 # [m] minimum size of an occlusion (square side size) - detection_range: 8.0 # [m] distance away from the crosswalk-ego path intersection that is checked for occlusion free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 14dcf31dc9a03..3104aba1f3dad 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -126,12 +126,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for occlusions cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_occluded_object_velocity = + getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); cp.occlusion_slow_down_velocity = getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); - cp.occlusion_detection_range = - getOrDeclareParameter(node, ns + ".occlusion.detection_range"); cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); } diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index c5e7d267caf08..95dbe771b5195 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -53,7 +54,7 @@ lanelet::BasicPoint2d interpolate_point( bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const geometry_msgs::msg::Point & path_intersection, + const geometry_msgs::msg::Point & path_intersection, const double detection_range, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; @@ -79,11 +80,9 @@ bool is_crosswalk_occluded( const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d()); const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d()); const auto dist = lanelet::geometry::distance2d(center_segment.second, path_inter); - if (dist < params.occlusion_detection_range) { - const auto target_left = - interpolate_point(left_segment, params.occlusion_detection_range - dist); - const auto target_right = - interpolate_point(right_segment, params.occlusion_detection_range - dist); + if (dist < detection_range) { + const auto target_left = interpolate_point(left_segment, detection_range - dist); + const auto target_right = interpolate_point(right_segment, detection_range - dist); incoming_areas.push_back( {left_segment.second, target_left, target_right, right_segment.second}); } @@ -99,6 +98,14 @@ bool is_crosswalk_occluded( return false; } +double calculate_detection_range( + const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity) +{ + constexpr double min_ego_velocity = 1.0; + const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); + return time_to_crosswalk > 0.0 ? time_to_crosswalk / object_velocity : 20.0; +} + void update_occlusion_timers( std::optional & initial_time, std::optional & most_recent_slowdown_time, const rclcpp::Time & now, diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 3ba1fd1b3d532..43ae747e8326b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -50,16 +50,20 @@ lanelet::BasicPoint2d interpolate_point( /// @brief check if the crosswalk is occluded /// @param crosswalk_lanelet lanelet of the crosswalk -/// @param occupancy_grid occupancy grid +/// @param occupancy_grid occupancy grid with the occlusion information /// @param path_intersection intersection between the crosswalk and the ego path +/// @param detection_range range away from the crosswalk until occlusions are considered /// @param params parameters /// @return true if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const geometry_msgs::msg::Point & path_intersection, + const geometry_msgs::msg::Point & path_intersection, const double detection_range, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); +double calculate_detection_range( + const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity); + /// @brief update timers so that the slowdown activates if the initial time is older than the buffer /// @param initial_time initial time /// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 701db31f07fd1..56be80541ac81 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -234,24 +234,32 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *path, path_intersects, static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } - // Apply slow down if the crosswalk is occluded - const auto use_occluded_space = - planner_param_.occlusion_enable && !path_intersects.empty() && - calcSignedArcLength(path->points, ego_pos, path_intersects.front()) > 0.0; - if (use_occluded_space) { - if (is_crosswalk_occluded( - crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), planner_param_)) - update_occlusion_timers( - current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(), - planner_param_.occlusion_time_buffer); - else - current_initial_occlusion_time_.reset(); - - const auto is_last_occlusion_within_time_buffer = - most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= - planner_param_.occlusion_time_buffer; - if (is_last_occlusion_within_time_buffer) - applySafetySlowDownSpeed(*path, path_intersects, planner_param_.occlusion_slow_down_velocity); + // Apply safety slow down speed if the crosswalk is occluded + if (planner_param_.occlusion_enable && !path_intersects.empty()) { + const auto dist_ego_to_crosswalk = + calcSignedArcLength(path->points, ego_pos, path_intersects.front()); + const auto detection_range = calculate_detection_range( + planner_param_.min_object_velocity, dist_ego_to_crosswalk, + planner_data_->current_velocity->twist.linear.x); + const auto is_ego_on_the_crosswalk = + dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!is_ego_on_the_crosswalk) { + if (is_crosswalk_occluded( + crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, + planner_param_)) + update_occlusion_timers( + current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(), + planner_param_.occlusion_time_buffer); + else + current_initial_occlusion_time_.reset(); + + const auto is_last_occlusion_within_time_buffer = + most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= + planner_param_.occlusion_time_buffer; + if (is_last_occlusion_within_time_buffer) + applySafetySlowDownSpeed( + *path, path_intersects, planner_param_.occlusion_slow_down_velocity); + } } recordTime(2); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 3692f046f524d..75de02df05fce 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -156,10 +156,10 @@ class CrosswalkModule : public SceneModuleInterface bool look_pedestrian; // param for occlusions bool occlusion_enable; + double occlusion_occluded_object_velocity; double occlusion_slow_down_velocity; double occlusion_time_buffer; double occlusion_min_size; - double occlusion_detection_range; int occlusion_free_space_max; int occlusion_occupied_min; }; From d736dab429da9424f5a997a927914d01ca5ff0c0 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 22 Jan 2024 22:09:26 +0900 Subject: [PATCH 14/26] Add option to ignore crosswalk with traffic light + apply max decel/jerk Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 1 + .../src/manager.cpp | 4 +++- .../src/occluded_crosswalk.cpp | 10 ++++++++++ .../src/occluded_crosswalk.hpp | 7 +++++++ .../src/scene_crosswalk.cpp | 11 ++++++++--- .../src/scene_crosswalk.hpp | 3 ++- 6 files changed, 31 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 55b14f5f77eca..e5bf20b8f75a3 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -78,3 +78,4 @@ min_size: 0.5 # [m] minimum size of an occlusion (square side size) free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid + ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 3104aba1f3dad..de3125647e0ea 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -129,11 +129,13 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_occluded_object_velocity = getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); cp.occlusion_slow_down_velocity = - getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + cp.occlusion_ignore_with_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 95dbe771b5195..e3e38e27ba82c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -51,6 +51,16 @@ lanelet::BasicPoint2d interpolate_point( return segment.second + extra_distance * direction_vector; } +bool is_crosswalk_ignored( + const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light) +{ + const auto traffic_lights_reg_elems = + crosswalk_lanelet.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + const bool has_skip_attribute = crosswalk_lanelet.hasAttribute("skip_occluded_slowdown"); + return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute; +} + bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 43ae747e8326b..0b3c945d35834 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -48,6 +48,13 @@ bool is_occluded( lanelet::BasicPoint2d interpolate_point( const lanelet::BasicSegment2d & segment, const double extra_distance); +/// @brief check if the crosswalk should be ignored +/// @param crosswalk_lanelet lanelet of the crosswalk +/// @param ignore_with_traffic_light if true, ignore the crosswalk if it has a traffic light +/// @return true if the crosswalk should be ignored +bool is_crosswalk_ignored( + const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light); + /// @brief check if the crosswalk is occluded /// @param crosswalk_lanelet lanelet of the crosswalk /// @param occupancy_grid occupancy grid with the occlusion information diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 56be80541ac81..4fe12306118ab 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -235,7 +235,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } // Apply safety slow down speed if the crosswalk is occluded - if (planner_param_.occlusion_enable && !path_intersects.empty()) { + if ( + planner_param_.occlusion_enable && !path_intersects.empty() && + !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); const auto detection_range = calculate_detection_range( @@ -256,9 +258,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto is_last_occlusion_within_time_buffer = most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= planner_param_.occlusion_time_buffer; - if (is_last_occlusion_within_time_buffer) + if (is_last_occlusion_within_time_buffer) { + const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); applySafetySlowDownSpeed( - *path, path_intersects, planner_param_.occlusion_slow_down_velocity); + *path, path_intersects, + std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + } } } recordTime(2); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 75de02df05fce..6de34cafd2c62 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -157,11 +157,12 @@ class CrosswalkModule : public SceneModuleInterface // param for occlusions bool occlusion_enable; double occlusion_occluded_object_velocity; - double occlusion_slow_down_velocity; + float occlusion_slow_down_velocity; double occlusion_time_buffer; double occlusion_min_size; int occlusion_free_space_max; int occlusion_occupied_min; + bool occlusion_ignore_with_traffic_light; }; struct ObjectInfo From 830ee7e0a0385cd43c9d2b6ddd601794ef0e6a4c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 23 Jan 2024 10:12:30 +0900 Subject: [PATCH 15/26] Small refactor Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 42 +++++++++++-------- 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index e3e38e27ba82c..e95563c8c2acd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -61,26 +61,19 @@ bool is_crosswalk_ignored( return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute; } -bool is_crosswalk_occluded( - const lanelet::ConstLanelet & crosswalk_lanelet, - const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +std::vector calculate_detection_areas( + const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, + const double detection_range) { - grid_map::GridMap grid_map; - grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); - const lanelet::BasicPoint2d path_inter(path_intersection.x, path_intersection.y); - - const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); - std::vector incoming_areas; + std::vector detection_areas = { + crosswalk_lanelet.polygon2d().basicPolygon()}; const std::vector> segment_getters = { [](const auto & ls) -> lanelet::BasicSegment2d { return {ls[1].basicPoint2d(), ls[0].basicPoint2d()}; }, [](const auto & ls) -> lanelet::BasicSegment2d { - const auto size = ls.size(); - return {ls[size - 2].basicPoint2d(), ls[size - 1].basicPoint2d()}; + return {ls[ls.size() - 2].basicPoint2d(), ls[ls.size() - 1].basicPoint2d()}; }}; if ( crosswalk_lanelet.centerline2d().size() > 1 && crosswalk_lanelet.leftBound2d().size() > 1 && @@ -89,19 +82,32 @@ bool is_crosswalk_occluded( const auto center_segment = segment_getter(crosswalk_lanelet.centerline2d()); const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d()); const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d()); - const auto dist = lanelet::geometry::distance2d(center_segment.second, path_inter); + const auto dist = lanelet::geometry::distance2d(center_segment.second, crosswalk_origin); if (dist < detection_range) { const auto target_left = interpolate_point(left_segment, detection_range - dist); const auto target_right = interpolate_point(right_segment, detection_range - dist); - incoming_areas.push_back( + detection_areas.push_back( {left_segment.second, target_left, target_right, right_segment.second}); } } } - incoming_areas.push_back(crosswalk_lanelet.polygon2d().basicPolygon()); - for (const auto & incoming_area : incoming_areas) { + return detection_areas; +} + +bool is_crosswalk_occluded( + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const geometry_msgs::msg::Point & path_intersection, const double detection_range, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +{ + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + + const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); + for (const auto & detection_area : calculate_detection_areas( + crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { grid_map::Polygon poly; - for (const auto & p : incoming_area) poly.addVertex(grid_map::Position(p.x(), p.y())); + for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y())); for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; } From 610496c7af747079f8327fb315258188b4189e66 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 23 Jan 2024 16:14:24 +0900 Subject: [PATCH 16/26] [WIP] option to ignore occlusions behind predicted objects Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 2 + .../src/manager.cpp | 4 ++ .../src/occluded_crosswalk.cpp | 54 ++++++++++++++++++- .../src/occluded_crosswalk.hpp | 10 +++- .../src/scene_crosswalk.cpp | 4 +- .../src/scene_crosswalk.hpp | 2 + 6 files changed, 71 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index e5bf20b8f75a3..101cc77a5f5e6 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -79,3 +79,5 @@ free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored + min_objects_velocity: 0.5 # [m/s] minimum velocity for a predicted object to be ignored diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index de3125647e0ea..a4bafc0a2dbe1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -136,6 +136,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); cp.occlusion_ignore_with_traffic_light = getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + cp.occlusion_ignore_behind_predicted_objects = + getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); + cp.occlusion_min_objects_velocity = + getOrDeclareParameter(node, ns + ".occlusion.min_objects_velocity"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index e95563c8c2acd..cb5d89f02c204 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -16,8 +16,10 @@ #include "behavior_velocity_crosswalk_module/util.hpp" +#include #include #include +#include #include #include @@ -94,15 +96,62 @@ std::vector calculate_detection_areas( return detection_areas; } +void clear_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects, + const double min_object_velocity) +{ + const auto angle_cmp = [&](const auto & p1, const auto & p2) { + const auto d1 = p1 - grid_map.getPosition(); + const auto d2 = p2 - grid_map.getPosition(); + return std::atan2(d1.y(), d1.x()) < std::atan2(d2.y(), d2.x()); + }; + cv::Mat img; // DEBUG + if (grid_map::GridMapCvConverter::toImage( + grid_map, "layer", CV_16UC4, 0, 100, img)) + cv::imwrite("/home/mclement/Pictures/original.png", img); + const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition(); + const auto range = grid_map.getLength().maxCoeff() / 2.0; + for (const auto & object : objects) { + const lanelet::BasicPoint2d object_position = { + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y}; + if ( + object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_object_velocity && + lanelet::geometry::distance2d(grid_map_position, object_position) < range) { + lanelet::BasicPoints2d edge_points; + for (const auto & edge_point : tier4_autoware_utils::toPolygon2d(object).outer()) + edge_points.push_back(edge_point); + std::sort(edge_points.begin(), edge_points.end(), angle_cmp); + // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); + grid_map::Polygon polygon_to_clear; + polygon_to_clear.addVertex(edge_points.front()); + polygon_to_clear.addVertex( + interpolate_point({grid_map_position, edge_points.front()}, 10.0 * range)); + polygon_to_clear.addVertex( + interpolate_point({grid_map_position, edge_points.back()}, 10.0 * range)); + polygon_to_clear.addVertex(edge_points.back()); + for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it) + grid_map.at("layer", *it) = 0; + } + } + if (grid_map::GridMapCvConverter::toImage( + grid_map, "layer", CV_16UC4, 0, 100, img)) + cv::imwrite("/home/mclement/Pictures/result.png", img); +} + bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + if (params.occlusion_ignore_behind_predicted_objects) + clear_behind_objects(grid_map, dynamic_objects, params.occlusion_min_objects_velocity); const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); for (const auto & detection_area : calculate_detection_areas( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { @@ -115,11 +164,12 @@ bool is_crosswalk_occluded( } double calculate_detection_range( - const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity) + const double occluded_object_velocity, const double dist_ego_to_crosswalk, + const double ego_velocity) { constexpr double min_ego_velocity = 1.0; const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); - return time_to_crosswalk > 0.0 ? time_to_crosswalk / object_velocity : 20.0; + return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } void update_occlusion_timers( diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 0b3c945d35834..22968fc3d9a65 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -27,6 +27,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -60,16 +61,23 @@ bool is_crosswalk_ignored( /// @param occupancy_grid occupancy grid with the occlusion information /// @param path_intersection intersection between the crosswalk and the ego path /// @param detection_range range away from the crosswalk until occlusions are considered +/// @param dynamic_objects dynamic objects /// @param params parameters /// @return true if the crosswalk is occluded bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); +/// @brief calculate the distance away from the crosswalk that should be checked for occlusions +/// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions +/// @param dist_ego_to_crosswalk distance between ego and the crosswalk +/// @param ego_velocity current velocity of ego double calculate_detection_range( - const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity); + const double occluded_object_velocity, const double dist_ego_to_crosswalk, + const double ego_velocity); /// @brief update timers so that the slowdown activates if the initial time is older than the buffer /// @param initial_time initial time diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 4fe12306118ab..7725e7048f11e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -241,14 +241,14 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); const auto detection_range = calculate_detection_range( - planner_param_.min_object_velocity, dist_ego_to_crosswalk, + planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, planner_data_->current_velocity->twist.linear.x); const auto is_ego_on_the_crosswalk = dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; if (!is_ego_on_the_crosswalk) { if (is_crosswalk_occluded( crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, - planner_param_)) + objects_ptr->objects, planner_param_)) update_occlusion_timers( current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(), planner_param_.occlusion_time_buffer); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 6de34cafd2c62..e8de8702d25b8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -163,6 +163,8 @@ class CrosswalkModule : public SceneModuleInterface int occlusion_free_space_max; int occlusion_occupied_min; bool occlusion_ignore_with_traffic_light; + bool occlusion_ignore_behind_predicted_objects; + double occlusion_min_objects_velocity; }; struct ObjectInfo From 882927c9695f7b754492e5df1193b5b2f728b7f0 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 24 Jan 2024 12:54:43 +0900 Subject: [PATCH 17/26] [DEBUG] launch behavior_velocity_planner in gdb+konsole Signed-off-by: Maxime CLEMENT --- .../behavior_planning/behavior_planning.launch.xml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 90ebd23215be5..baeaa41723534 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -218,7 +218,9 @@ - + + + @@ -261,12 +263,8 @@ - - - + - - From 6c2e993d402608b66b92e8438433c116e5c579fa Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 24 Jan 2024 14:32:53 +0900 Subject: [PATCH 18/26] Fix masking behind predicted objects Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/occluded_crosswalk.cpp | 20 +++++++------------ .../src/scene_crosswalk.hpp | 1 + 4 files changed, 11 insertions(+), 13 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 101cc77a5f5e6..d50119ed79094 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -81,3 +81,4 @@ ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored min_objects_velocity: 0.5 # [m/s] minimum velocity for a predicted object to be ignored + extra_objects_size: 0.5 # [m] extra size added to the objects when masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index a4bafc0a2dbe1..21a842f7b14dd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -140,6 +140,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_min_objects_velocity = getOrDeclareParameter(node, ns + ".occlusion.min_objects_velocity"); + cp.occlusion_extra_objects_size= + getOrDeclareParameter(node, ns + ".occlusion.extra_objects_size"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index cb5d89f02c204..f2fefac5f6de6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -16,10 +16,8 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include #include #include -#include #include #include @@ -99,20 +97,18 @@ std::vector calculate_detection_areas( void clear_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects, - const double min_object_velocity) + const double min_object_velocity, const double extra_object_size) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); const auto d2 = p2 - grid_map.getPosition(); return std::atan2(d1.y(), d1.x()) < std::atan2(d2.y(), d2.x()); }; - cv::Mat img; // DEBUG - if (grid_map::GridMapCvConverter::toImage( - grid_map, "layer", CV_16UC4, 0, 100, img)) - cv::imwrite("/home/mclement/Pictures/original.png", img); const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition(); const auto range = grid_map.getLength().maxCoeff() / 2.0; - for (const auto & object : objects) { + for (auto object : objects) { + object.shape.dimensions.x += extra_object_size; + object.shape.dimensions.y += extra_object_size; const lanelet::BasicPoint2d object_position = { object.kinematics.initial_pose_with_covariance.pose.position.x, object.kinematics.initial_pose_with_covariance.pose.position.y}; @@ -120,7 +116,8 @@ void clear_behind_objects( object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_object_velocity && lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; - for (const auto & edge_point : tier4_autoware_utils::toPolygon2d(object).outer()) + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); @@ -135,9 +132,6 @@ void clear_behind_objects( grid_map.at("layer", *it) = 0; } } - if (grid_map::GridMapCvConverter::toImage( - grid_map, "layer", CV_16UC4, 0, 100, img)) - cv::imwrite("/home/mclement/Pictures/result.png", img); } bool is_crosswalk_occluded( @@ -151,7 +145,7 @@ bool is_crosswalk_occluded( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); if (params.occlusion_ignore_behind_predicted_objects) - clear_behind_objects(grid_map, dynamic_objects, params.occlusion_min_objects_velocity); + clear_behind_objects(grid_map, dynamic_objects, params.occlusion_min_objects_velocity, params.occlusion_extra_objects_size); const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); for (const auto & detection_area : calculate_detection_areas( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e8de8702d25b8..42ab42fb8f18b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -165,6 +165,7 @@ class CrosswalkModule : public SceneModuleInterface bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; double occlusion_min_objects_velocity; + double occlusion_extra_objects_size; }; struct ObjectInfo From 65687bd20370f449f2c29bf9b8bc255a3116e8dc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 24 Jan 2024 14:34:50 +0900 Subject: [PATCH 19/26] Revert "[DEBUG] launch behavior_velocity_planner in gdb+konsole" This reverts commit 391c7bd322a4e95953675a629f5c7d42cfd2d98e. Signed-off-by: Maxime CLEMENT --- .../behavior_planning/behavior_planning.launch.xml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index baeaa41723534..90ebd23215be5 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -218,9 +218,7 @@ - - - + @@ -263,8 +261,12 @@ - + + + + + From 0622e479365c7d6319eca4256dc9fce8539e3cb1 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 24 Jan 2024 14:38:19 +0900 Subject: [PATCH 20/26] pre-commit Signed-off-by: Maxime CLEMENT --- .../behavior_velocity_crosswalk_module/src/manager.cpp | 2 +- .../src/occluded_crosswalk.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 21a842f7b14dd..c2e43c484a00d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -140,7 +140,7 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_min_objects_velocity = getOrDeclareParameter(node, ns + ".occlusion.min_objects_velocity"); - cp.occlusion_extra_objects_size= + cp.occlusion_extra_objects_size = getOrDeclareParameter(node, ns + ".occlusion.extra_objects_size"); } diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index f2fefac5f6de6..867245437a2fb 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -117,8 +117,7 @@ void clear_behind_objects( lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - for (const auto & edge_point : object_polygon.outer()) - edge_points.push_back(edge_point); + for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); std::sort(edge_points.begin(), edge_points.end(), angle_cmp); // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); grid_map::Polygon polygon_to_clear; @@ -145,7 +144,9 @@ bool is_crosswalk_occluded( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); if (params.occlusion_ignore_behind_predicted_objects) - clear_behind_objects(grid_map, dynamic_objects, params.occlusion_min_objects_velocity, params.occlusion_extra_objects_size); + clear_behind_objects( + grid_map, dynamic_objects, params.occlusion_min_objects_velocity, + params.occlusion_extra_objects_size); const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); for (const auto & detection_area : calculate_detection_areas( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { From 891dc9631281d5c45eba0c83a98f238a97f7d7f5 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 26 Jan 2024 09:58:53 +0900 Subject: [PATCH 21/26] Refactor and add param to NOT ignore behind pedestrians Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 1 + .../src/manager.cpp | 8 ++-- .../src/occluded_crosswalk.cpp | 42 +++++++++++++------ .../src/occluded_crosswalk.hpp | 18 ++++++++ .../src/scene_crosswalk.hpp | 3 +- 5 files changed, 56 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index d50119ed79094..5c61469f7390b 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -80,5 +80,6 @@ occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored + do_not_ignore_behind_pedestrians: true # [-] if true, occlusions behind pedestrians are not ignored min_objects_velocity: 0.5 # [m/s] minimum velocity for a predicted object to be ignored extra_objects_size: 0.5 # [m] extra size added to the objects when masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c2e43c484a00d..0d5f959198d68 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -138,10 +138,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); - cp.occlusion_min_objects_velocity = - getOrDeclareParameter(node, ns + ".occlusion.min_objects_velocity"); + cp.occlusion_do_not_ignore_behind_pedestrians = + getOrDeclareParameter(node, ns + ".occlusion.do_not_ignore_behind_pedestrians"); + cp.occlusion_ignore_velocity_threshold = + getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_threshold"); cp.occlusion_extra_objects_size = - getOrDeclareParameter(node, ns + ".occlusion.extra_objects_size"); + getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 867245437a2fb..5c91a7f5fdd5d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -94,10 +94,30 @@ std::vector calculate_detection_areas( return detection_areas; } -void clear_behind_objects( - grid_map::GridMap & grid_map, +std::vector select_and_inflate_objects( const std::vector & objects, - const double min_object_velocity, const double extra_object_size) + const double velocity_threshold, const bool skip_pedestrians, const double inflate_size) +{ + std::vector selected_objects; + for (const auto & o : objects) { + const auto skip = + (skip_pedestrians && + o.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) || + o.kinematics.initial_twist_with_covariance.twist.linear.x >= velocity_threshold; + if (!skip) { + auto selected_object = o; + selected_object.shape.dimensions.x += inflate_size; + selected_object.shape.dimensions.y += inflate_size; + selected_objects.push_back(selected_object); + } + } + return selected_objects; +} + +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); @@ -107,14 +127,10 @@ void clear_behind_objects( const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition(); const auto range = grid_map.getLength().maxCoeff() / 2.0; for (auto object : objects) { - object.shape.dimensions.x += extra_object_size; - object.shape.dimensions.y += extra_object_size; const lanelet::BasicPoint2d object_position = { object.kinematics.initial_pose_with_covariance.pose.position.x, object.kinematics.initial_pose_with_covariance.pose.position.y}; - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_object_velocity && - lanelet::geometry::distance2d(grid_map_position, object_position) < range) { + if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); @@ -143,10 +159,12 @@ bool is_crosswalk_occluded( grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); - if (params.occlusion_ignore_behind_predicted_objects) - clear_behind_objects( - grid_map, dynamic_objects, params.occlusion_min_objects_velocity, - params.occlusion_extra_objects_size); + if (params.occlusion_ignore_behind_predicted_objects) { + const auto objects = select_and_inflate_objects( + dynamic_objects, params.occlusion_ignore_velocity_threshold, + params.occlusion_do_not_ignore_behind_pedestrians, params.occlusion_extra_objects_size); + clear_occlusions_behind_objects(grid_map, objects); + } const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); for (const auto & detection_area : calculate_detection_areas( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 22968fc3d9a65..337b24fae929c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -79,6 +79,24 @@ double calculate_detection_range( const double occluded_object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity); +/// @brief select a subset of objects meeting the velocity threshold and inflate their shape +/// @param objects input objects +/// @param velocity_threshold minimum velocity for an object to be selected +/// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities +/// @param inflate_size [m] size by which the shape of the selected objects are inflated +/// @return selected and inflated objects +std::vector select_and_inflate_objects( + const std::vector & objects, + const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); + +/// @brief clear occlusions behind the given objects +/// @details masks behind the object assuming rays from the center of the grid map +/// @param grid_map grid map +/// @param objects objects +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects); + /// @brief update timers so that the slowdown activates if the initial time is older than the buffer /// @param initial_time initial time /// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 42ab42fb8f18b..e17b2c67c27cc 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -164,7 +164,8 @@ class CrosswalkModule : public SceneModuleInterface int occlusion_occupied_min; bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; - double occlusion_min_objects_velocity; + bool occlusion_do_not_ignore_behind_pedestrians; + double occlusion_ignore_velocity_threshold; double occlusion_extra_objects_size; }; From ea4193b739efdb81029d53885849e309db0ea1d1 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 26 Jan 2024 15:12:06 +0900 Subject: [PATCH 22/26] Change params to set the ignore vel threshold for each label Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 6 ++-- .../src/manager.cpp | 28 ++++++++++++++++--- .../src/occluded_crosswalk.cpp | 14 ++++------ .../src/scene_crosswalk.hpp | 3 +- 4 files changed, 34 insertions(+), 17 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 5c61469f7390b..380e7333a84e5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -80,6 +80,8 @@ occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored - do_not_ignore_behind_pedestrians: true # [-] if true, occlusions behind pedestrians are not ignored - min_objects_velocity: 0.5 # [m/s] minimum velocity for a predicted object to be ignored + ignore_velocity_thresholds: + default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_thresholds: [0.0] # velocities of the custom labels extra_objects_size: 0.5 # [m] extra size added to the objects when masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 0d5f959198d68..aeff5e27cea7b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -138,10 +139,29 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); - cp.occlusion_do_not_ignore_behind_pedestrians = - getOrDeclareParameter(node, ns + ".occlusion.do_not_ignore_behind_pedestrians"); - cp.occlusion_ignore_velocity_threshold = - getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_threshold"); + + cp.occlusion_ignore_velocity_thresholds.resize( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); + const auto get_label = [](const std::string & s) { + if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") + return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") + return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + }; + const auto custom_labels = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); + const auto custom_velocities = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds"); + for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) { + cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx]; + } cp.occlusion_extra_objects_size = getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); } diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 5c91a7f5fdd5d..19a5ecfa35821 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -96,16 +96,12 @@ std::vector calculate_detection_areas( std::vector select_and_inflate_objects( const std::vector & objects, - const double velocity_threshold, const bool skip_pedestrians, const double inflate_size) + const std::vector velocity_thresholds, const double inflate_size) { std::vector selected_objects; for (const auto & o : objects) { - const auto skip = - (skip_pedestrians && - o.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) || - o.kinematics.initial_twist_with_covariance.twist.linear.x >= velocity_threshold; - if (!skip) { + const auto vel_threshold = velocity_thresholds[o.classification.front().label]; + if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { auto selected_object = o; selected_object.shape.dimensions.x += inflate_size; selected_object.shape.dimensions.y += inflate_size; @@ -161,8 +157,8 @@ bool is_crosswalk_occluded( if (params.occlusion_ignore_behind_predicted_objects) { const auto objects = select_and_inflate_objects( - dynamic_objects, params.occlusion_ignore_velocity_threshold, - params.occlusion_do_not_ignore_behind_pedestrians, params.occlusion_extra_objects_size); + dynamic_objects, params.occlusion_ignore_velocity_thresholds, + params.occlusion_extra_objects_size); clear_occlusions_behind_objects(grid_map, objects); } const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e17b2c67c27cc..d2468e7f31aa7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -164,8 +164,7 @@ class CrosswalkModule : public SceneModuleInterface int occlusion_occupied_min; bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; - bool occlusion_do_not_ignore_behind_pedestrians; - double occlusion_ignore_velocity_threshold; + std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size; }; From 5a8901bc526902668fd5e25c8b8fc32bc4296aad Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 29 Jan 2024 11:31:20 +0900 Subject: [PATCH 23/26] rename param Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 380e7333a84e5..f8b9e5dbf1327 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -84,4 +84,4 @@ default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) custom_thresholds: [0.0] # velocities of the custom labels - extra_objects_size: 0.5 # [m] extra size added to the objects when masking the occlusions + extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions From 20736ad7a9d322af45ddc1ee99cbee2df2fcba2b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 2 Feb 2024 17:11:18 +0900 Subject: [PATCH 24/26] fix timer Signed-off-by: Maxime CLEMENT --- .../src/occluded_crosswalk.cpp | 9 -------- .../src/occluded_crosswalk.hpp | 9 -------- .../src/scene_crosswalk.cpp | 23 +++++++++++-------- 3 files changed, 14 insertions(+), 27 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 19a5ecfa35821..5d0ecd810d845 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -180,13 +180,4 @@ double calculate_detection_range( const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } - -void update_occlusion_timers( - std::optional & initial_time, - std::optional & most_recent_slowdown_time, const rclcpp::Time & now, - const double buffer) -{ - if (!initial_time) initial_time = now; - if ((now - *initial_time).seconds() >= buffer) most_recent_slowdown_time = now; -} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 337b24fae929c..3005263c23f43 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -96,15 +96,6 @@ std::vector select_and_infl void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects); - -/// @brief update timers so that the slowdown activates if the initial time is older than the buffer -/// @param initial_time initial time -/// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer -/// @param buffer [s] time buffer -void update_occlusion_timers( - std::optional & initial_time, - std::optional & most_recent_slowdown_time, const rclcpp::Time & now, - const double buffer); } // namespace behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7725e7048f11e..afe4889b6b5a2 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -235,6 +236,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } // Apply safety slow down speed if the crosswalk is occluded + const auto now = clock_->now(); + const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { + return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); + }; if ( planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { @@ -248,21 +253,21 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (!is_ego_on_the_crosswalk) { if (is_crosswalk_occluded( crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, - objects_ptr->objects, planner_param_)) - update_occlusion_timers( - current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(), - planner_param_.occlusion_time_buffer); - else + objects_ptr->objects, planner_param_)) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now; + if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal{})) + most_recent_occlusion_time_ = now; + } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater{})) { current_initial_occlusion_time_.reset(); + } - const auto is_last_occlusion_within_time_buffer = - most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= - planner_param_.occlusion_time_buffer; - if (is_last_occlusion_within_time_buffer) { + if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal{})) { const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); applySafetySlowDownSpeed( *path, path_intersects, std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + } else { + most_recent_occlusion_time_.reset(); } } } From b681fe79230902dcfc2dac6aa7b5072c1a181994 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 2 Feb 2024 17:30:38 +0900 Subject: [PATCH 25/26] add ego lateral offset to detection range Signed-off-by: Maxime CLEMENT --- .../src/scene_crosswalk.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index afe4889b6b5a2..519f2673465d8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -245,9 +245,11 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); - const auto detection_range = calculate_detection_range( - planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, - planner_data_->current_velocity->twist.linear.x); + const auto detection_range = + planner_data_->vehicle_info_.max_lateral_offset_m + + calculate_detection_range( + planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, + planner_data_->current_velocity->twist.linear.x); const auto is_ego_on_the_crosswalk = dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; if (!is_ego_on_the_crosswalk) { From f48deca9dbd672d4ba74fd36bab0a4c6059db93d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Sat, 3 Feb 2024 10:48:01 +0900 Subject: [PATCH 26/26] ignore occlusions only if the traffic light is red Signed-off-by: Maxime CLEMENT --- .../README.md | 44 ++++++++++ .../config/crosswalk.param.yaml | 2 +- .../docs/with_occlusion.svg | 86 +++++++++++++++++++ .../src/manager.cpp | 4 +- .../src/occluded_crosswalk.cpp | 10 --- .../src/occluded_crosswalk.hpp | 7 -- .../src/scene_crosswalk.cpp | 7 +- .../src/scene_crosswalk.hpp | 2 +- 8 files changed, 138 insertions(+), 24 deletions(-) create mode 100644 planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 4c7b214763c31..7d6acbfc6aa9a 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -193,6 +193,50 @@ document. | `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | | `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +### Occlusion + +This feature makes ego slow down for a crosswalk that is occluded. + +Occlusion of the crosswalk is determined using the occupancy grid. +An occlusion is a square of size `min_size` of occluded cells +(i.e., their values are between `free_space_max` and `occupied_min`) +of size `min_size`. +If an occlusion is found within range of the crosswalk, +then the velocity limit at the crosswalk is set to `slow_down_velocity` (or more to not break limits set by `max_slow_down_jerk` and `max_slow_down_accel`). +The range is calculated from the intersection between the ego path and the crosswalk and is equal to the time taken by ego to reach the crosswalk times the `occluded_object_velocity`. +This range is meant to be large when ego is far from the crosswalk and small when ego is close. + +In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken +after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter. + +To ignore occlusions when the pedestrian light is red, `ignore_with_red_traffic_light` should be set to true. + +To ignore temporary occlusions caused by moving objects, +`ignore_behind_predicted_objects` should be set to true. +By default, occlusions behind an object with velocity higher than `ignore_velocity_thresholds.default` are ignored. +This velocity threshold can be specified depending on the object type by specifying the object class label and velocity threshold in the parameter lists `ignore_velocity_thresholds.custom_labels` and `ignore_velocity_thresholds.custom_thresholds`. +To inflate the masking behind objects, their footprint can be made bigger using `extra_predicted_objects_size`. + +
+ ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} +
+ +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_red_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | + ### Others In the `common` namespace, the following parameters are defined. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index f8b9e5dbf1327..09ce64593ff46 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -78,7 +78,7 @@ min_size: 0.5 # [m] minimum size of an occlusion (square side size) free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid - ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored ignore_velocity_thresholds: default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg new file mode 100644 index 0000000000000..a98995e0da3a9 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occluded area +
+
+
+ + + + + + + + + + + +
+
+
obstacle
+
+
+ + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index aeff5e27cea7b..39f88d5389c2b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -135,8 +135,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); - cp.occlusion_ignore_with_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + cp.occlusion_ignore_with_red_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 5d0ecd810d845..2455b36d5883f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -51,16 +51,6 @@ lanelet::BasicPoint2d interpolate_point( return segment.second + extra_distance * direction_vector; } -bool is_crosswalk_ignored( - const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light) -{ - const auto traffic_lights_reg_elems = - crosswalk_lanelet.regulatoryElementsAs(); - const bool has_traffic_light = !traffic_lights_reg_elems.empty(); - const bool has_skip_attribute = crosswalk_lanelet.hasAttribute("skip_occluded_slowdown"); - return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute; -} - std::vector calculate_detection_areas( const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, const double detection_range) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 3005263c23f43..a76fdeb549b88 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -49,13 +49,6 @@ bool is_occluded( lanelet::BasicPoint2d interpolate_point( const lanelet::BasicSegment2d & segment, const double extra_distance); -/// @brief check if the crosswalk should be ignored -/// @param crosswalk_lanelet lanelet of the crosswalk -/// @param ignore_with_traffic_light if true, ignore the crosswalk if it has a traffic light -/// @return true if the crosswalk should be ignored -bool is_crosswalk_ignored( - const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light); - /// @brief check if the crosswalk is occluded /// @param crosswalk_lanelet lanelet of the crosswalk /// @param occupancy_grid occupancy grid with the occlusion information diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 519f2673465d8..00ace56f41d7c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -240,9 +240,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); }; - if ( - planner_param_.occlusion_enable && !path_intersects.empty() && - !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { + const auto is_crosswalk_ignored = + (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) || + crosswalk_.hasAttribute("skip_occluded_slowdown"); + if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); const auto detection_range = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d2468e7f31aa7..5ce5d4020928a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -162,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface double occlusion_min_size; int occlusion_free_space_max; int occlusion_occupied_min; - bool occlusion_ignore_with_traffic_light; + bool occlusion_ignore_with_red_traffic_light; bool occlusion_ignore_behind_predicted_objects; std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size;