Add additional data fields that can be reported by rangefinder sensors.

PiperOrigin-RevId: 848316991
Change-Id: Idbf7ba81b4da711a22c23302c8782ab2b0b98d82

src/user/user_api.cc

@@ -1274,14 +1274,6 @@ int mjs_sensorDim(const mjsSensor* sensor) {
   case mjSENS_CLOCK:
     return 1;
 
-  case mjSENS_RANGEFINDER:
-    if (sensor->objtype == mjOBJ_CAMERA) {
-      const mjCCamera* camera = static_cast<const mjCCamera*>(
-          static_cast<mjCSensor*>(sensor->element)->get_obj());
-      return camera->spec.resolution[0] * camera->spec.resolution[1];
-    }
-    return 1;  // site-attached: single ray
-
   case mjSENS_CAMPROJECTION:
     return 2;
 
@@ -1321,6 +1313,18 @@ int mjs_sensorDim(const mjsSensor* sensor) {
                    static_cast<mjCSensor*>(sensor->element)->get_obj())
                    ->nvert();
 
+  case mjSENS_RANGEFINDER:
+    {
+      int size = mju_raydataSize(sensor->intprm[0]);
+      int num_rays = 1;
+      if (sensor->objtype == mjOBJ_CAMERA) {
+        const mjCCamera* camera = static_cast<const mjCCamera*>(
+            static_cast<mjCSensor*>(sensor->element)->get_obj());
+        num_rays = camera->spec.resolution[0] * camera->spec.resolution[1];
+      }
+      return size * num_rays;
+    }
+
   case mjSENS_USER:
     return sensor->dim;
 

src/user/user_objects.cc

@@ -7149,7 +7149,6 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
 mjtDataType sensorDatatype(mjtSensor type) {
   switch (type) {
   case mjSENS_TOUCH:
-  case mjSENS_RANGEFINDER:
   case mjSENS_INSIDESITE:
     return mjDATATYPE_POSITIVE;
 
@@ -7196,6 +7195,7 @@ mjtDataType sensorDatatype(mjtSensor type) {
   case mjSENS_SUBTREEANGMOM:
   case mjSENS_GEOMDIST:
   case mjSENS_GEOMFROMTO:
+  case mjSENS_RANGEFINDER:
   case mjSENS_CONTACT:
   case mjSENS_TACTILE:
   case mjSENS_E_POTENTIAL:
@@ -7329,9 +7329,26 @@ void mjCSensor::Compile(void) {
       break;
 
     case mjSENS_RANGEFINDER:
-      // must be attached to site or camera
-      if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
-        throw mjCError(this, "sensor must be attached to site or camera");
+      {
+        // must be attached to site or camera
+        if (objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
+          throw mjCError(this, "sensor must be attached to site or camera");
+        }
+
+        // check for dataspec correctness
+        int dataspec = intprm[0];
+        if (dataspec <= 0) {
+          throw mjCError(this, "data spec (intprm[0]) must be positive, got %d", nullptr, dataspec);
+        }
+        int mask = (1 << mjNRAYDATA) - 1;
+        if (!(dataspec & mask)) {
+          throw mjCError(this, "data spec intprm[0]=%d must have at least one bit set of the first "
+                         "mjNRAYDATA bits", nullptr, dataspec);
+        }
+        if (dataspec & ~mask) {
+          throw mjCError(this, "data spec intprm[0]=%d has bits set beyond the first "
+                         "mjNRAYDATA bits", nullptr, dataspec);
+        }
       }
       break;