Skip to content

feat(autoware_planning_rviz_plugin): add pose visualization as an arrow #27

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

Open
wants to merge 6 commits into
base: main
Choose a base branch
from
Open

feat(autoware_planning_rviz_plugin): add pose visualization as an arrow #27

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8b35726
feat(autoware_planning_rviz_plugin): add pose visualization propertie…
ralwing Nov 18, 2025
ac9abba
fix vetrices estimation
ralwing Nov 18, 2025
d558ebe
remove duplicated calculations
ralwing Nov 18, 2025
390324c
improve code organization
ralwing Nov 18, 2025
0064494
fix(display_base): improve arrow head and shaft dimensions for better…
ralwing Nov 18, 2025
12457d8
formatting
ralwing Nov 18, 2025
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
87 changes: 86 additions & 1 deletion autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_},
property_point_radius_{"Radius", 0.1, "", &property_point_view_},
property_point_offset_{"Offset", 0.0, "", &property_point_view_},
// pose
property_pose_view_{"View Pose", false, "", this},
property_pose_alpha_{"Alpha", 1.0, "", &property_pose_view_},
property_pose_color_{"Color", QColor(255, 60, 0), "", &property_pose_view_},
property_pose_arrow_length_{"Arrow Length", 0.3, "", &property_pose_view_},
property_pose_offset_{"Offset", 0.0, "", &property_pose_view_},
// slope
property_slope_text_view_{"View Text Slope", false, "", this},
property_slope_text_scale_{"Scale", 0.3, "", &property_slope_text_view_}
Expand All @@ -167,6 +173,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
// initialize point
property_point_alpha_.setMin(0.0);
property_point_alpha_.setMax(1.0);
// initialize pose
property_pose_alpha_.setMin(0.0);
property_pose_alpha_.setMax(1.0);
property_pose_arrow_length_.setMin(0.0);

updateVehicleInfo();
}
Expand Down Expand Up @@ -534,7 +544,14 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>

footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2);
footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8);

// Estimate for both points (circles) and poses (arrows)
const size_t point_triangles =
property_point_view_.getBool() ? 8 * 3 : 0; // 8 triangles per circle
const size_t pose_triangles =
property_pose_view_.getBool() ? 9 : 0; // 3 triangles per arrow (9 vertices)
point_manual_object_->estimateVertexCount(
msg_ptr->points.size() * (point_triangles + pose_triangles));
point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST);

preVisualizePathFootprintDetail(msg_ptr);
Expand Down Expand Up @@ -613,6 +630,68 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
}
}

