Merge branch 'athenaz/gi_for_sensors' of https://github.com/athenaz2/…

…gz-sim into athenaz/gi_for_sensors
gazebosim · Sep 5, 2024 · 6a7a6c2 · 6a7a6c2
2 parents 3f64dd0 + 420231d
commit 6a7a6c2
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/examples/worlds/ground_spacecraft_testbed.sdf b/examples/worlds/ground_spacecraft_testbed.sdf
@@ -2,9 +2,9 @@
 <!--
   Spacecraft thruster plugin demo
 
-  Send commands to a single thruster:
+  Send commands to a single thruster (corrected 09/01/24):
 
-      gz topic -p 'normalized:[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]' -t /dart/command/duty_cycle --msgtype gz.msgs.Actuators
+      gz topic -p 'normalized:[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]' -t /kth_freeflyer/command/duty_cycle --msgtype gz.msgs.Actuators
 -->
 <sdf version="1.6">
   <world name="ground_testbed">

diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc
@@ -753,6 +753,15 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
   if (!this->initialized)
     return;
 
+  // sanity check to make sure point cloud data size matches other fields
+  if (_msg.data().size() !=  _msg.row_step() * _msg.height())
+  {
+    gzerr << "Invalid point cloud message. "
+          << "Point cloud data size != row_step * height."
+          << std::endl;
+    return;
+  }
+
   // Get data from the message
   const char *msgBuffer = _msg.data().data();