From d357ca52a25f34c736113e50349449b8e859a5fa Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 1 Dec 2022 21:17:15 +0900 Subject: [PATCH] feat(tier4_planning_rviz_plugin): add offset from baselink param (#2384) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe Signed-off-by: kminoda --- .../include/trajectory_footprint/display.hpp | 1 + .../src/trajectory_footprint/display.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp index 540c590324810..d3964f9e7ce07 100644 --- a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp @@ -67,6 +67,7 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vehicle_length_; rviz_common::properties::FloatProperty * property_vehicle_width_; rviz_common::properties::FloatProperty * property_rear_overhang_; + rviz_common::properties::FloatProperty * property_offset_; Ogre::ManualObject * trajectory_point_manual_object_; rviz_common::properties::BoolProperty * property_trajectory_point_view_; diff --git a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp index 7ab59c239f01c..b9c32aea58be3 100644 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp @@ -44,6 +44,9 @@ AutowareTrajectoryFootprintDisplay::AutowareTrajectoryFootprintDisplay() property_rear_overhang_ = new rviz_common::properties::FloatProperty( "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), this); + property_offset_ = new rviz_common::properties::FloatProperty( + "Offset from BaseLink", 0.0, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); property_vehicle_length_->setMin(0.0); property_vehicle_width_->setMin(0.0); property_rear_overhang_->setMin(0.0); @@ -158,6 +161,8 @@ void AutowareTrajectoryFootprintDisplay::processMessage( trajectory_point_manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + const float offset_from_baselink = property_offset_->getFloat(); + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); /* @@ -169,8 +174,8 @@ void AutowareTrajectoryFootprintDisplay::processMessage( color.a = property_trajectory_footprint_alpha_->getFloat(); const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; const float left = -info->width / 2.0; const float right = info->width / 2.0;