DRAFT: Add XAI RVIZ2 Plugins

perception_msgs_rendering/include/perception_msgs/rendering/object_state/object_state.hpp

@@ -49,6 +49,7 @@ class ColourValue;
 class Quaternion;
 class SceneManager;
 class SceneNode;
+class ManualObject;
 }  // namespace Ogre
 
 namespace perception_msgs {
@@ -224,6 +225,15 @@ class ObjectState {
    */
   void setVisualizeMesh(const bool& val);
 
+  // Hoverboard visualization
+  void setVisualizeHoverboard(const bool& val);
+  void setHoverboardThickness(const float& val);
+  void setHoverboardCornerRadius(const float& val);
+  void setHoverboardGlow(const bool& val);
+  void setHoverboardGlowParams(const float& height, const float& intensity);
+  void setHoverboardCapStyle(int style);
+  void setHoverboardCornerSegments(int segs);
+
   /**
    * @brief Set the Bounding Box Dimensions explicitly (e.g. for EgoData where dimensions are not part of the EGO-State-Model)
    *
@@ -387,6 +397,8 @@ class ObjectState {
   std::shared_ptr<rviz_rendering::Shape> bbox_;
   std::shared_ptr<rviz_rendering::Shape> bbox_cone_;
   std::shared_ptr<rviz_rendering::Shape> bbox_mesh_;
+  Ogre::ManualObject* hoverboard_mo_ = nullptr;
+  Ogre::ManualObject* hoverboard_glow_mo_ = nullptr;
   std::shared_ptr<rviz_rendering::Arrow> vel_arrow_;
   std::shared_ptr<rviz_rendering::Arrow> acc_arrow_;
   std::shared_ptr<rviz_rendering::MovableText> text_;
@@ -411,6 +423,7 @@ class ObjectState {
   bool indicate_direction_ = true;
   bool visualize_bounding_box_ = true;
   bool visualize_mesh_ = false;
+  bool visualize_hoverboard_ = false;
   bool visualize_velocity_ = true;
   float velocity_scale_ = 1.0;
   bool velocity_height_ = false;
@@ -437,6 +450,17 @@ class ObjectState {
   std::string text_probabilities_;
   std::string material;
 
+  // Hoverboard params
+  float hoverboard_thickness_ = 0.12f;
+  float hoverboard_corner_radius_ = 0.35f;
+  bool hoverboard_glow_ = true;
+  float hoverboard_glow_height_ = 0.7f;
+  float hoverboard_glow_intensity_ = 0.6f;
+  int hoverboard_cap_style_ = 2; // 0=square,1=bevel,2=round
+  int hoverboard_corner_segments_ = 12;
+  std::string hoverboard_material_name_ = "ObjectHoverboard/Tile";
+  std::string hoverboard_glow_material_name_ = "ObjectHoverboard/Glow";
+
   const double kFixedMeshHeightCar = 1.5;
   const double kFixedMeshHeightTruck = 4.0;
   const double kFixedMeshHeightBus = 4.0;
@@ -447,4 +471,4 @@ class ObjectState {
 
 }  // namespace rendering
 
-}  // namespace perception_msgs
+}  // namespace perception_msgs

perception_msgs_rendering/src/rendering/object_state/object_state.cpp

@@ -32,6 +32,7 @@ SOFTWARE.
 #include <OgreEntity.h>
 #include <OgreMaterial.h>
 #include <OgreMaterialManager.h>
+#include <OgreManualObject.h>
 #include <OgreMovableObject.h>
 #include <OgrePrerequisites.h>
 #include <OgreQuaternion.h>
@@ -43,6 +44,7 @@ SOFTWARE.
 #include <OgreVector.h>
 
 #include <cmath>
+#include <algorithm>
 
 #include <rviz_rendering/mesh_loader.hpp>
 #include "rviz_rendering/objects/shape.hpp"
@@ -68,7 +70,17 @@ ObjectState::ObjectState(const std::unordered_map<unsigned int, Ogre::ColourValu
   text_color_ = text_color;
 }
 
-ObjectState::~ObjectState() { scene_manager_->destroySceneNode(scene_node_); }
+ObjectState::~ObjectState() {
+  if (hoverboard_mo_) {
+    scene_manager_->destroyManualObject(hoverboard_mo_);
+    hoverboard_mo_ = nullptr;
+  }
+  if (hoverboard_glow_mo_) {
+    scene_manager_->destroyManualObject(hoverboard_glow_mo_);
+    hoverboard_glow_mo_ = nullptr;
+  }
+  scene_manager_->destroySceneNode(scene_node_);
+}
 
 void ObjectState::setObjectState(const perception_msgs::msg::ObjectState& state) {
   object_state_ = state;
@@ -125,6 +137,8 @@ void ObjectState::setObjectStatePredictions(
   predictions_ = predictions;
   //Fill billboard_line_predictions_ with empty vectors
   billboard_line_predictions_.clear();
+  text_prob_vector_.clear();
+  bbox_predictions_.clear();
   for (size_t i = 0; i < predictions.size(); i++) {
     billboard_line_predictions_.push_back(std::shared_ptr<rviz_rendering::BillboardLine>());
     text_prob_vector_.push_back(std::shared_ptr<rviz_rendering::MovableText>());
@@ -145,7 +159,7 @@ void ObjectState::setObjectStatePredictions(
     setObjectPredictionsVizDefault(predictions[i].states, billboard_line_predictions_[i], bbox_predictions_[i],
                                    line_color, point_color);
 
-    if (visualize_predictions_ && visualize_prediction_probabilities_) {
+    if (visualize_predictions_ && visualize_prediction_probabilities_ && !predictions[i].states.empty()) {
       setObjectPredictionProbabilityText(predictions[i].probability, predictions[i].states[0], text_prob_vector_[i]);
     }
   }
@@ -169,6 +183,23 @@ void ObjectState::setVisualizeBoundingBox(const bool& val) { visualize_bounding_
 
 void ObjectState::setVisualizeMesh(const bool& val) { visualize_mesh_ = val; }
 
+void ObjectState::setVisualizeHoverboard(const bool& val) { visualize_hoverboard_ = val; }
+void ObjectState::setHoverboardThickness(const float& val) { hoverboard_thickness_ = std::max(0.0f, val); }
+void ObjectState::setHoverboardCornerRadius(const float& val) { hoverboard_corner_radius_ = std::max(0.0f, val); }
+void ObjectState::setHoverboardGlow(const bool& val) { hoverboard_glow_ = val; }
+void ObjectState::setHoverboardGlowParams(const float& height, const float& intensity) {
+  hoverboard_glow_height_ = std::max(0.0f, height);
+  hoverboard_glow_intensity_ = std::max(0.0f, std::min(1.0f, intensity));
+}
+
+void ObjectState::setHoverboardCapStyle(int style) {
+  hoverboard_cap_style_ = std::max(0, std::min(2, style));
+}
+
+void ObjectState::setHoverboardCornerSegments(int segs) {
+  hoverboard_corner_segments_ = std::max(3, std::min(64, segs));
+}
+
 void ObjectState::setVisualizeVelocity(const bool& val) { visualize_velocity_ = val; }
 
 void ObjectState::setVelocityScale(const float& val) { velocity_scale_ = val; }
@@ -240,6 +271,17 @@ void ObjectState::setObjectStateVizDefault(const perception_msgs::msg::ObjectSta
   }
 
   if (visualize_mesh_) {
+    // Clean up any previously created mesh node and its attached objects to avoid leaks
+    if (mesh_node_) {
+      // Detach and destroy all attached movable objects (e.g., entities)
+      while (mesh_node_->numAttachedObjects() > 0) {
+        Ogre::MovableObject* mo = mesh_node_->getAttachedObject(0);
+        mesh_node_->detachObject(mo);
+        scene_manager_->destroyMovableObject(mo);
+      }
+      scene_manager_->destroySceneNode(mesh_node_);
+      mesh_node_ = nullptr;
+    }
     //load mesh to render based on classification
     Ogre::Entity* entity;
     Ogre::MeshPtr mesh;
@@ -371,6 +413,164 @@ void ObjectState::setObjectStateVizDefault(const perception_msgs::msg::ObjectSta
     bbox_cone_->setPosition(cone_pos);
   }
 
+  // Hoverboard tile (rounded rectangle with optional upward glow)
+  if (visualize_hoverboard_) {
+    // Create tile and glow materials if missing
+    if (!Ogre::MaterialManager::getSingleton().resourceExists(hoverboard_material_name_)) {
+      Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(
+          hoverboard_material_name_, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+      if (!mat.isNull()) {
+        Ogre::Technique* tech = mat->getTechnique(0);
+        if (!tech) tech = mat->createTechnique();
+        Ogre::Pass* pass = tech->getPass(0);
+        if (!pass) pass = tech->createPass();
+        pass->setLightingEnabled(false);
+        pass->setDepthCheckEnabled(true);
+        pass->setDepthWriteEnabled(true);
+        pass->setCullingMode(Ogre::CULL_NONE);
+        pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+        pass->setVertexColourTracking(Ogre::TVC_DIFFUSE);
+      }
+    }
+    if (!Ogre::MaterialManager::getSingleton().resourceExists(hoverboard_glow_material_name_)) {
+      Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(
+          hoverboard_glow_material_name_, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+      if (!mat.isNull()) {
+        Ogre::Technique* tech = mat->getTechnique(0);
+        if (!tech) tech = mat->createTechnique();
+        Ogre::Pass* pass = tech->getPass(0);
+        if (!pass) pass = tech->createPass();
+        pass->setLightingEnabled(false);
+        pass->setDepthCheckEnabled(true);
+        pass->setDepthWriteEnabled(false);
+        pass->setCullingMode(Ogre::CULL_NONE);
+        pass->setSceneBlending(Ogre::SBT_ADD);
+        pass->setVertexColourTracking(Ogre::TVC_DIFFUSE);
+      }
+    }
+
+    // Ensure manual objects exist
+    if (!hoverboard_mo_) {
+      hoverboard_mo_ = scene_manager_->createManualObject();
+      hoverboard_mo_->setDynamic(true);
+      scene_node_->attachObject(hoverboard_mo_);
+    }
+    if (!hoverboard_glow_mo_) {
+      hoverboard_glow_mo_ = scene_manager_->createManualObject();
+      hoverboard_glow_mo_->setDynamic(true);
+      scene_node_->attachObject(hoverboard_glow_mo_);
+    }
+
+    // Dimensions and corner resolution
+    const float L = static_cast<float>(bbox_dims_.x);
+    const float W = static_cast<float>(bbox_dims_.y);
+    float r = hoverboard_corner_radius_;
+    const float rmax = 0.5f * std::min(L, W) - 1e-3f;
+    if (r > rmax) r = std::max(0.0f, rmax);
+    int seg = (hoverboard_cap_style_ == 2) ? hoverboard_corner_segments_ : 1; // round uses configured segments
+    const float zBot = static_cast<float>(-0.5 * bbox_dims_.z);
+    const float zTop = zBot + hoverboard_thickness_;
+
+    std::vector<Ogre::Vector3> boundary;
+    boundary.reserve(4 * (seg + 1));
+    auto arc = [&](float cx, float cy, float a0, float a1) {
+      if (hoverboard_cap_style_ == 0) {
+        return; // square handled separately
+      }
+      if (hoverboard_cap_style_ == 1) {
+        // bevel: straight cut between the two edge-offset points
+        boundary.emplace_back(cx + r * std::cos(a0), cy + r * std::sin(a0), zTop);
+        boundary.emplace_back(cx + r * std::cos(a1), cy + r * std::sin(a1), zTop);
+        return;
+      }
+      // round corner
+      for (int i = 0; i <= seg; ++i) {
+        float t = static_cast<float>(i) / static_cast<float>(seg);
+        float a = a0 + t * (a1 - a0);
+        boundary.emplace_back(cx + r * std::cos(a), cy + r * std::sin(a), zTop);
+      }
+    };
+    const float hx = 0.5f * L - r;
+    const float hy = 0.5f * W - r;
+    if (hoverboard_cap_style_ == 0 || r <= 1e-6f) {
+      // Square corners: rectangle boundary
+      boundary.emplace_back(+0.5f * L, +0.5f * W, zTop);
+      boundary.emplace_back(-0.5f * L, +0.5f * W, zTop);
+      boundary.emplace_back(-0.5f * L, -0.5f * W, zTop);
+      boundary.emplace_back(+0.5f * L, -0.5f * W, zTop);
+    } else {
+      const float PI = static_cast<float>(Ogre::Math::PI);
+      const float HALF_PI = static_cast<float>(Ogre::Math::HALF_PI);
+      arc(+hx, +hy, 0.0f, HALF_PI);      // top-right
+      arc(-hx, +hy, HALF_PI, PI);        // top-left
+      arc(-hx, -hy, PI, 1.5f * PI);      // bottom-left
+      arc(+hx, -hy, 1.5f * PI, 2.0f * PI); // bottom-right
+    }
+
+    Ogre::ColourValue tile_col = color;
+
+    // Build tile
+    hoverboard_mo_->clear();
+    hoverboard_mo_->begin(hoverboard_material_name_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
+    // top face
+    Ogre::Vector3 cTop(0, 0, zTop);
+    for (size_t i = 0; i < boundary.size(); ++i) {
+      const Ogre::Vector3 &p0 = boundary[i];
+      const Ogre::Vector3 &p1 = boundary[(i + 1) % boundary.size()];
+      hoverboard_mo_->position(cTop); hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(p0);   hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(p1);   hoverboard_mo_->colour(tile_col);
+    }
+    // sides
+    for (size_t i = 0; i < boundary.size(); ++i) {
+      Ogre::Vector3 t0 = boundary[i];
+      Ogre::Vector3 t1 = boundary[(i + 1) % boundary.size()];
+      Ogre::Vector3 b0(t0.x, t0.y, zBot);
+      Ogre::Vector3 b1(t1.x, t1.y, zBot);
+      hoverboard_mo_->position(t0); hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(b0); hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(b1); hoverboard_mo_->colour(tile_col);
+
+      hoverboard_mo_->position(t0); hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(b1); hoverboard_mo_->colour(tile_col);
+      hoverboard_mo_->position(t1); hoverboard_mo_->colour(tile_col);
+    }
+    hoverboard_mo_->end();
+
+    // Glow skirt
+    hoverboard_glow_mo_->clear();
+    if (hoverboard_glow_ && hoverboard_glow_height_ > 1e-4f && hoverboard_glow_intensity_ > 1e-4f) {
+      Ogre::ColourValue cBottom = tile_col;
+      cBottom.r *= hoverboard_glow_intensity_;
+      cBottom.g *= hoverboard_glow_intensity_;
+      cBottom.b *= hoverboard_glow_intensity_;
+      Ogre::ColourValue cTop = cBottom; cTop.a = 0.0f;
+      hoverboard_glow_mo_->begin(hoverboard_glow_material_name_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
+      const float zG0 = zTop;
+      const float zG1 = zTop + hoverboard_glow_height_;
+      for (size_t i = 0; i < boundary.size(); ++i) {
+        Ogre::Vector3 e0 = boundary[i];
+        Ogre::Vector3 e1 = boundary[(i + 1) % boundary.size()];
+        Ogre::Vector3 g0(e0.x, e0.y, zG0);
+        Ogre::Vector3 g1(e1.x, e1.y, zG0);
+        Ogre::Vector3 g2(e1.x, e1.y, zG1);
+        Ogre::Vector3 g3(e0.x, e0.y, zG1);
+        hoverboard_glow_mo_->position(g0); hoverboard_glow_mo_->colour(cBottom);
+        hoverboard_glow_mo_->position(g1); hoverboard_glow_mo_->colour(cBottom);
+        hoverboard_glow_mo_->position(g2); hoverboard_glow_mo_->colour(cTop);
+
+        hoverboard_glow_mo_->position(g0); hoverboard_glow_mo_->colour(cBottom);
+        hoverboard_glow_mo_->position(g2); hoverboard_glow_mo_->colour(cTop);
+        hoverboard_glow_mo_->position(g3); hoverboard_glow_mo_->colour(cTop);
+      }
+      hoverboard_glow_mo_->end();
+    }
+  } else {
+    // cleanup if previously created
+    if (hoverboard_mo_) { scene_manager_->destroyManualObject(hoverboard_mo_); hoverboard_mo_ = nullptr; }
+    if (hoverboard_glow_mo_) { scene_manager_->destroyManualObject(hoverboard_glow_mo_); hoverboard_glow_mo_ = nullptr; }
+  }
+
   if (visualize_velocity_) {
     vel_arrow_ =
         std::make_shared<rviz_rendering::Arrow>(scene_manager_, scene_node_, 1.0f, 0.5f, 0.3f,
@@ -527,6 +727,8 @@ void ObjectState::setObjectStateTextDefault(const perception_msgs::msg::ObjectSt
 void ObjectState::setObjectPredictionProbabilityText(const double& probability,
                                                      const perception_msgs::msg::ObjectState& state,
                                                      std::shared_ptr<rviz_rendering::MovableText>& text_prob) {
+  // reset text buffer per prediction label to avoid uncontrolled growth
+  text_probabilities_.clear();
   if (probability >= 0.0) {
     std::ostringstream textStream;
     textStream << std::fixed << std::setprecision(1)