Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix timestamp issues for rendering sensors (indigo-devel) #551

Merged
merged 11 commits into from
Feb 28, 2017
Merged

Fix timestamp issues for rendering sensors (indigo-devel) #551

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
d92a409
Fix for issue #408. The last measurement time is the time that gazebo…
lucasw Mar 18, 2016
42ad400
#408 Making a test for multicamra that shows the timestamps are curre…
lucasw Mar 23, 2016
5fa7cc8
#408 Make the multi camera timestamps current rather than outdated, a…
lucasw Mar 23, 2016
d8cb647
Adding depth camera world to use in test to make depth camera have ri…
lucasw Mar 29, 2016
61cb624
Disabling this test because of #409
lucasw Mar 29, 2016
2bf30a1
#408 Created test for depth camera, which fails, so next make it pass
lucasw Mar 30, 2016
b3265d8
#408 also test points publication
lucasw Mar 30, 2016
113bdfd
#408 fix timestamps for depth image, test passes now
lucasw Mar 30, 2016
e916337
#408 Increasing max time because some systems are taking 0.6 seconds …
lucasw Apr 28, 2016
0d23e1f
Adding @j-rivero default disabling of tests that require a display
lucasw Oct 18, 2016
87c4dbf
change LastUpdateTime to LastMeasurementTime
iche033 Feb 10, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 19 additions & 0 deletions gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_plugins)

option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF)

find_package(catkin REQUIRED COMPONENTS
message_generation
gazebo_msgs
Expand Down Expand Up @@ -357,11 +359,28 @@ install(DIRECTORY test
)

# Tests
# These need to be run with -j1 flag because gazebo can't be run
# in parallel.
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(set_model_state-test
test/set_model_state_test/set_model_state_test.test
test/set_model_state_test/set_model_state_test.cpp)
add_rostest(test/range/range_plugin.test)
target_link_libraries(set_model_state-test ${catkin_LIBRARIES})

if (ENABLE_DISPLAY_TESTS)
add_rostest_gtest(depth_camera-test
test/camera/depth_camera.test
test/camera/depth_camera.cpp)
target_link_libraries(depth_camera-test ${catkin_LIBRARIES})
add_rostest_gtest(multicamera-test
test/camera/multicamera.test
test/camera/multicamera.cpp)
target_link_libraries(multicamera-test ${catkin_LIBRARIES})
add_rostest_gtest(camera-test
test/camera/camera.test
test/camera/camera.cpp)
target_link_libraries(camera-test ${catkin_LIBRARIES})
endif()
endif()
2 changes: 2 additions & 0 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
20 changes: 13 additions & 7 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,9 +77,9 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
const std::string &_format)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
# endif

if (!this->parentSensor->IsActive())
Expand All @@ -92,12 +92,18 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
{
if ((*this->image_connect_count_) > 0)
{
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_update_time_ >= this->update_period_)
// OnNewFrame is triggered at the gazebo sensor <update_rate>
// while there is also a plugin <updateRate> that can throttle the
// rate down further (but then why not reduce the sensor rate?
// what is the use case?).
// Setting the <updateRate> to zero will make this plugin
// update at the gazebo sensor <update_rate>, update_period_ will be
// zero and the conditional always will be true.
if (sensor_update_time - this->last_update_time_ >= this->update_period_)
{
this->PutCameraData(_image);
this->PublishCameraInfo();
this->last_update_time_ = cur_time;
this->PutCameraData(_image, sensor_update_time);
this->PublishCameraInfo(sensor_update_time);
this->last_update_time_ = sensor_update_time;
}
}
}
Expand Down
9 changes: 4 additions & 5 deletions gazebo_plugins/src/gazebo_ros_camera_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -626,15 +626,14 @@ void GazeboRosCameraUtils::PublishCameraInfo()
if (this->camera_info_pub_.getNumSubscribers() > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_info_update_time_ >= this->update_period_)
if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_)
{
this->PublishCameraInfo(this->camera_info_pub_);
this->last_info_update_time_ = cur_time;
this->last_info_update_time_ = this->sensor_update_time_;
}
}
}
Expand Down
28 changes: 16 additions & 12 deletions gazebo_plugins/src/gazebo_ros_depth_camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,9 +192,9 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif
if (this->parentSensor->IsActive())
{
Expand Down Expand Up @@ -232,9 +232,9 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif
if (!this->parentSensor->IsActive())
{
Expand Down Expand Up @@ -302,9 +302,9 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,

//ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif

if (!this->parentSensor->IsActive())
Expand All @@ -316,7 +316,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
else
{
if ((*this->image_connect_count_) > 0)
{
this->PutCameraData(_image);
// TODO(lucasw) publish camera info with depth image
// this->PublishCameraInfo(sensor_update_time);
}
}
}

Expand Down Expand Up @@ -502,15 +506,15 @@ void GazeboRosDepthCamera::PublishCameraInfo()
if (this->depth_info_connect_count_ > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
this->sensor_update_time_ = sensor_update_time;
if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
{
this->PublishCameraInfo(this->depth_image_camera_info_pub_);
this->last_depth_image_camera_info_update_time_ = cur_time;
this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time);
this->last_depth_image_camera_info_update_time_ = sensor_update_time;
}
}
}
Expand Down
47 changes: 18 additions & 29 deletions gazebo_plugins/src/gazebo_ros_multicamera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,52 +102,41 @@ 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]);
}
}
17 changes: 8 additions & 9 deletions gazebo_plugins/src/gazebo_ros_openni_kinect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,9 +189,9 @@ void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
# endif
if (this->parentSensor->IsActive())
{
Expand Down Expand Up @@ -231,9 +231,9 @@ void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image,

//ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
# endif

if (this->parentSensor->IsActive())
Expand Down Expand Up @@ -447,15 +447,14 @@ void GazeboRosOpenniKinect::PublishCameraInfo()
if (this->depth_info_connect_count_ > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
if (this->sensor_update_time_ - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
{
this->PublishCameraInfo(this->depth_image_camera_info_pub_);
this->last_depth_image_camera_info_update_time_ = cur_time;
this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_;
}
}
}
Expand Down
21 changes: 10 additions & 11 deletions gazebo_plugins/src/gazebo_ros_prosilica.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,9 +132,9 @@ void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image,
// should do nothing except turning camera on/off, as we are using service.
/// @todo: consider adding thumbnailing feature here if subscribed.
# if GAZEBO_MAJOR_VERSION >= 7
common::Time sensor_update_time = this->parentSensor_->LastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
# else
common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime();
common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
# endif

// as long as ros is connected, parent is active
Expand All @@ -153,12 +153,11 @@ void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image,
{
if ((*this->image_connect_count_) > 0)
{
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_update_time_ >= this->update_period_)
if (sensor_update_time - this->last_update_time_ >= this->update_period_)
{
this->PutCameraData(_image, sensor_update_time);
this->PublishCameraInfo(sensor_update_time);
this->last_update_time_ = cur_time;
this->last_update_time_ = sensor_update_time;
}
}
}
Expand Down Expand Up @@ -227,9 +226,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiCameraInfoMsg->header.frame_id = this->frame_name_;

# if GAZEBO_MAJOR_VERSION >= 7
common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime();
common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
# else
common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime();
common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime();
# endif
this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec;
this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec;
Expand Down Expand Up @@ -285,9 +284,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->image_msg_.header.frame_id = this->frame_name_;

# if GAZEBO_MAJOR_VERSION >= 7
common::Time lastRenderTime = this->parentSensor_->LastUpdateTime();
common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime();
# else
common::Time lastRenderTime = this->parentSensor_->GetLastUpdateTime();
common::Time lastRenderTime = this->parentSensor_->GetLastMeasurementTime();
# endif
this->image_msg_.header.stamp.sec = lastRenderTime.sec;
this->image_msg_.header.stamp.nsec = lastRenderTime.nsec;
Expand All @@ -313,9 +312,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiImageMsg = &image;
this->roiImageMsg->header.frame_id = this->frame_name_;
# if GAZEBO_MAJOR_VERSION >= 7
common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime();
common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
# else
common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime();
common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime();
# endif
this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec;
this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec;
Expand Down
Loading