[jsk_rviz_plugins/BoundingBoxArray] Fix coords orientation.

closes jsk-ros-pkg#528
garaemon · Dec 5, 2015 · 9c46b66 · 9c46b66
1 parent 59fdbd6
commit 9c46b66
Showing 1 changed file with 9 additions and 11 deletions.
diff --git a/jsk_rviz_plugins/src/bounding_box_array_display.cpp b/jsk_rviz_plugins/src/bounding_box_array_display.cpp
@@ -306,17 +306,15 @@ namespace jsk_rviz_plugins
       ShapePtr shape = shapes_[i];
       Ogre::Vector3 position;
       Ogre::Quaternion quaternion;
-      if(!context_->getFrameManager()->transform(box.header, box.pose,
-                                                 position,
-                                                 quaternion)) {
-        ROS_ERROR( "Error transforming pose"
-                   "'%s' from frame '%s' to frame '%s'",
-                   qPrintable( getName() ), box.header.frame_id.c_str(),
-                   qPrintable( fixed_frame_ ));
-        return;                 // return?
-      }
-      shape->setPosition(position);
-      shape->setOrientation(quaternion);
+      Ogre::Vector3 p(box.pose.position.x,
+                      box.pose.position.y,
+                      box.pose.position.z);
+      Ogre::Quaternion q(box.pose.orientation.w,
+                         box.pose.orientation.x,
+                         box.pose.orientation.y,
+                         box.pose.orientation.z);
+      shape->setPosition(p);
+      shape->setOrientation(q);
       Ogre::Vector3 dimensions;
       dimensions[0] = box.dimensions.x;
       dimensions[1] = box.dimensions.y;