From d8480546590e5bda251ea840eadabb0d6dddf855 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 17 May 2024 17:03:28 -0400 Subject: [PATCH] update navigation service config to include bounding regions --- services/motion/motion.go | 2 +- services/navigation/builtin/builtin.go | 22 +++++++++++++++++++- services/navigation/builtin/builtin_test.go | 3 +++ services/navigation/data/nav_cfg.json | 23 +++++++++++++++++++++ 4 files changed, 48 insertions(+), 2 deletions(-) diff --git a/services/motion/motion.go b/services/motion/motion.go index 2333901e13d..08e26799d98 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -56,7 +56,7 @@ type MoveOnGlobeReq struct { Obstacles []*spatialmath.GeoGeometry // Optional motion configuration MotionCfg *MotionConfiguration - + // Set of obstacles which the robot must remain within while navigating BoundingRegions []*spatialmath.GeoGeometry Extra map[string]interface{} } diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go index 9e632146cae..6ee5f962c2e 100644 --- a/services/navigation/builtin/builtin.go +++ b/services/navigation/builtin/builtin.go @@ -45,6 +45,7 @@ var ( 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") + errGeomWithTranslation = errors.New("geometries specified through navigation are not allowed to have a translation") ) const ( @@ -99,6 +100,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 +183,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, errGeomWithTranslation + } + } + } + + // Ensure bounding regions have no translation + for _, region := range conf.BoundingRegions { + for _, geoms := range region.Geometries { + if !geoms.TranslationOffset.ApproxEqual(r3.Vector{}) { + return nil, errGeomWithTranslation } } } @@ -225,6 +236,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 @@ -368,6 +380,12 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, return err } + // Parse bounding regions from the configuration + newBoundingRegions, err := spatialmath.GeoGeometriesFromConfigs(svcConfig.Obstacles) + if err != nil { + return err + } + // Create explore motion service // Note: this service will disappear after the explore motion model is integrated into builtIn exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}} @@ -381,6 +399,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 +543,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) diff --git a/services/navigation/builtin/builtin_test.go b/services/navigation/builtin/builtin_test.go index e7424d9b916..8e06da3732a 100644 --- a/services/navigation/builtin/builtin_test.go +++ b/services/navigation/builtin/builtin_test.go @@ -574,6 +574,9 @@ 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 setupStartWaypoint(ctx context.Context, t *testing.T, logger logging.Logger) startWaypointState { diff --git a/services/navigation/data/nav_cfg.json b/services/navigation/data/nav_cfg.json index 4c6ce0de08a..767e6ecd1cc 100644 --- a/services/navigation/data/nav_cfg.json +++ b/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" }