Merge pull request #25 from FabianThomsen/fix/offset

Fix offset in CAM visualization

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<int, DENMRenderObject> denms_;

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

etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp

@@ -32,6 +32,7 @@ SOFTWARE.
 #include <OgreTechnique.h>
 
 #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"
@@ -101,12 +102,20 @@ void CAMDisplay::processMessage(etsi_its_cam_msgs::msg::CAM::ConstSharedPtr msg)
 {
   // Generate CAM render object from message
   rclcpp::Time now = rviz_node_->now();
-  CAMRenderObject cam(*msg, now, getLeapSecondInsertionsSince2004((uint64_t)now.seconds()));
+  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<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
@@ -121,8 +130,12 @@ 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
@@ -134,29 +147,58 @@ 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
       // 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

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);
@@ -76,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",
@@ -139,8 +135,6 @@ void DENMDisplay::update(float, float)
     // create arrow object
     std::shared_ptr<rviz_rendering::Arrow> arrow = std::make_shared<rviz_rendering::Arrow>(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);

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;
   }