From 3656937ca835a71a54793c2bc88520e8d02d806f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 14:59:09 -0400 Subject: [PATCH] add BoudingRegions to arguments of MoveOnGlobe + update server test --- services/motion/builtin/move_request.go | 4 ++-- services/motion/motion.go | 4 +++- services/motion/pbhelpers.go | 19 +++++++++++++++++++ services/motion/server_test.go | 8 ++++++++ services/navigation/navigation.go | 2 +- 5 files changed, 33 insertions(+), 4 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f63a0f2c637..9bed3e9e682 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -446,7 +446,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // ensure arguments are well behaved obstacles := req.Obstacles if obstacles == nil { - obstacles = []*spatialmath.GeoObstacle{} + obstacles = []*spatialmath.GeoGeometry{} } if req.Destination == nil { return nil, errors.New("destination cannot be nil") @@ -518,7 +518,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( return nil, err } - geomsRaw := spatialmath.GeoObstaclesToGeometries(obstacles, origin) + geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, origin) mr, err := ms.createBaseMoveRequest( ctx, diff --git a/services/motion/motion.go b/services/motion/motion.go index ee74e1902e3..b1979ec3005 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -56,7 +56,9 @@ type MoveOnGlobeReq struct { Obstacles []*spatialmath.GeoGeometry // Optional motion configuration MotionCfg *MotionConfiguration - Extra map[string]interface{} + //Set of obstacles which the robot must remain within while navigating + BoundingRegions []*spatialmath.GeoGeometry + Extra map[string]interface{} } func (r MoveOnGlobeReq) String() string { diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index ea62cafee21..720f9302417 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -186,6 +186,13 @@ func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) { } req.Obstacles = obstaclesProto } + if len(r.BoundingRegions) > 0 { + obstaclesProto := make([]*commonpb.GeoGeometry, 0, len(r.Obstacles)) + for _, obstacle := range r.BoundingRegions { + obstaclesProto = append(obstaclesProto, spatialmath.GeoGeometryToProtobuf(obstacle)) + } + req.BoundingRegions = obstaclesProto + } return req, nil } @@ -212,6 +219,17 @@ func moveOnGlobeRequestFromProto(req *pb.MoveOnGlobeRequest) (MoveOnGlobeReq, er } obstacles = append(obstacles, convObst) } + + boundingRegionGeometriesProto := req.GetBoundingRegions() + boundingRegionGeometries := make([]*spatialmath.GeoGeometry, 0, len(boundingRegionGeometriesProto)) + for _, eachProtoObst := range boundingRegionGeometriesProto { + convObst, err := spatialmath.GeoGeometryFromProtobuf(eachProtoObst) + if err != nil { + return MoveOnGlobeReq{}, err + } + boundingRegionGeometries = append(boundingRegionGeometries, convObst) + } + protoComponentName := req.GetComponentName() if protoComponentName == nil { return MoveOnGlobeReq{}, errors.New("received nil *commonpb.ResourceName") @@ -232,6 +250,7 @@ func moveOnGlobeRequestFromProto(req *pb.MoveOnGlobeRequest) (MoveOnGlobeReq, er MovementSensorName: movementSensorName, Obstacles: obstacles, MotionCfg: motionCfg, + BoundingRegions: boundingRegionGeometries, Extra: req.Extra.AsMap(), }, nil } diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 78e03b64cd3..d6095d97e96 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -221,6 +221,10 @@ func TestServerMoveOnGlobe(t *testing.T) { spatialmath.GeoGeometryToProtobuf(geoGeometry1), spatialmath.GeoGeometryToProtobuf(geoGeometry2), } + boundingRegionGeoms := []*commonpb.GeoGeometry{ + spatialmath.GeoGeometryToProtobuf(geoGeometry1), + spatialmath.GeoGeometryToProtobuf(geoGeometry2), + } angularDegsPerSec := 1. linearMPerSec := 2. planDeviationM := 3. @@ -252,6 +256,7 @@ func TestServerMoveOnGlobe(t *testing.T) { PositionPollingFrequencyHz: &positionPollingFrequencyHz, ObstacleDetectors: obstacleDetectorsPB, }, + BoundingRegions: boundingRegionGeoms, } firstExecutionID := uuid.New() @@ -274,6 +279,9 @@ func TestServerMoveOnGlobe(t *testing.T) { test.That(t, req.MotionCfg.ObstacleDetectors[0].CameraName, test.ShouldResemble, camera.Named("camera 1")) test.That(t, req.MotionCfg.ObstacleDetectors[1].VisionServiceName, test.ShouldResemble, vision.Named("vision service 2")) test.That(t, req.MotionCfg.ObstacleDetectors[1].CameraName, test.ShouldResemble, camera.Named("camera 2")) + test.That(t, len(req.BoundingRegions), test.ShouldEqual, 2) + test.That(t, req.BoundingRegions[0], test.ShouldResemble, geoGeometry1) + test.That(t, req.BoundingRegions[1], test.ShouldResemble, geoGeometry2) return firstExecutionID, nil } moveOnGlobeResponse, err := server.MoveOnGlobe(context.Background(), moveOnGlobeRequest) diff --git a/services/navigation/navigation.go b/services/navigation/navigation.go index 94be97e5dab..746b62f71d7 100644 --- a/services/navigation/navigation.go +++ b/services/navigation/navigation.go @@ -92,7 +92,7 @@ type Service interface { AddWaypoint(ctx context.Context, point *geo.Point, extra map[string]interface{}) error RemoveWaypoint(ctx context.Context, id primitive.ObjectID, extra map[string]interface{}) error - Obstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoObstacle, error) + Obstacles(ctx context.Context, extra map[string]interface{}) ([]*spatialmath.GeoGeometry, error) Paths(ctx context.Context, extra map[string]interface{}) ([]*Path, error)