From 34b45d09c129b355a258c45d9bc58bc1daacc318 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:52:53 -0700 Subject: [PATCH] #408 Make the multi camera timestamps current rather than outdated, also reuse the same update code --- .../gazebo_plugins/gazebo_ros_multicamera.h | 2 + gazebo_plugins/src/gazebo_ros_multicamera.cpp | 46 ++++++++----------- 2 files changed, 20 insertions(+), 28 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h index 233194551..aa9970541 100644 --- 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 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 index 6a9a1f941..248a252be 100644 --- 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]); } }