[bugfix] Prevent rviz from crashing if a Point3D is NaN

jrl-umi3218 · Dec 12, 2023 · 9b8d842 · 9b8d842
1 parent 6a2f05b
commit 9b8d842
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 2 deletions.
diff --git a/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h
@@ -1,5 +1,6 @@
 #pragma once
 #include "InteractiveMarkerWidget.h"
+#include "utils.h"
 
 namespace mc_rtc_rviz
 {
@@ -25,7 +26,16 @@ struct TransformInteractiveMarkerWidget : public InteractiveMarkerWidget
 
   void handleRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) override;
 
-  void update(const Eigen::Vector3d & t) { marker_.update(t); }
+  void update(const Eigen::Vector3d & t)
+  {
+    if(is_nan(t))
+    {
+      mc_rtc::log::error("Could not update marker {}: invalid value in coordinates ({})", id2name(request_id_),
+                         t.transpose());
+      return;
+    }
+    marker_.update(t);
+  }
   void update(const sva::PTransformd & pos) { marker_.update(pos); }
 
 protected:

diff --git a/mc_rtc_rviz_panel/src/utils.cpp b/mc_rtc_rviz_panel/src/utils.cpp
@@ -230,7 +230,7 @@ vm::InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly)
   return int_marker;
 }
 
-visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & /*pos*/,
+visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & pos,
                                           const mc_rtc::gui::Color & color,
                                           double scale = 0.02)
 {
@@ -244,6 +244,13 @@ visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & /*pos*/,
   m.color.r = static_cast<float>(color.r);
   m.color.g = static_cast<float>(color.g);
   m.color.b = static_cast<float>(color.b);
+  m.pose.orientation.w = 1.0;
+  m.pose.orientation.x = 0.0;
+  m.pose.orientation.y = 0.0;
+  m.pose.orientation.z = 0.0;
+  m.pose.position.x = pos.x();
+  m.pose.position.y = pos.y();
+  m.pose.position.z = pos.z();
   return m;
 }
 

diff --git a/mc_rtc_rviz_panel/src/utils.h b/mc_rtc_rviz_panel/src/utils.h
@@ -16,6 +16,13 @@ namespace vm = visualization_msgs;
 namespace mc_rtc_rviz
 {
 
+// Check if at least one values in x.array() is nan
+template<typename Derived>
+inline bool is_nan(const Eigen::MatrixBase<Derived> & x)
+{
+  return x.array().isNaN().any();
+}
+
 template<typename Derived>
 inline bool is_in_range(const Eigen::MatrixBase<Derived> & x, double min = -10e10, double max = 10e10)
 {