From 82a152c1ac80d28bde1f1e0d3b6a54ec1cc51507 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 31 Aug 2022 14:27:44 +0900 Subject: [PATCH] feat(tier4_planning_rviz_plugin): add velocity_text to path_with_lane_id (#1735) * feat(tier4_planning_rviz_plugin): add velocity_text to path_with_lane_id Signed-off-by: kosuke55 * fix pre-commit Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../include/path_with_lane_id/display.hpp | 6 ++ .../src/path_with_lane_id/display.cpp | 57 ++++++++++++++++++- 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp index 53f54202a9cce..df01ffc73c466 100644 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -35,6 +36,7 @@ #include #include +#include namespace rviz_plugins { @@ -62,6 +64,8 @@ private Q_SLOTS: const QColor & color_min, const QColor & color_max, const double ratio); Ogre::ManualObject * path_manual_object_; Ogre::ManualObject * velocity_manual_object_; + std::vector velocity_texts_; + std::vector velocity_text_nodes_; rviz_common::properties::BoolProperty * property_path_view_; rviz_common::properties::BoolProperty * property_velocity_view_; rviz_common::properties::FloatProperty * property_path_width_; @@ -70,6 +74,8 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_path_alpha_; rviz_common::properties::FloatProperty * property_velocity_alpha_; rviz_common::properties::FloatProperty * property_velocity_scale_; + rviz_common::properties::BoolProperty * property_velocity_text_view_; + rviz_common::properties::FloatProperty * property_velocity_text_scale_; rviz_common::properties::BoolProperty * property_path_color_view_; rviz_common::properties::BoolProperty * property_velocity_color_view_; rviz_common::properties::FloatProperty * property_vel_max_; diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp index 590c7ac9e0320..0ae03ffecce28 100644 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp @@ -85,7 +85,10 @@ AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); property_velocity_color_ = new rviz_common::properties::ColorProperty( "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - + property_velocity_text_view_ = new rviz_common::properties::BoolProperty( + "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); + property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); property_vel_max_ = new rviz_common::properties::FloatProperty( "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); property_vel_max_->setMin(0.0); @@ -96,6 +99,12 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() if (initialized()) { scene_manager_->destroyManualObject(path_manual_object_); scene_manager_->destroyManualObject(velocity_manual_object_); + for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + scene_manager_->destroySceneNode(node); + } } } @@ -168,6 +177,29 @@ void AutowarePathWithLaneIdDisplay::processMessage( // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & e = msg_ptr->points.at(point_idx); /* @@ -236,6 +268,29 @@ void AutowarePathWithLaneIdDisplay::processMessage( e.point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); velocity_manual_object_->colour(color); } + + /* + * Velocity Text + */ + if (property_velocity_text_view_->getBool()) { + Ogre::Vector3 position; + position.x = e.point.pose.position.x; + position.y = e.point.pose.position.y; + position.z = e.point.pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + double vel = e.point.longitudinal_velocity_mps; + text->setCaption( + std::to_string(static_cast(std::floor(vel))) + "." + + std::to_string(static_cast(std::floor(vel * 100)))); + text->setCharacterHeight(property_velocity_text_scale_->getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } } path_manual_object_->end();