// Pose - visualized as arrows showing pose direction
if (property_pose_view_.getBool()) {
Ogre::ColourValue color;
color = rviz_common::properties::qtToOgre(property_pose_color_.getColor());
color.a = property_pose_alpha_.getFloat();

const double offset = property_pose_offset_.getFloat();
const double yaw = tf2::getYaw(pose.orientation);
const double arrow_length = property_pose_arrow_length_.getFloat();
constexpr static double HEAD_LENGTH_RATIO = 0.4;
constexpr static double HEAD_WIDTH_RATIO = 0.5;
constexpr static double SHAFT_WIDTH_RATIO = 0.15;

const double head_length = arrow_length * HEAD_LENGTH_RATIO;
const double head_width = arrow_length * HEAD_WIDTH_RATIO;
const double shaft_length = arrow_length - head_length;
const double shaft_width = arrow_length * SHAFT_WIDTH_RATIO;

const double cos_yaw = std::cos(yaw);
const double sin_yaw = std::sin(yaw);
const double origin_x = pose.position.x + offset * cos_yaw;
const double origin_y = pose.position.y + offset * sin_yaw;
const double origin_z = pose.position.z;
const double p0_x = origin_x - shaft_width / 2.0 * sin_yaw;
const double p0_y = origin_y + shaft_width / 2.0 * cos_yaw;
const double p1_x = origin_x + shaft_width / 2.0 * sin_yaw;
const double p1_y = origin_y - shaft_width / 2.0 * cos_yaw;
const double p2_x = origin_x + shaft_length * cos_yaw + shaft_width / 2.0 * sin_yaw;
const double p2_y = origin_y + shaft_length * sin_yaw - shaft_width / 2.0 * cos_yaw;
const double p3_x = origin_x + shaft_length * cos_yaw - shaft_width / 2.0 * sin_yaw;
const double p3_y = origin_y + shaft_length * sin_yaw + shaft_width / 2.0 * cos_yaw;
Comment on lines +656 to +663
Copy link

Copilot AI Nov 18, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[nitpick] The arrow geometry calculations use repeated expressions like 'shaft_width / 2.0 * sin_yaw' and 'shaft_width / 2.0 * cos_yaw'. Extract these as named constants (e.g., 'half_shaft_width_perp_x' and 'half_shaft_width_perp_y') to improve readability and reduce potential errors from repeated calculations.

Suggested change
const double p0_x = origin_x - shaft_width / 2.0 * sin_yaw;
const double p0_y = origin_y + shaft_width / 2.0 * cos_yaw;
const double p1_x = origin_x + shaft_width / 2.0 * sin_yaw;
const double p1_y = origin_y - shaft_width / 2.0 * cos_yaw;
const double p2_x = origin_x + shaft_length * cos_yaw + shaft_width / 2.0 * sin_yaw;
const double p2_y = origin_y + shaft_length * sin_yaw - shaft_width / 2.0 * cos_yaw;
const double p3_x = origin_x + shaft_length * cos_yaw - shaft_width / 2.0 * sin_yaw;
const double p3_y = origin_y + shaft_length * sin_yaw + shaft_width / 2.0 * cos_yaw;
const double half_shaft_width_perp_x = shaft_width / 2.0 * sin_yaw;
const double half_shaft_width_perp_y = shaft_width / 2.0 * cos_yaw;
const double p0_x = origin_x - half_shaft_width_perp_x;
const double p0_y = origin_y + half_shaft_width_perp_y;
const double p1_x = origin_x + half_shaft_width_perp_x;
const double p1_y = origin_y - half_shaft_width_perp_y;
const double p2_x = origin_x + shaft_length * cos_yaw + half_shaft_width_perp_x;
const double p2_y = origin_y + shaft_length * sin_yaw - half_shaft_width_perp_y;
const double p3_x = origin_x + shaft_length * cos_yaw - half_shaft_width_perp_x;
const double p3_y = origin_y + shaft_length * sin_yaw + half_shaft_width_perp_y;

Copilot uses AI. Check for mistakes.

point_manual_object_->position(p0_x, p0_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(p1_x, p1_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(p2_x, p2_y, origin_z);
point_manual_object_->colour(color);

point_manual_object_->position(p0_x, p0_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(p2_x, p2_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(p3_x, p3_y, origin_z);
point_manual_object_->colour(color);

// Arrow head (triangle)
const double tip_x = origin_x + arrow_length * cos_yaw;
const double tip_y = origin_y + arrow_length * sin_yaw;
const double base_left_x = origin_x + shaft_length * cos_yaw - head_width / 2.0 * sin_yaw;
const double base_left_y = origin_y + shaft_length * sin_yaw + head_width / 2.0 * cos_yaw;
const double base_right_x = origin_x + shaft_length * cos_yaw + head_width / 2.0 * sin_yaw;
const double base_right_y = origin_y + shaft_length * sin_yaw - head_width / 2.0 * cos_yaw;

point_manual_object_->position(base_left_x, base_left_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(base_right_x, base_right_y, origin_z);
point_manual_object_->colour(color);
point_manual_object_->position(tip_x, tip_y, origin_z);
point_manual_object_->colour(color);
}

visualizePathFootprintDetail(msg_ptr, p_idx);
}

Expand Down Expand Up @@ -683,6 +762,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
rviz_common::properties::FloatProperty property_point_radius_;
rviz_common::properties::FloatProperty property_point_offset_;

rviz_common::properties::BoolProperty property_pose_view_;
rviz_common::properties::FloatProperty property_pose_alpha_;
rviz_common::properties::ColorProperty property_pose_color_;
rviz_common::properties::FloatProperty property_pose_arrow_length_;
rviz_common::properties::FloatProperty property_pose_offset_;

rviz_common::properties::BoolProperty property_slope_text_view_;
rviz_common::properties::FloatProperty property_slope_text_scale_;

Expand Down
Loading