apply origin rotation to inertia box visualization (#1171)

Signed-off-by: Jonas Otto <[email protected]>
ros2 · Mar 20, 2024 · 2273179 · 2273179
1 parent 175110b
commit 2273179
Showing 1 changed file with 7 additions and 1 deletion.
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
@@ -897,8 +897,14 @@ void RobotLink::createInertia(const urdf::LinkConstSharedPtr & link)
       link->inertial->origin.position.x,
       link->inertial->origin.position.y,
       link->inertial->origin.position.z);
+
+    double x, y, z, w;
+    link->inertial->origin.rotation.getQuaternion(x, y, z, w);
+    Ogre::Quaternion originRotate(w, x, y, z);
+
     Ogre::Quaternion rotate(box_rot.W(), box_rot.X(), box_rot.Y(), box_rot.Z());
-    Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(translate, rotate);
+    Ogre::SceneNode * offset_node = inertia_node_->createChildSceneNode(
+      translate, originRotate * rotate);
     inertia_shape_ = new Shape(Shape::Cube, scene_manager_, offset_node);
 
     inertia_shape_->setColor(1, 0, 0, 1);