fix handling of empty object list in CPM plugin

ika-rwth-aachen · Oct 11, 2024 · f07fba5 · f07fba5
1 parent c2fb23f
commit f07fba5
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 8 deletions.
diff --git a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp
@@ -94,7 +94,7 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM
   uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(
       etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg));
 
-  CPMRenderObject cpm(*msg, 0); // TODO: do we need this?
+  CPMRenderObject cpm(*msg, 0); // TODO: do not use render object here; can be done with StationId and ReferenceTime
 
   cpm_render_objects_.clear();
 

diff --git a/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp b/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp
@@ -28,22 +28,23 @@ namespace etsi_its_msgs {
 namespace displays {
 
 CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object) {
-                                
+  try {
     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);
-
     geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, object);
-
     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_.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);
+  } catch(const std::exception& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("CPMRenderObject"), "Unavailable PerceivedObject from CPM: %s", e.what());
+  }
+
+  uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm));
+  header_.stamp = rclcpp::Time(nanosecs);
 
-    station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm);
+  station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm);
 }
 
 bool CPMRenderObject::validateFloats() {