Skip to content

Commit

Permalink
Add valid path's visualization
Browse files Browse the repository at this point in the history 
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Aug 2, 2022
1 parent 1a71ad1 commit f985c80
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 17 deletions.
3 changes: 2 additions & 1 deletion ...ng/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,8 @@ using marker_utils::CollisionCheckDebug;
using visualization_msgs::msg::MarkerArray;
MarkerArray showObjectInfo(
const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns);
MarkerArray showAllValidLaneChangePath(
const std::vector<LaneChangePath> & lanes, std::string && ns);
MarkerArray showLerpedPose(
const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
MarkerArray showEgoPredictedPaths(
Expand Down
2 changes: 1 addition & 1 deletion ...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ class LaneChangeModule : public SceneModuleInterface

void setObjectDebugVisualization() const;
mutable std::unordered_map<std::string, CollisionCheckDebug> object_debug_;
mutable std::vector<LaneChangePath> candidate_path_;
mutable std::vector<LaneChangePath> debug_valid_path_;
};
} // namespace behavior_path_planner

Expand Down
30 changes: 16 additions & 14 deletions planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,27 +60,29 @@ MarkerArray showObjectInfo(
return marker_array;
}

MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns)
MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
{
if (lanes.empty()) {
return MarkerArray{};
}

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

constexpr auto colors = colorsList();
const auto loop_size = std::min(lanes.size(), colors.size());

marker_array.markers.reserve(loop_size);

for (std::size_t idx = 0; idx < loop_size; ++idx) {
const auto & color = colors.at(idx);
if (lanes.at(idx).path.points.empty()) {
continue;
}

const auto & color = colors.at(idx);
Marker marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::LINE_STRIP,
createMarkerScale(0.1, 0.1, 0.0), createMarkerColor(color[0], color[1], color[2], 0.9));

"map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0),
createMarkerColor(color[0], color[1], color[2], 0.9));
marker.points.reserve(lanes.at(idx).path.points.size());

for (const auto & point : lanes.at(idx).path.points) {
Expand All @@ -97,12 +99,12 @@ MarkerArray showLerpedPose(
{
MarkerArray marker_array;
int32_t id{0};
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};

for (const auto & [uuid, info] : obj_debug_vec) {
Marker marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::POINTS,
createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 0.3, 1.0, 0.9));

"map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3),
createMarkerColor(1.0, 0.3, 1.0, 0.9));
marker.points.reserve(info.lerped_path.size());

for (const auto & point : info.lerped_path) {
Expand All @@ -123,8 +125,8 @@ MarkerArray showEgoPredictedPaths(

MarkerArray marker_array;
constexpr auto colors = colorsList();

constexpr float scale_val = 0.2;
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};

int32_t id{0};
for (const auto & [uuid, info] : obj_debug_vec) {
Expand All @@ -135,7 +137,7 @@ MarkerArray showEgoPredictedPaths(
const auto & color = colors.at(idx);

Marker marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::LINE_STRIP,
"map", current_time, ns, ++id, Marker::LINE_STRIP,
createMarkerScale(scale_val, scale_val, scale_val),
createMarkerColor(color[0], color[1], color[2], 0.9));

Expand Down Expand Up @@ -164,7 +166,6 @@ MarkerArray showPolygon(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP,
createMarkerScale(scale_val, scale_val, scale_val), createMarkerColor(1.0, 1.0, 1.0, 0.9));
Marker obj_marker = ego_marker;

MarkerArray marker_array;

constexpr auto colors = colorsList();
Expand Down Expand Up @@ -211,12 +212,13 @@ MarkerArray showPolygonPose(
MarkerArray marker_array;
int32_t id{0};
size_t idx{0};
const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};

for (const auto & [uuid, info] : obj_debug_vec) {
const auto & color = colors.at(idx);
Marker marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::POINTS,
createMarkerScale(0.2, 0.2, 0.2), createMarkerColor(color[0], color[1], color[2], 0.999));
"map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2),
createMarkerColor(color[0], color[1], color[2], 0.999));
marker.points.reserve(2);
marker.points.push_back(info.expected_ego_pose.position);
marker.points.push_back(info.expected_obj_pose.position);
Expand Down
4 changes: 3 additions & 1 deletion planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,7 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
if (valid_paths.empty()) {
return std::make_pair(false, false);
}
candidate_path_ = valid_paths;
debug_valid_path_ = valid_paths;

// select safe path
object_debug_.clear();
Expand Down Expand Up @@ -535,6 +535,7 @@ bool LaneChangeModule::hasFinishedLaneChange() const

void LaneChangeModule::setObjectDebugVisualization() const
{
using marker_utils::lane_change_markers::showAllValidLaneChangePath;
using marker_utils::lane_change_markers::showLerpedPose;
using marker_utils::lane_change_markers::showObjectInfo;
using marker_utils::lane_change_markers::showPolygon;
Expand All @@ -549,6 +550,7 @@ void LaneChangeModule::setObjectDebugVisualization() const
add(showLerpedPose(object_debug_, "lerp_pose_before_true"));
add(showPolygonPose(object_debug_, "expected_pose"));
add(showPolygon(object_debug_, "lerped_polygon"));
add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths"));
}

} // namespace behavior_path_planner

0 comments on commit f985c80

Please sign in to comment.