#408 Make the multi camera timestamps current rather than outdated, a…

…lso reuse the same update code
ros-simulation · Feb 16, 2017 · 5fa7cc8 · 5fa7cc8
1 parent 42ad400
commit 5fa7cc8
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 28 deletions.
diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h
@@ -42,6 +42,8 @@ namespace gazebo
 
     std::vector<GazeboRosCameraUtils*> utils;
 
+    protected: void OnNewFrame(const unsigned char *_image,
+                   GazeboRosCameraUtils* util);
     /// \brief Update the controller
     /// FIXME: switch to function vectors
     protected: virtual void OnNewFrameLeft(const unsigned char *_image,

diff --git a/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_plugins/src/gazebo_ros_multicamera.cpp
@@ -102,52 +102,42 @@ void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-// Update the controller
-void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
-    unsigned int _width, unsigned int _height, unsigned int _depth,
-    const std::string &_format)
+
+void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
+    GazeboRosCameraUtils* util)
 {
   GazeboRosCameraUtils* util = this->utils[0];
 # if GAZEBO_MAJOR_VERSION >= 7
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
+  common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
 # else
-  util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
+  common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
 # endif
 
   if (util->parentSensor_->IsActive())
   {
-    common::Time cur_time = util->world_->GetSimTime();
-    if (cur_time - util->last_update_time_ >= util->update_period_)
+    if (sensor_update_time - util->last_update_time_ >= util->update_period_)
     {
-      util->PutCameraData(_image);
-      util->PublishCameraInfo();
-      util->last_update_time_ = cur_time;
+      util->PutCameraData(_image, sensor_update_time);
+      util->PublishCameraInfo(sensor_update_time);
+      util->last_update_time_ = sensor_update_time;
     }
   }
 }
 
+// Update the controller
+void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
+    unsigned int _width, unsigned int _height, unsigned int _depth,
+    const std::string &_format)
+{
+  OnNewFrame(_image, this->utils[0]);
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 // Update the controller
 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
     unsigned int _width, unsigned int _height, unsigned int _depth,
     const std::string &_format)
 {
-  GazeboRosCameraUtils* util = this->utils[1];
-# if GAZEBO_MAJOR_VERSION >= 7
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
-# else
-  util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
-# endif
-
-  if (util->parentSensor_->IsActive())
-  {
-    common::Time cur_time = util->world_->GetSimTime();
-    if (cur_time - util->last_update_time_ >= util->update_period_)
-    {
-      util->PutCameraData(_image);
-      util->PublishCameraInfo();
-      util->last_update_time_ = cur_time;
-    }
-  }
+  OnNewFrame(_image, this->utils[1]);
 }
 }