diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp index 9b676ecafa2c3..9d39fa32417f5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp @@ -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); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 7eca420e1adb7..3d0fbf72bdf9d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -271,12 +271,25 @@ class AvoidanceModule : public SceneModuleInterface // debug mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; - void setDebugData( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; - void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector 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 & avoidance_debug_msg_array) const; + double getLateralMarginFromVelocity(const double velocity) const; double getRSSLongitudinalDistance( diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index b4075932b80ab..fb8322dffeed3 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -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)); @@ -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( @@ -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); @@ -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; @@ -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)); @@ -495,7 +482,7 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( for (const auto & linestring : linestrings) { const auto id = static_cast(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)); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 4420525bfa14e..a0889186d13c5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -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. @@ -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(avoidance_path.path); output.reference_path = getPreviousModuleOutput().reference_path; @@ -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; @@ -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) { @@ -3293,22 +3298,8 @@ void AvoidanceModule::setDebugData( add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); - std::vector avoidable_target_objects; - std::vector 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));