cleanup, formatting

etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp

@@ -60,5 +60,32 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) {
   EXPECT_NEAR(longitude, cpm_ts_access::getLongitude(cpm), 1e-7);
   EXPECT_NEAR(altitude, cpm_ts_access::getAltitude(cpm), 1e-2);
 
-  // TODO: check object list access
+  etsi_its_cpm_ts_msgs::msg::PerceivedObject object;
+  gm::Vector3 dimensions;
+  dimensions.x = randomDouble(0.1, 25.6);
+  dimensions.y = randomDouble(0.1, 25.6);
+  dimensions.z = randomDouble(0.1, 25.6);
+  cpm_ts_access::setDimensionsOfPerceivedObject(object, dimensions);
+  EXPECT_NEAR(dimensions.x, cpm_ts_access::getDimensionsOfPerceivedObject(object).x, 1e-1);
+  EXPECT_NEAR(dimensions.y, cpm_ts_access::getDimensionsOfPerceivedObject(object).y, 1e-1);
+  EXPECT_NEAR(dimensions.z, cpm_ts_access::getDimensionsOfPerceivedObject(object).z, 1e-1);
+
+  gm::Vector3 velocity;
+  velocity.x = randomDouble(-163.83, 163.83);
+  velocity.y = randomDouble(-163.83, 163.83);
+  velocity.z = randomDouble(-163.83, 163.83);
+  cpm_ts_access::setVelocityOfPerceivedObject(object, velocity);
+  EXPECT_NEAR(velocity.x, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).x, 1e-2);
+  EXPECT_NEAR(velocity.y, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).y, 1e-2);
+  EXPECT_NEAR(velocity.z, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).z, 1e-2);
+
+  gm::Vector3 acceleration;
+  acceleration.x = randomDouble(-16.0, 16.0);
+  acceleration.y = randomDouble(-16.0, 16.0);
+  acceleration.z = randomDouble(-16.0, 16.0);
+  cpm_ts_access::setAccelerationOfPerceivedObject(object, acceleration);
+  EXPECT_NEAR(acceleration.x, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).x, 1e-1);
+  EXPECT_NEAR(acceleration.y, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).y, 1e-1);
+  EXPECT_NEAR(acceleration.z, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).z, 1e-1);
+
 }

etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp

@@ -34,47 +34,40 @@ SOFTWARE.
 
 #include <rclcpp/rclcpp.hpp>
 
