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

Jump to bottom

feat(avoidance): output avoidance target marker that are always shown in rviz #3582

Merged
merged 1 commit into from
May 8, 2023
Merged

feat(avoidance): output avoidance target marker that are always shown in rviz #3582

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
8 changes: 1 addition & 7 deletions planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,18 +58,12 @@ MarkerArray createAvoidLineMarkerArray(

MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns);

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

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

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

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

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

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

Expand Down
19 changes: 16 additions & 3 deletions ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,12 +271,25 @@ class AvoidanceModule : public SceneModuleInterface
// debug
mutable DebugData debug_data_;
mutable std::shared_ptr<AvoidanceDebugMsgArray> debug_msg_ptr_;
void setDebugData(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;
void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;
mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;

/**
* @brief fill debug markers.
*/
void updateDebugMarker(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;

/**
* @brief fill information markers that are shown in Rviz by default.
*/
void updateInfoMarker(const AvoidancePlanningData & data) const;

/**
* @brief fill debug msg that are published as a topic.
*/
void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;

double getLateralMarginFromVelocity(const double velocity) const;

double getRSSLongitudinalDistance(
Expand Down
161 changes: 74 additions & 87 deletions planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,34 @@ MarkerArray createObjectsCubeMarkerArray(
return msg;
}

MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;

for (const auto & object : objects) {
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));

const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;

for (const auto & p : object.envelope_poly.outer()) {
marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
}

marker.points.push_back(marker.points.front());
marker.id = uuidToInt32(object.object.object_id);
msg.markers.push_back(marker);
}

return msg;
}

MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;

Marker marker = createDefaultMarker(
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));

Expand Down Expand Up @@ -110,6 +133,40 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
return msg;
}

MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 4);

appendMarkerArray(
createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
createMarkerColor(1.0, 1.0, 0.0, 0.8)),
&msg);

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);

return msg;
}

MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 4);

appendMarkerArray(
createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
createMarkerColor(1.0, 0.0, 0.0, 0.8)),
&msg);

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);

return msg;
}

} // namespace

MarkerArray createEgoStatusMarkerArray(
Expand Down Expand Up @@ -241,7 +298,7 @@ MarkerArray createAvoidLineMarkerArray(

for (const auto & sl : shift_lines_local) {
// ROS_ERROR("sl: s = (%f, %f), g = (%f, %f)", sl.start.x, sl.start.y, sl.end.x, sl.end.y);
Marker basic_marker = createDefaultMarker(
auto basic_marker = createDefaultMarker(
"map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5),
createMarkerColor(r, g, b, 0.9));
basic_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
Expand Down Expand Up @@ -337,84 +394,28 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st
return msg;
}

MarkerArray createAvoidableTargetObjectsMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns)
MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 3);

appendMarkerArray(
createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
createMarkerColor(1.0, 1.0, 0.0, 0.8)),
&msg);

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);

{
for (const auto & object : objects) {
const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;

{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L,
Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(1.0, 1.0, 1.0, 0.999));

for (const auto & p : object.envelope_poly.outer()) {
marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
}

marker.points.push_back(marker.points.front());
marker.id = uuidToInt32(object.object.object_id);
msg.markers.push_back(marker);
}
ObjectDataArray avoidable;
ObjectDataArray unavoidable;
for (const auto & o : objects) {
if (o.is_avoidable) {
avoidable.push_back(o);
} else {
unavoidable.push_back(o);
}
}

return msg;
}

MarkerArray createUnavoidableTargetObjectsMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
msg.markers.reserve(objects.size() * 3);
msg.markers.reserve(objects.size() * 4);

appendMarkerArray(
createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
createMarkerColor(1.0, 0.0, 0.0, 0.8)),
&msg);

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);

{
for (const auto & object : objects) {
const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;

{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L,
Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(1.0, 1.0, 1.0, 0.999));

for (const auto & p : object.envelope_poly.outer()) {
marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
}

marker.points.push_back(marker.points.front());
marker.id = uuidToInt32(object.object.object_id);
msg.markers.push_back(marker);
}
}
}
appendMarkerArray(avoidableObjectsMarkerArray(avoidable, "avoidable_" + ns), &msg);
appendMarkerArray(unAvoidableObjectsMarkerArray(unavoidable, "unavoidable_" + ns), &msg);

return msg;
}

