Skip to content

Commit

Permalink
Merge pull request autowarefoundation#840 from satoshi-ota/hotfix/avo…
Browse files Browse the repository at this point in the history 
…idance-safety-check

fix(avoidance): avoidance safety check
  • Loading branch information
@takayuki5168
takayuki5168 authored Sep 15, 2023
2 parents 3e5d0c4 + b117d8f commit 872b409
Show file tree
Hide file tree
Showing 11 changed files with 235 additions and 207 deletions.
2 changes: 0 additions & 2 deletions ...ning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons

MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);

MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns);

Expand Down
4 changes: 0 additions & 4 deletions ...ng/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers
{
using behavior_path_planner::LaneChangePath;
using visualization_msgs::msg::MarkerArray;
MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
MarkerArray showAllValidLaneChangePath(
const std::vector<LaneChangePath> & lanes, std::string && ns);
MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
MarkerArray createLaneChangingVirtualWallMarker(
const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
const rclcpp::Time & now, const std::string & ns);
Expand Down
5 changes: 5 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray(
const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);

MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);

MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);

MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
} // namespace marker_utils

#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
7 changes: 4 additions & 3 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -556,9 +556,6 @@ struct DebugData
// shift path
std::vector<double> proposed_spline_shift;

// future pose
PathWithLaneId path_with_planned_velocity;

// avoidance require objects
ObjectDataArray unavoidable_objects;

Expand All @@ -568,6 +565,10 @@ struct DebugData
// tmp for plot
PathWithLaneId center_line;

// collision check debug map
utils::path_safety_checker::CollisionCheckDebugMap collision_check;

// debug msg array
AvoidanceDebugMsgArray avoidance_debug_msg_array;
};

Expand Down
11 changes: 6 additions & 5 deletions ...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,20 +179,21 @@ struct SafetyCheckParams
struct CollisionCheckDebug
{
std::string unsafe_reason; ///< Reason indicating unsafe situation.
Pose current_pose{}; ///< Ego vehicle's current pose.
Twist current_twist{}; ///< Ego vehicle's current velocity and rotation.
Twist object_twist{}; ///< Detected object's velocity and rotation.
Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle.
Pose current_obj_pose{}; ///< Detected object's current pose.
Twist object_twist{}; ///< Detected object's velocity and rotation.
Pose expected_obj_pose{}; ///< Predicted future pose of object.
double rss_longitudinal{0.0}; ///< Longitudinal RSS measure.
double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object.
double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon.
double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon.
bool is_front{false}; ///< True if object is in front of ego vehicle.
bool is_safe{false}; ///< True if situation is deemed safe.
std::vector<Pose> lerped_path; ///< Interpolated ego vehicle path.
Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon.
Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon.
std::vector<PoseWithVelocityStamped> ego_predicted_path; ///< ego vehicle's predicted path.
std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path; ///< object's predicted path.
Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon.
Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon.
};
using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
using CollisionCheckDebugMap =
Expand Down
6 changes: 0 additions & 6 deletions planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -416,12 +416,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
return msg;
}

MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
return createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8));
}

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const ObjectDataArray & objects, std::string && ns)
{
Expand Down
149 changes: 0 additions & 149 deletions planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,40 +36,6 @@ using geometry_msgs::msg::Point;
using tier4_autoware_utils::createDefaultMarker;
using tier4_autoware_utils::createMarkerScale;

MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
Marker obj_marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(0.5, 0.5, 0.5), colors::aqua());

MarkerArray marker_array;
int32_t id{0};

marker_array.markers.reserve(obj_debug_vec.size());

int idx{0};

for (const auto & [uuid, info] : obj_debug_vec) {
obj_marker.id = ++id;
obj_marker.pose = info.current_pose;

std::ostringstream ss;

ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason
<< "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
<< "\nEgo to obj: " << info.inter_vehicle_distance
<< "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
<< "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
<< "\nPosition: " << (info.is_front ? "front" : "back")
<< "\nSafe: " << (info.is_safe ? "Yes" : "No");

obj_marker.text = ss.str();

marker_array.markers.push_back(obj_marker);
}
return marker_array;
}

MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
{
if (lanes.empty()) {
Expand Down Expand Up @@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes
return marker_array;
}

MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
MarkerArray marker_array;
int32_t id{0};
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
marker_array.markers.reserve(obj_debug_vec.size());

for (const auto & [uuid, info] : obj_debug_vec) {
Marker marker = createDefaultMarker(
"map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3),
colors::magenta());
marker.points.reserve(info.lerped_path.size());

for (const auto & point : info.lerped_path) {
marker.points.push_back(point.position);
}

marker_array.markers.push_back(marker);
}
return marker_array;
}

MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
if (obj_debug_vec.empty()) {
return MarkerArray{};
}

constexpr float scale_val{0.2};
int32_t id{0};
const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
Marker ego_marker = createDefaultMarker(
"map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val),
colors::green());
Marker obj_marker = ego_marker;

auto text_marker = createDefaultMarker(
"map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
createMarkerScale(1.5, 1.5, 1.5), colors::white());

MarkerArray marker_array;

const auto reserve_size = obj_debug_vec.size();

marker_array.markers.reserve(reserve_size * 4);

int32_t idx = {0};

for (const auto & [uuid, info] : obj_debug_vec) {
const auto color = info.is_safe ? colors::green() : colors::red();
const auto & ego_polygon = info.extended_ego_polygon.outer();
const auto poly_z = info.current_pose.position.z; // temporally
ego_marker.id = ++id;
ego_marker.color = color;
ego_marker.points.reserve(ego_polygon.size());
for (const auto & p : ego_polygon) {
ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
}
marker_array.markers.push_back(ego_marker);

std::ostringstream ss;
text_marker.id = ego_marker.id;
ss << ++idx;
text_marker.text = ss.str();
text_marker.pose = info.expected_ego_pose;

marker_array.markers.push_back(text_marker);

const auto & obj_polygon = info.extended_obj_polygon.outer();
obj_marker.id = ++id;
obj_marker.color = color;
obj_marker.points.reserve(obj_polygon.size());
for (const auto & p : obj_polygon) {
obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
}
marker_array.markers.push_back(obj_marker);

text_marker.id = obj_marker.id;
text_marker.pose = info.expected_obj_pose;
marker_array.markers.push_back(text_marker);

ego_marker.points.clear();
obj_marker.points.clear();
}

return marker_array;
}

MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
const auto colors = colors::colors_list();
const auto loop_size = std::min(colors.size(), obj_debug_vec.size());
MarkerArray marker_array;
int32_t id{0};
size_t idx{0};
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
marker_array.markers.reserve(obj_debug_vec.size());

for (const auto & [uuid, info] : obj_debug_vec) {
const auto & color = colors.at(idx);
Marker marker = createDefaultMarker(
"map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color);
marker.points.reserve(2);
marker.points.push_back(info.expected_ego_pose.position);
marker.points.push_back(info.expected_obj_pose.position);
marker_array.markers.push_back(marker);
++idx;
if (idx >= loop_size) {
break;
}
}

return marker_array;
}

MarkerArray createLaneChangingVirtualWallMarker(
const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
const rclcpp::Time & now, const std::string & ns)
Expand Down
Loading

0 comments on commit 872b409

Please sign in to comment.