-namespace Ogre
-{
+namespace Ogre {
 class ManualObject;
 }
 
-namespace rviz_common
-{
-namespace properties
-{
-  class ColorProperty;
-  class FloatProperty;
+namespace rviz_common {
+namespace properties {
+class ColorProperty;
+class FloatProperty;
 }  // namespace properties
 }  // namespace rviz_common
 
-namespace etsi_its_msgs
-{
-namespace displays
-{
+namespace etsi_its_msgs {
+namespace displays {
 
 /**
  * @class CPMDisplay
  * @brief Displays an etsi_its_cpm_msgs::CollectivePerceptionMessage
  */
-class CPMDisplay : public
-  rviz_common::RosTopicDisplay<etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage>
-{
+class CPMDisplay : public rviz_common::RosTopicDisplay<etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage> {
   Q_OBJECT
 
-public:
+ public:
   CPMDisplay();
   ~CPMDisplay() override;
 
   void onInitialize() override;
 
   void reset() override;
 
-protected:
+ protected:
   void processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) override;
   void update(float wall_dt, float ros_dt) override;
 
-  Ogre::ManualObject * manual_object_;
+  Ogre::ManualObject *manual_object_;
 
   rclcpp::Node::SharedPtr rviz_node_;
 
@@ -88,9 +81,6 @@ class CPMDisplay : public
   // Rendering objects
   std::vector<std::shared_ptr<CPMRenderObject>> cpm_render_objects_;
 
-  //vector of Poses
-  std::vector<geometry_msgs::msg::Pose> poses_;
-
   std::vector<std::shared_ptr<rviz_rendering::Shape>> bboxs_;
   std::vector<std::shared_ptr<rviz_rendering::MovableText>> texts_;
 };

etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp

@@ -45,9 +45,14 @@ namespace displays
 class CPMRenderObject
 {
   public:
-    // CPMRenderObject() {
-    //     // Initialize members with default values if necessary
-    // }
+    /**
+     * @brief Construct a new CPMRenderObject object
+     *
+     * @param cpm
+     * @param receive_time
+     * @param n_leap_seconds
+     * @param number_of_object
+     */
     CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second, uint8_t number_of_object=0);
 
     /**
@@ -99,18 +104,20 @@ class CPMRenderObject
      */
     geometry_msgs::msg::Vector3 getVelocity();
 
+    /**
+     * @brief Get number of objects in CPM-object
+     *
+     * @return uint8_t
+     */
     uint8_t getNumberOfObjects();
 
   private:
     // member variables
-    std_msgs::msg::Header header;
-    uint8_t number_of_objects_;
-    uint32_t station_id;
-    int station_type;
-    geometry_msgs::msg::Pose pose;
-    geometry_msgs::msg::Vector3 dimensions;
-    geometry_msgs::msg::Vector3 velocity;
-    geometry_msgs::msg::Quaternion orientation;
+    std_msgs::msg::Header header_;
+    uint32_t station_id_;
+    geometry_msgs::msg::Pose pose_;
+    geometry_msgs::msg::Vector3 dimensions_;
+    geometry_msgs::msg::Vector3 velocity_;
 
 };
 

etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp

@@ -131,11 +131,6 @@ void CPMDisplay::update(float, float) {
   bboxs_.clear();
   texts_.clear();
   for (auto it = cpms_.begin(); it != cpms_.end(); ++it) {
-    //info logger for cpms size
-    RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Size of cpms: %d", cpms_.size());
-
-    //logger for cpm_render_objects size
-    RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Size of cpm_render_objects: %d", cpm_render_objects_.size());
     for (const auto& cpm_ptr : cpm_render_objects_) {
       // Dereference the shared_ptr to access the CPMRenderObject
       CPMRenderObject& cpm = *cpm_ptr;
@@ -153,8 +148,6 @@ void CPMDisplay::update(float, float) {
       std::string fixed_frame = fixed_frame_.toStdString();
       geometry_msgs::msg::PoseStamped pose_origin;
       pose_origin.header = cpm.getHeader();
-      //logger for the header frame id
-      RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Header frame id: %s", cpm.getHeader().frame_id.c_str());
       pose_origin.pose.position.x = 0;
       pose_origin.pose.position.y = 0;
       pose_origin.pose.position.z = 0;
@@ -214,7 +207,10 @@ void CPMDisplay::update(float, float) {
           text += "\n";
         }
         if (show_speed_->getBool()) {
-          // text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h";
+          geometry_msgs::msg::Vector3 velocity = cpm.getVelocity();
+          //get magnitude of velocity
+          double speed = sqrt(pow(velocity.x, 2) + pow(velocity.y, 2) + pow(velocity.z, 2));
+          text += "Speed: " + std::to_string((int)(speed * 3.6)) + " km/h";
         }
         if (!text.size()) return;
         std::shared_ptr<rviz_rendering::MovableText> text_render =

etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp

@@ -33,45 +33,39 @@ CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerception
     etsi_its_cpm_ts_msgs::msg::PerceivedObject object = etsi_its_cpm_ts_msgs::access::getPerceivedObject(
         etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(cpm), number_of_object);
 
-    pose.position = etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject(object);
-    pose.orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(object);
-    dimensions = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object);
-    velocity = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object);
-    //header.frame_id = position.header.frame_id;
-    header.frame_id = "map";
+    geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, object);
 
-    uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm));
+    pose_.position = utm_position.point;
+    pose_.orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(object);
+    dimensions_ = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object);
+    velocity_ = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object);
 
-    header.stamp = rclcpp::Time(nanosecs);
+    header_.frame_id =  utm_position.header.frame_id;
+    uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm));
+    header_.stamp = rclcpp::Time(nanosecs);
 
-    uint32_t station_id = etsi_its_cpm_ts_msgs::access::getStationID(cpm);
-    //hardcoded station_id to 10 for testing
-    station_id = 10;
+    station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm);
 }
 
 bool CPMRenderObject::validateFloats() {
   bool valid = true;
-  valid = valid && rviz_common::validateFloats(pose);
-  valid = valid && rviz_common::validateFloats(dimensions);
-  valid = valid && rviz_common::validateFloats(velocity);
+  valid = valid && rviz_common::validateFloats(pose_);
+  valid = valid && rviz_common::validateFloats(dimensions_);
+  valid = valid && rviz_common::validateFloats(velocity_);
   return valid;
 }
 
-double CPMRenderObject::getAge(rclcpp::Time now) { return (now - header.stamp).seconds(); }
+double CPMRenderObject::getAge(rclcpp::Time now) { return (now - header_.stamp).seconds(); }
 
-std_msgs::msg::Header CPMRenderObject::getHeader() { return header; }
+std_msgs::msg::Header CPMRenderObject::getHeader() { return header_; }
 
-uint32_t CPMRenderObject::getStationID() { return station_id; }
+uint32_t CPMRenderObject::getStationID() { return station_id_; }
 
-geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose; }
-
-uint8_t CPMRenderObject::getNumberOfObjects() {
-  return number_of_objects_; 
-}
+geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose_; }
 
-geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions; }
+geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions_; }
 
-geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity; }
+geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity_; }
 
 }  // namespace displays
 }  // namespace etsi_its_msgs