Skip to content

Commit ba4b755

Browse files
authored
feat(static_obstacle_avoidance): output safety factor (autowarefoundation#10000)
* feat(safety_check): convert to SafetyFactor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(static_obstacle_avoidance): use safety factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(bpp): output detail Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent d4e488a commit ba4b755

File tree

5 files changed

+29
-5
lines changed

5 files changed

+29
-5
lines changed

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -549,7 +549,7 @@ class SceneModuleInterface
549549
if (stop_pose_.has_value()) {
550550
planning_factor_interface_->add(
551551
path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP,
552-
SafetyFactorArray{});
552+
SafetyFactorArray{}, true, 0.0, 0.0, stop_pose_.value().detail);
553553
return;
554554
}
555555

@@ -559,7 +559,7 @@ class SceneModuleInterface
559559

560560
planning_factor_interface_->add(
561561
path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN,
562-
SafetyFactorArray{});
562+
SafetyFactorArray{}, true, 0.0, 0.0, slow_pose_.value().detail);
563563
}
564564

565565
void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include <autoware_perception_msgs/msg/predicted_path.hpp>
2626
#include <geometry_msgs/msg/pose.hpp>
2727
#include <geometry_msgs/msg/twist.hpp>
28+
#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
2829

2930
#include <cmath>
3031
#include <string>
@@ -283,6 +284,9 @@ double calculateRoughDistanceToObjects(
283284
CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
284285
void updateCollisionCheckDebugMap(
285286
CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe);
287+
288+
tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array(
289+
const CollisionCheckDebugMap & debug_map);
286290
} // namespace autoware::behavior_path_planner::utils::path_safety_checker
287291

288292
#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

+14
Original file line numberDiff line numberDiff line change
@@ -883,4 +883,18 @@ double calculateRoughDistanceToObjects(
883883
return min_distance;
884884
}
885885

886+
tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array(
887+
const CollisionCheckDebugMap & debug_map)
888+
{
889+
tier4_planning_msgs::msg::SafetyFactorArray safety_factors;
890+
for (const auto & [uuid, data] : debug_map) {
891+
tier4_planning_msgs::msg::SafetyFactor safety_factor;
892+
safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT;
893+
safety_factor.is_safe = data.is_safe;
894+
safety_factor.object_id = autoware::universe_utils::toUUIDMsg(uuid);
895+
safety_factor.points.push_back(data.current_obj_pose.position);
896+
safety_factors.factors.push_back(safety_factor);
897+
}
898+
return safety_factors;
899+
}
886900
} // namespace autoware::behavior_path_planner::utils::path_safety_checker

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
1919
#include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp"
20+
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
2021
#include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp"
2122
#include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp"
2223
#include "autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp"
@@ -134,7 +135,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
134135
if (finish_distance > -1.0e-03) {
135136
planning_factor_interface_->add(
136137
start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose,
137-
PlanningFactor::SHIFT_LEFT, SafetyFactorArray{});
138+
PlanningFactor::SHIFT_LEFT,
139+
utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
138140
}
139141
}
140142

@@ -154,7 +156,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
154156
if (finish_distance > -1.0e-03) {
155157
planning_factor_interface_->add(
156158
start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose,
157-
PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{});
159+
PlanningFactor::SHIFT_RIGHT,
160+
utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
158161
}
159162
}
160163
}

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -1211,9 +1211,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const
12111211
const uint16_t planning_factor_direction = std::invoke([&output]() {
12121212
return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT;
12131213
});
1214+
12141215
planning_factor_interface_->add(
12151216
output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start,
1216-
sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift);
1217+
sl_back.end, planning_factor_direction,
1218+
utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check), true, 0.0,
1219+
output.lateral_shift);
12171220

12181221
output.path_candidate = shifted_path.path;
12191222
return output;

0 commit comments

Comments
 (0)