From 8b357264c02494b4b289f2e052a594d3f7bf3003 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Grzegorz=20G=C5=82owacki?= Date: Tue, 18 Nov 2025 08:46:07 +0100 Subject: [PATCH 1/6] feat(autoware_planning_rviz_plugin): add pose visualization properties and rendering MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Grzegorz Głowacki --- .../path/display_base.hpp | 84 ++++++++++++++++++- 1 file changed, 83 insertions(+), 1 deletion(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index beec37b..44134f7 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -142,6 +142,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay 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_} @@ -167,6 +173,11 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // initialize point property_point_alpha_.setMin(0.0); property_point_alpha_.setMax(1.0); + property_point_radius_.setMin(0.0); + // initialize pose + property_pose_alpha_.setMin(0.0); + property_pose_alpha_.setMax(1.0); + property_pose_arrow_length_.setMin(0.0); updateVehicleInfo(); } @@ -534,7 +545,13 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay 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() ? 3 : 0; // 3 triangles per arrow + 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); @@ -613,6 +630,65 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay } } + // 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(); + const double head_length = arrow_length * 0.4; + const double head_width = arrow_length * 0.5; // Make head wider and more visible + const double shaft_length = arrow_length - head_length; + const double shaft_width = arrow_length * 0.15; + + const double origin_x = pose.position.x + offset * std::cos(yaw); + const double origin_y = pose.position.y + offset * std::sin(yaw); + const double origin_z = pose.position.z; + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + 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; + + 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); } @@ -683,6 +759,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay 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_; From ac9abba69189e56f4fdc28a9db8e8cf45ee93baa Mon Sep 17 00:00:00 2001 From: ralwing <58466562+ralwing@users.noreply.github.com> Date: Tue, 18 Nov 2025 08:58:54 +0100 Subject: [PATCH 2/6] fix vetrices estimation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Grzegorz Głowacki --- .../include/autoware_planning_rviz_plugin/path/display_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index 44134f7..df1161e 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -549,7 +549,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // 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() ? 3 : 0; // 3 triangles per arrow + 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); From d558ebe795035dad184447afb373979634c4f9a6 Mon Sep 17 00:00:00 2001 From: ralwing <58466562+ralwing@users.noreply.github.com> Date: Tue, 18 Nov 2025 09:03:06 +0100 Subject: [PATCH 3/6] remove duplicated calculations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Grzegorz Głowacki --- .../autoware_planning_rviz_plugin/path/display_base.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index df1161e..c1e5986 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -644,12 +644,11 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay const double shaft_length = arrow_length - head_length; const double shaft_width = arrow_length * 0.15; - const double origin_x = pose.position.x + offset * std::cos(yaw); - const double origin_y = pose.position.y + offset * std::sin(yaw); - const double origin_z = pose.position.z; - 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; From 390324c038507197b319bb53c89d74e74ff99154 Mon Sep 17 00:00:00 2001 From: ralwing <58466562+ralwing@users.noreply.github.com> Date: Tue, 18 Nov 2025 09:04:04 +0100 Subject: [PATCH 4/6] improve code organization MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Signed-off-by: Grzegorz Głowacki --- .../include/autoware_planning_rviz_plugin/path/display_base.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index c1e5986..ebc100a 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -173,7 +173,6 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // initialize point property_point_alpha_.setMin(0.0); property_point_alpha_.setMax(1.0); - property_point_radius_.setMin(0.0); // initialize pose property_pose_alpha_.setMin(0.0); property_pose_alpha_.setMax(1.0); From 00644948a39ec050b7eab57e0a1b5597f07d6e9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Grzegorz=20G=C5=82owacki?= Date: Tue, 18 Nov 2025 09:01:47 +0100 Subject: [PATCH 5/6] fix(display_base): improve arrow head and shaft dimensions for better visibility MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Grzegorz Głowacki --- .../path/display_base.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index ebc100a..f731758 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -638,10 +638,14 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay const double offset = property_pose_offset_.getFloat(); const double yaw = tf2::getYaw(pose.orientation); const double arrow_length = property_pose_arrow_length_.getFloat(); - const double head_length = arrow_length * 0.4; - const double head_width = arrow_length * 0.5; // Make head wider and more visible + 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 * 0.15; + const double shaft_width = arrow_length * SHAFT_WIDTH_RATIO; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); From 12457d8754024f90d9d714f5440dc399ea58bcca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Grzegorz=20G=C5=82owacki?= Date: Tue, 18 Nov 2025 15:42:51 +0100 Subject: [PATCH 6/6] formatting MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Grzegorz Głowacki --- .../autoware_planning_rviz_plugin/path/display_base.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp index f731758..b9f18c7 100644 --- a/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp +++ b/autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/path/display_base.hpp @@ -547,8 +547,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // 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) + 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);