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(lane_change): add metrics to valid paths visualization #9737

Merged
merged 2 commits into from
Dec 24, 2024
+51 −11
Merged

fix(lane_change): add metrics to valid paths visualization #9737

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7628561
fix(lane_change): add metrics to valid paths visualization
zulfaqar-azmi-t4 Dec 23, 2024
82137b2
fix cpp-check error
zulfaqar-azmi-t4 Dec 24, 2024
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: 1 addition & 1 deletion ...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ using autoware::behavior_path_planner::lane_change::Debug;
using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
using visualization_msgs::msg::MarkerArray;
MarkerArray showAllValidLaneChangePath(
const std::vector<LaneChangePath> & lanes, std::string && ns);
const std::vector<LaneChangePath> & lane_change_paths, 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
60 changes: 50 additions & 10 deletions ...ing/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,34 +38,74 @@ using autoware::universe_utils::createDefaultMarker;
using autoware::universe_utils::createMarkerScale;
using geometry_msgs::msg::Point;

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

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

const auto colors = colors::colors_list();
const auto loop_size = std::min(lanes.size(), colors.size());
const auto loop_size = std::min(lane_change_paths.size(), colors.size());
mkquda marked this conversation as resolved.
Show resolved Hide resolved
marker_array.markers.reserve(loop_size);

const auto info_prep_to_string =
[](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
std::ostringstream ss_text;
ss_text << std::setprecision(3) << "vel: " << info.velocity.prepare
<< "[m/s] | lon_acc: " << info.longitudinal_acceleration.prepare
<< "[m/s2] | t: " << info.duration.prepare << "[s] | L: " << info.length.prepare
<< "[m]";
return ss_text.str();
};

const auto info_lc_to_string =
[](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
std::ostringstream ss_text;
ss_text << std::setprecision(3) << "vel: " << info.velocity.lane_changing
<< "[m/s] | lon_acc: " << info.longitudinal_acceleration.lane_changing
<< "[m/s2] | lat_acc: " << info.lateral_acceleration
<< "[m/s2] | t: " << info.duration.lane_changing
<< "[s] | L: " << info.length.lane_changing << "[m]";
return ss_text.str();
};

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

std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]";
const auto & color = colors.at(idx);
Marker marker = createDefaultMarker(
"map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color);
marker.points.reserve(lanes.at(idx).path.points.size());
const auto & points = lc_path.path.points;
auto marker = createDefaultMarker(
"map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0),
color);
marker.points.reserve(points.size());

for (const auto & point : lanes.at(idx).path.points) {
for (const auto & point : points) {
marker.points.push_back(point.point.pose.position);
}

auto text_marker = createDefaultMarker(
"map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
createMarkerScale(0.1, 0.1, 0.8), colors::yellow());
const auto prep_idx = points.size() / 4;
text_marker.pose = points.at(prep_idx).point.pose;
text_marker.pose.position.z += 2.0;
text_marker.text = info_prep_to_string(lc_path.info);
marker_array.markers.push_back(text_marker);

const auto lc_idx = points.size() / 2;
text_marker.id = ++id;
text_marker.pose = points.at(lc_idx).point.pose;
text_marker.text = info_lc_to_string(lc_path.info);
marker_array.markers.push_back(text_marker);

marker_array.markers.push_back(marker);
}
return marker_array;
Expand Down
Loading