From e90be4bab07e831647e29150ce58d00276a56b81 Mon Sep 17 00:00:00 2001 From: Kenji Miyake Date: Sat, 16 Apr 2022 17:53:44 +0900 Subject: [PATCH 1/3] feat(tier4_planning_rviz_plugins): add vehicle_info to PathFootprintDisplay Signed-off-by: Kenji Miyake --- .../include/path_footprint/display.hpp | 6 +++++ common/tier4_planning_rviz_plugin/package.xml | 1 + .../src/path_footprint/display.cpp | 22 ++++++++++++++++++- 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp index 78f87bd6ec03..21442120b006 100644 --- a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -37,6 +38,9 @@ namespace rviz_plugins { +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + class AutowarePathFootprintDisplay : public rviz_common::MessageFilterDisplay { @@ -72,6 +76,8 @@ private Q_SLOTS: } float length, width, rear_overhang; }; + + std::shared_ptr vehicle_info_; std::shared_ptr vehicle_footprint_info_; private: diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 3a937abeb5eb..065c3ad463b1 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -19,6 +19,7 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs + vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp index 9806be61bdc3..6a52e84c693a 100644 --- a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp @@ -88,6 +88,20 @@ void AutowarePathFootprintDisplay::processMessage( return; } + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + e.what()); + } + } + Ogre::Vector3 position; Ogre::Quaternion orientation; if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { @@ -176,7 +190,13 @@ void AutowarePathFootprintDisplay::updateVehicleInfo() float width{property_vehicle_width_->getFloat()}; float rear_overhang{property_rear_overhang_->getFloat()}; - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + } } } // namespace rviz_plugins From 5d278ddd5aa9f0afb503256d90ddda16accc0401 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 23 Apr 2022 13:46:35 +0900 Subject: [PATCH 2/3] add vehicle_info to other footprint displays Signed-off-by: Takayuki Murooka --- .../path_with_lane_id_footprint/display.hpp | 5 +++++ .../include/trajectory_footprint/display.hpp | 5 +++++ .../path_with_lane_id_footprint/display.cpp | 22 ++++++++++++++++++- .../src/trajectory_footprint/display.cpp | 22 ++++++++++++++++++- 4 files changed, 52 insertions(+), 2 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp index 19c7146ee92a..bfad59025b4a 100644 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -37,6 +38,9 @@ namespace rviz_plugins { +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + class AutowarePathWithLaneIdFootprintDisplay : public rviz_common::MessageFilterDisplay { @@ -72,6 +76,7 @@ private Q_SLOTS: } float length, width, rear_overhang; }; + std::shared_ptr vehicle_info_; std::shared_ptr vehicle_footprint_info_; private: 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 5c2002160d83..540c59032481 100644 --- a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -37,6 +38,9 @@ namespace rviz_plugins { +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + class AutowareTrajectoryFootprintDisplay : public rviz_common::MessageFilterDisplay { @@ -79,6 +83,7 @@ private Q_SLOTS: } float length, width, rear_overhang; }; + std::shared_ptr vehicle_info_; std::shared_ptr vehicle_footprint_info_; private: diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp index 31463d40896f..3e55852fd866 100644 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp @@ -88,6 +88,20 @@ void AutowarePathWithLaneIdFootprintDisplay::processMessage( return; } + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + e.what()); + } + } + Ogre::Vector3 position; Ogre::Quaternion orientation; if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { @@ -178,7 +192,13 @@ void AutowarePathWithLaneIdFootprintDisplay::updateVehicleInfo() float width{property_vehicle_width_->getFloat()}; float rear_overhang{property_rear_overhang_->getFloat()}; - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + } } } // namespace rviz_plugins 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 4eb63fcd1201..5e9484ce6163 100644 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp @@ -115,6 +115,20 @@ void AutowareTrajectoryFootprintDisplay::processMessage( return; } + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + e.what()); + } + } + Ogre::Vector3 position; Ogre::Quaternion orientation; if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { @@ -241,7 +255,13 @@ void AutowareTrajectoryFootprintDisplay::updateVehicleInfo() float width{property_vehicle_width_->getFloat()}; float rear_overhang{property_rear_overhang_->getFloat()}; - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + } } } // namespace rviz_plugins From c913b4610571e2f6ec46929dc9023a2c82cb26af Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 23 Apr 2022 14:16:42 +0900 Subject: [PATCH 3/3] fix the scope of local variables Signed-off-by: Takayuki Murooka --- .../src/path_footprint/display.cpp | 8 ++++---- .../src/path_with_lane_id_footprint/display.cpp | 8 ++++---- .../src/trajectory_footprint/display.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp index 6a52e84c693a..5e58d6da0362 100644 --- a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp @@ -186,15 +186,15 @@ void AutowarePathFootprintDisplay::updateVisualization() void AutowarePathFootprintDisplay::updateVehicleInfo() { - float length{property_vehicle_length_->getFloat()}; - float width{property_vehicle_width_->getFloat()}; - float rear_overhang{property_rear_overhang_->getFloat()}; - if (vehicle_info_) { vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); } else { + const float length{property_vehicle_length_->getFloat()}; + const float width{property_vehicle_width_->getFloat()}; + const float rear_overhang{property_rear_overhang_->getFloat()}; + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); } } diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp index 3e55852fd866..d3378b887bd1 100644 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp @@ -188,15 +188,15 @@ void AutowarePathWithLaneIdFootprintDisplay::updateVisualization() void AutowarePathWithLaneIdFootprintDisplay::updateVehicleInfo() { - float length{property_vehicle_length_->getFloat()}; - float width{property_vehicle_width_->getFloat()}; - float rear_overhang{property_rear_overhang_->getFloat()}; - if (vehicle_info_) { vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); } else { + const float length{property_vehicle_length_->getFloat()}; + const float width{property_vehicle_width_->getFloat()}; + const float rear_overhang{property_rear_overhang_->getFloat()}; + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); } } 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 5e9484ce6163..7ab59c239f01 100644 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp @@ -251,15 +251,15 @@ void AutowareTrajectoryFootprintDisplay::updateVisualization() void AutowareTrajectoryFootprintDisplay::updateVehicleInfo() { - float length{property_vehicle_length_->getFloat()}; - float width{property_vehicle_width_->getFloat()}; - float rear_overhang{property_rear_overhang_->getFloat()}; - if (vehicle_info_) { vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); } else { + const float length{property_vehicle_length_->getFloat()}; + const float width{property_vehicle_width_->getFloat()}; + const float rear_overhang{property_rear_overhang_->getFloat()}; + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); } }