Fix physics based sensor update rate in lockstep mode (#2863)

* fix physics based sensor update rate in lockstep mode Signed-off-by: Ian Chen <ichen@osrfoundation.org> * merge lines and update changelog Signed-off-by: Ian Chen <ichen@osrfoundation.org>
gazebosim · Oct 29, 2020 · 7ffc25a · 7ffc25a
1 parent 98e67e6
commit 7ffc25a
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 1 deletion.
diff --git a/Changelog.md b/Changelog.md
@@ -2,6 +2,9 @@
 
 ## Gazebo 9.xx.x (202x-xx-xx)
 
+1. Fix physics based sensor update rate in lockstep mode
+    * [Pull request #2863](https://github.com/osrf/gazebo/pull/2863)
+
 ## Gazebo 9.15.0 (2020-09-30)
 
 1. More enhancement for Windows build

diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc
@@ -174,8 +174,17 @@ void Sensor::Update(const bool _force)
   {
     if (this->useStrictRate)
     {
+      // rendering sensors (IMAGE category) has its own mechanism
+      // for throttling and lockstepping with physics. So throttle just
+      // physics sensors
+      if (this->dataPtr->category != IMAGE && !this->NeedsUpdate() && !_force)
+        return;
+
       if (this->UpdateImpl(_force))
+      {
+        this->lastUpdateTime = this->world->SimTime();;
         this->dataPtr->updated();
+      }
     }
     else
     {

diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc
@@ -762,7 +762,7 @@ void SensorManager::SensorContainer::RunLoop()
       if (!g_sensorsDirty)
         return;
 
-      // Get the minimum update rate from the sensors.
+      // Get the maximum update rate from the sensors.
       for (Sensor_V::iterator iter = this->sensors.begin();
           iter != this->sensors.end() && !this->stop; ++iter)
       {

diff --git a/test/worlds/laser_hit_strict_rate_test.world b/test/worlds/laser_hit_strict_rate_test.world
@@ -32,6 +32,32 @@
       <static>true</static>
       <link name="link">
         <sensor name="laser" type="ray">
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>4</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.26889</min_angle>
+                <max_angle>2.268899</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.0</min>
+              <max>20</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+          <always_on>1</always_on>
+          <update_rate>250</update_rate>
+          <visualize>true</visualize>
+        </sensor>
+      </link>
+    </model>
+
+    <model name="ray_model2">
+      <static>true</static>
+      <link name="link2">
+        <sensor name="laser2" type="ray">
           <ray>
             <scan>
               <horizontal>
@@ -53,5 +79,6 @@
         </sensor>
       </link>
     </model>
+
   </world>
 </sdf>