Skip to content

Commit

Permalink
refactor(behavior_path_plannner): commonize collision check markers (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#4798)

* refactor(behavior_path_plannner): commonize collision check markers

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Update planning/behavior_path_planner/src/marker_utils/utils.cpp

Co-authored-by: Satoshi OTA <[email protected]>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4 @satoshi-ota
zulfaqar-azmi-t4 and satoshi-ota committed Sep 15, 2023
1 parent be42024 commit b117d8f
Show file tree
Hide file tree
Showing 7 changed files with 183 additions and 169 deletions.
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_
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
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
161 changes: 160 additions & 1 deletion planning/behavior_path_planner/src/marker_utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include "behavior_path_planner/marker_utils/utils.hpp"

#include "behavior_path_planner/marker_utils/colors.hpp"
#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
#include "behavior_path_planner/utils/utils.hpp"
Expand All @@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker;
CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
{
CollisionCheckDebug debug;
debug.current_pose = obj.initial_pose.pose;
debug.current_obj_pose = obj.initial_pose.pose;
debug.current_twist = obj.initial_twist.twist;
return {tier4_autoware_utils::toHexString(obj.uuid), debug};
}
Expand Down Expand Up @@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray(
return msg;
}

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

int32_t id{0};
const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();

constexpr float line_scale_val{0.2};
const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val);

auto default_line_marker = [&](const auto & color = colors::green()) {
return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color);
};

constexpr float text_scale_val{1.5};
const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val);

auto default_text_marker = [&]() {
return createDefaultMarker(
"map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
text_marker_scale, colors::white());
};

auto default_cube_marker =
[&](const auto & width, const auto & depth, const auto & color = colors::green()) {
return createDefaultMarker(
"map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE,
createMarkerScale(width, depth, 1.0), color);
};

MarkerArray marker_array;
marker_array.markers.reserve(
obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj

int32_t idx = {0};
for (const auto & [uuid, info] : obj_debug_vec) {
const auto color = info.is_safe ? colors::green() : colors::red();
const auto poly_z = info.current_obj_pose.position.z; // temporally

const auto insert_polygon_marker = [&](const auto & polygon) {
marker_array.markers.emplace_back();
auto & polygon_marker = marker_array.markers.back();
polygon_marker = default_line_marker(color);
polygon_marker.points.reserve(polygon.outer().size());
for (const auto & p : polygon.outer()) {
polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z));
}
};

insert_polygon_marker(info.extended_ego_polygon);
insert_polygon_marker(info.extended_obj_polygon);

const auto str_idx = std::to_string(++idx);
const auto insert_text_marker = [&](const auto & pose) {
marker_array.markers.emplace_back();
auto & text_marker = marker_array.markers.back();
text_marker = default_text_marker();
text_marker.text = str_idx;
text_marker.pose = pose;
};

insert_text_marker(info.expected_ego_pose);
insert_text_marker(info.expected_obj_pose);

const auto insert_cube_marker = [&](const auto & pose) {
marker_array.markers.emplace_back();
auto & cube_marker = marker_array.markers.back();
cube_marker = default_cube_marker(1.0, 1.0, color);
cube_marker.pose = pose;
};
insert_cube_marker(info.current_obj_pose);
}
return marker_array;
}

MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
int32_t id{0};
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3);
const auto default_arrow_marker = [&](const auto & color) {
return createDefaultMarker(
"map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color);
};

MarkerArray marker_array;
marker_array.markers.reserve(std::accumulate(
obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL,
[&](const auto current_sum, const auto & obj_debug) {
const auto & [uuid, info] = obj_debug;
return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2;
}));

for (const auto & [uuid, info] : obj_debug_vec) {
const auto insert_marker = [&](const auto & path, const auto & color) {
for (const auto & pose : path) {
marker_array.markers.emplace_back();
auto & marker = marker_array.markers.back();
marker = default_arrow_marker(color);
marker.pose = pose.pose;
}
};

insert_marker(info.ego_predicted_path, colors::aqua());
insert_marker(info.obj_predicted_path, colors::yellow());
const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) {
// instead of checking for distance, inserting a new marker might be more efficient
marker_array.markers.emplace_back();
auto & marker = marker_array.markers.back();
marker = default_arrow_marker(color);
marker.pose = pose;
marker.pose.position.z += 0.05;
};

insert_expected_pose_marker(info.expected_ego_pose, colors::red());
insert_expected_pose_marker(info.expected_obj_pose, colors::red());
}
return marker_array;
}

MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
{
int32_t id{0};
auto default_text_marker = [&]() {
return createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING,
createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
};

MarkerArray marker_array;

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

int idx{0};

for (const auto & [uuid, info] : obj_debug_vec) {
auto safety_check_info_text = default_text_marker();
safety_check_info_text.pose = info.current_obj_pose;

std::ostringstream ss;

ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason
<< "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
<< "\nEgo to obj: " << info.inter_vehicle_distance
<< "\nExtended polygon: " << (info.is_front ? "ego" : "object")
<< "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
<< "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
<< "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
<< "\nSafe: " << (info.is_safe ? "Yes" : "No");

safety_check_info_text.text = ss.str();
marker_array.markers.push_back(safety_check_info_text);
}
return marker_array;
}

} // namespace marker_utils
Loading

0 comments on commit b117d8f

Please sign in to comment.