Draft: Add new plugin to visualize geofence based on polygon

rviz_default_plugins/CMakeLists.txt

@@ -109,6 +109,7 @@ set(rviz_default_plugins_headers_to_moc
   include/rviz_default_plugins/displays/pointcloud/transformers/flat_color_pc_transformer.hpp
   include/rviz_default_plugins/displays/pointcloud/transformers/intensity_pc_transformer.hpp
   include/rviz_default_plugins/displays/polygon/polygon_display.hpp
+  include/rviz_default_plugins/displays/geofence/geofence_display.hpp
   include/rviz_default_plugins/displays/pose/pose_display.hpp
   include/rviz_default_plugins/displays/pose_array/pose_array_display.hpp
   include/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.hpp
@@ -194,6 +195,7 @@ set(rviz_default_plugins_source_files
   src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp
   src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.cpp
   src/rviz_default_plugins/displays/polygon/polygon_display.cpp
+  src/rviz_default_plugins/displays/geofence/geofence_display.cpp
   src/rviz_default_plugins/displays/pose/pose_display.cpp
   src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp
   src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp

rviz_default_plugins/include/rviz_default_plugins/displays/geofence/geofence_display.hpp

@@ -0,0 +1,64 @@
+/*
+ * Geofence display for geometry_msgs::msg::PolygonStamped
+ */
+
+#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__GEOFENCE__GEOFENCE_DISPLAY_HPP_
+#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__GEOFENCE__GEOFENCE_DISPLAY_HPP_
+
+#include "geometry_msgs/msg/polygon_stamped.hpp"
+
+#include "rviz_common/message_filter_display.hpp"
+
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace Ogre
+{
+class ManualObject;
+}
+
+namespace rviz_common
+{
+namespace properties
+{
+class ColorProperty;
+class FloatProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace rviz_default_plugins
+{
+namespace displays
+{
+
+class RVIZ_DEFAULT_PLUGINS_PUBLIC GeofenceDisplay : public
+  rviz_common::MessageFilterDisplay<geometry_msgs::msg::PolygonStamped>
+{
+  Q_OBJECT
+
+public:
+  GeofenceDisplay();
+  ~GeofenceDisplay() override;
+
+  void onInitialize() override;
+  void reset() override;
+
+protected:
+  void processMessage(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) override;
+
+private:
+  Ogre::ManualObject * manual_object_;
+  Ogre::MaterialPtr material_;
+
+  // Properties
+  rviz_common::properties::ColorProperty * color_property_;
+  rviz_common::properties::FloatProperty * bottom_alpha_property_;
+  rviz_common::properties::FloatProperty * top_alpha_property_;
+  rviz_common::properties::FloatProperty * height_property_;
+  rviz_common::properties::FloatProperty * thickness_property_;
+};
+
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#endif  // RVIZ_DEFAULT_PLUGINS__DISPLAYS__GEOFENCE__GEOFENCE_DISPLAY_HPP_
+

rviz_default_plugins/plugins_description.xml

@@ -187,6 +187,17 @@
     <message_type>geometry_msgs/msg/PolygonStamped</message_type>
   </class>
 
+  <class
+    name="rviz_default_plugins/Geofence"
+    type="rviz_default_plugins::displays::GeofenceDisplay"
+    base_class_type="rviz_common::Display"
+  >
+    <description>
+      Visualizes a geofence from geometry_msgs::PolygonStamped as vertical edge bands with a vertical fade. Edges are rendered as thin 3D rectangles with a configurable height, thickness, and alpha gradient.
+    </description>
+    <message_type>geometry_msgs/msg/PolygonStamped</message_type>
+  </class>
+
   <class
     name="rviz_default_plugins/Pose"
     type="rviz_default_plugins::displays::PoseDisplay"

rviz_default_plugins/src/rviz_default_plugins/displays/geofence/geofence_display.cpp

@@ -0,0 +1,207 @@
+/*
+ * Geofence display - renders polygon edges as thin 3D quads with vertical fade.
+ */
+
+#include "rviz_default_plugins/displays/geofence/geofence_display.hpp"
+
+#include <OgreManualObject.h>
+#include <OgreMaterialManager.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreTechnique.h>
+
+#include <string>
+#include <vector>
+
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/frame_manager_iface.hpp"
+#include "rviz_common/logging.hpp"
+#include "rviz_common/properties/color_property.hpp"
+#include "rviz_common/properties/float_property.hpp"
+#include "rviz_common/validate_floats.hpp"
+#include "rviz_rendering/material_manager.hpp"
+
+namespace rviz_default_plugins
+{
+namespace displays
+{
+
+namespace
+{
+bool validateFloats(const geometry_msgs::msg::PolygonStamped::ConstSharedPtr & msg)
+{
+  return rviz_common::validateFloats(msg->polygon.points);
+}
+}
+
+GeofenceDisplay::GeofenceDisplay()
+{
+  color_property_ = new rviz_common::properties::ColorProperty(
+    "Color", QColor(25, 255, 0),
+    "Base color for geofence edges.", this, SLOT(queueRender()));
+
+  bottom_alpha_property_ = new rviz_common::properties::FloatProperty(
+    "Bottom Alpha", 0.8f,
+    "Alpha at the bottom of the edge (0..1).", this, SLOT(queueRender()));
+  bottom_alpha_property_->setMin(0.0);
+  bottom_alpha_property_->setMax(1.0);
+
+  top_alpha_property_ = new rviz_common::properties::FloatProperty(
+    "Top Alpha", 0.05f,
+    "Alpha at the top of the edge (0..1).", this, SLOT(queueRender()));
+  top_alpha_property_->setMin(0.0);
+  top_alpha_property_->setMax(1.0);
+
+  height_property_ = new rviz_common::properties::FloatProperty(
+    "Height", 2.0f,
+    "Extrusion height of the edges (meters).", this, SLOT(queueRender()));
+  height_property_->setMin(0.0);
+
+  thickness_property_ = new rviz_common::properties::FloatProperty(
+    "Thickness", 0.05f,
+    "Visual thickness of the edge band (meters).", this, SLOT(queueRender()));
+  thickness_property_->setMin(0.0);
+
+  static int geofence_count = 0;
+  std::string material_name = "GeofenceMaterial" + std::to_string(geofence_count++);
+  material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name);
+  material_->setCullingMode(Ogre::CULL_NONE);
+}
+
+GeofenceDisplay::~GeofenceDisplay()
+{
+  if (initialized()) {
+    scene_manager_->destroyManualObject(manual_object_);
+  }
+}
+
+void GeofenceDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  manual_object_ = scene_manager_->createManualObject();
+  manual_object_->setDynamic(true);
+  scene_node_->attachObject(manual_object_);
+}
+
+void GeofenceDisplay::reset()
+{
+  MFDClass::reset();
+  manual_object_->clear();
+}
+
+void GeofenceDisplay::processMessage(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
+{
+  if (!validateFloats(msg)) {
+    setStatus(
+      rviz_common::properties::StatusProperty::Error, "Topic",
+      "Message contained invalid floating point values (nans or infs)");
+    return;
+  }
+
+  rclcpp::Time msg_time(msg->header.stamp, RCL_ROS_TIME);
+  if (!updateFrame(msg->header.frame_id, msg_time)) {
+    setMissingTransformToFixedFrame(msg->header.frame_id);
+    return;
+  }
+  setTransformOk();
+
+  manual_object_->clear();
+
+  const auto base_color_qt = color_property_->getColor();
+  Ogre::ColourValue base_color(
+    static_cast<float>(base_color_qt.redF()),
+    static_cast<float>(base_color_qt.greenF()),
+    static_cast<float>(base_color_qt.blueF()),
+    1.0f);
+
+  const float alpha_bottom = std::max(0.0f, std::min(1.0f, bottom_alpha_property_->getFloat()));
+  const float alpha_top = std::max(0.0f, std::min(1.0f, top_alpha_property_->getFloat()));
+  const float height = std::max(0.0f, height_property_->getFloat());
+  const float thickness = std::max(0.0f, thickness_property_->getFloat());
+
+  // If any alpha < 1, enable alpha blending
+  rviz_rendering::MaterialManager::enableAlphaBlending(
+    material_, std::min(alpha_bottom, alpha_top));
+
+  const size_t n = msg->polygon.points.size();
+  if (n < 2 || height <= 0.0f) {
+    return;
+  }
+
+  // Two quads per edge if thickness > 0, otherwise one quad centered on the edge normal.
+  // We'll use OT_TRIANGLE_LIST and per-vertex colors for vertical fade.
+  manual_object_->begin(
+    material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST, "rviz_rendering");
+
+  // Helper lambda to push a quad as two triangles with vertical fade.
+  unsigned int vertex_index = 0;
+  auto add_quad = [&](
+      const Ogre::Vector3 & b0,
+      const Ogre::Vector3 & b1,
+      const Ogre::Vector3 & t1,
+      const Ogre::Vector3 & t0) {
+      // Vertex order: b0, b1, t1, t0
+      const unsigned int start_index = vertex_index;
+
+      Ogre::ColourValue bottom_color = base_color; bottom_color.a = alpha_bottom;
+      Ogre::ColourValue top_color = base_color; top_color.a = alpha_top;
+
+      manual_object_->position(b0); manual_object_->colour(bottom_color);
+      manual_object_->position(b1); manual_object_->colour(bottom_color);
+      manual_object_->position(t1); manual_object_->colour(top_color);
+      manual_object_->position(t0); manual_object_->colour(top_color);
+
+      // Two triangles: (0,1,2) and (0,2,3)
+      manual_object_->triangle(start_index + 0, start_index + 1, start_index + 2);
+      manual_object_->triangle(start_index + 0, start_index + 2, start_index + 3);
+      vertex_index += 4;
+    };
+
+  const Ogre::Vector3 z_up(0.0f, 0.0f, 1.0f);
+
+  for (size_t i = 0; i < n; ++i) {
+    const auto & p0 = msg->polygon.points[i];
+    const auto & p1 = msg->polygon.points[(i + 1) % n];
+
+    Ogre::Vector3 P0(p0.x, p0.y, p0.z);
+    Ogre::Vector3 P1(p1.x, p1.y, p1.z);
+
+    Ogre::Vector3 edge = P1 - P0;
+    Ogre::Vector3 edge_xy(edge.x, edge.y, 0.0f);
+    if (edge_xy.squaredLength() < 1e-10f) {
+      continue;  // skip degenerate edge
+    }
+    edge_xy.normalise();
+    Ogre::Vector3 normal(-edge_xy.y, edge_xy.x, 0.0f);  // perpendicular in XY
+
+    const float hw = thickness * 0.5f;
+
+    // Base and top points for this edge
+    Ogre::Vector3 B0 = P0;
+    Ogre::Vector3 B1 = P1;
+    Ogre::Vector3 T0 = P0 + height * z_up;
+    Ogre::Vector3 T1 = P1 + height * z_up;
+
+    if (thickness > 1e-6f) {
+      // Left face (offset -normal)
+      Ogre::Vector3 off = -normal * hw;
+      add_quad(B0 + off, B1 + off, T1 + off, T0 + off);
+      // Right face (offset +normal)
+      off = normal * hw;
+      add_quad(B0 + off, B1 + off, T1 + off, T0 + off);
+    } else {
+      // Single centered face: create a very thin band by offsetting half-width on each side
+      Ogre::Vector3 off = normal * 1e-3f;  // virtually zero thickness
+      add_quad(B0 - off, B1 - off, T1 - off, T0 - off);
+      add_quad(B0 + off, B1 + off, T1 + off, T0 + off);
+    }
+  }
+
+  manual_object_->end();
+}
+
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::GeofenceDisplay, rviz_common::Display)

scripts/publish_dummy_geofence.sh

@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+# Publish a dummy geometry_msgs/PolygonStamped in the map frame (by default)
+# Usage:
+#   ./scripts/publish_dummy_geofence.sh [<topic> [<frame_id> [<rate_hz>]]]
+# Examples:
+#   ./scripts/publish_dummy_geofence.sh               # /geofence, map, 1 Hz
+#   ./scripts/publish_dummy_geofence.sh /geofence map 5
+
+TOPIC="${1:-/geofence}"
+FRAME_ID="${2:-map}"
+RATE="${3:-1}"
+
+echo "Publishing dummy polygon to ${TOPIC} in frame '${FRAME_ID}' at ${RATE} Hz..."
+
+# A simple pentagon-like geofence footprint (meters)
+read -r -d '' MSG << YAML || true
+header:
+  frame_id: ${FRAME_ID}
+polygon:
+  points:
+  - {x: 0.0,  y: 0.0,  z: 0.0}
+  - {x: 10.0, y: 0.0,  z: 0.0}
+  - {x: 10.0, y: 6.0,  z: 0.0}
+  - {x: 5.0,  y: 10.0, z: 0.0}
+  - {x: 0.0,  y: 6.0,  z: 0.0}
+YAML
+
+ros2 topic pub -r "${RATE}" "${TOPIC}" geometry_msgs/msg/PolygonStamped "${MSG}"
+