From 02486654e0bb6d9667d36e57f17b9928d763a67d Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 3 Jun 2024 13:39:09 +0000 Subject: [PATCH 1/9] Fix offset and sim time --- .../src/displays/CAM/cam_display.cpp | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 1033b6635..8c12d27ce 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -100,7 +100,8 @@ void CAMDisplay::reset() void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) { // Generate CAM render object from message - rclcpp::Time now = rviz_node_->now(); + static rclcpp::Clock clock; + rclcpp::Time now = clock.now(); // Always use the current time for the render object, no sim time CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); if (!cam.validateFloats()) { setStatus( @@ -119,10 +120,18 @@ void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) void CAMDisplay::update(float, float) { + //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Updating CAM display after " << wall_dt << " seconds, " << ros_dt << "seconds in ROS."); // Check for outdated CAMs for (auto it = cams_.begin(); it != cams_.end(); ) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) it = cams_.erase(it); - else ++it; + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) + { + //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); + it = cams_.erase(it); + } + else + { + ++it; + } } // Render all valid cams @@ -155,8 +164,14 @@ void CAMDisplay::update(float, float) // the reference point shall be the ground position of the centre of the front side of // the bounding box of the vehicle. // https://www.etsi.org/deliver/etsi_en/302600_302699/30263702/01.03.01_30/en_30263702v010301v.pdf - position.x-=dimensions.x/2.0; - position.z+=dimensions.z/2.0; + tf2::Quaternion q; + tf2::fromMsg(pose.orientation, q); + tf2::Matrix3x3 m(q); + tf2::Vector3 v(-dimensions.x/2.0, 0.0, dimensions.z/2.0); + v = m*v; + position.x += v.x(); + position.y += v.y(); + position.z += v.z(); } // set pose of child scene node of bounding-box From 5fd3c27f5d368f3553e42180cfae1f62aa8d3659 Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 3 Jun 2024 14:05:13 +0000 Subject: [PATCH 2/9] Clamp instead of force wall time --- .../src/displays/CAM/cam_display.cpp | 136 ++++++++---------- 1 file changed, 59 insertions(+), 77 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 8c12d27ce..9bb91b254 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -24,63 +24,49 @@ SOFTWARE. #include "displays/CAM/cam_display.hpp" -#include -#include -#include #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/bool_property.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/parse_color.hpp" -namespace etsi_its_msgs -{ -namespace displays -{ +namespace etsi_its_msgs { +namespace displays { -CAMDisplay::CAMDisplay() -{ +CAMDisplay::CAMDisplay() { // General Properties - buffer_timeout_ = new rviz_common::properties::FloatProperty( - "Timeout", 0.1f, - "Time (in s) until objects disappear", this); + buffer_timeout_ = + new rviz_common::properties::FloatProperty("Timeout", 0.1f, "Time (in s) until objects disappear", this); buffer_timeout_->setMin(0); - bb_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 1.0f, - "Scale of objects", this); + bb_scale_ = new rviz_common::properties::FloatProperty("Scale", 1.0f, "Scale of objects", this); bb_scale_->setMin(0.01); - color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(25, 0, 255), - "Object color", this); - show_meta_ = new rviz_common::properties::BoolProperty("Metadata", true, - "Show metadata as text next to objects", this); - text_color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(25, 0, 255), - "Text color", show_meta_); + color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Object color", this); + show_meta_ = + new rviz_common::properties::BoolProperty("Metadata", true, "Show metadata as text next to objects", this); + text_color_property_ = + new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Text color", show_meta_); char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); - show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, - "Show StationID", show_meta_); - show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, - "Show speed", show_meta_); - + show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, "Show StationID", show_meta_); + show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, "Show speed", show_meta_); } -CAMDisplay::~CAMDisplay() -{ - if (initialized() ) { +CAMDisplay::~CAMDisplay() { + if (initialized()) { scene_manager_->destroyManualObject(manual_object_); } } -void CAMDisplay::onInitialize() -{ +void CAMDisplay::onInitialize() { RTDClass::onInitialize(); auto nodeAbstraction = context_->getRosNodeAbstraction().lock(); @@ -91,54 +77,49 @@ void CAMDisplay::onInitialize() scene_node_->attachObject(manual_object_); } -void CAMDisplay::reset() -{ +void CAMDisplay::reset() { RTDClass::reset(); manual_object_->clear(); } -void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) -{ +void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) { // Generate CAM render object from message - static rclcpp::Clock clock; - rclcpp::Time now = clock.now(); // Always use the current time for the render object, no sim time + rclcpp::Time now = + rclcpp::Time(std::clamp(rviz_node_->now().nanoseconds(), etsi_its_cam_msgs::msg::TimestampIts::MIN, + etsi_its_cam_msgs::msg::TimestampIts::MAX)); CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); if (!cam.validateFloats()) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; } // Check if Station ID is already present in list auto it = cams_.find(cam.getStationID()); - if (it != cams_.end()) it->second = cam; // Key exists, update the value - else cams_.insert(std::make_pair(cam.getStationID(), cam)); + if (it != cams_.end()) + it->second = cam; // Key exists, update the value + else + cams_.insert(std::make_pair(cam.getStationID(), cam)); return; } -void CAMDisplay::update(float, float) -{ +void CAMDisplay::update(float, float) { //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Updating CAM display after " << wall_dt << " seconds, " << ros_dt << "seconds in ROS."); // Check for outdated CAMs - for (auto it = cams_.begin(); it != cams_.end(); ) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) - { - //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); - it = cams_.erase(it); - } - else - { - ++it; - } + for (auto it = cams_.begin(); it != cams_.end();) { + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { + //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); + it = cams_.erase(it); + } else { + ++it; + } } // Render all valid cams bboxs_.clear(); texts_.clear(); - for(auto it = cams_.begin(); it != cams_.end(); ++it) { - + for (auto it = cams_.begin(); it != cams_.end(); ++it) { CAMRenderObject cam = it->second; Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; @@ -158,8 +139,7 @@ void CAMDisplay::update(float, float) geometry_msgs::msg::Vector3 dimensions = cam.getDimensions(); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - if(3 <= cam.getStationType() && cam.getStationType() <= 11) - { + if (3 <= cam.getStationType() && cam.getStationType() <= 11) { // If the station type of the originating ITS-S is set to one out of the values 3 to 11 // the reference point shall be the ground position of the centre of the front side of // the bounding box of the vehicle. @@ -167,8 +147,8 @@ void CAMDisplay::update(float, float) tf2::Quaternion q; tf2::fromMsg(pose.orientation, q); tf2::Matrix3x3 m(q); - tf2::Vector3 v(-dimensions.x/2.0, 0.0, dimensions.z/2.0); - v = m*v; + tf2::Vector3 v(-dimensions.x / 2.0, 0.0, dimensions.z / 2.0); + v = m * v; position.x += v.x(); position.y += v.y(); position.z += v.z(); @@ -179,14 +159,15 @@ void CAMDisplay::update(float, float) child_scene_node->setOrientation(orientation); // create boundind-box object - std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); + std::shared_ptr bbox = + std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); // set the dimensions of bounding box Ogre::Vector3 dims; double scale = bb_scale_->getFloat(); - dims.x = dimensions.x*scale; - dims.y = dimensions.y*scale; - dims.z = dimensions.z*scale; + dims.x = dimensions.x * scale; + dims.y = dimensions.y * scale; + dims.z = dimensions.z * scale; bbox->setScale(dims); // set the color of bounding box Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); @@ -194,19 +175,20 @@ void CAMDisplay::update(float, float) bboxs_.push_back(bbox); // Visualize meta-information as text - if(show_meta_->getBool()) { + if (show_meta_->getBool()) { std::string text; - if(show_station_id_->getBool()) { - text+="StationID: " + std::to_string(cam.getStationID()); - text+="\n"; + if (show_station_id_->getBool()) { + text += "StationID: " + std::to_string(cam.getStationID()); + text += "\n"; } - if(show_speed_->getBool()) { - text+="Speed: " + std::to_string((int)(cam.getSpeed()*3.6)) + " km/h"; + if (show_speed_->getBool()) { + text += "Speed: " + std::to_string((int)(cam.getSpeed() * 3.6)) + " km/h"; } - if(!text.size()) return; - std::shared_ptr text_render = std::make_shared(text, "Liberation Sans", char_height_->getFloat()); + if (!text.size()) return; + std::shared_ptr text_render = + std::make_shared(text, "Liberation Sans", char_height_->getFloat()); double height = dims.z; - height+=text_render->getBoundingRadius(); + height += text_render->getBoundingRadius(); Ogre::Vector3 offs(0.0, 0.0, height); // There is a bug in rviz_rendering::MovableText::setGlobalTranslation https://github.com/ros2/rviz/issues/974 text_render->setGlobalTranslation(offs); From 28b13da324c43303e0916cc6c083536488ca1b63 Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 3 Jun 2024 14:51:51 +0000 Subject: [PATCH 3/9] Revert "Clamp instead of force wall time" This reverts commit 2df1ef8162c5df65dc44216b41832b35a9945a84. --- .../src/displays/CAM/cam_display.cpp | 136 ++++++++++-------- 1 file changed, 77 insertions(+), 59 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 9bb91b254..8c12d27ce 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -24,49 +24,63 @@ SOFTWARE. #include "displays/CAM/cam_display.hpp" -#include +#include +#include #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/bool_property.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/parse_color.hpp" -namespace etsi_its_msgs { -namespace displays { +namespace etsi_its_msgs +{ +namespace displays +{ -CAMDisplay::CAMDisplay() { +CAMDisplay::CAMDisplay() +{ // General Properties - buffer_timeout_ = - new rviz_common::properties::FloatProperty("Timeout", 0.1f, "Time (in s) until objects disappear", this); + buffer_timeout_ = new rviz_common::properties::FloatProperty( + "Timeout", 0.1f, + "Time (in s) until objects disappear", this); buffer_timeout_->setMin(0); - bb_scale_ = new rviz_common::properties::FloatProperty("Scale", 1.0f, "Scale of objects", this); + bb_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 1.0f, + "Scale of objects", this); bb_scale_->setMin(0.01); - color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Object color", this); - show_meta_ = - new rviz_common::properties::BoolProperty("Metadata", true, "Show metadata as text next to objects", this); - text_color_property_ = - new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Text color", show_meta_); + color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 0, 255), + "Object color", this); + show_meta_ = new rviz_common::properties::BoolProperty("Metadata", true, + "Show metadata as text next to objects", this); + text_color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 0, 255), + "Text color", show_meta_); char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); - show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, "Show StationID", show_meta_); - show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, "Show speed", show_meta_); + show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, + "Show StationID", show_meta_); + show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, + "Show speed", show_meta_); + } -CAMDisplay::~CAMDisplay() { - if (initialized()) { +CAMDisplay::~CAMDisplay() +{ + if (initialized() ) { scene_manager_->destroyManualObject(manual_object_); } } -void CAMDisplay::onInitialize() { +void CAMDisplay::onInitialize() +{ RTDClass::onInitialize(); auto nodeAbstraction = context_->getRosNodeAbstraction().lock(); @@ -77,49 +91,54 @@ void CAMDisplay::onInitialize() { scene_node_->attachObject(manual_object_); } -void CAMDisplay::reset() { +void CAMDisplay::reset() +{ RTDClass::reset(); manual_object_->clear(); } -void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) { +void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) +{ // Generate CAM render object from message - rclcpp::Time now = - rclcpp::Time(std::clamp(rviz_node_->now().nanoseconds(), etsi_its_cam_msgs::msg::TimestampIts::MIN, - etsi_its_cam_msgs::msg::TimestampIts::MAX)); + static rclcpp::Clock clock; + rclcpp::Time now = clock.now(); // Always use the current time for the render object, no sim time CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); if (!cam.validateFloats()) { - setStatus(rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; } // Check if Station ID is already present in list auto it = cams_.find(cam.getStationID()); - if (it != cams_.end()) - it->second = cam; // Key exists, update the value - else - cams_.insert(std::make_pair(cam.getStationID(), cam)); + if (it != cams_.end()) it->second = cam; // Key exists, update the value + else cams_.insert(std::make_pair(cam.getStationID(), cam)); return; } -void CAMDisplay::update(float, float) { +void CAMDisplay::update(float, float) +{ //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Updating CAM display after " << wall_dt << " seconds, " << ros_dt << "seconds in ROS."); // Check for outdated CAMs - for (auto it = cams_.begin(); it != cams_.end();) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { - //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); - it = cams_.erase(it); - } else { - ++it; - } + for (auto it = cams_.begin(); it != cams_.end(); ) { + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) + { + //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); + it = cams_.erase(it); + } + else + { + ++it; + } } // Render all valid cams bboxs_.clear(); texts_.clear(); - for (auto it = cams_.begin(); it != cams_.end(); ++it) { + for(auto it = cams_.begin(); it != cams_.end(); ++it) { + CAMRenderObject cam = it->second; Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; @@ -139,7 +158,8 @@ void CAMDisplay::update(float, float) { geometry_msgs::msg::Vector3 dimensions = cam.getDimensions(); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - if (3 <= cam.getStationType() && cam.getStationType() <= 11) { + if(3 <= cam.getStationType() && cam.getStationType() <= 11) + { // If the station type of the originating ITS-S is set to one out of the values 3 to 11 // the reference point shall be the ground position of the centre of the front side of // the bounding box of the vehicle. @@ -147,8 +167,8 @@ void CAMDisplay::update(float, float) { tf2::Quaternion q; tf2::fromMsg(pose.orientation, q); tf2::Matrix3x3 m(q); - tf2::Vector3 v(-dimensions.x / 2.0, 0.0, dimensions.z / 2.0); - v = m * v; + tf2::Vector3 v(-dimensions.x/2.0, 0.0, dimensions.z/2.0); + v = m*v; position.x += v.x(); position.y += v.y(); position.z += v.z(); @@ -159,15 +179,14 @@ void CAMDisplay::update(float, float) { child_scene_node->setOrientation(orientation); // create boundind-box object - std::shared_ptr bbox = - std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); + std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); // set the dimensions of bounding box Ogre::Vector3 dims; double scale = bb_scale_->getFloat(); - dims.x = dimensions.x * scale; - dims.y = dimensions.y * scale; - dims.z = dimensions.z * scale; + dims.x = dimensions.x*scale; + dims.y = dimensions.y*scale; + dims.z = dimensions.z*scale; bbox->setScale(dims); // set the color of bounding box Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); @@ -175,20 +194,19 @@ void CAMDisplay::update(float, float) { bboxs_.push_back(bbox); // Visualize meta-information as text - if (show_meta_->getBool()) { + if(show_meta_->getBool()) { std::string text; - if (show_station_id_->getBool()) { - text += "StationID: " + std::to_string(cam.getStationID()); - text += "\n"; + if(show_station_id_->getBool()) { + text+="StationID: " + std::to_string(cam.getStationID()); + text+="\n"; } - if (show_speed_->getBool()) { - text += "Speed: " + std::to_string((int)(cam.getSpeed() * 3.6)) + " km/h"; + if(show_speed_->getBool()) { + text+="Speed: " + std::to_string((int)(cam.getSpeed()*3.6)) + " km/h"; } - if (!text.size()) return; - std::shared_ptr text_render = - std::make_shared(text, "Liberation Sans", char_height_->getFloat()); + if(!text.size()) return; + std::shared_ptr text_render = std::make_shared(text, "Liberation Sans", char_height_->getFloat()); double height = dims.z; - height += text_render->getBoundingRadius(); + height+=text_render->getBoundingRadius(); Ogre::Vector3 offs(0.0, 0.0, height); // There is a bug in rviz_rendering::MovableText::setGlobalTranslation https://github.com/ros2/rviz/issues/974 text_render->setGlobalTranslation(offs); From ca4d8b4c4573ccfa09db79bbc1660538890d3970 Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 10 Jun 2024 08:56:38 +0000 Subject: [PATCH 4/9] Finally fix time issues --- .../src/displays/CAM/cam_display.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 8c12d27ce..0b103a8ba 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -100,9 +100,17 @@ void CAMDisplay::reset() void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) { // Generate CAM render object from message - static rclcpp::Clock clock; - rclcpp::Time now = clock.now(); // Always use the current time for the render object, no sim time - CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); + rclcpp::Time now = rviz_node_->now(); + uint64_t nanosecs = now.nanoseconds(); + if (nanosecs == 0) + { + setStatus( + rviz_common::properties::StatusProperty::Warn, "Topic", + "Message received before clock got a valid time"); + return; + } + + CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004(static_cast(now.seconds()))); if (!cam.validateFloats()) { setStatus( rviz_common::properties::StatusProperty::Error, "Topic", @@ -120,12 +128,10 @@ void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) void CAMDisplay::update(float, float) { - //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Updating CAM display after " << wall_dt << " seconds, " << ros_dt << "seconds in ROS."); // Check for outdated CAMs for (auto it = cams_.begin(); it != cams_.end(); ) { if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { - //RCLCPP_INFO_STREAM(rviz_node_->get_logger(), "Removing CAM with StationID " << it->first << " from display, age: " << it->second.getAge(rviz_node_->now()) << "."); it = cams_.erase(it); } else From 541efe90f12d7d5af4f653cbfddce328ca06b1de Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 10 Jun 2024 08:57:39 +0000 Subject: [PATCH 5/9] Cleanup --- .../src/displays/CAM/cam_display.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 0b103a8ba..51a24ac78 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -112,10 +112,10 @@ void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004(static_cast(now.seconds()))); if (!cam.validateFloats()) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; } // Check if Station ID is already present in list @@ -130,14 +130,14 @@ void CAMDisplay::update(float, float) { // Check for outdated CAMs for (auto it = cams_.begin(); it != cams_.end(); ) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) - { - it = cams_.erase(it); - } - else - { - ++it; - } + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) + { + it = cams_.erase(it); + } + else + { + ++it; + } } // Render all valid cams From 082ee0b310ae3f47843ec58efb74fd3f047e47e2 Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Mon, 10 Jun 2024 11:50:07 +0000 Subject: [PATCH 6/9] Fix float precision in OGRE --- .../src/displays/CAM/cam_display.cpp | 38 +++++++++++++++---- 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 51a24ac78..ce8c4e558 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -32,6 +32,7 @@ SOFTWARE. #include #include "rviz_common/display_context.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" #include "rviz_common/frame_manager_iface.hpp" #include "rviz_common/logging.hpp" #include "rviz_common/properties/color_property.hpp" @@ -130,12 +131,10 @@ void CAMDisplay::update(float, float) { // Check for outdated CAMs for (auto it = cams_.begin(); it != cams_.end(); ) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) - { + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { it = cams_.erase(it); } - else - { + else { ++it; } } @@ -149,21 +148,44 @@ void CAMDisplay::update(float, float) Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; if (!context_->getFrameManager()->getTransform(cam.getHeader(), sn_position, sn_orientation)) { + // Check if transform exists setMissingTransformToFixedFrame(cam.getHeader().frame_id); return; } + // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. + // So we get the transfrom manually from tf2: + std::string fixed_frame = fixed_frame_.toStdString(); + geometry_msgs::msg::PoseStamped pose_origin; + pose_origin.header = cam.getHeader(); + pose_origin.pose.position.x = 0; + pose_origin.pose.position.y = 0; + pose_origin.pose.position.z = 0; + pose_origin.pose.orientation.w = 1; + pose_origin.pose.orientation.x = 0; + pose_origin.pose.orientation.y = 0; + pose_origin.pose.orientation.z = 0; + geometry_msgs::msg::PoseStamped pose_fixed_frame = context_->getTransformationManager()->getCurrentTransformer()->transform(pose_origin, fixed_frame); + geometry_msgs::msg::TransformStamped transform_to_fixed_frame; + transform_to_fixed_frame.header = pose_fixed_frame.header; + transform_to_fixed_frame.transform.translation.x = pose_fixed_frame.pose.position.x; + transform_to_fixed_frame.transform.translation.y = pose_fixed_frame.pose.position.y; + transform_to_fixed_frame.transform.translation.z = pose_fixed_frame.pose.position.z; + transform_to_fixed_frame.transform.rotation = pose_fixed_frame.pose.orientation; + setTransformOk(); - // set pose of scene node - scene_node_->setPosition(sn_position); - scene_node_->setOrientation(sn_orientation); + // set pose of scene node to origin (=fixed frame)! + scene_node_->setPosition(Ogre::Vector3{0.0f, 0.0f, 0.0f}); + scene_node_->setOrientation(Ogre::Quaternion{1.0f, 0.0f, 0.0f, 0.0f}); auto child_scene_node = scene_node_->createChildSceneNode(); - // Set position of scene node + // Set position of scene node to the position relative to the fixed frame geometry_msgs::msg::Pose pose = cam.getPose(); geometry_msgs::msg::Vector3 dimensions = cam.getDimensions(); + tf2::doTransform(pose, pose, transform_to_fixed_frame); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if(3 <= cam.getStationType() && cam.getStationType() <= 11) { // If the station type of the originating ITS-S is set to one out of the values 3 to 11 From 005a43e85afa125b7fc17a317575d17e02c49921 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guido=20K=C3=BCppers?= Date: Wed, 10 Jul 2024 10:23:04 +0200 Subject: [PATCH 7/9] minor refactoring of denm plugin --- .../include/displays/DENM/denm_display.hpp | 3 +-- .../include/displays/DENM/denm_render_object.hpp | 2 +- .../src/displays/DENM/denm_display.cpp | 6 ------ .../src/displays/DENM/denm_render_object.cpp | 14 +++++++------- 4 files changed, 9 insertions(+), 16 deletions(-) diff --git a/etsi_its_rviz_plugins/include/displays/DENM/denm_display.hpp b/etsi_its_rviz_plugins/include/displays/DENM/denm_display.hpp index b4d904836..9d18125c0 100644 --- a/etsi_its_rviz_plugins/include/displays/DENM/denm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/DENM/denm_display.hpp @@ -1,6 +1,5 @@ #pragma once -#include "etsi_its_cam_msgs/msg/cam.hpp" #include "etsi_its_denm_msgs/msg/denm.hpp" #include "displays/DENM/denm_render_object.hpp" @@ -57,7 +56,7 @@ class DENMDisplay : public // Properties rviz_common::properties::BoolProperty *show_meta_, *show_station_id_, *show_cause_code_, *show_sub_cause_code_; - rviz_common::properties::FloatProperty *buffer_timeout_, *bb_scale_, *char_height_; + rviz_common::properties::FloatProperty *buffer_timeout_, *char_height_; rviz_common::properties::ColorProperty *color_property_, *text_color_property_; std::unordered_map denms_; diff --git a/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp b/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp index 470aaab51..804470b0c 100644 --- a/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp +++ b/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp @@ -22,7 +22,7 @@ namespace displays class DENMRenderObject { public: - DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, rclcpp::Time receive_time, uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second); + DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second); /** * @brief This function validates all float variables that are part of a DENMRenderObject diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp index a7167123f..d7c6f6df5 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp @@ -28,10 +28,6 @@ DENMDisplay::DENMDisplay() "Timeout", 0.1f, "Time (in s) until visualizations disappear", this); buffer_timeout_->setMin(0); - bb_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 1.0f, - "Scale of visualization", this); - bb_scale_->setMin(0.01); color_property_ = new rviz_common::properties::ColorProperty( "Color", QColor(255, 0, 25), "Color", this); @@ -139,8 +135,6 @@ void DENMDisplay::update(float, float) // create arrow object std::shared_ptr arrow = std::make_shared(scene_manager_, child_scene_node, shaft_length, shaft_diameter, head_length, head_diameter); - // set the dimensions of arrow - double scale = bb_scale_->getFloat(); // set the color of arrow Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); arrow->setColor(bb_color); diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp index d2349eaa7..efe979099 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp @@ -5,7 +5,7 @@ namespace etsi_its_msgs namespace displays { - DENMRenderObject::DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, rclcpp::Time receive_time, uint16_t n_leap_seconds) { + DENMRenderObject::DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, uint16_t n_leap_seconds) { int zone; bool northp; @@ -20,10 +20,10 @@ namespace displays sub_cause_code_type = etsi_its_denm_msgs::access::getSubCauseCodeType(denm); double heading; // 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - if(etsi_its_denm_msgs::access::getIsHeadingPresent(denm)){ + if(etsi_its_denm_msgs::access::getIsHeadingPresent(denm)) { heading = (90-etsi_its_denm_msgs::access::getHeading(denm))*M_PI/180.0; } - else{ + else { heading = 0*M_PI/180.0; } while(heading<0) heading+=2*M_PI; @@ -32,10 +32,10 @@ namespace displays orientation.setRPY(0.0, 0.0, heading); pose.orientation = tf2::toMsg(orientation); - if(etsi_its_denm_msgs::access::getIsSpeedPresent(denm)){ + if(etsi_its_denm_msgs::access::getIsSpeedPresent(denm)) { speed = etsi_its_denm_msgs::access::getSpeed(denm); } - else{ + else { speed = 0; } } @@ -67,11 +67,11 @@ namespace displays return speed; } - std::string DENMRenderObject::getCauseCode(){ + std::string DENMRenderObject::getCauseCode() { return cause_code_type; } - std::string DENMRenderObject::getSubCauseCode(){ + std::string DENMRenderObject::getSubCauseCode() { return sub_cause_code_type; } From 47907fd6d56f9a68887730fb4222cf904c03ad0b Mon Sep 17 00:00:00 2001 From: Fabian Thomsen Date: Wed, 10 Jul 2024 08:25:05 +0000 Subject: [PATCH 8/9] Apply 2 suggestion(s) to 1 file(s) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Guido Küppers --- etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index ce8c4e558..5646ea49c 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -103,11 +103,10 @@ void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg) // Generate CAM render object from message rclcpp::Time now = rviz_node_->now(); uint64_t nanosecs = now.nanoseconds(); - if (nanosecs == 0) - { + if (nanosecs == 0) { setStatus( - rviz_common::properties::StatusProperty::Warn, "Topic", - "Message received before clock got a valid time"); + rviz_common::properties::StatusProperty::Warn, "Topic", + "Message received before clock got a valid time"); return; } From befc76e9dde61aaa56f0032ba29a6149aab86b09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Guido=20K=C3=BCppers?= Date: Wed, 10 Jul 2024 10:46:55 +0200 Subject: [PATCH 9/9] fix denm plugin --- etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp index d7c6f6df5..c7012a063 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp @@ -72,7 +72,7 @@ void DENMDisplay::processMessage(etsi_its_denm_msgs::msg::DENM::ConstSharedPtr m { // Generate DENM render object from message rclcpp::Time now = rviz_node_->now(); - DENMRenderObject denm(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); // 5 leap seconds in 2023 + DENMRenderObject denm(*msg, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); // 5 leap seconds in 2023 if (!denm.validateFloats()) { setStatus( rviz_common::properties::StatusProperty::Error, "Topic",