MarkerArray createOtherObjectsMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, const std::string & ns)
MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns)
{
using behavior_path_planner::utils::convertToSnakeCase;

Expand Down Expand Up @@ -451,24 +452,10 @@ MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std:
objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8));
}

MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;

appendMarkerArray(
createObjectsCubeMarkerArray(
objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0),
createMarkerColor(1.0, 0.0, 1.0, 0.9)),
&msg);
appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);

return msg;
}

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns)
const ObjectDataArray & objects, std::string && ns)
{
Marker marker = createDefaultMarker(
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0));

Expand All @@ -495,7 +482,7 @@ MarkerArray createOverhangFurthestLineStringMarkerArray(

for (const auto & linestring : linestrings) {
const auto id = static_cast<int>(linestring.id());
Marker marker = createDefaultMarker(
auto marker = createDefaultMarker(
"map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0),
createMarkerColor(r, g, b, 0.999));

Expand Down
49 changes: 20 additions & 29 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,9 +125,8 @@ bool AvoidanceModule::isExecutionRequested() const
const auto avoid_data = avoidance_data_;
#endif

if (parameters_->publish_debug_marker) {
setDebugData(avoid_data, path_shifter_, debug_data_);
}
updateInfoMarker(avoid_data);
updateDebugMarker(avoid_data, path_shifter_, debug_data_);

#ifndef USE_OLD_ARCHITECTURE
// there is object that should be avoid. return true.
Expand Down Expand Up @@ -2800,11 +2799,8 @@ BehaviorModuleOutput AvoidanceModule::plan()
updateEgoBehavior(data, avoidance_path);
}

if (parameters_->publish_debug_marker) {
setDebugData(avoidance_data_, path_shifter_, debug_data_);
} else {
debug_marker_.markers.clear();
}
updateInfoMarker(avoidance_data_);
updateDebugMarker(avoidance_data_, path_shifter_, debug_data_);

output.path = std::make_shared<PathWithLaneId>(avoidance_path.path);
output.reference_path = getPreviousModuleOutput().reference_path;
Expand Down Expand Up @@ -3242,7 +3238,16 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con
return turn_signal_info;
}

void AvoidanceModule::setDebugData(
void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
{
using marker_utils::avoidance_marker::createTargetObjectsMarkerArray;

info_marker_.markers.clear();
appendMarkerArray(
createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_);
}

void AvoidanceModule::updateDebugMarker(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const
{
using marker_utils::createLaneletsAreaMarkerArray;
Expand All @@ -3251,25 +3256,25 @@ void AvoidanceModule::setDebugData(
using marker_utils::createPoseMarkerArray;
using marker_utils::createShiftLengthMarkerArray;
using marker_utils::createShiftLineMarkerArray;
using marker_utils::avoidance_marker::createAvoidableTargetObjectsMarkerArray;
using marker_utils::avoidance_marker::createAvoidLineMarkerArray;
using marker_utils::avoidance_marker::createEgoStatusMarkerArray;
using marker_utils::avoidance_marker::createOtherObjectsMarkerArray;
using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray;
using marker_utils::avoidance_marker::createPredictedVehiclePositions;
using marker_utils::avoidance_marker::createSafetyCheckMarkerArray;
using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray;
using marker_utils::avoidance_marker::createUnavoidableTargetObjectsMarkerArray;
using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray;
using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray;
using tier4_autoware_utils::appendMarkerArray;

debug_marker_.markers.clear();

if (!parameters_->publish_debug_marker) {
return;
}

const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();

const auto add = [this](const MarkerArray & added) {
tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
};
const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); };

const auto addAvoidLine =
[&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.1) {
Expand All @@ -3293,22 +3298,8 @@ void AvoidanceModule::setDebugData(

add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug));

std::vector<ObjectData> avoidable_target_objects;
std::vector<ObjectData> unavoidable_target_objects;
for (const auto & object : data.target_objects) {
if (object.is_avoidable) {
avoidable_target_objects.push_back(object);
} else {
unavoidable_target_objects.push_back(object);
}
}

add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0));
add(
createAvoidableTargetObjectsMarkerArray(avoidable_target_objects, "avoidable_target_objects"));
add(createUnavoidableTargetObjectsMarkerArray(
unavoidable_target_objects, "unavoidable_target_objects"));

add(createOtherObjectsMarkerArray(
data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD));
Expand Down