Skip to content

Commit

Permalink
Fix timestamp issues for rendering sensors (#538)
Browse files Browse the repository at this point in the history 
Fix timestamp issues for rendering sensors

It affects to gazebo_ros_comera_utils, gazebo_ros_openni_kinnect and gazebo_ros_prosilica.
  • Loading branch information
@iche033 @j-rivero
iche033 authored and j-rivero committed Feb 14, 2017
1 parent ab36b5d commit 4421754
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 26 deletions.
9 changes: 4 additions & 5 deletions gazebo_plugins/src/gazebo_ros_camera_utils.cpp
View file
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
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
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
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

0 comments on commit 4421754

Please sign in to comment.