From 68cb41f95bf52bb1c3dc5d4d6cc8ae458308f021 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Fri, 23 Jun 2023 03:49:24 +0200 Subject: [PATCH 01/23] fix(points_preprocessor): single input topic should be first element (#4044) Signed-off-by: Tim Clephas --- sensing/pointcloud_preprocessor/launch/preprocessor.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 4b3fac1baa65f..246931629cfee 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -66,7 +66,9 @@ def generate_launch_description(): [ "'points_raw/concatenated' if len(", LaunchConfiguration("input_points_raw_list"), - ") > 1 else 'input_points_raw0'", + ") > 1 else ", + LaunchConfiguration("input_points_raw_list"), + "[0]", ] ), ), From f68374b2ea5bfd535269b42656ddedee4e1f5370 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 23 Jun 2023 12:04:35 +0900 Subject: [PATCH 02/23] feat(avoidance): improve avoidance judgement logic for pedestrian & bicycle (#4016) * feat(avoidance): don't avoid pedestrian and bicycle on crosswalk Signed-off-by: satoshi-ota * feat(avoidance): avoid pedestrian/bicycle near centerline Signed-off-by: satoshi-ota * feat(utils): use geometry distance Signed-off-by: satoshi-ota * chore(avoidance): add comment Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 6 ++ .../avoidance/avoidance_module.cpp | 1 + .../src/utils/avoidance/utils.cpp | 67 +++++++++++++++++++ 3 files changed, 74 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index beb1da66aa4a9..811587ea1c276 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -28,6 +28,12 @@ using behavior_path_planner::PlannerData; bool isOnRight(const ObjectData & obj); +bool isVehicleTypeObject(const ObjectData & object); + +bool isWithinCrosswalk( + const ObjectData & object, + const std::shared_ptr & overall_graphs); + bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index bbbdcdb995e31..51312dbf6d333 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3178,6 +3178,7 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 0a15a5dc05eee..06ca1c3cc979e 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -110,6 +110,54 @@ bool isTargetObjectType( return parameters->object_parameters.at(t).enable; } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto t = utils::getHighestProbLabel(object.object.classification); + + if (t == ObjectClassification::UNKNOWN) { + return false; + } + + if (t == ObjectClassification::PEDESTRIAN) { + return false; + } + + if (t == ObjectClassification::BICYCLE) { + return false; + } + + return true; +} + +bool isWithinCrosswalk( + const ObjectData & object, + const std::shared_ptr & overall_graphs) +{ + using Point = boost::geometry::model::d2::point_xy; + + const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position; + const Point p_object{p.x, p.y}; + + // get conflicting crosswalk crosswalk + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicts = + overall_graphs->conflictingInGraph(object.overhang_lanelet, PEDESTRIAN_GRAPH_ID); + + constexpr double THRESHOLD = 2.0; + for (const auto & crosswalk : conflicts) { + auto polygon = crosswalk.polygon2d().basicPolygon(); + + boost::geometry::correct(polygon); + + // ignore objects around the crosswalk + if (boost::geometry::distance(p_object, polygon) < THRESHOLD) { + return true; + } + } + + return false; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -752,6 +800,7 @@ void filterTargetObjects( const auto object_parameter = parameters->object_parameters.at(t); if (!isTargetObjectType(o.object, parameters)) { + o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; data.other_objects.push_back(o); continue; } @@ -873,6 +922,24 @@ void filterTargetObjects( } } + // for non vehicle type object + if (!isVehicleTypeObject(o)) { + if (isWithinCrosswalk(o, rh->getOverallGraphPtr())) { + // avoidance module ignore pedestrian and bicycle around crosswalk + o.reason = "CrosswalkUser"; + data.other_objects.push_back(o); + } else { + // if there is no crosswalk near the object, avoidance module avoids pedestrian and bicycle + // no matter how it is shifted. + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + } + continue; + } + + // from here condition check for vehicle type objects. + const auto stop_time_longer_than_threshold = o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; From cdab0e6e7e4e824a15d1a726f3e37e634305bd11 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 23 Jun 2023 12:31:07 +0900 Subject: [PATCH 03/23] refactor(occpuancy grid map): move param to yaml (#4038) --- ...robabilistic_occupancy_grid_map.launch.xml | 6 ++ .../launch/perception.launch.xml | 4 ++ .../launch/simulator.launch.xml | 4 ++ .../binary_bayes_filter_updater.param.yaml | 9 +++ ...erscan_based_occupancy_grid_map.param.yaml | 6 +- ...tcloud_based_occupancy_grid_map.param.yaml | 6 +- ...aserscan_based_occupancy_grid_map_node.hpp | 2 + ...intcloud_based_occupancy_grid_map_node.hpp | 2 + ...y_grid_map_binary_bayes_filter_updater.hpp | 12 +--- .../occupancy_grid_map_updater_interface.hpp | 2 + ...serscan_based_occupancy_grid_map.launch.py | 59 +++++++++++-------- ...ntcloud_based_occupancy_grid_map.launch.py | 39 +++++++----- ...aserscan_based_occupancy_grid_map_node.cpp | 47 +++++++++------ ...intcloud_based_occupancy_grid_map_node.cpp | 41 ++++++++----- ...y_grid_map_binary_bayes_filter_updater.cpp | 23 +++++++- 15 files changed, 178 insertions(+), 84 deletions(-) create mode 100644 perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 80ee3963553d6..0f2712a03247e 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -10,6 +10,8 @@ + + @@ -22,6 +24,8 @@ + + @@ -36,6 +40,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 47569ed2bd82c..131388b3764b5 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -9,6 +9,8 @@ + + @@ -84,6 +86,8 @@ + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index b4cd8d0cf91e2..e46528f73a0b8 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -3,6 +3,8 @@ + + @@ -49,6 +51,8 @@ + + diff --git a/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml b/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml new file mode 100644 index 0000000000000..afb776d9fd961 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + binary_bayes_filter: + probability_matrix: + occupied_to_occupied: 0.95 + occupied_to_free: 0.05 + free_to_occupied: 0.2 + free_to_free: 0.8 + v_ratio: 10.0 diff --git a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml index 3568ac7ec47a0..b8ec4d7f24046 100644 --- a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml @@ -7,7 +7,11 @@ # ray-tracing center: main sensor frame is preferable like: "velodyne_top" scan_origin_frame: "base_link" - use_height_filter: true + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: false map_length: 100.0 map_width: 100.0 diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml index 7a1a1b13ee335..6f00e9521c7ef 100644 --- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml @@ -3,7 +3,11 @@ map_length: 100.0 # [m] map_resolution: 0.5 # [m] - use_height_filter: true + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: false # use sensor pointcloud to filter obstacle pointcloud filter_obstacle_pointcloud_by_raw_pointcloud: false diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index fbef72e29e0f2..3a0f1f5ea7433 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -95,6 +95,8 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node std::string gridmap_origin_frame_; std::string scan_origin_frame_; bool use_height_filter_; + double min_height_; + double max_height_; bool enable_single_frame_mode_; }; diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index dc1b268123fcc..595937b89bbdc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -85,6 +85,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node std::string gridmap_origin_frame_; std::string scan_origin_frame_; bool use_height_filter_; + double min_height_; + double max_height_; bool enable_single_frame_mode_; bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index 31e6fd8cce8fa..d8f3cb6556bf2 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -27,20 +27,14 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface public: enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; OccupancyGridMapBBFUpdater( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) - { - probability_matrix_(Index::OCCUPIED, Index::OCCUPIED) = 0.95; - probability_matrix_(Index::FREE, Index::OCCUPIED) = - 1.0 - probability_matrix_(OCCUPIED, OCCUPIED); - probability_matrix_(Index::FREE, Index::FREE) = 0.8; - probability_matrix_(Index::OCCUPIED, Index::FREE) = 1.0 - probability_matrix_(FREE, FREE); - } + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + void initRosParam(rclcpp::Node & node) override; private: inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o); Eigen::Matrix2f probability_matrix_; + double v_ratio_; }; } // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp index f7ae908c4b0d0..9305313ddc2fc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp @@ -18,6 +18,7 @@ #include "cost_value.hpp" #include +#include namespace costmap_2d { @@ -32,6 +33,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D } virtual ~OccupancyGridMapUpdaterInterface() = default; virtual bool update(const Costmap2D & single_frame_occupancy_grid_map) = 0; + virtual void initRosParam(rclcpp::Node & node) = 0; }; } // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 3bdc3df09f1ce..26c7482943333 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -26,25 +26,15 @@ import yaml -def overwrite_config(param_dict, launch_config_name, node_params_name, context): - if LaunchConfiguration(launch_config_name).perform(context) != "": - param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context) - - def launch_setup(context, *args, **kwargs): # load parameter files param_file = LaunchConfiguration("param_file").perform(context) with open(param_file, "r") as f: laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] - overwrite_config( - laserscan_based_occupancy_grid_map_node_params, - "map_origin", - "gridmap_origin_frame", - context, - ) - overwrite_config( - laserscan_based_occupancy_grid_map_node_params, "scan_origin", "scan_origin_frame", context - ) + + updater_param_file = LaunchConfiguration("updater_param_file").perform(context) + with open(updater_param_file, "r") as f: + occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"] composable_nodes = [ ComposableNode( @@ -52,9 +42,18 @@ def launch_setup(context, *args, **kwargs): plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", name="pointcloud_to_laserscan_node", remappings=[ - ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/output/laserscan", LaunchConfiguration("output/laserscan")), - ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")), + ( + "~/input/pointcloud", + LaunchConfiguration("input/obstacle_pointcloud"), + ), + ( + "~/output/laserscan", + LaunchConfiguration("output/laserscan"), + ), + ( + "~/output/pointcloud", + LaunchConfiguration("output/pointcloud"), + ), ("~/output/ray", LaunchConfiguration("output/ray")), ("~/output/stixel", LaunchConfiguration("output/stixel")), ], @@ -90,12 +89,19 @@ def launch_setup(context, *args, **kwargs): name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), - ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ( + "~/input/obstacle_pointcloud", + LaunchConfiguration("input/obstacle_pointcloud"), + ), + ( + "~/input/raw_pointcloud", + LaunchConfiguration("input/raw_pointcloud"), + ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], parameters=[ laserscan_based_occupancy_grid_map_node_params, + occupancy_grid_map_updater_params, { "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), "input_obstacle_and_raw_pointcloud": LaunchConfiguration( @@ -148,24 +154,27 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "false"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), - add_launch_arg("map_origin", "base_link"), - add_launch_arg("sensor_origin", "base_link"), add_launch_arg("output", "occupancy_grid"), add_launch_arg("output/laserscan", "virtual_scan/laserscan"), add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"), add_launch_arg("output/ray", "virtual_scan/ray"), add_launch_arg("output/stixel", "virtual_scan/stixel"), - add_launch_arg("input_obstacle_pointcloud", "false"), - add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), add_launch_arg( "param_file", get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/laserscan_based_occupancy_grid_map.param.yaml", ), + add_launch_arg( + "updater_type", + "binary_bayes_filter", + ), + add_launch_arg( + "updater_param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/updater.param.yaml", + ), add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), - add_launch_arg("map_origin", ""), - add_launch_arg("scan_origin", ""), set_container_executable, set_container_mt_executable, ] diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 1a78072935526..f6cd4200f6e4b 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -38,15 +38,10 @@ def launch_setup(context, *args, **kwargs): pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ "ros__parameters" ] - overwrite_config( - pointcloud_based_occupancy_grid_map_node_params, - "map_origin", - "gridmap_origin_frame", - context, - ) - overwrite_config( - pointcloud_based_occupancy_grid_map_node_params, "scan_origin", "scan_origin_frame", context - ) + + updater_param_file = LaunchConfiguration("updater_param_file").perform(context) + with open(updater_param_file, "r") as f: + occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"] composable_nodes = [ ComposableNode( @@ -54,11 +49,20 @@ def launch_setup(context, *args, **kwargs): plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ - ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ( + "~/input/obstacle_pointcloud", + LaunchConfiguration("input/obstacle_pointcloud"), + ), + ( + "~/input/raw_pointcloud", + LaunchConfiguration("input/raw_pointcloud"), + ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[pointcloud_based_occupancy_grid_map_node_params], + parameters=[ + pointcloud_based_occupancy_grid_map_node_params, + occupancy_grid_map_updater_params, + ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] @@ -112,8 +116,15 @@ def add_launch_arg(name: str, default_value=None): get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/pointcloud_based_occupancy_grid_map.param.yaml", ), - add_launch_arg("map_origin", ""), - add_launch_arg("scan_origin", ""), + add_launch_arg( + "updater_type", + "binary_bayes_filter", + ), + add_launch_arg( + "updater_param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/updater.param.yaml", + ), set_container_executable, set_container_mt_executable, ] diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 248e436bd1732..e6f8ca17757c2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -51,18 +51,20 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( using std::placeholders::_3; /* params */ - map_frame_ = declare_parameter("map_frame", "map"); - base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); - scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); - use_height_filter_ = declare_parameter("use_height_filter", true); - enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); - const double map_length{declare_parameter("map_length", 100.0)}; - const double map_width{declare_parameter("map_width", 100.0)}; - const double map_resolution{declare_parameter("map_resolution", 0.5)}; - const bool input_obstacle_pointcloud{declare_parameter("input_obstacle_pointcloud", true)}; - const bool input_obstacle_and_raw_pointcloud{ - declare_parameter("input_obstacle_and_raw_pointcloud", true)}; + map_frame_ = this->declare_parameter("map_frame"); + base_link_frame_ = this->declare_parameter("base_link_frame"); + gridmap_origin_frame_ = this->declare_parameter("gridmap_origin_frame"); + scan_origin_frame_ = this->declare_parameter("scan_origin_frame"); + use_height_filter_ = this->declare_parameter("height_filter.use_height_filter"); + min_height_ = this->declare_parameter("height_filter.min_height"); + max_height_ = this->declare_parameter("height_filter.max_height"); + enable_single_frame_mode_ = this->declare_parameter("enable_single_frame_mode"); + const double map_length = this->declare_parameter("map_length"); + const double map_width = this->declare_parameter("map_width"); + const double map_resolution = this->declare_parameter("map_resolution"); + const bool input_obstacle_pointcloud = this->declare_parameter("input_obstacle_pointcloud"); + const bool input_obstacle_and_raw_pointcloud = + this->declare_parameter("input_obstacle_and_raw_pointcloud"); /* Subscriber and publisher */ laserscan_sub_.subscribe( @@ -90,9 +92,19 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( _3)); occupancy_grid_map_pub_ = create_publisher("~/output/occupancy_grid_map", 1); - /* Occupancy grid */ - occupancy_grid_map_updater_ptr_ = std::make_shared( - map_length / map_resolution, map_width / map_resolution, map_resolution); + const std::string updater_type = this->declare_parameter("updater_type"); + if (updater_type == "binary_bayes_filter") { + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_width / map_resolution, map_resolution); + } else { + RCLCPP_WARN( + get_logger(), + "specified occupancy grid map updater type [%s] is not found, use binary_bayes_filter", + updater_type.c_str()); + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_width / map_resolution, map_resolution); + } + occupancy_grid_map_updater_ptr_->initRosParam(*this); } PointCloud2::SharedPtr LaserscanBasedOccupancyGridMapNode::convertLaserscanToPointCLoud2( @@ -132,14 +144,13 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa PointCloud2 cropped_obstacle_pc{}; PointCloud2 cropped_raw_pc{}; if (use_height_filter_) { - constexpr float min_height = -1.0, max_height = 2.0; if (!utils::cropPointcloudByHeight( - *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height, + *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_obstacle_pc)) { return; } if (!utils::cropPointcloudByHeight( - *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) { + *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) { return; } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e00a79dc29e66..54395477b0892 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -51,16 +51,18 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( using std::placeholders::_2; /* params */ - map_frame_ = declare_parameter("map_frame", "map"); - base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); - scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); - use_height_filter_ = declare_parameter("use_height_filter", true); - enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); + map_frame_ = this->declare_parameter("map_frame"); + base_link_frame_ = this->declare_parameter("base_link_frame"); + gridmap_origin_frame_ = this->declare_parameter("gridmap_origin_frame"); + scan_origin_frame_ = this->declare_parameter("scan_origin_frame"); + use_height_filter_ = this->declare_parameter("height_filter.use_height_filter"); + min_height_ = this->declare_parameter("height_filter.min_height"); + max_height_ = this->declare_parameter("height_filter.max_height"); + enable_single_frame_mode_ = this->declare_parameter("enable_single_frame_mode"); filter_obstacle_pointcloud_by_raw_pointcloud_ = - declare_parameter("filter_obstacle_pointcloud_by_raw_pointcloud", false); - const double map_length{declare_parameter("map_length", 100.0)}; - const double map_resolution{declare_parameter("map_resolution", 0.5)}; + this->declare_parameter("filter_obstacle_pointcloud_by_raw_pointcloud"); + const double map_length = this->declare_parameter("map_length"); + const double map_resolution = this->declare_parameter("map_resolution"); /* Subscriber and publisher */ obstacle_pointcloud_sub_.subscribe( @@ -74,9 +76,19 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( std::bind(&PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw, this, _1, _2)); occupancy_grid_map_pub_ = create_publisher("~/output/occupancy_grid_map", 1); - /* Occupancy grid */ - occupancy_grid_map_updater_ptr_ = std::make_shared( - map_length / map_resolution, map_length / map_resolution, map_resolution); + const std::string updater_type = this->declare_parameter("updater_type"); + if (updater_type == "binary_bayes_filter") { + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_length / map_resolution, map_resolution); + } else { + RCLCPP_WARN( + get_logger(), + "specified occupancy grid map updater type [%s] is not found, use binary_bayes_filter", + updater_type.c_str()); + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_length / map_resolution, map_resolution); + } + occupancy_grid_map_updater_ptr_->initRosParam(*this); // initialize debug tool { @@ -101,14 +113,13 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( PointCloud2 cropped_obstacle_pc{}; PointCloud2 cropped_raw_pc{}; if (use_height_filter_) { - constexpr float min_height = -1.0, max_height = 2.0; if (!utils::cropPointcloudByHeight( - *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height, + *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_obstacle_pc)) { return; } if (!utils::cropPointcloudByHeight( - *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) { + *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) { return; } } diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 2438a05bbc5fc..01763b00d2a87 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -20,6 +20,26 @@ namespace costmap_2d { + +OccupancyGridMapBBFUpdater::OccupancyGridMapBBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) +: OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) +{ +} + +void OccupancyGridMapBBFUpdater::initRosParam(rclcpp::Node & node) +{ + probability_matrix_(Index::OCCUPIED, Index::OCCUPIED) = + node.declare_parameter("binary_bayes_filter.probability_matrix.occupied_to_occupied"); + probability_matrix_(Index::FREE, Index::OCCUPIED) = + node.declare_parameter("binary_bayes_filter.probability_matrix.occupied_to_free"); + probability_matrix_(Index::FREE, Index::FREE) = + node.declare_parameter("binary_bayes_filter.probability_matrix.free_to_free"); + probability_matrix_(Index::OCCUPIED, Index::FREE) = + node.declare_parameter("binary_bayes_filter.probability_matrix.free_to_occupied"); + v_ratio_ = node.declare_parameter("binary_bayes_filter.v_ratio"); +} + inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( const unsigned char & z, const unsigned char & o) { @@ -37,7 +57,7 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); } else if (z == occupancy_cost_value::NO_INFORMATION) { - constexpr float inv_v_ratio = 1.f / 10.f; + const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } return std::min( @@ -57,4 +77,5 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } return true; } + } // namespace costmap_2d From 75eecc7be5b5f4e0dcf7b3c0a1197f5b3a1d6e33 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 23 Jun 2023 13:12:56 +0900 Subject: [PATCH 04/23] feat(behavior_path_planner): use new framework (#4051) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f521407053e27..4463620dcc4a9 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) +set(COMPILE_WITH_OLD_ARCHITECTURE FALSE) set(common_src src/steering_factor_interface.cpp From 99ec28bcf3aece7daa82a940f62ac66c724320d3 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 15:27:42 +0900 Subject: [PATCH 05/23] feat(dynamic_avoidance): apply LPF to shift length, and positive relative velocity (#4047) Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 50 +++++++++++++++++-- planning/behavior_path_planner/package.xml | 1 + .../dynamic_avoidance_module.cpp | 35 +++++++++++-- 3 files changed, 76 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 1dadf8c02ad5e..217104eb7c938 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -67,9 +67,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface public: struct DynamicAvoidanceObject { - explicit DynamicAvoidanceObject( + DynamicAvoidanceObject( const PredictedObject & predicted_object, const double arg_path_projected_vel) - : pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), + pose(predicted_object.kinematics.initial_pose_with_covariance.pose), path_projected_vel(arg_path_projected_vel), shape(predicted_object.shape) { @@ -78,9 +79,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - const geometry_msgs::msg::Pose pose; - const double path_projected_vel; - const autoware_auto_perception_msgs::msg::Shape shape; + std::string uuid; + geometry_msgs::msg::Pose pose; + double path_projected_vel; + autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths{}; bool is_left; @@ -124,7 +126,45 @@ class DynamicAvoidanceModule : public SceneModuleInterface const DynamicAvoidanceObject & object) const; std::vector target_objects_; + // std::vector prev_target_objects_; std::shared_ptr parameters_; + + struct ObjectsVariable + { + void resetCurrentUuids() { current_uuids_.clear(); } + void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); } + void removeCounterUnlessUpdated() + { + std::vector obsolete_uuids; + for (const auto & key_and_value : variable_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == + current_uuids_.end()) { + obsolete_uuids.push_back(key_and_value.first); + } + } + + for (const auto & obsolete_uuid : obsolete_uuids) { + variable_.erase(obsolete_uuid); + } + } + + std::optional get(const std::string & uuid) const + { + if (variable_.count(uuid) != 0) { + return variable_.at(uuid); + } + return std::nullopt; + } + void update(const std::string & uuid, const double new_variable) + { + variable_.emplace(uuid, new_variable); + } + + std::unordered_map variable_; + std::vector current_uuids_; + }; + mutable ObjectsVariable prev_objects_min_bound_lat_offset_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index cd7311f3308bb..97453b0325cef 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -62,6 +62,7 @@ route_handler rtc_interface sensor_msgs + signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 47e8c73c394ae..d8860c86ae112 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "signal_processing/lowpass_filter_1d.hpp" #include #include @@ -128,6 +129,19 @@ void appendExtractedPolygonMarker( marker_array.markers.push_back(marker); } + +template +std::optional getObjectFromUuid(const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -211,6 +225,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() // 3. create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; + prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { const auto obstacle_poly = calcDynamicObstaclePolygon(object); if (obstacle_poly) { @@ -218,8 +233,11 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() appendObjectMarker(info_marker_, object.pose); appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + + prev_objects_min_bound_lat_offset_.addCurrentUuid(object.uuid); } } + prev_objects_min_bound_lat_offset_.removeCounterUnlessUpdated(); BehaviorModuleOutput output; output.path = prev_module_path; @@ -389,6 +407,7 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } +// NOTE: object does not have const only to update min_bound_lat_offset. std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { @@ -455,10 +474,8 @@ std::optional DynamicAvoidanceModule::calcDynam motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); const double signed_lon_length = motion_utils::calcSignedArcLength( prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); - if (relative_velocity == 0.0) { - return std::numeric_limits::max(); - } - return signed_lon_length / relative_velocity; + const double positive_relative_velocity = std::max(relative_velocity, 1.0); + return signed_lon_length / positive_relative_velocity; }(); if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { @@ -535,13 +552,21 @@ std::optional DynamicAvoidanceModule::calcDynam const double max_bound_lat_offset = max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); + // filter min_bound_lat_offset + const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid); + const double filtered_min_bound_lat_offset = + prev_min_bound_lat_offset + ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_bound_lat_offset, 0.3) + : min_bound_lat_offset; + prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset); + // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { obj_inner_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, min_bound_lat_offset, 0.0) + path_with_backward_margin.points.at(i).point.pose, 0.0, filtered_min_bound_lat_offset, 0.0) .position); obj_outer_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( From deffac2a874de02a7cbe1d02133a48cc8cd754bf Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 15:28:30 +0900 Subject: [PATCH 06/23] refactor(crosswalk): minor refactoring (#4046) * refactor(crosswalk): minor refactoring Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.cpp | 78 +++++++++---------- .../src/scene_crosswalk.hpp | 8 +- 2 files changed, 43 insertions(+), 43 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7047fc7b56f2e..ca39796c616ba 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -194,52 +194,52 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto logger_, planner_param_.show_processing_time, "- step2: %f ms", stop_watch_.toc("total_processing_time", false)); - StopFactor stop_factor{}; - StopFactor stop_factor_rtc{}; - - const auto nearest_stop_point = findNearestStopPoint(ego_path, stop_factor); - const auto rtc_stop_point = findRTCStopPoint(ego_path, stop_factor_rtc); + const auto nearest_stop_point_with_factor = findNearestStopPointWithFactor(ego_path); + const auto rtc_stop_point_with_factor = findRTCStopPointWithFactor(ego_path); RCLCPP_INFO_EXPRESSION( logger_, planner_param_.show_processing_time, "- step3: %f ms", stop_watch_.toc("total_processing_time", false)); - setSafe(!nearest_stop_point); + setSafe(!nearest_stop_point_with_factor); if (isActivated()) { - if (!nearest_stop_point) { - if (!rtc_stop_point) { + if (!nearest_stop_point_with_factor) { + if (!rtc_stop_point_with_factor) { setDistance(std::numeric_limits::lowest()); return true; } const auto crosswalk_distance = - calcSignedArcLength(ego_path.points, ego_pos, getPoint(rtc_stop_point.get())); + calcSignedArcLength(ego_path.points, ego_pos, getPoint(rtc_stop_point_with_factor->first)); setDistance(crosswalk_distance); return true; } - const auto target_velocity = calcTargetVelocity(nearest_stop_point.get(), ego_path); + const auto target_velocity = + calcTargetVelocity(nearest_stop_point_with_factor->first, ego_path); insertDecelPointWithDebugInfo( - nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity), - *path); + nearest_stop_point_with_factor->first, + std::max(planner_param_.min_slow_down_velocity, target_velocity), *path); return true; } - if (nearest_stop_point) { - insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_factor.stop_pose, - VelocityFactor::UNKNOWN); - } else if (rtc_stop_point) { - insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path); - planning_utils::appendStopReason(stop_factor_rtc, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_factor_rtc.stop_pose, - VelocityFactor::UNKNOWN); - } + const auto & stop_point_with_factor = + [&]() -> boost::optional> { + if (nearest_stop_point_with_factor) + return nearest_stop_point_with_factor.get(); + else if (rtc_stop_point_with_factor) + return rtc_stop_point_with_factor.get(); + return {}; + }(); + + const auto & stop_factor = stop_point_with_factor->second; + insertDecelPointWithDebugInfo(stop_point_with_factor->first, 0.0, *path); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_factor.stop_pose, + VelocityFactor::UNKNOWN); RCLCPP_INFO_EXPRESSION( logger_, planner_param_.show_processing_time, "- step4: %f ms", @@ -291,8 +291,8 @@ boost::optional> CrosswalkModule::g return {}; } -boost::optional CrosswalkModule::findRTCStopPoint( - const PathWithLaneId & ego_path, StopFactor & stop_factor) +boost::optional> +CrosswalkModule::findRTCStopPointWithFactor(const PathWithLaneId & ego_path) { const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -311,14 +311,17 @@ boost::optional CrosswalkModule::findRTCStopPoint( return {}; } + StopFactor stop_factor; stop_factor.stop_pose = stop_pose.get(); - return stop_pose.get().position; + return std::make_pair(stop_pose.get().position, stop_factor); } -boost::optional CrosswalkModule::findNearestStopPoint( - const PathWithLaneId & ego_path, StopFactor & stop_factor) +boost::optional> +CrosswalkModule::findNearestStopPointWithFactor(const PathWithLaneId & ego_path) { + StopFactor stop_factor; + bool found_pedestrians = false; bool found_stuck_vehicle = false; @@ -485,7 +488,7 @@ boost::optional CrosswalkModule::findNearestStopPoint stop_factor.stop_pose = stop_pose.get(); - return stop_pose.get().position; + return std::make_pair(stop_pose.get().position, stop_factor); } std::pair CrosswalkModule::getAttentionRange(const PathWithLaneId & ego_path) @@ -844,18 +847,15 @@ bool CrosswalkModule::isStuckVehicle( return false; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - - double near_attention_range = 0.0; - double far_attention_range = 0.0; if (path_intersects_.size() < 2) { return false; } - near_attention_range = calcSignedArcLength(ego_path.points, ego_pos, path_intersects_.front()); - far_attention_range = calcSignedArcLength(ego_path.points, ego_pos, path_intersects_.back()); - near_attention_range = far_attention_range; - far_attention_range += planner_param_.stuck_vehicle_attention_range; + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double near_attention_range = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects_.back()); + const double far_attention_range = + near_attention_range + planner_param_.stuck_vehicle_attention_range; const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index c382d09e9ec6e..d53bc30471426 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -107,11 +107,11 @@ class CrosswalkModule : public SceneModuleInterface private: int64_t module_id_; - boost::optional findRTCStopPoint( - const PathWithLaneId & ego_path, StopFactor & stop_factor); + boost::optional> findRTCStopPointWithFactor( + const PathWithLaneId & ego_path); - boost::optional findNearestStopPoint( - const PathWithLaneId & ego_path, StopFactor & stop_factor); + boost::optional> findNearestStopPointWithFactor( + const PathWithLaneId & ego_path); boost::optional> getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const; From 385ef5bf7a22b932a06a713c19b6d0e1fcd3510a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 15:28:55 +0900 Subject: [PATCH 07/23] feat(behavior_velocity_traffic_light_module): print not found traffic ids for warning (#4045) Signed-off-by: Takayuki Murooka --- .../src/scene.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 925dfb0470b98..679a2f0cb9320 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -442,9 +442,15 @@ bool TrafficLightModule::getHighestConfidenceTrafficSignal( } } if (!found) { + std::string not_found_traffic_ids{""}; + for (size_t i = 0; i < traffic_lights.size(); ++i) { + const int id = static_cast(traffic_lights.at(i)).id(); + not_found_traffic_ids += (i != 0 ? "," : "") + std::to_string(id); + } + RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s).", - reason.c_str()); + logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s) due to %s.", + not_found_traffic_ids.c_str(), reason.c_str()); return false; } return true; From 7c77bbdefc5f2aa2bbec05fe1a8d5ac4c8374b94 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 23 Jun 2023 16:37:15 +0900 Subject: [PATCH 08/23] =?UTF-8?q?fix(behavior=5Fpath=5Fplanner):=20print?= =?UTF-8?q?=20avoidance=20and=20lane=20change=20debug=20mes=E2=80=A6=20(#4?= =?UTF-8?q?058)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix(behavior_path_planner): print avoidance and lane change debug message Signed-off-by: Zulfaqar Azmi --- .../behavior_path_planner_node.hpp | 2 -- .../behavior_path_planner/planner_manager.hpp | 8 ++++++++ .../src/behavior_path_planner_node.cpp | 3 +-- .../behavior_path_planner/src/planner_manager.cpp | 13 +++++++++++++ 4 files changed, 22 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 60152bdf42bb1..ea0e990a8b259 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -234,10 +234,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish debug messages */ -#ifdef USE_OLD_ARCHITECTURE void publishSceneModuleDebugMsg( const std::shared_ptr & debug_messages_data_ptr); -#endif /** * @brief publish path candidate diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index c91065d4a0a5d..108ca29f161b1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" #include @@ -225,6 +226,11 @@ class PlannerManager */ void print() const; + /** + * @brief visit each module and get debug information. + */ + std::shared_ptr getDebugMsg(); + private: /** * @brief run the module and publish RTC status. @@ -461,6 +467,8 @@ class PlannerManager mutable std::vector debug_info_; + mutable std::shared_ptr debug_msg_ptr_; + bool verbose_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a853661891e75..bc3711b72e648 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1295,6 +1295,7 @@ void BehaviorPathPlannerNode::run() publishPathCandidate(bt_manager_->getSceneModules(), planner_data_); publishSceneModuleDebugMsg(bt_manager_->getAllSceneModuleDebugMsgData()); #else + publishSceneModuleDebugMsg(planner_manager_->getDebugMsg()); publishPathCandidate(planner_manager_->getSceneModuleManagers(), planner_data_); publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_); stop_reason_publisher_->publish(planner_manager_->getStopReasons()); @@ -1416,7 +1417,6 @@ void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) bound_publisher_->publish(msg); } -#ifdef USE_OLD_ARCHITECTURE void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( const std::shared_ptr & debug_messages_data_ptr) { @@ -1430,7 +1430,6 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( debug_lane_change_msg_array_publisher_->publish(*lane_change_debug_message); } } -#endif #ifdef USE_OLD_ARCHITECTURE void BehaviorPathPlannerNode::publishPathCandidate( diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a84f13fd5a9d0..c9d4d2c191a4c 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -718,4 +718,17 @@ void PlannerManager::print() const RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +std::shared_ptr PlannerManager::getDebugMsg() +{ + debug_msg_ptr_ = std::make_shared(); + for (const auto & approved_module : approved_module_ptrs_) { + approved_module->acceptVisitor(debug_msg_ptr_); + } + + for (const auto & candidate_module : candidate_module_ptrs_) { + candidate_module->acceptVisitor(debug_msg_ptr_); + } + return debug_msg_ptr_; +} + } // namespace behavior_path_planner From a0a0b1a91200f0f12077a52917129efd3b1ea605 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 16:48:43 +0900 Subject: [PATCH 09/23] feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching (#4048) * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 10 +++--- .../dynamic_avoidance_module.hpp | 23 +++++++++++- .../src/behavior_path_planner_node.cpp | 2 ++ .../dynamic_avoidance_module.cpp | 35 +++++++++++++++---- .../dynamic_avoidance/manager.cpp | 4 +++ 5 files changed, 63 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index a4ff684839321..0b8f35bac3131 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -12,21 +12,23 @@ motorcycle: true pedestrian: false - min_obstacle_vel: 1.0 # [m/s] + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 drivable_area_generation: - lat_offset_from_obstacle: 1.3 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] # for same directional object overtaking_object: max_time_to_collision: 3.0 # [s] start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 5.0 # [s] + end_duration_to_avoid: 8.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 9.0 # [s] + start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 217104eb7c938..799c0aad6e307 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -47,6 +47,7 @@ struct DynamicAvoidanceParameters bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; + int successive_num_to_entry_dynamic_avoidance_condition{0}; // drivable area generation double lat_offset_from_obstacle{0.0}; @@ -87,6 +88,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool is_left; }; + struct DynamicAvoidanceObjectCandidate + { + DynamicAvoidanceObject object; + int alive_counter; + + static std::optional getObjectFromUuid( + const std::vector & objects, const std::string & target_uuid) + { + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; + } + }; #ifdef USE_OLD_ARCHITECTURE DynamicAvoidanceModule( @@ -119,12 +138,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface private: bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjects() const; + std::vector calcTargetObjectsCandidate() const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; std::optional calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; + std::vector + prev_target_objects_candidate_; std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bc3711b72e648..1359b1afc86fc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -743,6 +743,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); + p.successive_num_to_entry_dynamic_avoidance_condition = + declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); } { // drivable_area_generation diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index d8860c86ae112..c75ed08b7a5ce 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -192,7 +192,19 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - target_objects_ = calcTargetObjects(); + // calculate target objects candidate + const auto target_objects_candidate = calcTargetObjectsCandidate(); + prev_target_objects_candidate_ = target_objects_candidate; + + // calculate target objects considering flickering suppress + target_objects_.clear(); + for (const auto & target_object_candidate : target_objects_candidate) { + if ( + parameters_->successive_num_to_entry_dynamic_avoidance_condition <= + target_object_candidate.alive_counter) { + target_objects_.push_back(target_object_candidate.object); + } + } } ModuleStatus DynamicAvoidanceModule::updateState() @@ -293,8 +305,8 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjects() const +std::vector +DynamicAvoidanceModule::calcTargetObjectsCandidate() const { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; @@ -329,7 +341,7 @@ DynamicAvoidanceModule::calcTargetObjects() const // 4. check if object will cut into the ego lane. // NOTE: The oncoming object will be ignored. constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects; + std::vector output_objects_candidate; for (const bool is_left : {true, false}) { for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { const auto reliable_predicted_path = std::max_element( @@ -358,13 +370,24 @@ DynamicAvoidanceModule::calcTargetObjects() const continue; } + // get previous object if it exists + const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( + prev_target_objects_candidate_, object.uuid); + const int alive_counter = + prev_target_object_candidate + ? std::min( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + prev_target_object_candidate->alive_counter + 1) + : 0; + auto target_object = object; target_object.is_left = is_left; - output_objects.push_back(target_object); + output_objects_candidate.push_back( + DynamicAvoidanceObjectCandidate{target_object, alive_counter}); } } - return output_objects; + return output_objects_candidate; } std::pair DynamicAvoidanceModule::getAdjacentLanes( diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index d3212a7c0b1d4..b9ca4ffa964f7 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -49,6 +49,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); + + updateParam( + parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", + p->successive_num_to_entry_dynamic_avoidance_condition); } { // drivable_area_generation From 702947346bb48f89a93aab1a88cc7d60a2a9ea4d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 23 Jun 2023 17:33:38 +0900 Subject: [PATCH 10/23] fix(avoidance): update logic to keep waiting approval (#4059) * revert "fix(avoidance): don't clear waiting approval if raw shift line exists (#4012)" This reverts commit 8577563ca1dca2994f099c7c47307ad57cbce875. * fix(avoidance): update logic to keep waiting approval Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 51312dbf6d333..1d5142c81c0f9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2695,9 +2695,11 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() [](const auto & o) { return !o.is_avoidable; }); const auto candidate = planCandidate(); - if (!avoidance_data_.unapproved_raw_sl.empty()) { + if (!data.safe_new_sl.empty()) { updateCandidateRTCStatus(candidate); waitApproval(); + } else if (path_shifter_.getShiftLines().empty()) { + waitApproval(); } else if (all_unavoidable) { waitApproval(); } else { From 25f9d6a03adf91ed2312741b9acdf937560245a9 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 23 Jun 2023 17:41:10 +0900 Subject: [PATCH 11/23] fix(lane_change): fix debug visualization now showing (#4061) * fix(lane_change): fix debug visualization now showing Signed-off-by: Zulfaqar Azmi * renaming the markers Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene_module/lane_change/interface.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index c23bf807f121c..81df39cb8f789 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -116,6 +116,7 @@ ModuleStatus LaneChangeInterface::updateState() const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); + setObjectDebugVisualization(); if (is_safe) { module_type_->toNormalState(); return ModuleStatus::RUNNING; @@ -270,6 +271,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); is_abort_path_approved_ = false; + setObjectDebugVisualization(); + return out; } @@ -300,6 +303,10 @@ void LaneChangeInterface::setData(const std::shared_ptr & dat void LaneChangeInterface::setObjectDebugVisualization() const { + debug_marker_.markers.clear(); + if (!parameters_->publish_debug_marker) { + return; + } using marker_utils::lane_change_markers::showAllValidLaneChangePath; using marker_utils::lane_change_markers::showLerpedPose; using marker_utils::lane_change_markers::showObjectInfo; @@ -315,9 +322,8 @@ void LaneChangeInterface::setObjectDebugVisualization() const }; add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "lerp_pose_before_true")); - add(showPolygonPose(debug_data, "expected_pose")); - add(showPolygon(debug_data, "lerped_polygon")); + add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showPolygon(debug_data, "ego_and_target_polygon_relation")); add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); } From e70df1239c67d8ae86a7a8bf8d6c83bc20e7ece7 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 23 Jun 2023 18:02:39 +0900 Subject: [PATCH 12/23] feat(dynamic_avoidance): ignore cut-out vehicle (#4049) * feat(dynamic_avoidance): ignore cut-out vehicle Signed-off-by: Takayuki Murooka * update cut out decision policy Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 8 +- .../dynamic_avoidance_module.cpp | 89 ++++++++++++------- 2 files changed, 61 insertions(+), 36 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 799c0aad6e307..4898188c71319 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -69,10 +69,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface struct DynamicAvoidanceObject { DynamicAvoidanceObject( - const PredictedObject & predicted_object, const double arg_path_projected_vel) + const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), - path_projected_vel(arg_path_projected_vel), + vel(arg_vel), + lat_vel(arg_lat_vel), shape(predicted_object.shape) { for (const auto & path : predicted_object.kinematics.predicted_paths) { @@ -82,7 +83,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string uuid; geometry_msgs::msg::Pose pose; - double path_projected_vel; + double vel; + double lat_vel; autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths{}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index c75ed08b7a5ce..a8a7f89c54021 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -73,20 +73,6 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & return geom_obj_point; } -double calcObstacleProjectedVelocity( - const std::vector & path_points, const PredictedObject & object) -{ - const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - - const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - - return obj_vel * std::cos(obj_yaw - path_yaw); -} - std::pair getMinMaxValues(const std::vector & vec) { const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); @@ -142,6 +128,21 @@ std::optional getObjectFromUuid(const std::vector & objects, const std::st } return *itr; } + +std::pair projectObstacleVelocityToTrajectory( + const std::vector & path_points, const PredictedObject & object) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + + const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); + + return std::make_pair( + obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -321,14 +322,14 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const continue; } - const double path_projected_vel = - calcObstacleProjectedVelocity(prev_module_path->points, predicted_object); + const auto [tangent_vel, normal_vel] = + projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); // check if velocity is high enough - if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) { + if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) { continue; } - input_objects.push_back(DynamicAvoidanceObject(predicted_object, path_projected_vel)); + input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel)); } // 2. calculate target lanes to filter obstacles @@ -338,7 +339,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); - // 4. check if object will cut into the ego lane. + // 4. check if object will cut into the ego lane or cut out to the next lane. // NOTE: The oncoming object will be ignored. constexpr double epsilon_path_lat_diff = 0.3; std::vector output_objects_candidate; @@ -352,7 +353,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const // Ignore object that will cut into the ego lane const bool will_object_cut_in = [&]() { - if (object.path_projected_vel < 0) { + if (object.vel < 0) { // Ignore oncoming object return false; } @@ -370,6 +371,29 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const continue; } + // Ignore object that will cut out to the next lane + const bool will_object_cut_out = [&]() { + if (object.vel < 0) { + // Ignore oncoming object + return false; + } + + constexpr double object_lat_vel_thresh = 0.3; + if (is_left) { + if (object_lat_vel_thresh < object.lat_vel) { + return true; + } + } else { + if (object.lat_vel < -object_lat_vel_thresh) { + return true; + } + } + return false; + }(); + if (will_object_cut_out) { + continue; + } + // get previous object if it exists const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( prev_target_objects_candidate_, object.uuid); @@ -489,7 +513,7 @@ std::optional DynamicAvoidanceModule::calcDynam getMinMaxValues(obj_lon_offset_vec); // calculate time to collision and apply it to drivable area extraction - const double relative_velocity = getEgoSpeed() - object.path_projected_vel; + const double relative_velocity = getEgoSpeed() - object.vel; const double time_to_collision = [&]() { const auto prev_module_path = getPreviousModuleOutput().path; const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); @@ -505,19 +529,18 @@ std::optional DynamicAvoidanceModule::calcDynam return std::nullopt; } - if (0 <= object.path_projected_vel) { + if (0 <= object.vel) { const double limited_time_to_collision = std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); return std::make_pair( - raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, - raw_max_obj_lon_offset + object.path_projected_vel * limited_time_to_collision); + raw_min_obj_lon_offset + object.vel * limited_time_to_collision, + raw_max_obj_lon_offset + object.vel * limited_time_to_collision); } const double limited_time_to_collision = std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); return std::make_pair( - raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision, - raw_max_obj_lon_offset); + raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); }(); if (!obj_lon_offset) { @@ -527,15 +550,15 @@ std::optional DynamicAvoidanceModule::calcDynam const double max_obj_lon_offset = obj_lon_offset->second; // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= object.path_projected_vel); + const bool is_object_overtaking = (0.0 <= object.vel); const double start_length_to_avoid = - std::abs(object.path_projected_vel) * - (is_object_overtaking ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); + std::abs(object.vel) * (is_object_overtaking + ? parameters_->start_duration_to_avoid_overtaking_object + : parameters_->start_duration_to_avoid_oncoming_object); const double end_length_to_avoid = - std::abs(object.path_projected_vel) * (is_object_overtaking - ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); + std::abs(object.vel) * (is_object_overtaking + ? parameters_->end_duration_to_avoid_overtaking_object + : parameters_->end_duration_to_avoid_oncoming_object); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points); const size_t updated_obj_seg_idx = From 4ee69759ecfc00efc31aa710b4a62953f48cd518 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 23 Jun 2023 18:51:28 +0900 Subject: [PATCH 13/23] feat(mission_planner): add mrm route planner (#4027) * feat(mission_planner): add mrm route planner Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 165 +++++++++++++++++- .../src/mission_planner/mission_planner.hpp | 3 + 2 files changed, 160 insertions(+), 8 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 098f5e07ac071..7de19df1b3b4e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,7 +75,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), - normal_route_(nullptr) + normal_route_(nullptr), + mrm_route_(nullptr) { map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); @@ -155,6 +156,11 @@ void MissionPlanner::clear_route() // TODO(Takagi, Isamu): publish an empty route here } +void MissionPlanner::clear_mrm_route() +{ + mrm_route_ = nullptr; +} + void MissionPlanner::change_route(const LaneletRoute & route) { PoseWithUuidStamped goal; @@ -172,6 +178,23 @@ void MissionPlanner::change_route(const LaneletRoute & route) normal_route_ = std::make_shared(route); } +void MissionPlanner::change_mrm_route(const LaneletRoute & route) +{ + PoseWithUuidStamped goal; + goal.header = route.header; + goal.pose = route.goal_pose; + goal.uuid = route.uuid; + + arrival_checker_.set_goal(goal); + pub_route_->publish(route); + pub_mrm_route_->publish(route); + pub_marker_->publish(planner_->visualize(route)); + planner_->updateRoute(route); + + // update emergency route + mrm_route_ = std::make_shared(route); +} + LaneletRoute MissionPlanner::create_route( const std_msgs::msg::Header & header, const std::vector & route_segments, @@ -278,6 +301,10 @@ void MissionPlanner::on_set_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (mrm_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + } // Convert request to a new route. const auto route = create_route(req); @@ -313,6 +340,10 @@ void MissionPlanner::on_set_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (mrm_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + } // Plan route. const auto route = create_route(req); @@ -334,19 +365,124 @@ void MissionPlanner::on_set_mrm_route( const SetMrmRoute::Service::Request::SharedPtr req, const SetMrmRoute::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute for MRM - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + + if (!planner_->ready()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } + if (!odometry_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + + const auto prev_state = state_.state; + change_state(RouteState::Message::CHANGING); + + // Plan route. + const auto new_route = create_route(req); + + if (new_route.segments.empty()) { + change_state(prev_state); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "Failed to plan a new route."); + } + + // check route safety + // step1. if in mrm state, check with mrm route + if (mrm_route_) { + if (checkRerouteSafety(*mrm_route_, new_route)) { + // success to reroute + change_mrm_route(new_route); + res->status.success = true; + } else { + // failed to reroute + change_mrm_route(*mrm_route_); + res->status.success = false; + } + change_state(RouteState::Message::SET); + return; + } + + if (!normal_route_) { + // if it does not set normal route, just use the new planned route + change_mrm_route(new_route); + change_state(RouteState::Message::SET); + res->status.success = true; + return; + } + + // step2. if not in mrm state, check with normal route + if (checkRerouteSafety(*normal_route_, new_route)) { + // success to reroute + change_mrm_route(new_route); + res->status.success = true; + } else { + // Failed to reroute + change_route(*normal_route_); + res->status.success = false; + } + change_state(RouteState::Message::SET); } // NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr req, + const ClearMrmRoute::Service::Request::SharedPtr, const ClearMrmRoute::Service::Response::SharedPtr res) { - // TODO(Yutaka Shimizu): reroute for MRM - (void)req; - (void)res; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + + if (!planner_->ready()) { + change_state(RouteState::Message::SET); + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } + if (!odometry_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + } + if (!mrm_route_) { + throw component_interface_utils::NoEffectWarning("MRM route is not set"); + } + + change_state(RouteState::Message::CHANGING); + + if (!normal_route_) { + clear_mrm_route(); + change_state(RouteState::Message::UNSET); + res->success = true; + return; + } + + // check route safety + if (checkRerouteSafety(*mrm_route_, *normal_route_)) { + clear_mrm_route(); + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->success = true; + return; + } + + // reroute with normal goal + const std::vector empty_waypoints; + const auto new_route = create_route( + odometry_->header, empty_waypoints, normal_route_->goal_pose, + normal_route_->allow_modification); + + // check new route safety + if (new_route.segments.empty() || !checkRerouteSafety(*mrm_route_, new_route)) { + // failed to create a new route + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "Reroute with normal goal failed."); + change_mrm_route(*mrm_route_); + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->success = false; + } else { + clear_mrm_route(); + change_route(new_route); + change_state(RouteState::Message::SET); + res->success = true; + } } void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) @@ -367,6 +503,11 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt RCLCPP_ERROR(get_logger(), "Normal route has not set yet."); return; } + if (mrm_route_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "Cannot modify goal in the emergency state"); + return; + } if (normal_route_->uuid == msg->uuid) { // set to changing state @@ -413,6 +554,10 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); } + if (mrm_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -468,6 +613,10 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); } + if (mrm_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + } change_state(RouteState::Message::CHANGING); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index bd3655bc55b0d..d3ca6be7eb63f 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -77,7 +77,9 @@ class MissionPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); + void clear_mrm_route(); void change_route(const LaneletRoute & route); + void change_mrm_route(const LaneletRoute & route); LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req); LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req); LaneletRoute create_route( @@ -136,6 +138,7 @@ class MissionPlanner : public rclcpp::Node bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::shared_ptr normal_route_{nullptr}; + std::shared_ptr mrm_route_{nullptr}; }; } // namespace mission_planner From 4bc5ccce99efb3742421fb8b939737e8bbeda69d Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Fri, 23 Jun 2023 18:58:31 +0900 Subject: [PATCH 14/23] revert: "feat(behavior_path_planner): relax longitudinal_velocity_delta_time" (#4064) Revert "feat(behavior_path_planner): relax longitudinal_velocity_delta_time (#4039)" This reverts commit f6f3babcf2bcc0f9638dee055cb05c3590131feb. --- .../config/behavior_path_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index bfadf9497c0fd..a35f920dfb482 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -31,7 +31,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.3 # [s] + longitudinal_velocity_delta_time: 0.8 # [s] expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 From 87f9b86fa1023468fe09e2a0e67ed4dce6db52db Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 23 Jun 2023 19:01:14 +0900 Subject: [PATCH 15/23] fix(start_planner): not run other modules when back pull out (#4043) * fix(start_planner): not run other modules when back pull out Signed-off-by: kosuke55 * fix reorder and value name Signed-off-by: kosuke55 * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module_manager_interface.hpp | 16 ++++++++-------- .../start_planner/start_planner_module.hpp | 13 +++++++++++++ .../scene_module/avoidance/avoidance_module.cpp | 8 -------- .../src/scene_module/start_planner/manager.cpp | 6 +++++- .../start_planner/start_planner_module.cpp | 12 ++++++++++++ 5 files changed, 38 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 46a8d09c77ef2..596be548508fc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -49,13 +49,13 @@ class SceneModuleManagerInterface clock_(*node->get_clock()), logger_(node->get_logger().get_child(name)), name_(name), - enable_rtc_(config.enable_rtc), - max_module_num_(config.max_module_size), - priority_(config.priority), enable_simultaneous_execution_as_approved_module_( config.enable_simultaneous_execution_as_approved_module), enable_simultaneous_execution_as_candidate_module_( - config.enable_simultaneous_execution_as_candidate_module) + config.enable_simultaneous_execution_as_candidate_module), + enable_rtc_(config.enable_rtc), + max_module_num_(config.max_module_size), + priority_(config.priority) { for (const auto & rtc_type : rtc_types) { const auto snake_case_name = utils::convertToSnakeCase(name); @@ -281,16 +281,16 @@ class SceneModuleManagerInterface std::unordered_map> rtc_interface_ptr_map_; + bool enable_simultaneous_execution_as_approved_module_{false}; + + bool enable_simultaneous_execution_as_candidate_module_{false}; + private: bool enable_rtc_; size_t max_module_num_; size_t priority_; - - bool enable_simultaneous_execution_as_approved_module_{false}; - - bool enable_simultaneous_execution_as_candidate_module_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 3f6ea8f5b3799..a48c24b183111 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -104,6 +104,19 @@ class StartPlannerModule : public SceneModuleInterface { } + // set is_simultaneously_executable_ as false when backward driving. + // keep initial value to return it after finishing backward driving. + bool initial_value_simultaneously_executable_as_approved_module_; + bool initial_value_simultaneously_executable_as_candidate_module_; + void setInitialIsSimultaneousExecutableAsApprovedModule(const bool is_simultaneously_executable) + { + initial_value_simultaneously_executable_as_approved_module_ = is_simultaneously_executable; + }; + void setInitialIsSimultaneousExecutableAsCandidateModule(const bool is_simultaneously_executable) + { + initial_value_simultaneously_executable_as_candidate_module_ = is_simultaneously_executable; + }; + private: std::shared_ptr parameters_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 1d5142c81c0f9..ac98eb5a2b59b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -117,14 +117,6 @@ bool AvoidanceModule::isExecutionRequested() const { DEBUG_PRINT("AVOIDANCE isExecutionRequested"); -#ifndef USE_OLD_ARCHITECTURE - const auto is_driving_forward = - motion_utils::isDrivingForward(getPreviousModuleOutput().path->points); - if (!is_driving_forward || !(*is_driving_forward)) { - return false; - } -#endif - if (current_state_ == ModuleStatus::RUNNING) { return true; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index ab33c78c945ab..3b2917c94c8e1 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -39,8 +39,12 @@ void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&](const auto & m) { m->updateModuleParams(p); + m->setInitialIsSimultaneousExecutableAsApprovedModule( + enable_simultaneous_execution_as_approved_module_); + m->setInitialIsSimultaneousExecutableAsCandidateModule( + enable_simultaneous_execution_as_candidate_module_); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 263c8b2ea197a..67ad82a489ade 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -270,6 +270,10 @@ BehaviorModuleOutput StartPlannerModule::plan() }); if (status_.back_finished) { + setIsSimultaneousExecutableAsApprovedModule( + initial_value_simultaneously_executable_as_approved_module_); + setIsSimultaneousExecutableAsCandidateModule( + initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -283,6 +287,8 @@ BehaviorModuleOutput StartPlannerModule::plan() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } else { + setIsSimultaneousExecutableAsApprovedModule(false); + setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -394,6 +400,10 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() }); if (status_.back_finished) { + setIsSimultaneousExecutableAsApprovedModule( + initial_value_simultaneously_executable_as_approved_module_); + setIsSimultaneousExecutableAsCandidateModule( + initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -406,6 +416,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } else { + setIsSimultaneousExecutableAsApprovedModule(false); + setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); From 79024d349cc3430695097360f0c68863d95345b5 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Fri, 23 Jun 2023 21:27:21 +0900 Subject: [PATCH 16/23] docs(default_ad_api): fix link in default_ad_api (#4067) Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --- system/default_ad_api/document/fail-safe.md | 2 +- system/default_ad_api/document/interface.md | 2 +- system/default_ad_api/document/localization.md | 2 +- system/default_ad_api/document/motion.md | 2 +- system/default_ad_api/document/operation-mode.md | 2 +- system/default_ad_api/document/routing.md | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/system/default_ad_api/document/fail-safe.md b/system/default_ad_api/document/fail-safe.md index b9967089c5e3a..a5a8ecad3542e 100644 --- a/system/default_ad_api/document/fail-safe.md +++ b/system/default_ad_api/document/fail-safe.md @@ -2,4 +2,4 @@ ## Overview -The fail-safe API simply relays the MRM state. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/fail_safe/) for AD API specifications. +The fail-safe API simply relays the MRM state. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/fail-safe/) for AD API specifications. diff --git a/system/default_ad_api/document/interface.md b/system/default_ad_api/document/interface.md index f3fc6a389c294..ed5a69219471d 100644 --- a/system/default_ad_api/document/interface.md +++ b/system/default_ad_api/document/interface.md @@ -2,4 +2,4 @@ ## Overview -The interface API simply returns a version number. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/interface/) for AD API specifications. +The interface API simply returns a version number. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/interface/) for AD API specifications. diff --git a/system/default_ad_api/document/localization.md b/system/default_ad_api/document/localization.md index 866322ed807cf..6f27ae3079f0d 100644 --- a/system/default_ad_api/document/localization.md +++ b/system/default_ad_api/document/localization.md @@ -2,6 +2,6 @@ ## Overview -Unify the location initialization method to the service. The topic `/initialpose` from rviz is now only subscribed to by adapter node and converted to API call. This API call is forwarded to the pose initializer node so it can centralize the state of pose initialization. For other nodes that require initialpose, pose initializer node publishes as `/initialpose3d`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/) for AD API specifications. +Unify the location initialization method to the service. The topic `/initialpose` from rviz is now only subscribed to by adapter node and converted to API call. This API call is forwarded to the pose initializer node so it can centralize the state of pose initialization. For other nodes that require initialpose, pose initializer node publishes as `/initialpose3d`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/localization/) for AD API specifications. ![localization-architecture](images/localization.drawio.svg) diff --git a/system/default_ad_api/document/motion.md b/system/default_ad_api/document/motion.md index fd01a8f56ed7f..f13ee2a687808 100644 --- a/system/default_ad_api/document/motion.md +++ b/system/default_ad_api/document/motion.md @@ -2,7 +2,7 @@ ## Overview -Provides a hook for when the vehicle starts. It is typically used for announcements that call attention to the surroundings. Add a pause function to the vehicle_cmd_gate, and API will control it based on vehicle stopped and start requested. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/motion/) for AD API specifications. +Provides a hook for when the vehicle starts. It is typically used for announcements that call attention to the surroundings. Add a pause function to the vehicle_cmd_gate, and API will control it based on vehicle stopped and start requested. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/motion/) for AD API specifications. ![motion-architecture](images/motion-architecture.drawio.svg) diff --git a/system/default_ad_api/document/operation-mode.md b/system/default_ad_api/document/operation-mode.md index 703f6aa47b50d..69b79ed9b1fd7 100644 --- a/system/default_ad_api/document/operation-mode.md +++ b/system/default_ad_api/document/operation-mode.md @@ -2,7 +2,7 @@ ## Overview -Introduce operation mode. It handles autoware engage, gate_mode, external_cmd_selector and control_mode abstractly. When the mode is changed, it will be in-transition state, and if the transition completion condition to that mode is not satisfied, it will be returned to the previous mode. Also, currently, the condition for mode change is only `WaitingForEngage` in `/autoware/state`, and the engage state is shared between modes. After introducing the operation mode, each mode will have a transition available flag. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/operation_mode/) for AD API specifications. +Introduce operation mode. It handles autoware engage, gate_mode, external_cmd_selector and control_mode abstractly. When the mode is changed, it will be in-transition state, and if the transition completion condition to that mode is not satisfied, it will be returned to the previous mode. Also, currently, the condition for mode change is only `WaitingForEngage` in `/autoware/state`, and the engage state is shared between modes. After introducing the operation mode, each mode will have a transition available flag. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/operation_mode/) for AD API specifications. ![operation-mode-architecture](images/operation-mode-architecture.drawio.svg) diff --git a/system/default_ad_api/document/routing.md b/system/default_ad_api/document/routing.md index 899136f2d9e50..88969277e9263 100644 --- a/system/default_ad_api/document/routing.md +++ b/system/default_ad_api/document/routing.md @@ -2,6 +2,6 @@ ## Overview -Unify the route setting method to the service. This API supports two waypoint formats, poses and lanelet segments. The goal and checkpoint topics from rviz is only subscribed to by adapter node and converted to API call. This API call is forwarded to the mission planner node so it can centralize the state of routing. For other nodes that require route, mission planner node publishes as `/planning/mission_planning/route`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/) for AD API specifications. +Unify the route setting method to the service. This API supports two waypoint formats, poses and lanelet segments. The goal and checkpoint topics from rviz is only subscribed to by adapter node and converted to API call. This API call is forwarded to the mission planner node so it can centralize the state of routing. For other nodes that require route, mission planner node publishes as `/planning/mission_planning/route`. See the [autoware-documentation](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/routing/) for AD API specifications. ![routing-architecture](images/routing.drawio.svg) From 1b9232f5a9aedeae3946a13d5ceaae16b8c88508 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 23 Jun 2023 23:50:29 +0900 Subject: [PATCH 17/23] feat(behavior_path_planner): add turn signal debug marker (#4066) --- .../behavior_path_planner_node.hpp | 6 ++ .../behavior_path_planner/data_manager.hpp | 5 +- .../turn_signal_decider.hpp | 8 +- .../src/behavior_path_planner_node.cpp | 77 ++++++++++++++++++- .../src/turn_signal_decider.cpp | 8 +- 5 files changed, 99 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index ea0e990a8b259..d06a3aae68e4f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -219,6 +219,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; + rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; /** * @brief publish steering factor from intersection @@ -226,6 +227,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node void publish_steering_factor( const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal); + /** + * @brief publish turn signal debug info + */ + void publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data); + /** * @brief publish left and right bound */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index b682d28efeaa5..639f495a74598 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -150,12 +150,13 @@ struct PlannerData mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( - const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info) + const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, + TurnSignalDebugData & debug_data) { const auto & current_pose = self_odometry->pose.pose; const auto & current_vel = self_odometry->twist.twist.linear.x; return turn_signal_decider.getTurnSignal( - route_handler, path, turn_signal_info, current_pose, current_vel, parameters); + route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } template diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index 5691c03d31f6c..b550f132c27da 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -63,13 +63,19 @@ struct TurnSignalInfo geometry_msgs::msg::Pose required_end_point; }; +struct TurnSignalDebugData +{ + TurnSignalInfo intersection_turn_signal_info; + TurnSignalInfo behavior_turn_signal_info; +}; + class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( const std::shared_ptr & route_handler, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, - const BehaviorPathPlannerParameters & parameters); + const BehaviorPathPlannerParameters & parameters, TurnSignalDebugData & debug_data); TurnIndicatorsCommand resolve_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 1359b1afc86fc..229ae0eb679ea 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -78,6 +78,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/maximum_drivable_area", 1); } + debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); + bound_publisher_ = create_publisher("~/debug/bound", 1); // subscriber @@ -1339,12 +1341,13 @@ void BehaviorPathPlannerNode::computeTurnSignal( const BehaviorModuleOutput & output) { TurnIndicatorsCommand turn_signal; + TurnSignalDebugData debug_data; HazardLightsCommand hazard_signal; if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { - turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info); + turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info, debug_data); hazard_signal.command = HazardLightsCommand::DISABLE; } turn_signal.stamp = get_clock()->now(); @@ -1352,6 +1355,7 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal_publisher_->publish(turn_signal); hazard_signal_publisher_->publish(hazard_signal); + publish_turn_signal_debug_data(debug_data); publish_steering_factor(planner_data, turn_signal); } @@ -1386,6 +1390,77 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data) +{ + MarkerArray marker_array; + + const auto current_time = rclcpp::Time(); + constexpr double scale_x = 1.0; + constexpr double scale_y = 1.0; + constexpr double scale_z = 1.0; + const auto scale = tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z); + const auto desired_section_color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + const auto required_section_color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.999); + + // intersection turn signal info + { + const auto & turn_signal_info = debug_data.intersection_turn_signal_info; + + auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "intersection_turn_signal_desired_start", 0L, Marker::SPHERE, scale, + desired_section_color); + auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "intersection_turn_signal_desired_end", 0L, Marker::SPHERE, scale, + desired_section_color); + desired_start_marker.pose = turn_signal_info.desired_start_point; + desired_end_marker.pose = turn_signal_info.desired_end_point; + + auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "intersection_turn_signal_required_start", 0L, Marker::SPHERE, scale, + required_section_color); + auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "intersection_turn_signal_required_end", 0L, Marker::SPHERE, scale, + required_section_color); + required_start_marker.pose = turn_signal_info.required_start_point; + required_end_marker.pose = turn_signal_info.required_end_point; + + marker_array.markers.push_back(desired_start_marker); + marker_array.markers.push_back(desired_end_marker); + marker_array.markers.push_back(required_start_marker); + marker_array.markers.push_back(required_end_marker); + } + + // behavior turn signal info + { + const auto & turn_signal_info = debug_data.behavior_turn_signal_info; + + auto desired_start_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "behavior_turn_signal_desired_start", 0L, Marker::CUBE, scale, + desired_section_color); + auto desired_end_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "behavior_turn_signal_desired_end", 0L, Marker::CUBE, scale, + desired_section_color); + desired_start_marker.pose = turn_signal_info.desired_start_point; + desired_end_marker.pose = turn_signal_info.desired_end_point; + + auto required_start_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "behavior_turn_signal_required_start", 0L, Marker::CUBE, scale, + required_section_color); + auto required_end_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, "behavior_turn_signal_required_end", 0L, Marker::CUBE, scale, + required_section_color); + required_start_marker.pose = turn_signal_info.required_start_point; + required_end_marker.pose = turn_signal_info.required_end_point; + + marker_array.markers.push_back(desired_start_marker); + marker_array.markers.push_back(desired_end_marker); + marker_array.markers.push_back(required_start_marker); + marker_array.markers.push_back(required_end_marker); + } + + debug_turn_signal_info_publisher_->publish(marker_array); +} + void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) { constexpr double scale_x = 0.2; diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 57c5f149969ae..ac11941ec1eb6 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -41,8 +41,10 @@ double calc_distance( TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( const std::shared_ptr & route_handler, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, - const BehaviorPathPlannerParameters & parameters) + const BehaviorPathPlannerParameters & parameters, TurnSignalDebugData & debug_data) { + debug_data.behavior_turn_signal_info = turn_signal_info; + // Guard if (path.points.empty()) { return turn_signal_info.turn_signal; @@ -76,6 +78,10 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( extended_path, current_pose, current_vel, ego_seg_idx, *route_handler, nearest_dist_threshold, nearest_yaw_threshold); + if (intersection_turn_signal_info) { + debug_data.intersection_turn_signal_info = *intersection_turn_signal_info; + } + if (!intersection_turn_signal_info) { initialize_intersection_info(); const auto & desired_end_point = turn_signal_info.desired_end_point; From 6c6973b79de2682b7a01c580cbdf9888b502b64d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 23 Jun 2023 20:25:23 +0200 Subject: [PATCH 18/23] build: fix opencv dependency (#3987) Signed-off-by: Esteve Fernandez --- planning/behavior_path_planner/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 97453b0325cef..48c0762461d16 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -53,6 +53,7 @@ lane_departure_checker lanelet2_extension libboost-dev + libopencv-dev magic_enum motion_utils perception_utils From edcad42338a56cd2c7eaf0803f0861c56493608c Mon Sep 17 00:00:00 2001 From: atsushi yano <55824710+atsushi421@users.noreply.github.com> Date: Sat, 24 Jun 2023 08:24:56 +0900 Subject: [PATCH 19/23] perf(voxel_grid_downsample_filter): performance tuning (#3679) * perf(voxel_grid_downsample_filter): change to use faster_filter Signed-off-by: atsushi421 * perf(voxel_grid_downsample_filter): changed to not use pcl library Signed-off-by: atsushi421 * refactor: split faster_filter() into a separate class Signed-off-by: atsushi421 * style(pre-commit): autofix * chore: restore old filter function Signed-off-by: atsushi421 * chore: rename Signed-off-by: atsushi421 * refactor: split filter into multiple functions Signed-off-by: atsushi421 * refactor: improve readability Signed-off-by: atsushi421 * style(pre-commit): autofix * fix: pre-commit error Signed-off-by: atsushi421 * fix: copyright Signed-off-by: atsushi421 * fix: add const to calc_centroid() Signed-off-by: atsushi421 * refactor: define TransformInfo in separate file Signed-off-by: atsushi421 * style(pre-commit): autofix * refactor: improve readability Signed-off-by: atsushi421 * chore: fix copyright Signed-off-by: atsushi421 * chore: fix copyright Signed-off-by: atsushi421 * fix: typo Signed-off-by: atsushi421 --------- Signed-off-by: atsushi421 Co-authored-by: atsushi421 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../pointcloud_preprocessor/CMakeLists.txt | 23 +++ .../crop_box_filter_nodelet.hpp | 1 + .../faster_voxel_grid_downsample_filter.hpp | 93 +++++++++ .../voxel_grid_downsample_filter_nodelet.hpp | 15 +- .../pointcloud_preprocessor/filter.hpp | 14 +- .../ring_outlier_filter_nodelet.hpp | 1 + .../transform_info.hpp | 43 +++++ .../faster_voxel_grid_downsample_filter.cpp | 178 ++++++++++++++++++ .../voxel_grid_downsample_filter_nodelet.cpp | 25 ++- .../pointcloud_preprocessor/src/filter.cpp | 3 +- 10 files changed, 375 insertions(+), 21 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp create mode 100644 sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index d8d561a9c5098..47d05b8c6ce47 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -41,6 +41,21 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base pcl_ros ) +add_library(faster_voxel_grid_downsample_filter SHARED + src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +) + +target_include_directories(faster_voxel_grid_downsample_filter PUBLIC + "$" + "$" +) + +ament_target_dependencies(faster_voxel_grid_downsample_filter + pcl_conversions + rclcpp + sensor_msgs +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_data_nodelet.cpp @@ -65,6 +80,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base + faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} @@ -167,6 +183,13 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS faster_voxel_grid_downsample_filter EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 3b392401de49b..1406679ce0dba 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -54,6 +54,7 @@ #define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp new file mode 100644 index 0000000000000..d92b511d40c69 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -0,0 +1,93 @@ +// 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. + +#pragma once + +#include "pointcloud_preprocessor/transform_info.hpp" + +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ + +class FasterVoxelGridDownsampleFilter +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +public: + FasterVoxelGridDownsampleFilter(); + void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); + void set_field_offsets(const PointCloud2ConstPtr & input); + void filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger); + +private: + struct Centroid + { + float x; + float y; + float z; + uint32_t point_count_; + + Centroid() : x(0), y(0), z(0) {} + Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; } + + void add_point(float _x, float _y, float _z) + { + this->x += _x; + this->y += _y; + this->z += _z; + this->point_count_++; + } + + Eigen::Vector4f calc_centroid() const + { + Eigen::Vector4f centroid( + (this->x / this->point_count_), (this->y / this->point_count_), + (this->z / this->point_count_), 1.0f); + return centroid; + } + }; + + Eigen::Vector3f inverse_voxel_size_; + std::vector xyz_fields_; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + Eigen::Vector3f get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset); + + bool get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel); + + std::unordered_map calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel); + + void copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info); +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index 7ebf80c2da958..ae285b4fa9239 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -52,6 +52,7 @@ #define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -66,10 +67,16 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info); + private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index f995be741272c..47c0f2b403faa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -52,6 +52,8 @@ #ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ #define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#include "pointcloud_preprocessor/transform_info.hpp" + #include #include #include @@ -134,18 +136,6 @@ class Filter : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: - struct TransformInfo - { - TransformInfo() - { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - need_transform = false; - } - - Eigen::Matrix4f eigen_transform; - bool need_transform; - }; - /** \brief The input PointCloud2 subscriber. */ rclcpp::Subscription::SharedPtr sub_input_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index a906eb913d863..ab7d4e0012304 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -17,6 +17,7 @@ #include "autoware_point_types/types.hpp" #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp new file mode 100644 index 0000000000000..a4806555695b5 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp @@ -0,0 +1,43 @@ +// 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. + +#pragma once + +#include + +namespace pointcloud_preprocessor +{ + +/** + * This holds the coordinate transformation information of the point cloud. + * Usage example: + * \code + * if (transform_info.need_transform) { + * point = transform_info.eigen_transform * point; + * } + * \endcode + */ +struct TransformInfo +{ + TransformInfo() + { + eigen_transform = Eigen::Matrix4f::Identity(4, 4); + need_transform = false; + } + + Eigen::Matrix4f eigen_transform; + bool need_transform; +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp new file mode 100644 index 0000000000000..7c302eec20431 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -0,0 +1,178 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + +namespace pointcloud_preprocessor +{ + +FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter() +{ + pcl::for_each_type::type>( + pcl::detail::FieldAdder(xyz_fields_)); + offset_initialized_ = false; +} + +void FasterVoxelGridDownsampleFilter::set_voxel_size( + float voxel_size_x, float voxel_size_y, float voxel_size_z) +{ + inverse_voxel_size_ = + Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); +} + +void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + offset_initialized_ = true; +} + +void FasterVoxelGridDownsampleFilter::filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger) +{ + // Check if the field offset has been set + if (!offset_initialized_) { + set_field_offsets(input); + } + + // Compute the minimum and maximum voxel coordinates + Eigen::Vector3i min_voxel, max_voxel; + if (!get_min_max_voxel(input, min_voxel, max_voxel)) { + RCLCPP_ERROR( + logger, + "Voxel size is too small for the input dataset. " + "Integer indices would overflow."); + output = *input; + return; + } + + // Storage for mapping voxel coordinates to centroids + auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel); + + // Initialize the output + output.row_step = voxel_centroid_map.size() * input->point_step; + output.data.resize(output.row_step); + output.width = voxel_centroid_map.size(); + pcl_conversions::fromPCL(xyz_fields_, output.fields); + output.is_dense = true; // we filter out invalid points + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + // Copy the centroids to the output + copy_centroids_to_output(voxel_centroid_map, output, transform_info); +} + +Eigen::Vector3f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset) +{ + Eigen::Vector3f point( + *reinterpret_cast(&input->data[global_offset + x_offset_]), + *reinterpret_cast(&input->data[global_offset + y_offset_]), + *reinterpret_cast(&input->data[global_offset + z_offset_])); + return point; +} + +bool FasterVoxelGridDownsampleFilter::get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel) +{ + // Compute the minimum and maximum point coordinates + Eigen::Vector3f min_point, max_point; + min_point.setConstant(FLT_MAX); + max_point.setConstant(-FLT_MAX); + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + min_point = min_point.cwiseMin(point); + max_point = max_point.cwiseMax(point); + } + } + + // Check that the voxel size is not too small, given the size of the data + if ( + ((static_cast((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) * + (static_cast((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) * + (static_cast((max_point[2] - min_point[2]) * inverse_voxel_size_[2]) + 1)) > + static_cast(std::numeric_limits::max())) { + return false; + } + + // Compute the minimum and maximum voxel coordinates + min_voxel[0] = static_cast(std::floor(min_point[0] * inverse_voxel_size_[0])); + min_voxel[1] = static_cast(std::floor(min_point[1] * inverse_voxel_size_[1])); + min_voxel[2] = static_cast(std::floor(min_point[2] * inverse_voxel_size_[2])); + max_voxel[0] = static_cast(std::floor(max_point[0] * inverse_voxel_size_[0])); + max_voxel[1] = static_cast(std::floor(max_point[1] * inverse_voxel_size_[1])); + max_voxel[2] = static_cast(std::floor(max_point[2] * inverse_voxel_size_[2])); + + return true; +} + +std::unordered_map +FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel) +{ + std::unordered_map voxel_centroid_map; + // Compute the number of divisions needed along all axis + Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones(); + // Set up the division multiplier + Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]); + + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + // Calculate the voxel index to which the point belongs + int ijk0 = static_cast(std::floor(point[0] * inverse_voxel_size_[0]) - min_voxel[0]); + int ijk1 = static_cast(std::floor(point[1] * inverse_voxel_size_[1]) - min_voxel[1]); + int ijk2 = static_cast(std::floor(point[2] * inverse_voxel_size_[2]) - min_voxel[2]); + uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2]; + + // Add the point to the corresponding centroid + if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) { + voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]); + } else { + voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]); + } + } + } + + return voxel_centroid_map; +} + +void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info) +{ + size_t output_data_size = 0; + for (const auto & pair : voxel_centroid_map) { + Eigen::Vector4f centroid = pair.second.calc_centroid(); + if (transform_info.need_transform) { + centroid = transform_info.eigen_transform * centroid; + } + *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; + *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; + *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + output_data_size += output.point_step; + } +} + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index 5b22986a1b6b8..f11f37397a142 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -50,6 +50,8 @@ #include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" +#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + #include #include #include @@ -64,9 +66,9 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( { // set initial parameters { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); } using std::placeholders::_1; @@ -74,6 +76,8 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1)); } +// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. void VoxelGridDownsampleFilterComponent::filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { @@ -95,6 +99,19 @@ void VoxelGridDownsampleFilterComponent::filter( output.header = input->header; } +// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the +// filter nodes conform to new API. Then delete the old `filter()` defined above. +void VoxelGridDownsampleFilterComponent::faster_filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output, const TransformInfo & transform_info) +{ + std::scoped_lock lock(mutex_); + FasterVoxelGridDownsampleFilter faster_voxel_filter; + faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); + faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); +} + rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback( const std::vector & p) { diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index f0b716c73b5f1..d5843be395adf 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -124,7 +124,8 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. - std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"}; + std::set supported_nodes = { + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From 7b6bff236710cf7bb0c1b097362fcac523b54846 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Sat, 24 Jun 2023 16:34:19 +0900 Subject: [PATCH 20/23] feat(obstacle_cruise_planner): change cruise color to orange (#4065) Signed-off-by: Shumpei Wakabayashi --- planning/obstacle_cruise_planner/src/node.cpp | 2 +- .../src/pid_based_planner/pid_based_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index bf705759e0422..0020ef1c8e960 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1251,7 +1251,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { // obstacle const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.5, 0.5); + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.6, 0.1); debug_marker.markers.push_back(obstacle_marker); // collision points diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index bca5ad3f3cf41..ffd2f97b1eb89 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -314,7 +314,7 @@ std::vector PIDBasedPlanner::planCruise( stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. - markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.5, 0.5); + markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); // cruise obstacle From 40f931129ebbcbb9c63788ea53816c12e18ad2fd Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 26 Jun 2023 07:23:24 +0900 Subject: [PATCH 21/23] refactor(lane_change): use common logger (#4072) Signed-off-by: Takamasa Horibe --- .../scene_module/lane_change/normal.hpp | 2 + .../src/scene_module/lane_change/normal.cpp | 55 +++++-------------- 2 files changed, 16 insertions(+), 41 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index e79400231cfb0..6572d4f8221b0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -107,6 +107,8 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo calcTurnSignalInfo() override; bool isValidPath(const PathWithLaneId & path) const override; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; class NormalLaneChangeBT : public NormalLaneChange diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 96238ecfb203d..548d23642581e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -561,9 +561,7 @@ bool NormalLaneChange::getLaneChangePaths( minimum_prepare_length); if (prepare_length < target_length) { - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "prepare length is shorter than distance to target lane!!"); + RCLCPP_DEBUG(logger_, "prepare length is shorter than distance to target lane!!"); break; } @@ -571,9 +569,7 @@ bool NormalLaneChange::getLaneChangePaths( original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "prepare segment is empty!!"); + RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); continue; } @@ -587,8 +583,7 @@ bool NormalLaneChange::getLaneChangePaths( // that case, the lane change shouldn't be executed. if (target_length_from_lane_change_start_pose > 0.0) { RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); break; } @@ -621,9 +616,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "length of lane changing path is longer than length to goal!!"); + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } @@ -636,9 +629,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + common_parameter.lane_change_finish_judge_buffer + next_lane_change_buffer > s_goal) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "length of lane changing path is longer than length to goal!!"); + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } } @@ -649,9 +640,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target segment is empty!! something wrong..."); + RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); continue; } @@ -673,9 +662,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target_lane_reference_path is empty!!"); + RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); continue; } @@ -693,9 +680,7 @@ bool NormalLaneChange::getLaneChangePaths( lc_velocity, terminal_lane_changing_velocity, lc_time); if (!candidate_path) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "no candidate path!!"); + RCLCPP_DEBUG(logger_, "no candidate path!!"); continue; } @@ -704,9 +689,7 @@ bool NormalLaneChange::getLaneChangePaths( minimum_lane_changing_velocity, common_parameter, direction); if (!is_valid) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "invalid candidate path!!"); + RCLCPP_DEBUG(logger_, "invalid candidate path!!"); continue; } @@ -927,18 +910,14 @@ bool NormalLaneChange::getAbortPath() lane_change_parameters_->abort_delta_time + lane_change_parameters_->aborting_time); if (abort_start_idx >= abort_return_idx) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "abort start idx and return idx is equal. can't compute abort path."); + RCLCPP_ERROR(logger_, "abort start idx and return idx is equal. can't compute abort path."); return false; } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, direction)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "insufficient distance to abort."); + RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -965,9 +944,7 @@ bool NormalLaneChange::getAbortPath() path_shifter.setLateralAccelerationLimit(max_lateral_acc); if (lateral_jerk > lane_change_parameters_->abort_max_lateral_jerk) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "Aborting jerk is too strong. lateral_jerk = " << lateral_jerk); + RCLCPP_ERROR(logger_, "Aborting jerk is too strong. lateral_jerk = %f", lateral_jerk); return false; } @@ -975,9 +952,7 @@ bool NormalLaneChange::getAbortPath() // offset front side // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()), - "failed to generate abort shifted path."); + RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } const auto arc_position = lanelet::utils::getArcCoordinates( @@ -1094,9 +1069,7 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment( const double s_start = arc_length_from_current - backward_path_length; const double s_end = arc_length_from_current + prepare_length; - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()).get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); + RCLCPP_DEBUG(logger_, "start: %f, end: %f", s_start, s_end); PathWithLaneId prepare_segment = getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); From e9d4e9fa7dd03935a7752556ab63eefa48aaf2be Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 26 Jun 2023 09:08:29 +0900 Subject: [PATCH 22/23] fix(crosswalk): return if the stop point with factor is boost::none (#4071) Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index ca39796c616ba..2ac4521c4c79e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -234,6 +234,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return {}; }(); + if (!stop_point_with_factor) { + return false; + } + const auto & stop_factor = stop_point_with_factor->second; insertDecelPointWithDebugInfo(stop_point_with_factor->first, 0.0, *path); planning_utils::appendStopReason(stop_factor, stop_reason); From a3c6ea22d3f4cdcaa810ae9a85fe44652014f2f0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 26 Jun 2023 10:18:43 +0900 Subject: [PATCH 23/23] feat(avoidance): set additional buffer margin independently (#4041) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 9 ++++++++- .../utils/avoidance/avoidance_module_data.hpp | 5 ++--- .../src/behavior_path_planner_node.cpp | 2 +- .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++-- .../src/scene_module/avoidance/manager.cpp | 2 +- .../behavior_path_planner/src/utils/avoidance/utils.cpp | 2 +- 6 files changed, 15 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index f7867ea647e13..79aca0867cce9 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -33,6 +33,7 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: @@ -41,6 +42,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bus: @@ -49,6 +51,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 trailer: @@ -57,6 +60,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 unknown: @@ -65,6 +69,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bicycle: @@ -73,6 +78,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: @@ -81,6 +87,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: @@ -89,6 +96,7 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 + avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 lower_distance_for_polygon_expansion: 30.0 # [m] @@ -124,7 +132,6 @@ avoidance: # avoidance lateral parameters lateral: - lateral_collision_margin: 1.0 # [m] lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] road_shoulder_safety_margin: 0.3 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3dd5668f43ddf..187dca71ae6c8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -58,6 +58,8 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; + double avoid_margin_lateral{1.0}; + double safety_buffer_lateral{1.0}; double safety_buffer_longitudinal{0.0}; @@ -159,9 +161,6 @@ struct AvoidanceParameters // force avoidance double threshold_time_force_avoidance_for_stopped_vehicle; - // we want to keep this lateral margin when avoiding - double lateral_collision_margin; - // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction double longitudinal_collision_margin_min_distance; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 229ae0eb679ea..f7336dad3f895 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -586,6 +586,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() param.moving_time_threshold = declare_parameter(ns + "moving_time_threshold"); param.max_expand_ratio = declare_parameter(ns + "max_expand_ratio"); param.envelope_buffer_margin = declare_parameter(ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = declare_parameter(ns + "avoid_margin_lateral"); param.safety_buffer_lateral = declare_parameter(ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = declare_parameter(ns + "safety_buffer_longitudinal"); @@ -649,7 +650,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.lateral_collision_margin = declare_parameter(ns + "lateral_collision_margin"); p.road_shoulder_safety_margin = declare_parameter(ns + "road_shoulder_safety_margin"); p.lateral_execution_threshold = declare_parameter(ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ac98eb5a2b59b..8c0d6d4acd48a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -620,7 +620,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - parameters_->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getSharpAvoidanceDistance( helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); @@ -3285,7 +3285,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_parameter = parameters_->object_parameters.at(t); const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - p->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getMinimumAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant = diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 321a5a72cc7b7..90dbfa960dbfe 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -52,6 +52,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); + updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); @@ -82,7 +83,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorlateral_execution_threshold); updateParam( parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); - updateParam(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin); updateParam( parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 06ca1c3cc979e..8d33faf82e645 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -894,7 +894,7 @@ void filterTargetObjects( // calculate avoid_margin dynamically // NOTE: This calculation must be after calculating to_road_shoulder_distance. const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - parameters->lateral_collision_margin + 0.5 * vehicle_width; + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const double min_safety_lateral_distance = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; const auto max_allowable_lateral_distance =