RSDK-7466 - Change navigation config to include bounding regions (#3971)

services/motion/motion.go

@@ -54,11 +54,11 @@ type MoveOnGlobeReq struct {
 	MovementSensorName resource.Name
 	// Static obstacles that should be navigated around
 	Obstacles []*spatialmath.GeoGeometry
+	// Set of bounds which the robot must remain within while navigating
+	BoundingRegions []*spatialmath.GeoGeometry
 	// Optional motion configuration
 	MotionCfg *MotionConfiguration
-
-	BoundingRegions []*spatialmath.GeoGeometry
-	Extra           map[string]interface{}
+	Extra     map[string]interface{}
 }
 
 func (r MoveOnGlobeReq) String() string {

services/navigation/builtin/builtin.go

@@ -38,13 +38,18 @@ var (
 		navigation.NoMap:  {navigation.ModeManual, navigation.ModeExplore},
 		navigation.GPSMap: {navigation.ModeManual, navigation.ModeWaypoint, navigation.ModeExplore},
 	}
+	geomWithTranslation = "geometries specified through navigation are not allowed to have a translation"
 
 	errNegativeDegPerSec                  = errors.New("degs_per_sec must be non-negative if set")
 	errNegativeMetersPerSec               = errors.New("meters_per_sec must be non-negative if set")
 	errNegativePositionPollingFrequencyHz = errors.New("position_polling_frequency_hz must be non-negative if set")
 	errNegativeObstaclePollingFrequencyHz = errors.New("obstacle_polling_frequency_hz must be non-negative if set")
 	errNegativePlanDeviationM             = errors.New("plan_deviation_m must be non-negative if set")
 	errNegativeReplanCostFactor           = errors.New("replan_cost_factor must be non-negative if set")
+	errObstacleGeomWithTranslation        = errors.New("obstacle " + geomWithTranslation)
+	errBoundingRegionsGeomWithTranslation = errors.New("bounding region " + geomWithTranslation)
+	errObstacleGeomParse                  = errors.New("obstacle unable to be converted from geometry config")
+	errBoundingRegionsGeomParse           = errors.New("bounding regions unable to be converted from geometry config")
 )
 
 const (
@@ -99,6 +104,7 @@ type Config struct {
 	MetersPerSec float64 `json:"meters_per_sec,omitempty"`
 
 	Obstacles                  []*spatialmath.GeoGeometryConfig `json:"obstacles,omitempty"`
+	BoundingRegions            []*spatialmath.GeoGeometryConfig `json:"bounding_regions,omitempty"`
 	PositionPollingFrequencyHz float64                          `json:"position_polling_frequency_hz,omitempty"`
 	ObstaclePollingFrequencyHz float64                          `json:"obstacle_polling_frequency_hz,omitempty"`
 	PlanDeviationM             float64                          `json:"plan_deviation_m,omitempty"`
@@ -181,7 +187,16 @@ func (conf *Config) Validate(path string) ([]string, error) {
 	for _, obs := range conf.Obstacles {
 		for _, geoms := range obs.Geometries {
 			if !geoms.TranslationOffset.ApproxEqual(r3.Vector{}) {
-				return nil, errors.New("geometries specified through the navigation are not allowed to have a translation")
+				return nil, errObstacleGeomWithTranslation
+			}
+		}
+	}
+
+	// Ensure bounding regions have no translation
+	for _, region := range conf.BoundingRegions {
+		for _, geoms := range region.Geometries {
+			if !geoms.TranslationOffset.ApproxEqual(r3.Vector{}) {
+				return nil, errBoundingRegionsGeomWithTranslation
 			}
 		}
 	}
@@ -225,6 +240,7 @@ type builtIn struct {
 	// exploreMotionService will be removed once the motion explore model is integrated into motion builtin
 	exploreMotionService motion.Service
 	obstacles            []*spatialmath.GeoGeometry
+	boundingRegions      []*spatialmath.GeoGeometry
 
 	motionCfg        *motion.MotionConfiguration
 	replanCostFactor float64
@@ -365,7 +381,13 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies,
 	// Parse obstacles from the configuration
 	newObstacles, err := spatialmath.GeoGeometriesFromConfigs(svcConfig.Obstacles)
 	if err != nil {
-		return err
+		return errors.Wrap(errObstacleGeomParse, err.Error())
+	}
+
+	// Parse bounding regions from the configuration
+	newBoundingRegions, err := spatialmath.GeoGeometriesFromConfigs(svcConfig.BoundingRegions)
+	if err != nil {
+		return errors.Wrap(errBoundingRegionsGeomParse, err.Error())
 	}
 
 	// Create explore motion service
@@ -381,6 +403,7 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies,
 	svc.mapType = mapType
 	svc.motionService = motionSvc
 	svc.obstacles = newObstacles
+	svc.boundingRegions = newBoundingRegions
 	svc.replanCostFactor = replanCostFactor
 	svc.visionServicesByName = visionServicesByName
 	svc.motionCfg = &motion.MotionConfiguration{
@@ -524,6 +547,7 @@ func (svc *builtIn) moveToWaypoint(ctx context.Context, wp navigation.Waypoint,
 		MovementSensorName: svc.movementSensor.Name(),
 		Obstacles:          svc.obstacles,
 		MotionCfg:          svc.motionCfg,
+		BoundingRegions:    svc.boundingRegions,
 		Extra:              extra,
 	}
 	cancelCtx, cancelFn := context.WithCancel(ctx)

services/navigation/builtin/builtin_test.go

@@ -4,6 +4,7 @@ import (
 	"context"
 	"errors"
 	"math"
+	"strings"
 	"sync"
 	"testing"
 	"time"
@@ -574,6 +575,40 @@ func TestNavSetup(t *testing.T) {
 	paths, err := ns.Paths(ctx, nil)
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, paths, test.ShouldBeEmpty)
+
+	test.That(t, len(ns.(*builtIn).boundingRegions), test.ShouldEqual, 1)
+	test.That(t, len(ns.(*builtIn).obstacles), test.ShouldEqual, 1)
+}
+
+func TestNavSetUpFromFaultyConfig(t *testing.T) {
+	type testCase struct {
+		configPath    string
+		expectedError string
+	}
+	testCases := []testCase{
+		{
+			configPath: "../data/incorrect_obstacles_nav_cfg.json",
+			expectedError: `resource "rdk:service:navigation/test_navigation" not available;` +
+				` reason="resource build error: unsupported Geometry type : obstacle unable to be converted from geometry config"`,
+		},
+		{
+			configPath: "../data/incorrect_bounding_regions_nav_cfg.json",
+			expectedError: `resource "rdk:service:navigation/test_navigation" not available;` +
+				` reason="resource build error: unsupported Geometry type : bounding regions unable to be converted from geometry config"`,
+		},
+	}
+	ctx := context.Background()
+	logger := logging.NewTestLogger(t)
+	for _, tc := range testCases {
+		cfg, err := config.Read(ctx, tc.configPath, logger)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, cfg.Ensure(false, logger), test.ShouldBeNil)
+		myRobot, err := robotimpl.New(ctx, cfg, logger)
+		test.That(t, err, test.ShouldBeNil)
+		_, err = navigation.FromRobot(myRobot, "test_navigation")
+		test.That(t, err, test.ShouldNotBeNil)
+		test.That(t, strings.Contains(err.Error(), tc.expectedError), test.ShouldBeTrue)
+	}
 }
 
 func setupStartWaypoint(ctx context.Context, t *testing.T, logger logging.Logger) startWaypointState {
@@ -1697,9 +1732,8 @@ func TestValidateGeometry(t *testing.T) {
 	t.Run("fail case", func(t *testing.T) {
 		cfg = createBox(r3.Vector{X: 10, Y: 10, Z: 10})
 		_, err := cfg.Validate("")
-		expectedErr := "geometries specified through the navigation are not allowed to have a translation"
 		test.That(t, err, test.ShouldNotBeNil)
-		test.That(t, err.Error(), test.ShouldEqual, expectedErr)
+		test.That(t, errors.Is(err, errObstacleGeomWithTranslation), test.ShouldBeTrue)
 	})
 
 	t.Run("success case", func(t *testing.T) {

services/navigation/data/incorrect_bounding_regions_nav_cfg.json

@@ -0,0 +1,71 @@
+{
+    "components":
+    [{
+        "name": "test_camera",
+        "type": "camera",
+        "model": "fake"
+    },
+    {
+        "model": "fake",
+        "name": "test_base",
+        "type": "base"
+    },
+    {
+        "name": "test_movement",
+        "type": "movement_sensor",
+        "model": "fake"
+    }],
+    "services":
+    [{
+        "name": "blue_square",
+        "type": "vision",
+        "model": "color_detector",
+        "attributes": {
+            "segment_size_px": 100,
+            "detect_color": "#1C4599",
+            "hue_tolerance_pct": 0.07,
+            "value_cutoff_pct": 0.15
+        }
+    },
+    {
+        "name":"test_navigation",
+        "type": "navigation",
+        "attributes": {
+            "base":"test_base",
+            "movement_sensor":"test_movement",
+            "obstacle_detectors": [{
+                "vision_service": "blue_square",
+                "camera": "test_camera"
+            }],
+            "bounding_regions":
+            [{
+                "geometries":
+                [{
+                    "label":"aLabel",
+                    "type": "",
+                    "orientation":{
+                        "type":"ov_degrees",
+                        "value":{
+                            "X":1,
+                            "Y":0,
+                            "Z":0,
+                            "Th": 90
+                        }
+                    },
+                    "translation": {
+                        "x": 0,
+                        "y": 0,
+                        "z": 0
+                    }
+                }],
+                "location":{
+                    "latitude": 1,
+                    "longitude": 1
+                }
+            }],
+            "store":{
+                "type":"memory"
+            }
+        }
+    }]
+}

services/navigation/data/incorrect_obstacles_nav_cfg.json

@@ -0,0 +1,71 @@
+{
+    "components":
+    [{
+        "name": "test_camera",
+        "type": "camera",
+        "model": "fake"
+    },
+    {
+        "model": "fake",
+        "name": "test_base",
+        "type": "base"
+    },
+    {
+        "name": "test_movement",
+        "type": "movement_sensor",
+        "model": "fake"
+    }],
+    "services":
+    [{
+        "name": "blue_square",
+        "type": "vision",
+        "model": "color_detector",
+        "attributes": {
+            "segment_size_px": 100,
+            "detect_color": "#1C4599",
+            "hue_tolerance_pct": 0.07,
+            "value_cutoff_pct": 0.15
+        }
+    },
+    {
+        "name":"test_navigation",
+        "type": "navigation",
+        "attributes": {
+            "base":"test_base",
+            "movement_sensor":"test_movement",
+            "obstacle_detectors": [{
+                "vision_service": "blue_square",
+                "camera": "test_camera"
+            }],
+            "obstacles":
+            [{
+                "geometries":
+                [{
+                    "label":"aLabel",
+                    "type": "",
+                    "orientation":{
+                        "type":"ov_degrees",
+                        "value":{
+                            "X":1,
+                            "Y":0,
+                            "Z":0,
+                            "Th": 90
+                        }
+                    },
+                    "translation": {
+                        "x": 0,
+                        "y": 0,
+                        "z": 0
+                    }
+                }],
+                "location":{
+                    "latitude": 1,
+                    "longitude": 1
+                }
+            }],
+            "store":{
+                "type":"memory"
+            }
+        }
+    }]
+}

services/navigation/data/nav_cfg.json

@@ -60,6 +60,29 @@
                     "longitude": 1
                 }
             }],
+            "bounding_regions":
+            [{
+                "geometries":
+                [{
+                    "label":"aLabel2",
+                    "orientation":{
+                        "type":"ov_degrees",
+                        "value":{
+                            "X":1,
+                            "Y":0,
+                            "Z":0,
+                            "Th": -90
+                        }
+                    },
+                    "x":20,
+                    "y":20,
+                    "z":20
+                }],
+                "location":{
+                    "latitude": 2,
+                    "longitude": 2
+                }
+            }],
             "store":{
                 "type":"memory"
             }