Show link names in inertia error message (#874)

* Show link names in inertia error message * Suppress warning message for base link and static links Signed-off-by: Timon Engelke <timon.engelke@online.de>
ros2 · Aug 31, 2022 · 4df5aa3 · 4df5aa3
1 parent 4e976dc
commit 4df5aa3
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp b/rviz_default_plugins/src/rviz_default_plugins/robot/robot_link.cpp
@@ -885,9 +885,12 @@ void RobotLink::createInertia(const urdf::LinkConstSharedPtr & link)
     ignition::math::Quaterniond box_rot;
     if (!mass_matrix.EquivalentBox(box_scale, box_rot)) {
       // Invalid inertia, load with default scale
-      RVIZ_COMMON_LOG_ERROR_STREAM(
-        "The link is static or has unrealistic "
-        "inertia, so the equivalent inertia box will not be shown.\n");
+      if (link->parent_joint && link->parent_joint->type != urdf::Joint::FIXED) {
+        // Do not show error message for base link or static links
+        RVIZ_COMMON_LOG_ERROR_STREAM(
+          "The link " << link->name << " is has unrealistic "
+            "inertia, so the equivalent inertia box will not be shown.\n");
+      }
       return;
     }
     Ogre::Vector3 translate(