From ca5b144b087bc02cfa543606e86b50e7d09b0398 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 3 Sep 2025 12:10:59 +0200 Subject: [PATCH] add new plugin to visualize geofence based on polygon --- rviz_default_plugins/CMakeLists.txt | 2 + .../displays/geofence/geofence_display.hpp | 64 ++++++ rviz_default_plugins/plugins_description.xml | 11 + .../displays/geofence/geofence_display.cpp | 207 ++++++++++++++++++ scripts/publish_dummy_geofence.sh | 31 +++ 5 files changed, 315 insertions(+) create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/geofence/geofence_display.hpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/geofence/geofence_display.cpp create mode 100755 scripts/publish_dummy_geofence.sh diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 3734be749..74898cf4a 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/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 diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/geofence/geofence_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/geofence/geofence_display.hpp new file mode 100644 index 000000000..9b5c3bdb5 --- /dev/null +++ b/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 +{ + 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_ + diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 6b0b581f6..ee2d658fa 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -187,6 +187,17 @@ geometry_msgs/msg/PolygonStamped + + + 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. + + geometry_msgs/msg/PolygonStamped + + +#include +#include +#include +#include + +#include +#include + +#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(base_color_qt.redF()), + static_cast(base_color_qt.greenF()), + static_cast(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 // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::GeofenceDisplay, rviz_common::Display) diff --git a/scripts/publish_dummy_geofence.sh b/scripts/publish_dummy_geofence.sh new file mode 100755 index 000000000..e10e0fcdc --- /dev/null +++ b/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 [ [ []]] +# 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}" +