Port sensor update rate fix (#3353)

* resolve conflicts Signed-off-by: Ian Chen <ichen@openrobotics.org> * add lock Signed-off-by: Ian Chen <ichen@openrobotics.org> --------- Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · Nov 8, 2023 · 41b34e5 · 41b34e5
1 parent 391c4f2
commit 41b34e5
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 1 deletion.
diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc
@@ -238,8 +238,18 @@ 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))
+      {
+        std::lock_guard<std::mutex> lock(this->dataPtr->mutexLastUpdateTime);
+        this->lastUpdateTime = this->world->SimTime();;
         this->updated();
+      }
     }
     else
     {

diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc
@@ -761,7 +761,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>