add customizability to fake gantry and standardize default gantry model

Author: jgeaso1266

components/gantry/fake/gantry.go

@@ -42,9 +42,14 @@ func makeGantryModel(cfg resource.Config, newConf *Config) (referenceframe.Model
 	case modelPath != "":
 		model, err = referenceframe.KinematicModelFromFile(modelPath, cfg.Name)
 	default:
-		// if no arm model is specified, we return a fake arm with 1 dof and 0 spatial transformation
+		// if no gantry model is specified, we return a default one.
 		model, err = referenceframe.UnmarshalModelJSON(gantryModelJSON, cfg.Name)
 	}
+
+	if (len(model.DoF()) != 1) {
+		return nil, fmt.Errorf("gantry model must have exactly one degree of freedom, got %d", len(model.DoF()))
+	}
+
 	return model, err
 }
 
@@ -80,9 +85,9 @@ func NewGantry(conf resource.Config, logger logging.Logger) (gantry.Gantry, erro
 		testutils.NewUnimplementedResource(conf.ResourceName()),
 		resource.TriviallyReconfigurable{},
 		resource.TriviallyCloseable{},
-		[]float64{120},
+		[]float64{m.DoF()[0].Max / 2},
 		[]float64{50},
-		[]float64{350},
+		[]float64{m.DoF()[0].Max},
 		m,
 		logger,
 	}, nil

components/gantry/fake/test_gantry_model.json

@@ -6,7 +6,7 @@
       "id": "base",
       "parent": "world",
       "translation": {
-        "x": 0,
+        "x": -50,
         "y": 0,
         "z": 0
       }
@@ -20,11 +20,11 @@
         "z": 0
       },
       "geometry": {
-        "x": 150,
-        "y": 120,
+        "x": 50,
+        "y": 50,
         "z": 10,
         "translation": {
-          "x": -475,
+          "x": 0,
           "y": 0,
           "z": 5
         }
@@ -41,7 +41,7 @@
         "y": 0,
         "z": 0
       },
-      "max": 500,
+      "max": 100,
       "min": 0
     }
   ]

components/gantry/server_test.go

@@ -254,7 +254,7 @@ func TestServer(t *testing.T) {
 	t.Run("geometries", func(t *testing.T) {
 		// geometries only works for single axis (multi-axis is a controller of multiple single-axis gantries)
 		injectGantry.PositionFunc = func(ctx context.Context, extra map[string]interface{}) ([]float64, error) {
-			return []float64{1.0}, nil
+			return []float64{51.0}, nil
 		}
 		geometries, err := gantryServer.GetGeometries(context.Background(), &commonpb.GetGeometriesRequest{Name: testGantryName})
 		test.That(t, err, test.ShouldBeNil)
@@ -266,7 +266,7 @@ func TestServer(t *testing.T) {
 				&commonpb.Pose{
 					X:     1.0,
 					Y:     0.0,
-					Z:     0.0,
+					Z:     5.0,
 					OX:    0.0,
 					OY:    0.0,
 					OZ:    1.0,

referenceframe/testfiles/example_gantry.json

@@ -6,7 +6,7 @@
       "id": "base",
       "parent": "world",
       "translation": {
-        "x": 0,
+        "x": -50,
         "y": 0,
         "z": 0
       }
@@ -20,13 +20,13 @@
         "z": 0
       },
       "geometry": {
-        "x": 150,
-        "y": 120,
+        "x": 50,
+        "y": 50,
         "z": 10,
         "translation": {
           "x": 0,
           "y": 0,
-          "z": 0
+          "z": 5
         }
       }
     }
@@ -41,7 +41,7 @@
         "y": 0,
         "z": 0
       },
-      "max": 500,
+      "max": 100,
       "min": 0
     }
   ]

services/motion/builtin/builtin_test.go

@@ -193,14 +193,14 @@ func TestGetPose(t *testing.T) {
 	pose, err := ms.GetPose(context.Background(), "gantry1", "", nil, map[string]interface{}{})
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, pose.Parent(), test.ShouldEqual, referenceframe.World)
-	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 120)
+	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 0)
 	test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0)
 	test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 0)
 
 	pose, err = ms.GetPose(context.Background(), "arm1", "", nil, map[string]interface{}{})
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, pose.Parent(), test.ShouldEqual, referenceframe.World)
-	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 620)
+	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, 500)
 	test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0)
 	test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, 300)
 
@@ -233,7 +233,7 @@ func TestGetPose(t *testing.T) {
 
 	pose, err = ms.GetPose(context.Background(), "arm1", "testFrame2", transforms, map[string]interface{}{})
 	test.That(t, err, test.ShouldBeNil)
-	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, -620)
+	test.That(t, pose.Pose().Point().X, test.ShouldAlmostEqual, -500)
 	test.That(t, pose.Pose().Point().Y, test.ShouldAlmostEqual, 0)
 	test.That(t, pose.Pose().Point().Z, test.ShouldAlmostEqual, -300)
 	test.That(t, pose.Pose().Orientation().AxisAngles().RX, test.ShouldEqual, 0)