Adding option to transform map based on header timestamp (#1066)

* Adding option to transform map based on header timestamp * Adding map_display.h to PR * Adding second attempt to transform if the attempt with the correct time stamp fails
ros-visualization · Apr 29, 2017 · 3cb7b0d · 3cb7b0d
1 parent 54a1e95
commit 3cb7b0d
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 3 deletions.
diff --git a/src/rviz/default_plugin/map_display.cpp b/src/rviz/default_plugin/map_display.cpp
@@ -117,6 +117,11 @@ MapDisplay::MapDisplay()
                                            "Prefer UDP topic transport",
                                            this,
                                            SLOT( updateTopic() ));
+
+  transform_timestamp_property_ = new BoolProperty( "Use Timestamp", false,
+                                                    "Use map header timestamp when transforming",
+                                                    this,
+                                                    SLOT( transformMap() ));
 }
 
 MapDisplay::~MapDisplay()
@@ -702,9 +707,17 @@ void MapDisplay::transformMap()
     return;
   }
 
+  ros::Time transform_time;
+
+  if (transform_timestamp_property_->getBool())
+  {
+    transform_time = current_map_.header.stamp;
+  }
+
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_.info.origin, position, orientation))
+  if (!context_->getFrameManager()->transform(frame_, transform_time, current_map_.info.origin, position, orientation) &&
+      !context_->getFrameManager()->transform(frame_, ros::Time(0), current_map_.info.origin, position, orientation))
   {
     ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'",
                qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ ));

diff --git a/src/rviz/default_plugin/map_display.h b/src/rviz/default_plugin/map_display.h
@@ -96,6 +96,7 @@ protected Q_SLOTS:
   void updatePalette();
   /** @brief Show current_map_ in the scene. */
   void showMap();
+  void transformMap();
 
 protected:
   // overrides from Display
@@ -114,8 +115,6 @@ protected Q_SLOTS:
 
   void clear();
 
-  void transformMap();
-
   Ogre::ManualObject* manual_object_;
   Ogre::TexturePtr texture_;
   std::vector<Ogre::TexturePtr> palette_textures_;
@@ -144,6 +143,7 @@ protected Q_SLOTS:
   EnumProperty* color_scheme_property_;
 
   BoolProperty* unreliable_property_;
+  BoolProperty* transform_timestamp_property_;
 };
 
 } // namespace rviz