diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp index 250be0112..b330e0137 100644 --- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp @@ -211,6 +211,12 @@ void GazeboRosBlockLaser::OnNewLaserScans() if (this->topic_name_ != "") { common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime(); + if (sensor_update_time < last_update_time_) + { + ROS_WARN_NAMED("block_laser", "Negative sensor update time difference detected."); + last_update_time_ = sensor_update_time; + } + if (last_update_time_ < sensor_update_time) { this->PutLaserData(sensor_update_time); diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index edb90ff4d..8a59a129c 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -88,6 +88,12 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image, { if ((*this->image_connect_count_) > 0) { + if (sensor_update_time < this->last_update_time_) + { + ROS_WARN_NAMED("camera", "Negative sensor update time difference detected."); + this->last_update_time_ = sensor_update_time; + } + // OnNewFrame is triggered at the gazebo sensor // while there is also a plugin that can throttle the // rate down further (but then why not reduce the sensor rate? diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 1911db8f9..c125d731e 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -112,7 +112,14 @@ void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) #else common::Time current_time = this->world_->GetSimTime(); #endif + if (current_time < last_update_time_) + { + ROS_WARN_NAMED("joint_state_publisher", "Negative joint state update time difference detected."); + last_update_time_ = current_time; + } + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + if ( seconds_since_last_update > update_period_ ) { publishJointStates(); diff --git a/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_plugins/src/gazebo_ros_p3d.cpp index 57a381ab8..a252d222a 100644 --- a/gazebo_plugins/src/gazebo_ros_p3d.cpp +++ b/gazebo_plugins/src/gazebo_ros_p3d.cpp @@ -221,6 +221,12 @@ void GazeboRosP3D::UpdateChild() common::Time cur_time = this->world_->GetSimTime(); #endif + if (cur_time < this->last_time_) + { + ROS_WARN_NAMED("p3d", "Negative update time difference detected."); + this->last_time_ = cur_time; + } + // rate control if (this->update_rate_ > 0 && (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) diff --git a/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/src/gazebo_ros_range.cpp index b9dc37d74..043955232 100644 --- a/gazebo_plugins/src/gazebo_ros_range.cpp +++ b/gazebo_plugins/src/gazebo_ros_range.cpp @@ -248,6 +248,12 @@ void GazeboRosRange::OnNewLaserScans() #else common::Time cur_time = this->world_->GetSimTime(); #endif + if (cur_time < this->last_update_time_) + { + ROS_WARN_NAMED("range", "Negative sensor update time difference detected."); + this->last_update_time_ = cur_time; + } + if (cur_time - this->last_update_time_ >= this->update_period_) { common::Time sensor_update_time =