Revert "Smoothly move an Odometry's path given a moving frame_id (ros…

…-visualization#1631)"

This reverts commit a6e58df.

src/rviz/default_plugin/odometry_display.cpp

@@ -299,11 +299,16 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message
     }
   }
 
-  Ogre::Vector3 position(message->pose.pose.position.x, message->pose.pose.position.y,
-                         message->pose.pose.position.z);
-  Ogre::Quaternion orientation =
-      Ogre::Quaternion(message->pose.pose.orientation.w, message->pose.pose.orientation.x,
-                       message->pose.pose.orientation.y, message->pose.pose.orientation.z);
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position, orientation))
+  {
+    ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
+              message->header.frame_id.c_str(), qPrintable(fixed_frame_));
+    return;
+  }
+
+  // If we arrive here, we're good. Continue...
 
   // Create a scene node, and attach the arrow and the covariance to it
   Axes* axes = new Axes(scene_manager_, scene_node_, axes_length_property_->getFloat(),
@@ -367,22 +372,6 @@ void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
 
   assert(arrows_.size() == axes_.size());
   assert(axes_.size() == covariance_property_->sizeVisual());
-
-  // applyTransforms
-  if (!last_used_message_)
-    return;
-
-  Ogre::Vector3 position;
-  Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->getTransform(last_used_message_->header.frame_id, ros::Time(),
-                                                 position, orientation))
-  {
-    // the error output with setStatus is already generated by MessageFilterDisplay
-    return;
-  }
-
-  scene_node_->setPosition(position);
-  scene_node_->setOrientation(orientation);
 }
 
 void OdometryDisplay::reset()