Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(avoidance): avoidance safety check #840

Merged
merged 3 commits into from
Sep 15, 2023
+235 −207
Merged

fix(avoidance): avoidance safety check #840

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bd1329f
feat(avoidance): check incomming vehicle (#4864)
satoshi-ota Sep 4, 2023
be42024
fix(avoidance): clear rtc status in process on entry (#4926)
satoshi-ota Sep 12, 2023
b117d8f
refactor(behavior_path_plannner): commonize collision check markers (…
zulfaqar-azmi-t4 Aug 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions ...ning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading