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 #538

Merged
merged 12 commits into from
Feb 14, 2017
Merged

Fix timestamp issues for rendering sensors #538

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
d00bd47
Fix for issue #408. The last measurement time is the time that gazeb…
lucasw Mar 18, 2016
3fb2fea
#408 Making a test for multicamra that shows the timestamps are curre…
lucasw Mar 23, 2016
34b45d0
#408 Make the multi camera timestamps current rather than outdated, a…
lucasw Mar 23, 2016
d248877
Adding depth camera world to use in test to make depth camera have ri…
lucasw Mar 29, 2016
546f285
Disabling this test because of #409
lucasw Mar 29, 2016
58d47ae
#408 Created test for depth camera, which fails, so next make it pass
lucasw Mar 30, 2016
b1dec2f
#408 also test points publication
lucasw Mar 30, 2016
2be9a20
#408 fix timestamps for depth image, test passes now
lucasw Mar 30, 2016
9b56df9
#408 Increasing max time because some systems are taking 0.6 seconds …
lucasw Apr 28, 2016
3dc0df1
Adding @j-rivero default disabling of tests that require a display
lucasw Oct 18, 2016
d719719
change LastUpdateTime to LastMeasurementTime
iche033 Feb 10, 2017
a4608ce
Merge branch 'jade-devel' into camera_timestamp_408_other
j-rivero Feb 13, 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
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 @@ -629,15 +629,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
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 = ℑ
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
2 changes: 1 addition & 1 deletion gazebo_plugins/test/camera/depth_camera.world
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<!-- neither camera info is getting published, frame_id is empty
in points and both image headers -->
<depthImageTopicCameraInfoName>depth/camera_info</depthImageCameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>points</pointCloudTopicName>
<frameName>camera_link</frameName>
<!-- TODO(lucasw) is this used by depth camera at all? -->
Expand Down