From 8bc82deca3f32f4eef0e69aca4c964d07bb8461b Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 15 Aug 2024 11:33:44 -0400 Subject: [PATCH 01/16] tentative direction for PR --- services/motion/builtin/builtin.go | 41 ++++++++++++++---------------- services/motion/client.go | 25 +++--------------- services/motion/motion.go | 22 +++++++++++----- services/motion/pbhelpers.go | 36 ++++++++++++++++++++++++++ services/motion/server.go | 12 ++------- testutils/inject/motion_service.go | 20 +++------------ 6 files changed, 80 insertions(+), 76 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 33c0bbea02f..51ad81a0249 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -170,25 +170,13 @@ func (ms *builtIn) Close(ctx context.Context) error { return nil } -// Move takes a goal location and will plan and execute a movement to move a component specified by its name to that destination. -func (ms *builtIn) Move( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, -) (bool, error) { +func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) { ms.mu.RLock() defer ms.mu.RUnlock() operation.CancelOtherWithLabel(ctx, builtinOpLabel) - // get goal frame - goalFrameName := destination.Parent() - ms.logger.CDebugf(ctx, "goal given in frame of %q", goalFrameName) - - frameSys, err := ms.fsService.FrameSystem(ctx, worldState.Transforms()) + frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) if err != nil { return false, err } @@ -198,17 +186,16 @@ func (ms *builtIn) Move( if err != nil { return false, err } - - movingFrame := frameSys.Frame(componentName.ShortName()) - ms.logger.CDebugf(ctx, "frame system inputs: %v", fsInputs) + + movingFrame := frameSys.Frame(req.ComponentName.ShortName()) if movingFrame == nil { - return false, fmt.Errorf("component named %s not found in robot frame system", componentName.ShortName()) + return false, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName()) } // re-evaluate goalPose to be in the frame of World solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName - tf, err := frameSys.Transform(fsInputs, destination, solvingFrame) + tf, err := frameSys.Transform(fsInputs, &req.Destination, solvingFrame) if err != nil { return false, err } @@ -221,9 +208,9 @@ func (ms *builtIn) Move( Frame: movingFrame, StartConfiguration: fsInputs, FrameSystem: frameSys, - WorldState: worldState, - Constraints: constraints, - Options: extra, + WorldState: &req.WorldState, + Constraints: &req.Constraints, + Options: req.Extra, }) if err != nil { return false, err @@ -392,3 +379,13 @@ func (ms *builtIn) PlanHistory( defer ms.mu.RUnlock() return ms.state.PlanHistory(req) } + +func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { + moveReq, ok := cmd["Plan"] + if ok { + // call plan + } + if plan, ok := cmd["Execute"]; ok { + // call execute + } +} \ No newline at end of file diff --git a/services/motion/client.go b/services/motion/client.go index 2b5a6d89a6c..1dd597a872f 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -9,7 +9,6 @@ import ( "go.viam.com/utils/rpc" "go.viam.com/rdk/logging" - "go.viam.com/rdk/motionplan" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -43,30 +42,12 @@ func NewClientFromConn( return c, nil } -func (c *client) Move( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, -) (bool, error) { - ext, err := vprotoutils.StructToStructPb(extra) - if err != nil { - return false, err - } - worldStateMsg, err := worldState.ToProtobuf() +func (c *client) Move(ctx context.Context, req MoveReq) (bool, error) { + protoReq, err := req.toProto(c.name) if err != nil { return false, err } - resp, err := c.client.Move(ctx, &pb.MoveRequest{ - Name: c.name, - ComponentName: protoutils.ResourceNameToProto(componentName), - Destination: referenceframe.PoseInFrameToProtobuf(destination), - WorldState: worldStateMsg, - Constraints: constraints.ToProtobuf(), - Extra: ext, - }) + resp, err := c.client.Move(ctx, protoReq) if err != nil { return false, err } diff --git a/services/motion/motion.go b/services/motion/motion.go index 150d5465899..6f62a7128e7 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -40,6 +40,19 @@ type PlanHistoryReq struct { Extra map[string]interface{} } +// MoveReq describes the request to the Move interface method. +type MoveReq struct { + // ComponentName of the component to move + ComponentName resource.Name + // Goal destination the component should be moved to + Destination referenceframe.PoseInFrame + // The external environment to be considered for the duration of the move + WorldState referenceframe.WorldState + // Constraints which need to be satisfied during the movement + Constraints motionplan.Constraints + Extra map[string]interface{} +} + // MoveOnGlobeReq describes the request to the MoveOnGlobe interface method. type MoveOnGlobeReq struct { // ComponentName of the component to move @@ -329,14 +342,11 @@ type Service interface { // Move is the primary method to move multiple components or any object to a specified location. // Given a destination pose and a component, Move constructs a kinematic chain from goal to destination, - // solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself and other known objects. + // solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself + // and other known objects. Move( ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, + req MoveReq, ) (bool, error) // MoveOnMap moves a base component to a destination Pose on a SLAM map and returns a unique ExecutionID. diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index 7ba7316d41b..4e23c87c88c 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -11,7 +11,9 @@ import ( vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/protoutils" rprotoutils "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -152,6 +154,40 @@ func planStateFromProto(ps pb.PlanState) PlanState { } } +// toProto converts a MoveRequest to a *pb.MoveRequest. +func (r MoveReq) toProto(name string) (*pb.MoveRequest, error) { + ext, err := vprotoutils.StructToStructPb(r.Extra) + if err != nil { + return nil, err + } + worldStateMsg, err := r.WorldState.ToProtobuf() + if err != nil { + return nil, err + } + return &pb.MoveRequest{ + Name: name, + ComponentName: protoutils.ResourceNameToProto(r.ComponentName), + Destination: referenceframe.PoseInFrameToProtobuf(&r.Destination), + WorldState: worldStateMsg, + Constraints: r.Constraints.ToProtobuf(), + Extra: ext, + }, nil +} + +func moveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return MoveReq{}, err + } + return MoveReq{ + protoutils.ResourceNameFromProto(req.GetComponentName()), + *referenceframe.ProtobufToPoseInFrame(req.GetDestination()), + *worldState, + *motionplan.ConstraintsFromProtobuf(req.GetConstraints()), + req.Extra.AsMap(), + }, nil +} + // toProto converts a MoveOnGlobeRequest to a *pb.MoveOnGlobeRequest. func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) { ext, err := vprotoutils.StructToStructPb(r.Extra) diff --git a/services/motion/server.go b/services/motion/server.go index f96c9140b4d..859b8c521ae 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -7,7 +7,6 @@ import ( commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" - "go.viam.com/rdk/motionplan" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -30,18 +29,11 @@ func (server *serviceServer) Move(ctx context.Context, req *pb.MoveRequest) (*pb if err != nil { return nil, err } - worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + r, err := moveReqFromProto(req) if err != nil { return nil, err } - success, err := svc.Move( - ctx, - protoutils.ResourceNameFromProto(req.GetComponentName()), - referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - worldState, - motionplan.ConstraintsFromProtobuf(req.GetConstraints()), - req.Extra.AsMap(), - ) + success, err := svc.Move(ctx, r) return &pb.MoveResponse{Success: success}, err } diff --git a/testutils/inject/motion_service.go b/testutils/inject/motion_service.go index d350840817a..50bf3f0564f 100644 --- a/testutils/inject/motion_service.go +++ b/testutils/inject/motion_service.go @@ -3,7 +3,6 @@ package inject import ( "context" - "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" @@ -16,11 +15,7 @@ type MotionService struct { name resource.Name MoveFunc func( ctx context.Context, - componentName resource.Name, - grabPose *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, + req motion.MoveReq, ) (bool, error) MoveOnMapFunc func( ctx context.Context, @@ -65,18 +60,11 @@ func (mgs *MotionService) Name() resource.Name { } // Move calls the injected Move or the real variant. -func (mgs *MotionService) Move( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, -) (bool, error) { +func (mgs *MotionService) Move(ctx context.Context, req motion.MoveReq) (bool, error) { if mgs.MoveFunc == nil { - return mgs.Service.Move(ctx, componentName, destination, worldState, constraints, extra) + return mgs.Service.Move(ctx, req) } - return mgs.MoveFunc(ctx, componentName, destination, worldState, constraints, extra) + return mgs.MoveFunc(ctx, req) } // MoveOnMap calls the injected MoveOnMap or the real variant. From 12668f8769e830f474c426a7bebd7de66afe3125 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 5 Sep 2024 13:07:51 -0400 Subject: [PATCH 02/16] get the feature out for testing, not ready for merging --- referenceframe/primitives.go | 1 + robot/impl/resource_manager_test.go | 11 - services/motion/builtin/builtin.go | 301 +++++---- services/motion/builtin/builtin_test.go | 24 +- services/motion/client_test.go | 22 +- services/motion/explore/explore.go | 632 ------------------ services/motion/explore/explore_test.go | 386 ----------- services/motion/explore/explore_utils_test.go | 187 ------ services/motion/motion.go | 34 +- services/motion/pbhelpers.go | 5 +- services/motion/server_test.go | 19 +- services/navigation/builtin/builtin.go | 14 +- services/navigation/builtin/explore.go | 46 -- services/navigation/builtin/explore_test.go | 42 -- 14 files changed, 209 insertions(+), 1515 deletions(-) delete mode 100644 services/motion/explore/explore.go delete mode 100644 services/motion/explore/explore_test.go delete mode 100644 services/motion/explore/explore_utils_test.go delete mode 100644 services/navigation/builtin/explore.go delete mode 100644 services/navigation/builtin/explore_test.go diff --git a/referenceframe/primitives.go b/referenceframe/primitives.go index a8aa701b515..bf8851f4932 100644 --- a/referenceframe/primitives.go +++ b/referenceframe/primitives.go @@ -59,6 +59,7 @@ func JointPositionsFromRadians(radians []float64) *pb.JointPositions { // InputEnabled is a standard interface for all things that interact with the frame system // This allows us to figure out where they currently are, and then move them. // Input units are always in meters or radians. +// TODO: this really belongs in the motion service theres nothing stateful about the referenceframe package. type InputEnabled interface { CurrentInputs(ctx context.Context) ([]Input, error) GoToInputs(context.Context, ...[]Input) error diff --git a/robot/impl/resource_manager_test.go b/robot/impl/resource_manager_test.go index 7b521ab4122..8731821af94 100644 --- a/robot/impl/resource_manager_test.go +++ b/robot/impl/resource_manager_test.go @@ -50,7 +50,6 @@ import ( "go.viam.com/rdk/grpc" "go.viam.com/rdk/logging" "go.viam.com/rdk/module/modmaninterface" - "go.viam.com/rdk/motionplan" "go.viam.com/rdk/operation" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" @@ -463,16 +462,6 @@ func TestManagerAdd(t *testing.T) { test.That(t, resource1, test.ShouldEqual, injectBoard) injectMotionService := &inject.MotionService{} - injectMotionService.MoveFunc = func( - ctx context.Context, - componentName resource.Name, - grabPose *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { - return false, nil - } objectMResName := motion.Named("motion1") manager.resources.AddNode(objectMResName, resource.NewConfiguredGraphNode(resource.Config{}, injectMotionService, unknownModel)) motionService, err := manager.ResourceByName(objectMResName) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index af53bd90e9f..df7be38c091 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -23,12 +23,7 @@ import ( "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" - rdkutils "go.viam.com/rdk/utils" -) - -var ( - stateTTL = time.Hour * 24 - stateTTLCheckInterval = time.Minute + "go.viam.com/rdk/utils" ) func init() { @@ -46,6 +41,12 @@ func init() { ) } +// export keys to be used with DoCommand so they can be referenced by clients. +const ( + DoPlan = "plan" + DoExecute = "execute" +) + const ( builtinOpLabel = "motion-service" maxTravelDistanceMM = 5e6 // this is equivalent to 5km @@ -62,6 +63,11 @@ var ( defaultObstaclePollingHz = 1. ) +var ( + stateTTL = time.Hour * 24 + stateTTLCheckInterval = time.Minute +) + // inputEnabledActuator is an actuator that interacts with the frame system. // This allows us to figure out where the actuator currently is and then // move it. Input units are always in meters or radians. @@ -109,7 +115,7 @@ func (ms *builtIn) Reconfigure( return err } if config.LogFilePath != "" { - logger, err := rdkutils.NewFilePathDebugLogger(config.LogFilePath, "motion") + logger, err := utils.NewFilePathDebugLogger(config.LogFilePath, "motion") if err != nil { return err } @@ -174,127 +180,12 @@ func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) { ms.mu.RLock() defer ms.mu.RUnlock() - operation.CancelOtherWithLabel(ctx, builtinOpLabel) - - frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) - if err != nil { - return false, err - } - - // build maps of relevant components and inputs from initial inputs - fsInputs, resources, err := ms.fsService.CurrentInputs(ctx) - if err != nil { - return false, err - } - ms.logger.CDebugf(ctx, "frame system inputs: %v", fsInputs) - - movingFrame := frameSys.Frame(req.ComponentName.ShortName()) - if movingFrame == nil { - return false, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName()) - } - - // re-evaluate goalPose to be in the frame of World - solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName - tf, err := frameSys.Transform(fsInputs, &req.Destination, solvingFrame) + plan, err := ms.plan(ctx, req) if err != nil { return false, err } - goalPose, _ := tf.(*referenceframe.PoseInFrame) - - // the goal is to move the component to goalPose which is specified in coordinates of goalFrameName - plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: ms.logger, - Goal: goalPose, - Frame: movingFrame, - StartConfiguration: fsInputs, - FrameSystem: frameSys, - WorldState: &req.WorldState, - Constraints: &req.Constraints, - Options: req.Extra, - }) - if err != nil { - return false, err - } - - // move all the components - // Batch GoToInputs calls if possible; components may want to blend between inputs - combinedSteps := []map[string][][]referenceframe.Input{} - currStep := map[string][][]referenceframe.Input{} - for i, step := range plan.Trajectory() { - if i == 0 { - for name, inputs := range step { - if len(inputs) == 0 { - continue - } - currStep[name] = append(currStep[name], inputs) - } - continue - } - changed := "" - if len(currStep) > 0 { - reset := false - // Check if the current step moves only the same components as the previous step - // If so, batch the inputs - for name, inputs := range step { - if len(inputs) == 0 { - continue - } - if priorInputs, ok := currStep[name]; ok { - for i, input := range inputs { - if input != priorInputs[len(priorInputs)-1][i] { - if changed == "" { - changed = name - } - if changed != "" && changed != name { - // If the current step moves different components than the previous step, reset the batch - reset = true - break - } - } - } - } else { - // Previously moved components are no longer moving - reset = true - } - if reset { - break - } - } - if reset { - combinedSteps = append(combinedSteps, currStep) - currStep = map[string][][]referenceframe.Input{} - } - for name, inputs := range step { - if len(inputs) == 0 { - continue - } - currStep[name] = append(currStep[name], inputs) - } - } - } - combinedSteps = append(combinedSteps, currStep) - - for _, step := range combinedSteps { - for name, inputs := range step { - if len(inputs) == 0 { - continue - } - r, ok := resources[name] - if !ok { - return false, fmt.Errorf("plan had step for resource %s but no resource with that name found in framesystem", name) - } - if err := r.GoToInputs(ctx, inputs...); err != nil { - // If there is an error on GoToInputs, stop the component if possible before returning the error - if actuator, ok := r.(inputEnabledActuator); ok { - if stopErr := actuator.Stop(ctx, nil); stopErr != nil { - return false, errors.Wrap(err, stopErr.Error()) - } - } - return false, err - } - } - } - return true, nil + err = ms.execute(ctx, plan) + return err != nil, err } func (ms *builtIn) MoveOnMap(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { @@ -441,11 +332,159 @@ func (ms *builtIn) PlanHistory( } func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { - moveReq, ok := cmd["Plan"] - if ok { - // call plan + ms.mu.RLock() + defer ms.mu.RUnlock() + + resp := make(map[string]interface{}, 0) + if val, ok := cmd[DoPlan]; ok { + moveReq, err := utils.AssertType[motion.MoveReq](val) + if err != nil { + return nil, err + } + plan, err := ms.plan(ctx, moveReq) + if err != nil { + return nil, err + } + resp[DoPlan] = plan + } + if val, ok := cmd[DoExecute]; ok { + plan, err := utils.AssertType[motionplan.Plan](val) + if err != nil { + return nil, err + } + resp[DoExecute] = ms.execute(ctx, plan) != nil + } + return resp, nil +} + +func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Plan, error) { + operation.CancelOtherWithLabel(ctx, builtinOpLabel) + + frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) + if err != nil { + return nil, err + } + + // build maps of relevant components and inputs from initial inputs + fsInputs, _, err := ms.fsService.CurrentInputs(ctx) + if err != nil { + return nil, err + } + ms.logger.CDebugf(ctx, "frame system inputs: %v", fsInputs) + + movingFrame := frameSys.Frame(req.ComponentName.ShortName()) + if movingFrame == nil { + return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName()) + } + + // re-evaluate goalPose to be in the frame of World + solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName + tf, err := frameSys.Transform(fsInputs, &req.Destination, solvingFrame) + if err != nil { + return nil, err + } + goalPose, _ := tf.(*referenceframe.PoseInFrame) + + // the goal is to move the component to goalPose which is specified in coordinates of goalFrameName + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: ms.logger, + Goal: goalPose, + Frame: movingFrame, + StartConfiguration: fsInputs, + FrameSystem: frameSys, + WorldState: &req.WorldState, + Constraints: &req.Constraints, + Options: req.Extra, + }) + if err != nil { + return nil, err + } + return plan, nil +} + +func (ms *builtIn) execute(ctx context.Context, plan motionplan.Plan) error { + // build maps of relevant components and inputs from initial inputs + _, resources, err := ms.fsService.CurrentInputs(ctx) + if err != nil { + return err + } + + // Batch GoToInputs calls if possible; components may want to blend between inputs + combinedSteps := []map[string][][]referenceframe.Input{} + currStep := map[string][][]referenceframe.Input{} + for i, step := range plan.Trajectory() { + if i == 0 { + for name, inputs := range step { + if len(inputs) == 0 { + continue + } + currStep[name] = append(currStep[name], inputs) + } + continue + } + changed := "" + if len(currStep) > 0 { + reset := false + // Check if the current step moves only the same components as the previous step + // If so, batch the inputs + for name, inputs := range step { + if len(inputs) == 0 { + continue + } + if priorInputs, ok := currStep[name]; ok { + for i, input := range inputs { + if input != priorInputs[len(priorInputs)-1][i] { + if changed == "" { + changed = name + } + if changed != "" && changed != name { + // If the current step moves different components than the previous step, reset the batch + reset = true + break + } + } + } + } else { + // Previously moved components are no longer moving + reset = true + } + if reset { + break + } + } + if reset { + combinedSteps = append(combinedSteps, currStep) + currStep = map[string][][]referenceframe.Input{} + } + for name, inputs := range step { + if len(inputs) == 0 { + continue + } + currStep[name] = append(currStep[name], inputs) + } + } } - if plan, ok := cmd["Execute"]; ok { - // call execute + combinedSteps = append(combinedSteps, currStep) + + for _, step := range combinedSteps { + for name, inputs := range step { + if len(inputs) == 0 { + continue + } + r, ok := resources[name] + if !ok { + return fmt.Errorf("plan had step for resource %s but no resource with that name found in framesystem", name) + } + if err := r.GoToInputs(ctx, inputs...); err != nil { + // If there is an error on GoToInputs, stop the component if possible before returning the error + if actuator, ok := r.(inputEnabledActuator); ok { + if stopErr := actuator.Stop(ctx, nil); stopErr != nil { + return errors.Wrap(err, stopErr.Error()) + } + } + return err + } + } } -} \ No newline at end of file + return nil +} diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index eeee2272c1a..5debb5c4c7f 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -130,7 +130,7 @@ func TestMoveFailures(t *testing.T) { ctx := context.Background() t.Run("fail on not finding gripper", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("fakeCamera", spatialmath.NewPoseFromPoint(r3.Vector{X: 10.0, Y: 10.0, Z: 10.0})) - _, err = ms.Move(ctx, camera.Named("fake"), grabPose, nil, nil, nil) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: camera.Named("fake"), Destination: *grabPose}) test.That(t, err, test.ShouldNotBeNil) }) @@ -145,7 +145,7 @@ func TestMoveFailures(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) poseInFrame := referenceframe.NewPoseInFrame("frame2", spatialmath.NewZeroPose()) - _, err = ms.Move(ctx, arm.Named("arm1"), poseInFrame, worldState, nil, nil) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1"), Destination: *poseInFrame, WorldState: *worldState}) test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("frame2", "noParent")) }) } @@ -158,7 +158,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, gripper.Named("pieceGripper"), grabPose, nil, nil, nil) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -166,7 +166,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceArm", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, arm.Named("pieceArm"), grabPose, nil, nil, map[string]interface{}{}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("pieceArm"), Destination: *grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -174,7 +174,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, gripper.Named("pieceGripper"), grabPose, nil, nil, map[string]interface{}{}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -194,7 +194,10 @@ func TestMove(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) grabPose := referenceframe.NewPoseInFrame("testFrame2", spatialmath.NewPoseFromPoint(r3.Vector{X: -20, Y: -130, Z: -40})) - _, err = ms.Move(context.Background(), gripper.Named("pieceGripper"), grabPose, worldState, nil, nil) + _, err = ms.Move( + context.Background(), + motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose, WorldState: *worldState}, + ) test.That(t, err, test.ShouldBeNil) }) } @@ -242,7 +245,10 @@ func TestMoveWithObstacles(t *testing.T) { } worldState, err := referenceframe.WorldStateFromProtobuf(&commonpb.WorldState{Obstacles: obsMsgs}) test.That(t, err, test.ShouldBeNil) - _, err = ms.Move(context.Background(), gripper.Named("pieceArm"), grabPose, worldState, nil, nil) + _, err = ms.Move( + context.Background(), + motion.MoveReq{ComponentName: gripper.Named("pieceArm"), Destination: *grabPose, WorldState: *worldState}, + ) // This fails due to a large obstacle being in the way test.That(t, err, test.ShouldNotBeNil) }) @@ -416,7 +422,7 @@ func TestMultiplePieces(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/fake_tomato.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: -0, Y: -30, Z: -50})) - _, err = ms.Move(context.Background(), gripper.Named("gr"), grabPose, nil, nil, nil) + _, err = ms.Move(context.Background(), motion.MoveReq{ComponentName: gripper.Named("gr"), Destination: *grabPose}) test.That(t, err, test.ShouldBeNil) } @@ -568,7 +574,7 @@ func TestStoppableMoveFunctions(t *testing.T) { t.Run("stop during Move(...) call", func(t *testing.T) { calledStopFunc = false - success, err := ms.Move(ctx, injectArmName, goal, nil, nil, extra) + success, err := ms.Move(ctx, motion.MoveReq{ComponentName: injectArmName, Destination: *goal, Extra: extra}) testIfStoppable(t, success, err, failToReachGoalError) }) }) diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 4e33d355a1c..6fcba9b3763 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -89,14 +89,7 @@ func TestClient(t *testing.T) { receivedTransforms := make(map[string]*referenceframe.LinkInFrame) success := true - injectMS.MoveFunc = func( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { + injectMS.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { return success, nil } injectMS.GetPoseFunc = func( @@ -114,7 +107,7 @@ func TestClient(t *testing.T) { } // Move - result, err := client.Move(ctx, gripperName, zeroPoseInFrame, nil, nil, nil) + result, err := client.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: *zeroPoseInFrame}) test.That(t, err, test.ShouldBeNil) test.That(t, result, test.ShouldEqual, success) @@ -165,14 +158,7 @@ func TestClient(t *testing.T) { test.That(t, err, test.ShouldBeNil) passedErr := errors.New("fake move error") - injectMS.MoveFunc = func( - ctx context.Context, - componentName resource.Name, - grabPose *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { + injectMS.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { return false, passedErr } passedErr = errors.New("fake GetPose error") @@ -187,7 +173,7 @@ func TestClient(t *testing.T) { } // Move - resp, err := client2.Move(ctx, gripperName, zeroPoseInFrame, nil, nil, nil) + resp, err := client2.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: *zeroPoseInFrame}) test.That(t, err.Error(), test.ShouldContainSubstring, passedErr.Error()) test.That(t, resp, test.ShouldEqual, false) diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go deleted file mode 100644 index 03a89a393ae..00000000000 --- a/services/motion/explore/explore.go +++ /dev/null @@ -1,632 +0,0 @@ -// Package explore implements a motion service for exploration. This motion service model is a temporary model -// that will be incorporated into the rdk:builtin:motion service in the future. -package explore - -import ( - "context" - "fmt" - "math" - "strconv" - "strings" - "sync" - "time" - - "github.com/golang/geo/r3" - "github.com/google/uuid" - "github.com/pkg/errors" - goutils "go.viam.com/utils" - - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/base/kinematicbase" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/motionplan" - "go.viam.com/rdk/operation" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot/framesystem" - "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/vision" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/utils" -) - -var ( - model = resource.DefaultModelFamily.WithModel("explore") - errUnimplemented = errors.New("unimplemented") - // The distance a detected obstacle can be from a base to trigger the Move command to stop. - lookAheadDistanceMM = 500. - // successiveErrorLimit places a limit on the number of errors that can occur in a row when running - // checkForObstacles. - successiveErrorLimit = 5 - // Defines the limit on how far a potential kinematic base move action can be. For explore there is none. - defaultMoveLimitMM = math.Inf(1) - // The timeout for any individual move action on the kinematic base. - defaultExploreTimeout = 100 * time.Second - // The max angle the kinematic base spin action can be. - defaultMaxSpinAngleDegs = 180. -) - -func init() { - resource.RegisterService( - motion.API, - model, - resource.Registration[motion.Service, *Config]{ - Constructor: NewExplore, - WeakDependencies: []resource.Matcher{ - resource.TypeMatcher{Type: resource.APITypeComponentName}, - resource.SubtypeMatcher{Subtype: vision.SubtypeName}, - }, - }, - ) -} - -const ( - exploreOpLabel = "explore-motion-service" -) - -// moveResponse is the message type returned by both channels: obstacle detection and plan execution. -// It contains a bool stating if an obstacle was found or the execution was complete and/or any -// associated error. -type moveResponse struct { - err error - success bool -} - -// inputEnabledActuator is an actuator that interacts with the frame system. -// This allows us to figure out where the actuator currently is and then -// move it. Input units are always in meters or radians. -type inputEnabledActuator interface { - resource.Actuator - referenceframe.InputEnabled -} - -// obstacleDetectorObject provides a map for matching vision services to any and all cameras names they use. -type obstacleDetectorObject map[vision.Service]resource.Name - -// Config describes how to configure the service. As the explore motion service is not being surfaced to users, this is blank. -type Config struct{} - -// Validate here adds a dependency on the internal framesystem service. -func (c *Config) Validate(path string) ([]string, error) { - return []string{framesystem.InternalServiceName.String()}, nil -} - -// NewExplore returns a new explore motion service for the given robot. -func NewExplore(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger logging.Logger) (motion.Service, error) { - ms := &explore{ - Named: conf.ResourceName().AsNamed(), - logger: logger, - obstacleResponseChan: make(chan moveResponse), - executionResponseChan: make(chan moveResponse), - backgroundWorkers: &sync.WaitGroup{}, - } - - if err := ms.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - return ms, nil -} - -// Reconfigure updates the explore motion service when the config has changed. -func (ms *explore) Reconfigure( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, -) (err error) { - ms.resourceMutex.Lock() - defer ms.resourceMutex.Unlock() - // Iterate over dependencies and store components and services along with the frame service directly - components := make(map[resource.Name]resource.Resource) - services := make(map[resource.Name]resource.Resource) - for name, dep := range deps { - switch dep := dep.(type) { - case framesystem.Service: - ms.fsService = dep - case vision.Service: - services[name] = dep - default: - components[name] = dep - } - } - - ms.components = components - ms.services = services - return nil -} - -type explore struct { - resource.Named - logger logging.Logger - - processCancelFunc context.CancelFunc - obstacleResponseChan chan moveResponse - executionResponseChan chan moveResponse - backgroundWorkers *sync.WaitGroup - - // resourceMutex protects the connect to other resources, the frame service/system and prevent multiple - // Move and/or Reconfigure actions from being performed simultaneously. - resourceMutex sync.Mutex - components map[resource.Name]resource.Resource - services map[resource.Name]resource.Resource - fsService framesystem.Service - frameSystem referenceframe.FrameSystem -} - -func (ms *explore) MoveOnMap(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { - return uuid.Nil, errUnimplemented -} - -func (ms *explore) MoveOnGlobe( - ctx context.Context, - req motion.MoveOnGlobeReq, -) (motion.ExecutionID, error) { - return uuid.Nil, errUnimplemented -} - -func (ms *explore) GetPose( - ctx context.Context, - componentName resource.Name, - destinationFrame string, - supplementalTransforms []*referenceframe.LinkInFrame, - extra map[string]interface{}, -) (*referenceframe.PoseInFrame, error) { - return nil, errUnimplemented -} - -func (ms *explore) StopPlan( - ctx context.Context, - req motion.StopPlanReq, -) error { - return errUnimplemented -} - -func (ms *explore) ListPlanStatuses( - ctx context.Context, - req motion.ListPlanStatusesReq, -) ([]motion.PlanStatusWithID, error) { - return nil, errUnimplemented -} - -func (ms *explore) PlanHistory( - ctx context.Context, - req motion.PlanHistoryReq, -) ([]motion.PlanWithStatus, error) { - return nil, errUnimplemented -} - -func (ms *explore) Close(ctx context.Context) error { - ms.resourceMutex.Lock() - defer ms.resourceMutex.Unlock() - - if ms.processCancelFunc != nil { - ms.processCancelFunc() - } - utils.FlushChan(ms.obstacleResponseChan) - utils.FlushChan(ms.executionResponseChan) - ms.backgroundWorkers.Wait() - return nil -} - -// Move takes a goal location and will plan and execute a movement to move a component specified by its name -// to that destination. -func (ms *explore) Move( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, -) (bool, error) { - ms.resourceMutex.Lock() - defer ms.resourceMutex.Unlock() - - operation.CancelOtherWithLabel(ctx, exploreOpLabel) - - // Parse extras - motionCfg, err := parseMotionConfig(extra) - if err != nil { - return false, err - } - - // obstacleDetectors - obstacleDetectors, err := ms.createObstacleDetectors(motionCfg) - if err != nil { - return false, err - } - - // Create kinematic base - kb, err := ms.createKinematicBase(ctx, componentName, motionCfg) - if err != nil { - return false, err - } - - // Create motionplan plan - plan, err := ms.createMotionPlan(ctx, kb, destination, extra) - if err != nil { - return false, err - } - - // Start background processes - cancelCtx, cancel := context.WithCancel(ctx) - ms.processCancelFunc = cancel - defer ms.processCancelFunc() - - // Start polling for obstacles - ms.backgroundWorkers.Add(1) - goutils.ManagedGo(func() { - ms.checkForObstacles(cancelCtx, obstacleDetectors, kb, plan, *motionCfg.ObstaclePollingFreqHz) - }, ms.backgroundWorkers.Done) - - // Start executing plan - ms.backgroundWorkers.Add(1) - goutils.ManagedGo(func() { - ms.executePlan(cancelCtx, kb, plan) - }, ms.backgroundWorkers.Done) - - // this ensures that if the context is cancelled we always return early at the top of the loop - if err := ctx.Err(); err != nil { - return false, err - } - - select { - // if context was cancelled by the calling function, error out - case <-ctx.Done(): - return false, ctx.Err() - - // once execution responds: return the result to the caller - case resp := <-ms.executionResponseChan: - ms.logger.CDebugf(ctx, "execution completed: %v", resp) - if resp.err != nil { - return resp.success, resp.err - } - if resp.success { - return true, nil - } - - // if the checkPartialPlan process hit an error return it, otherwise exit - case resp := <-ms.obstacleResponseChan: - ms.logger.CDebugf(ctx, "obstacle response: %v", resp) - if resp.err != nil { - return resp.success, resp.err - } - if resp.success { - return false, nil - } - } - return false, errors.New("no obstacle was seen during movement to goal and goal was not reached") -} - -// checkForObstacles will continuously monitor the generated transient worldState for obstacles in the given -// motionplan plan. A response will be sent through the channel if an error occurs, the motionplan plan -// completes or an obstacle is detected in the given range. -func (ms *explore) checkForObstacles( - ctx context.Context, - obstacleDetectors []obstacleDetectorObject, - kb kinematicbase.KinematicBase, - plan motionplan.Plan, - obstaclePollingFrequencyHz float64, -) { - ms.logger.Debug("Current Plan") - ms.logger.Debug(plan) - - // Constantly check for obstacles in path at desired obstacle polling frequency - ticker := time.NewTicker(time.Duration(int(1000/obstaclePollingFrequencyHz)) * time.Millisecond) - defer ticker.Stop() - - var errCounterCurrentInputs, errCounterGenerateTransientWorldState int - for { - select { - case <-ctx.Done(): - return - case <-ticker.C: - - currentInputs, err := kb.CurrentInputs(ctx) - if err != nil { - ms.logger.Debugf("issue occurred getting current inputs from kinematic base: %v", err) - if errCounterCurrentInputs > successiveErrorLimit { - ms.obstacleResponseChan <- moveResponse{success: false} - return - } - errCounterCurrentInputs++ - } else { - errCounterCurrentInputs = 0 - } - - currentPose := spatialmath.NewPose( - r3.Vector{X: currentInputs[0].Value, Y: currentInputs[1].Value, Z: 0}, - &spatialmath.OrientationVector{OX: 0, OY: 0, OZ: 1, Theta: currentInputs[2].Value}, - ) - - // Look for new transient obstacles and add to worldState - worldState, err := ms.generateTransientWorldState(ctx, obstacleDetectors) - if err != nil { - ms.logger.CDebugf(ctx, "issue occurred generating transient worldState: %v", err) - if errCounterGenerateTransientWorldState > successiveErrorLimit { - ms.obstacleResponseChan <- moveResponse{success: false} - return - } - errCounterGenerateTransientWorldState++ - } else { - errCounterGenerateTransientWorldState = 0 - } - - // TODO: Generalize this fix to work for maps with non-transient obstacles. This current implementation - // relies on the plan being two steps: a start position and a goal position. - // JIRA Ticket: https://viam.atlassian.net/browse/RSDK-5964 - planTraj := plan.Trajectory() - planTraj[0][kb.Name().ShortName()] = currentInputs - ms.logger.Debugf("Current transient worldState: ", worldState.String()) - - executionState, err := motionplan.NewExecutionState( - plan, - 0, - plan.Trajectory()[0], - map[string]*referenceframe.PoseInFrame{ - kb.Kinematics().Name(): referenceframe.NewPoseInFrame(referenceframe.World, currentPose), - }, - ) - if err != nil { - ms.logger.Debugf("issue occurred getting execution state from kinematic base: %v", err) - if errCounterCurrentInputs > successiveErrorLimit { - ms.obstacleResponseChan <- moveResponse{success: false} - return - } - errCounterCurrentInputs++ - } - - // Check plan for transient obstacles - err = motionplan.CheckPlan( - kb.Kinematics(), - executionState, - worldState, - ms.frameSystem, - lookAheadDistanceMM, - ms.logger, - ) - if err != nil { - if strings.Contains(err.Error(), "found constraint violation or collision") { - ms.logger.CDebug(ctx, "collision found in given range") - ms.obstacleResponseChan <- moveResponse{success: true} - return - } - } - } - } -} - -// executePlan will carry out the desired motionplan plan. -func (ms *explore) executePlan(ctx context.Context, kb kinematicbase.KinematicBase, plan motionplan.Plan) { - steps, err := plan.Trajectory().GetFrameInputs(kb.Name().Name) - if err != nil { - ms.logger.Debugf("error in executePlan: %s", err) - return - } - for _, inputs := range steps { - select { - case <-ctx.Done(): - return - default: - if inputEnabledKb, ok := kb.(inputEnabledActuator); ok { - if err := inputEnabledKb.GoToInputs(ctx, inputs); err != nil { - // If there is an error on GoToInputs, stop the component if possible before returning the error - if stopErr := kb.Stop(ctx, nil); stopErr != nil { - ms.executionResponseChan <- moveResponse{err: err} - return - } - // If the error was simply a cancellation of context return without erroring out - if errors.Is(err, context.Canceled) { - return - } - ms.executionResponseChan <- moveResponse{err: err} - return - } - } else { - ms.executionResponseChan <- moveResponse{err: errors.New("unable to cast kinematic base to inputEnabledActuator")} - return - } - } - } - ms.executionResponseChan <- moveResponse{success: true} -} - -// generateTransientWorldState will create a new worldState with transient obstacles generated by the provided -// obstacleDetectors. -func (ms *explore) generateTransientWorldState( - ctx context.Context, - obstacleDetectors []obstacleDetectorObject, -) (*referenceframe.WorldState, error) { - geometriesInFrame := []*referenceframe.GeometriesInFrame{} - - // Iterate through provided obstacle detectors and their associated vision service and cameras - for _, obstacleDetector := range obstacleDetectors { - for visionService, cameraName := range obstacleDetector { - // Get detections as vision objects - detections, err := visionService.GetObjectPointClouds(ctx, cameraName.Name, nil) - if err != nil && strings.Contains(err.Error(), "does not implement a 3D segmenter") { - ms.logger.CInfof(ctx, "cannot call GetObjectPointClouds on %q as it does not implement a 3D segmenter", - visionService.Name()) - } else if err != nil { - return nil, err - } - - // Extract geometries from vision objects - geometries := []spatialmath.Geometry{} - for i, detection := range detections { - geometry := detection.Geometry - label := cameraName.Name + "_transientObstacle_" + strconv.Itoa(i) - if geometry.Label() != "" { - label += "_" + geometry.Label() - } - geometry.SetLabel(label) - geometries = append(geometries, geometry) - } - geometriesInFrame = append(geometriesInFrame, - referenceframe.NewGeometriesInFrame(cameraName.Name, geometries), - ) - } - } - - // Add geometries to worldState - worldState, err := referenceframe.NewWorldState(geometriesInFrame, nil) - if err != nil { - return nil, err - } - return worldState, nil -} - -// createKinematicBase will instantiate a kinematic base from the provided base name and motionCfg with -// associated kinematic base options. This will be a differential drive kinematic base. -func (ms *explore) createKinematicBase( - ctx context.Context, - baseName resource.Name, - motionCfg motion.MotionConfiguration, -) (kinematicbase.KinematicBase, error) { - // Select the base from the component list using the baseName - component, ok := ms.components[baseName] - if !ok { - return nil, resource.DependencyNotFoundError(baseName) - } - - b, ok := component.(base.Base) - if !ok { - return nil, fmt.Errorf("cannot get component of type %T because it is not a Case", component) - } - - // Generate kinematic base options from motionCfg - kinematicsOptions := kinematicbase.NewKinematicBaseOptions() - kinematicsOptions.NoSkidSteer = true - kinematicsOptions.UsePTGs = false - // PositionOnlyMode needs to be false to allow orientation info to be available in CurrentInputs as - // we need that to correctly place obstacles in world's reference frame. - kinematicsOptions.PositionOnlyMode = false - kinematicsOptions.AngularVelocityDegsPerSec = motionCfg.AngularDegsPerSec - kinematicsOptions.LinearVelocityMMPerSec = motionCfg.LinearMPerSec * 1000 - kinematicsOptions.Timeout = defaultExploreTimeout - kinematicsOptions.MaxSpinAngleDeg = defaultMaxSpinAngleDegs - kinematicsOptions.MaxMoveStraightMM = defaultMoveLimitMM - - // Create new kinematic base (differential drive) - kb, err := kinematicbase.WrapWithKinematics( - ctx, - b, - ms.logger, - nil, - []referenceframe.Limit{ - {Min: -defaultMoveLimitMM, Max: defaultMoveLimitMM}, - {Min: -defaultMoveLimitMM, Max: defaultMoveLimitMM}, - {Min: -2 * math.Pi, Max: 2 * math.Pi}, - }, - kinematicsOptions, - ) - if err != nil { - return nil, err - } - - return kb, nil -} - -// createObstacleDetectors will generate the list of obstacle detectors from the camera and vision services -// names provided in them motionCfg. -func (ms *explore) createObstacleDetectors(motionCfg motion.MotionConfiguration) ([]obstacleDetectorObject, error) { - var obstacleDetectors []obstacleDetectorObject - // Iterate through obstacleDetectorsNames - for _, obstacleDetectorsName := range motionCfg.ObstacleDetectors { - // Select the vision service from the service list using the vision service name in obstacleDetectorsNames - visionServiceResource, ok := ms.services[obstacleDetectorsName.VisionServiceName] - if !ok { - return nil, resource.DependencyNotFoundError(obstacleDetectorsName.VisionServiceName) - } - visionService, ok := visionServiceResource.(vision.Service) - if !ok { - return nil, fmt.Errorf("cannot get service of type %T because it is not a vision service", - visionServiceResource, - ) - } - - // Select the camera from the component list using the camera name in obstacleDetectorsNames - // Note: May need to be converted to a for loop if we accept multiple cameras for each vision service - cameraResource, ok := ms.components[obstacleDetectorsName.CameraName] - if !ok { - return nil, resource.DependencyNotFoundError(obstacleDetectorsName.CameraName) - } - cam, ok := cameraResource.(camera.Camera) - if !ok { - return nil, fmt.Errorf("cannot get component of type %T because it is not a camera", cameraResource) - } - obstacleDetectors = append(obstacleDetectors, obstacleDetectorObject{visionService: cam.Name()}) - } - - return obstacleDetectors, nil -} - -// createMotionPlan will construct a motion plan towards a specified destination using the given kinematic base. -// No position knowledge is assumed so every plan uses the base, and therefore world, origin as the starting location. -func (ms *explore) createMotionPlan( - ctx context.Context, - kb kinematicbase.KinematicBase, - destination *referenceframe.PoseInFrame, - extra map[string]interface{}, -) (motionplan.Plan, error) { - if destination.Pose().Point().Norm() >= defaultMoveLimitMM { - return nil, errors.Errorf("destination %v is above the defined limit of %v", destination.Pose().Point().String(), defaultMoveLimitMM) - } - - worldState, err := referenceframe.NewWorldState(nil, nil) - if err != nil { - return nil, err - } - - ms.frameSystem, err = ms.fsService.FrameSystem(ctx, worldState.Transforms()) - if err != nil { - return nil, err - } - - // replace origin base frame to remove original differential drive kinematic base geometry as it is overwritten by a bounding sphere - // during kinematic base creation - if err := ms.frameSystem.ReplaceFrame(referenceframe.NewZeroStaticFrame(kb.Kinematics().Name() + "_origin")); err != nil { - return nil, err - } - - // replace original base frame with one that knows how to move itself and allow planning for - if err := ms.frameSystem.ReplaceFrame(kb.Kinematics()); err != nil { - return nil, err - } - - inputs := make([]referenceframe.Input, len(kb.Kinematics().DoF())) - - f := kb.Kinematics() - - seedMap := map[string][]referenceframe.Input{f.Name(): inputs} - - goalPose, err := ms.fsService.TransformPose(ctx, destination, ms.frameSystem.World().Name(), nil) - if err != nil { - return nil, err - } - - ms.logger.CDebugf(ctx, "goal position: %v", goalPose.Pose().Point()) - return motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: ms.logger, - Goal: goalPose, - Frame: f, - StartConfiguration: seedMap, - FrameSystem: ms.frameSystem, - WorldState: worldState, - Constraints: nil, - Options: extra, - }) -} - -// parseMotionConfig extracts the MotionConfiguration from extra's. -func parseMotionConfig(extra map[string]interface{}) (motion.MotionConfiguration, error) { - motionCfgInterface, ok := extra["motionCfg"] - if !ok { - return motion.MotionConfiguration{}, errors.New("no motionCfg provided") - } - - motionCfg, ok := motionCfgInterface.(motion.MotionConfiguration) - if !ok { - return motion.MotionConfiguration{}, errors.New("could not interpret motionCfg field as an MotionConfiguration") - } - return motionCfg, nil -} diff --git a/services/motion/explore/explore_test.go b/services/motion/explore/explore_test.go deleted file mode 100644 index 658100c6193..00000000000 --- a/services/motion/explore/explore_test.go +++ /dev/null @@ -1,386 +0,0 @@ -package explore - -import ( - "context" - "fmt" - "testing" - "time" - - "github.com/golang/geo/r3" - "github.com/pkg/errors" - "go.viam.com/test" - goutils "go.viam.com/utils" - - "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/camera" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot/framesystem" - "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/spatialmath" -) - -var ( - testBaseName = resource.NewName(base.API, "test_base") - testCameraName1 = resource.NewName(camera.API, "test_camera1") - testCameraName2 = resource.NewName(camera.API, "test_camera2") - testFrameServiceName = resource.NewName(framesystem.API, "test_fs") - defaultCollisionBufferMM = 1e-8 - defaultObstaclePollingHz = 2. - defaultMotionCfg = motion.MotionConfiguration{ - AngularDegsPerSec: 0.25, - LinearMPerSec: 0.1, - ObstaclePollingFreqHz: &defaultObstaclePollingHz, - } -) - -type obstacleMetadata struct { - position r3.Vector - data int - label string -} - -func TestExplorePlanMove(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - - fakeBase, err := createFakeBase(ctx, logger) - test.That(t, err, test.ShouldBeNil) - - ms, err := createNewExploreMotionService(ctx, logger, fakeBase, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, ms, test.ShouldNotBeNil) - defer ms.Close(ctx) - - msStruct := ms.(*explore) - - // Create kinematic base - kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) - test.That(t, err, test.ShouldBeNil) - test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) - - cases := []struct { - description string - destination spatialmath.Pose - expectedMotionPlanLength int - }{ - { - description: "destination directly in front of base", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - expectedMotionPlanLength: 2, - }, - { - description: "destination directly behind the base", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -1000, Z: 0}), - expectedMotionPlanLength: 2, - }, - { - description: "destination off axis of base", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 1000, Y: 1000, Z: 0}), - expectedMotionPlanLength: 2, - }, - } - - for _, tt := range cases { - t.Run(tt.description, func(t *testing.T) { - dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) - plan, err := msStruct.createMotionPlan(ctx, kb, dest, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, len(plan.Path()), test.ShouldEqual, tt.expectedMotionPlanLength) - }) - } -} - -func TestExploreCheckForObstacles(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - - // Create fake camera - fakeCamera, err := createFakeCamera(ctx, logger, testCameraName1.Name) - test.That(t, err, test.ShouldBeNil) - - // Create fake base - fakeBase, err := createFakeBase(ctx, logger) - test.That(t, err, test.ShouldBeNil) - - // Create explore motion service - ms, err := createNewExploreMotionService(ctx, logger, fakeBase, []camera.Camera{fakeCamera}) - test.That(t, err, test.ShouldBeNil) - test.That(t, ms, test.ShouldNotBeNil) - defer ms.Close(ctx) - - msStruct := ms.(*explore) - - // Create kinematic base - kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) - test.That(t, err, test.ShouldBeNil) - test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) - - cases := []struct { - description string - destination spatialmath.Pose - obstacle []obstacleMetadata - detection bool - detectionErr error - }{ - { - description: "no obstacles in view", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - detection: false, - }, - { - description: "obstacle in path nearby", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacle: []obstacleMetadata{ - { - position: r3.Vector{X: 0, Y: 300, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - }, - detection: true, - }, - { - description: "obstacle in path farther away", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacle: []obstacleMetadata{ - { - position: r3.Vector{X: 0, Y: 800, Z: 0}, - data: 100, - label: "far_obstacle_in_path", - }, - }, - detection: false, - }, - { - description: "obstacle in path too far", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 9000, Z: 0}), - obstacle: []obstacleMetadata{ - { - position: r3.Vector{X: 0, Y: 1500, Z: 0}, - data: 100, - label: "far_obstacle_not_in_path", - }, - }, - detection: false, - detectionErr: errors.New("found collision between positions"), - }, - { - description: "obstacle off axis in path", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 1000, Y: 1000, Z: 0}), - obstacle: []obstacleMetadata{ - { - position: r3.Vector{X: 500, Y: 500, Z: 0}, - data: 100, - label: "obstacle on diagonal", - }, - }, - detection: true, - }, - { - description: "obstacle off axis not in path", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacle: []obstacleMetadata{ - { - position: r3.Vector{X: 500, Y: 500, Z: 0}, - data: 100, - label: "close_obstacle_not_in_path", - }, - }, - detection: false, - }, - } - - for _, tt := range cases { - t.Run(tt.description, func(t *testing.T) { - // Create motionplan plan - dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) - plan, err := msStruct.createMotionPlan( - ctx, - kb, - dest, - nil, - ) - test.That(t, err, test.ShouldBeNil) - test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) - test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 0) - - // Create a vision service using provided obstacles and place it in an obstacle DetectorPair object - visionService := createMockVisionService("", tt.obstacle) - - obstacleDetectors := []obstacleDetectorObject{ - {visionService: fakeCamera.Name()}, - } - - // Update and check worldState - worldState, err := msStruct.generateTransientWorldState(ctx, obstacleDetectors) - test.That(t, err, test.ShouldBeNil) - - if len(tt.obstacle) == 0 { - fs, err := msStruct.fsService.FrameSystem(ctx, nil) - test.That(t, err, test.ShouldBeNil) - - obstacles, err := worldState.ObstaclesInWorldFrame(fs, nil) - test.That(t, err, test.ShouldBeNil) - - for _, obs := range tt.obstacle { - collisionDetected, err := geometriesContainsPoint(obstacles.Geometries(), obs.position, defaultCollisionBufferMM) - test.That(t, err, test.ShouldBeNil) - test.That(t, collisionDetected, test.ShouldBeTrue) - } - } - - // Run check obstacles in of plan path - ctxTimeout, cancelFunc := context.WithTimeout(ctx, 1*time.Second) - defer cancelFunc() - - msStruct.backgroundWorkers.Add(1) - goutils.ManagedGo(func() { - msStruct.checkForObstacles(ctxTimeout, obstacleDetectors, kb, plan, *defaultMotionCfg.ObstaclePollingFreqHz) - }, msStruct.backgroundWorkers.Done) - - time.Sleep(20 * time.Millisecond) - - if tt.detection { - resp := <-msStruct.obstacleResponseChan - test.That(t, resp.success, test.ShouldEqual, tt.detection) - if tt.detectionErr == nil { - test.That(t, resp.err, test.ShouldBeNil) - } else { - test.That(t, resp.err, test.ShouldNotBeNil) - test.That(t, resp.err.Error(), test.ShouldContainSubstring, tt.detectionErr.Error()) - } - } - }) - } -} - -func TestMultipleObstacleDetectors(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - - // Create fake cameras - fakeCamera1, err := createFakeCamera(ctx, logger, testCameraName1.Name) - test.That(t, err, test.ShouldBeNil) - - fakeCamera2, err := createFakeCamera(ctx, logger, testCameraName2.Name) - test.That(t, err, test.ShouldBeNil) - - // Create fake base - fakeBase, err := createFakeBase(ctx, logger) - test.That(t, err, test.ShouldBeNil) - - // Create explore motion service - ms, err := createNewExploreMotionService(ctx, logger, fakeBase, []camera.Camera{fakeCamera1, fakeCamera2}) - test.That(t, err, test.ShouldBeNil) - test.That(t, ms, test.ShouldNotBeNil) - defer ms.Close(ctx) - - msStruct := ms.(*explore) - - // Create kinematic base - kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) - test.That(t, err, test.ShouldBeNil) - test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) - - cases := []struct { - description string - destination spatialmath.Pose - obstacleCamera1 obstacleMetadata - obstacleCamera2 obstacleMetadata - visionServiceCamLink []camera.Camera - }{ - { - description: "two independent vision services both detecting obstacles", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacleCamera1: obstacleMetadata{ - position: r3.Vector{X: 0, Y: 300, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - obstacleCamera2: obstacleMetadata{ - position: r3.Vector{X: 0, Y: 500, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, - }, - { - description: "two independent vision services only first detecting obstacle", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacleCamera1: obstacleMetadata{ - position: r3.Vector{X: 0, Y: 300, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - obstacleCamera2: obstacleMetadata{}, - visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, - }, - { - description: "two independent vision services only second detecting obstacle", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacleCamera1: obstacleMetadata{}, - obstacleCamera2: obstacleMetadata{ - position: r3.Vector{X: 0, Y: 300, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, - }, - { - description: "two vision services depending on same camera", - destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), - obstacleCamera1: obstacleMetadata{ - position: r3.Vector{X: 0, Y: 300, Z: 0}, - data: 100, - label: "close_obstacle_in_path", - }, - obstacleCamera2: obstacleMetadata{}, - visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, - }, - } - - for _, tt := range cases { - t.Run(tt.description, func(t *testing.T) { - // Create motionplan plan - dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) - plan, err := msStruct.createMotionPlan( - ctx, - kb, - dest, - nil, - ) - test.That(t, err, test.ShouldBeNil) - test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) - test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 0) - - // Create a vision service using provided obstacles and place it in an obstacle DetectorPair object - var obstacleDetectors []obstacleDetectorObject - for i, cam := range tt.visionServiceCamLink { - var obstacles []obstacleMetadata - if cam.Name().Name == testCameraName1.Name { - obstacles = append(obstacles, tt.obstacleCamera1) - } - if cam.Name().Name == testCameraName2.Name { - obstacles = append(obstacles, tt.obstacleCamera2) - } - visionService := createMockVisionService(fmt.Sprint(i), obstacles) - obstacleDetectors = append(obstacleDetectors, obstacleDetectorObject{visionService: cam.Name()}) - } - - // Run check obstacles in of plan path - ctxTimeout, cancelFunc := context.WithTimeout(ctx, 1*time.Second) - defer cancelFunc() - - msStruct.backgroundWorkers.Add(1) - goutils.ManagedGo(func() { - msStruct.checkForObstacles(ctxTimeout, obstacleDetectors, kb, plan, *defaultMotionCfg.ObstaclePollingFreqHz) - }, msStruct.backgroundWorkers.Done) - - resp := <-msStruct.obstacleResponseChan - test.That(t, resp.success, test.ShouldBeTrue) - test.That(t, resp.err, test.ShouldBeNil) - }) - } -} diff --git a/services/motion/explore/explore_utils_test.go b/services/motion/explore/explore_utils_test.go deleted file mode 100644 index 3cb02ab94fa..00000000000 --- a/services/motion/explore/explore_utils_test.go +++ /dev/null @@ -1,187 +0,0 @@ -package explore - -import ( - "context" - - "github.com/golang/geo/r3" - - "go.viam.com/rdk/components/base" - baseFake "go.viam.com/rdk/components/base/fake" - "go.viam.com/rdk/components/camera" - cameraFake "go.viam.com/rdk/components/camera/fake" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/pointcloud" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot/framesystem" - "go.viam.com/rdk/services/motion" - vSvc "go.viam.com/rdk/services/vision" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/testutils/inject" - "go.viam.com/rdk/vision" -) - -// createNewExploreMotionService creates a new motion service complete with base, camera (optional) and frame system. -// Note: The required vision service can/is added later as it will not affect the building of the frame system. -func createNewExploreMotionService(ctx context.Context, logger logging.Logger, fakeBase base.Base, cameras []camera.Camera, -) (motion.Service, error) { - var fsParts []*referenceframe.FrameSystemPart - deps := make(resource.Dependencies) - - // create base link - baseLink, err := createBaseLink(testBaseName.Name) - if err != nil { - return nil, err - } - fsParts = append(fsParts, &referenceframe.FrameSystemPart{FrameConfig: baseLink}) - deps[testBaseName] = fakeBase - - // create camera link - for _, cam := range cameras { - cameraLink, err := createCameraLink(cam.Name().Name, fakeBase.Name().Name) - if err != nil { - return nil, err - } - fsParts = append(fsParts, &referenceframe.FrameSystemPart{FrameConfig: cameraLink}) - deps[cam.Name()] = cam - } - - // create frame service - fs, err := createFrameSystemService(ctx, deps, fsParts, logger) - if err != nil { - return nil, err - } - deps[testFrameServiceName] = fs - - // create explore motion service - exploreMotionConf := resource.Config{ConvertedAttributes: &Config{}} - return NewExplore(ctx, deps, exploreMotionConf, logger) -} - -// createFakeBase instantiates a fake base. -func createFakeBase(ctx context.Context, logger logging.Logger) (base.Base, error) { - fakeBaseCfg := resource.Config{ - Name: testBaseName.Name, - API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{X: 300, Y: 200, Z: 100}}, - } - return baseFake.NewBase(ctx, nil, fakeBaseCfg, logger) -} - -// createFakeCamera instantiates a fake camera. -func createFakeCamera(ctx context.Context, logger logging.Logger, name string) (camera.Camera, error) { - fakeCameraCfg := resource.Config{ - Name: name, - API: camera.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 5}}, - ConvertedAttributes: &cameraFake.Config{ - Width: 1000, - Height: 1000, - }, - } - return cameraFake.NewCamera(ctx, resource.Dependencies{}, fakeCameraCfg, logger) -} - -// createMockVisionService instantiates a mock vision service with a custom version of GetObjectPointCloud that returns -// vision objects from a given set of points. -func createMockVisionService(visionSvcNum string, obstacles []obstacleMetadata) vSvc.Service { - mockVisionService := &inject.VisionService{} - - mockVisionService.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}, - ) ([]*vision.Object, error) { - var vObjects []*vision.Object - for _, obs := range obstacles { - obstaclePosition := spatialmath.NewPoseFromPoint(obs.position) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 100, Y: 100, Z: 100}, "test-case-2") - if err != nil { - return nil, err - } - - detection, err := vision.NewObjectWithLabel(pointcloud.New(), obs.label+visionSvcNum, box.ToProtobuf()) - if err != nil { - return nil, err - } - vObjects = append(vObjects, detection) - } - return vObjects, nil - } - return mockVisionService -} - -// createFrameSystemService will create a basic frame service from the list of parts. -func createFrameSystemService( - ctx context.Context, - deps resource.Dependencies, - fsParts []*referenceframe.FrameSystemPart, - logger logging.Logger, -) (framesystem.Service, error) { - fsSvc, err := framesystem.New(ctx, deps, logger) - if err != nil { - return nil, err - } - conf := resource.Config{ - ConvertedAttributes: &framesystem.Config{Parts: fsParts}, - } - if err := fsSvc.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - - return fsSvc, nil -} - -// createBaseLink instantiates the base to world link for the frame system. -func createBaseLink(baseName string) (*referenceframe.LinkInFrame, error) { - basePose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) - baseSphere, err := spatialmath.NewSphere(basePose, 10, "base-box") - if err != nil { - return nil, err - } - - baseLink := referenceframe.NewLinkInFrame( - referenceframe.World, - spatialmath.NewZeroPose(), - baseName, - baseSphere, - ) - return baseLink, nil -} - -// createCameraLink instantiates the camera to base link for the frame system. -func createCameraLink(camName, baseFrame string) (*referenceframe.LinkInFrame, error) { - camPose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) - camSphere, err := spatialmath.NewSphere(camPose, 5, "cam-sphere") - if err != nil { - return nil, err - } - - camLink := referenceframe.NewLinkInFrame( - baseFrame, - spatialmath.NewZeroPose(), - camName, - camSphere, - ) - return camLink, nil -} - -// geometriesContainsPoint is a helper function to test if a point is in a given geometry. -func geometriesContainsPoint(geometries []spatialmath.Geometry, point r3.Vector, collisionBufferMM float64) (bool, error) { - var collisionDetected bool - for _, geo := range geometries { - pointGeoCfg := spatialmath.GeometryConfig{ - Type: spatialmath.BoxType, - TranslationOffset: point, - } - pointGeo, err := pointGeoCfg.ParseConfig() - if err != nil { - return false, err - } - collides, err := geo.CollidesWith(pointGeo, collisionBufferMM) - if err != nil { - return false, err - } - if collides { - collisionDetected = true - } - } - return collisionDetected, nil -} diff --git a/services/motion/motion.go b/services/motion/motion.go index 6f62a7128e7..9f548be5193 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -200,7 +200,7 @@ type PlanWithStatus struct { // A Service controls the flow of moving components. // -// Move example: +// Move example: // this has to be changed // // motionService, err := motion.FromRobot(machine, "builtin") // @@ -340,32 +340,23 @@ type PlanWithStatus struct { type Service interface { resource.Resource - // Move is the primary method to move multiple components or any object to a specified location. + // Move is the primary method to move multiple components or any object to a specified location. // TODO change comment // Given a destination pose and a component, Move constructs a kinematic chain from goal to destination, // solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself // and other known objects. - Move( - ctx context.Context, - req MoveReq, - ) (bool, error) + Move(ctx context.Context, req MoveReq) (bool, error) // MoveOnMap moves a base component to a destination Pose on a SLAM map and returns a unique ExecutionID. // If the machine is already within PlanDeviationM of the goal, an error is returned. // Monitor progress with `GetPlan()` and `ListPlanStatuses()`, and check the machine's position via the SLAM service. // Designed for autonomous indoor navigation of rover bases. - MoveOnMap( - ctx context.Context, - req MoveOnMapReq, - ) (ExecutionID, error) + MoveOnMap(ctx context.Context, req MoveOnMapReq) (ExecutionID, error) // MoveOnGlobe moves a base component to a destination GPS point(latitude, longitude and returns a unique ExecutionID. // If the machine is already within PlanDeviationM of the goal, an error is returned. // This non-blocking method uses a movement sensor to verify the location of the base. // You can monitor progress with `GetPlan()` and `ListPlanStatuses()`. Designed for autonomous GPS navigation of rover bases. - MoveOnGlobe( - ctx context.Context, - req MoveOnGlobeReq, - ) (ExecutionID, error) + MoveOnGlobe(ctx context.Context, req MoveOnGlobeReq) (ExecutionID, error) // GetPose returns the location and orientation of a component within a frame system. // It returns a `PoseInFrame` describing the pose of the specified component relative to the specified destination frame. @@ -379,27 +370,18 @@ type Service interface { ) (*referenceframe.PoseInFrame, error) // StopPlan stops a base component being moved by an in progress `MoveOnGlobe()` or `MoveOnMap()` call. - StopPlan( - ctx context.Context, - req StopPlanReq, - ) error + StopPlan(ctx context.Context, req StopPlanReq) error // ListPlanStatuses returns the statuses of plans created by `MoveOnGlobe()` or `MoveOnMap()` since the motion service initialized. // It includes plans that are in progress or have changed state in the last 24 hours. // All repeated fields are in chronological order. - ListPlanStatuses( - ctx context.Context, - req ListPlanStatusesReq, - ) ([]PlanStatusWithID, error) + ListPlanStatuses(ctx context.Context, req ListPlanStatusesReq) ([]PlanStatusWithID, error) // PlanHistory returns the plan history of the most recent `MoveOnGlobe()` or `MoveOnMap()` call by default. // The history for earlier executions can be requested by providing an ExecutionID. // It returns a result if the execution is active or has changed state in the last 24 hours and the machine has not reinitialized. // Plans never change; replans always create new plans and replans share the ExecutionID of the previously executing plan. - PlanHistory( - ctx context.Context, - req PlanHistoryReq, - ) ([]PlanWithStatus, error) + PlanHistory(ctx context.Context, req PlanHistoryReq) ([]PlanWithStatus, error) } // ObstacleDetectorName pairs a vision service name with a camera name. diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index 4e23c87c88c..ea970082417 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -11,7 +11,6 @@ import ( vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/rdk/motionplan" - "go.viam.com/rdk/protoutils" rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -166,7 +165,7 @@ func (r MoveReq) toProto(name string) (*pb.MoveRequest, error) { } return &pb.MoveRequest{ Name: name, - ComponentName: protoutils.ResourceNameToProto(r.ComponentName), + ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), Destination: referenceframe.PoseInFrameToProtobuf(&r.Destination), WorldState: worldStateMsg, Constraints: r.Constraints.ToProtobuf(), @@ -180,7 +179,7 @@ func moveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { return MoveReq{}, err } return MoveReq{ - protoutils.ResourceNameFromProto(req.GetComponentName()), + rprotoutils.ResourceNameFromProto(req.GetComponentName()), *referenceframe.ProtobufToPoseInFrame(req.GetDestination()), *worldState, *motionplan.ConstraintsFromProtobuf(req.GetConstraints()), diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 3051deb6e5a..1c7072d3127 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -60,29 +60,14 @@ func TestServerMove(t *testing.T) { server, err = newServer(resources) test.That(t, err, test.ShouldBeNil) passedErr := errors.New("fake move error") - injectMS.MoveFunc = func( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { + injectMS.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { return false, passedErr } - _, err = server.Move(context.Background(), grabRequest) test.That(t, err, test.ShouldBeError, passedErr) // returns response - successfulMoveFunc := func( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { + successfulMoveFunc := func(ctx context.Context, req motion.MoveReq) (bool, error) { return true, nil } injectMS.MoveFunc = successfulMoveFunc diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go index e119e1fcd74..af41811187e 100644 --- a/services/navigation/builtin/builtin.go +++ b/services/navigation/builtin/builtin.go @@ -25,7 +25,6 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/motion/explore" "go.viam.com/rdk/services/navigation" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" @@ -392,11 +391,11 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, // Create explore motion service // Note: this service will disappear after the explore motion model is integrated into builtIn - exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}} - svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger) - if err != nil { - return err - } + // exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}} + // svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger) + // if err != nil { + // return err + // } svc.mode = navigation.ModeManual svc.base = baseComponent @@ -459,7 +458,8 @@ func (svc *builtIn) SetMode(ctx context.Context, mode navigation.Mode, extra map if len(svc.motionCfg.ObstacleDetectors) == 0 { return errors.New("explore mode requires at least one vision service") } - svc.startExploreMode(cancelCtx) + panic("no way to explore") + // svc.startExploreMode(cancelCtx) } return nil diff --git a/services/navigation/builtin/explore.go b/services/navigation/builtin/explore.go deleted file mode 100644 index ce8cd208489..00000000000 --- a/services/navigation/builtin/explore.go +++ /dev/null @@ -1,46 +0,0 @@ -package builtin - -import ( - "context" - "math" - "math/rand" - - "github.com/golang/geo/r3" - "go.viam.com/utils" - - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -const defaultDistanceMM = 50 * 1000 - -func (svc *builtIn) startExploreMode(ctx context.Context) { - svc.logger.CDebug(ctx, "startExploreMode called") - - svc.activeBackgroundWorkers.Add(1) - - utils.ManagedGo(func() { - // Send motionCfg parameters through extra until motionCfg can be added to Move() - extra := map[string]interface{}{"motionCfg": *svc.motionCfg} - - for { - if ctx.Err() != nil { - return - } - - // Choose a new random point using a normal distribution centered on the position directly the robot - randAngle := rand.NormFloat64() + math.Pi - destination := referenceframe.NewPoseInFrame(svc.base.Name().Name, spatialmath.NewPose( - r3.Vector{ - X: math.Sin(randAngle), - Y: math.Cos(randAngle), - Z: 0., - }.Normalize().Mul(defaultDistanceMM), spatialmath.NewOrientationVector())) - - _, err := svc.exploreMotionService.Move(ctx, svc.base.Name(), destination, nil, nil, extra) - if err != nil { - svc.logger.CDebugf(ctx, "error occurred when moving to point %v: %v", destination, err) - } - } - }, svc.activeBackgroundWorkers.Done) -} diff --git a/services/navigation/builtin/explore_test.go b/services/navigation/builtin/explore_test.go deleted file mode 100644 index 579f0532149..00000000000 --- a/services/navigation/builtin/explore_test.go +++ /dev/null @@ -1,42 +0,0 @@ -package builtin - -import ( - "context" - "testing" - "time" - - "github.com/golang/geo/r3" - "github.com/pkg/errors" - "go.viam.com/test" - - "go.viam.com/rdk/motionplan" - frame "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/testutils/inject" -) - -func TestExploreMode(t *testing.T) { - cancelCtx, cancel := context.WithCancel(context.Background()) - defer cancel() - ns, teardown := setupNavigationServiceFromConfig(t, "../data/nav_no_map_cfg.json") - - var points []r3.Vector - mockExploreMotionService := &inject.MotionService{} - mockExploreMotionService.MoveFunc = func(ctx context.Context, componentName resource.Name, - destination *frame.PoseInFrame, worldState *frame.WorldState, constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { - points = append(points, destination.Pose().Point()) - return false, errors.New("expected error") - } - - nsStruct := ns.(*builtIn) - nsStruct.exploreMotionService = mockExploreMotionService - - ctxTimeout, cancelFunc := context.WithTimeout(cancelCtx, 50*time.Millisecond) - defer cancelFunc() - nsStruct.startExploreMode(ctxTimeout) - <-ctxTimeout.Done() - teardown() - test.That(t, len(points), test.ShouldBeGreaterThan, 2) -} From 6473153033885705ea8614d5f1f9fbe32cb95244 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 5 Sep 2024 14:04:33 -0400 Subject: [PATCH 03/16] unmarshal argument --- services/motion/builtin/builtin.go | 15 +++++++++++++-- services/motion/builtin/builtin_test.go | 6 ++---- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index df7be38c091..1ed10bd36c0 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -3,6 +3,7 @@ package builtin import ( "context" + "encoding/json" "fmt" "sync" "time" @@ -337,10 +338,15 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m resp := make(map[string]interface{}, 0) if val, ok := cmd[DoPlan]; ok { - moveReq, err := utils.AssertType[motion.MoveReq](val) + bytes, err := json.Marshal(val) if err != nil { return nil, err } + var moveReq motion.MoveReq + err = json.Unmarshal(bytes, &moveReq) + if err != nil { + return nil, errors.New("couldn't unmarshal to motion.MoveReq") + } plan, err := ms.plan(ctx, moveReq) if err != nil { return nil, err @@ -348,10 +354,15 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m resp[DoPlan] = plan } if val, ok := cmd[DoExecute]; ok { - plan, err := utils.AssertType[motionplan.Plan](val) + bytes, err := json.Marshal(val) if err != nil { return nil, err } + var plan motionplan.Plan + err = json.Unmarshal(bytes, &plan) + if err != nil { + return nil, errors.New("couldn't unmarshal to motionplan.Plan") + } resp[DoExecute] = ms.execute(ctx, plan) != nil } return resp, nil diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 5debb5c4c7f..76df415c52a 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -194,10 +194,8 @@ func TestMove(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) grabPose := referenceframe.NewPoseInFrame("testFrame2", spatialmath.NewPoseFromPoint(r3.Vector{X: -20, Y: -130, Z: -40})) - _, err = ms.Move( - context.Background(), - motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose, WorldState: *worldState}, - ) + moveReq := motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose, WorldState: *worldState} + _, err = ms.Move(context.Background(), moveReq) test.That(t, err, test.ShouldBeNil) }) } From 6287b7e8a4073d133e54aafaf9c45b509859c4d6 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 18 Sep 2024 12:32:52 -0400 Subject: [PATCH 04/16] wip --- services/motion/builtin/builtin.go | 24 ++++++------- services/motion/client_test.go | 52 ++++++++++++++++++++++++++++ services/motion/motion_test.go | 26 ++++++++++++++ services/motion/server_test.go | 55 ++++++++++++++++++++++-------- 4 files changed, 130 insertions(+), 27 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 1ed10bd36c0..98598821af9 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -353,18 +353,18 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m } resp[DoPlan] = plan } - if val, ok := cmd[DoExecute]; ok { - bytes, err := json.Marshal(val) - if err != nil { - return nil, err - } - var plan motionplan.Plan - err = json.Unmarshal(bytes, &plan) - if err != nil { - return nil, errors.New("couldn't unmarshal to motionplan.Plan") - } - resp[DoExecute] = ms.execute(ctx, plan) != nil - } + // if val, ok := cmd[DoExecute]; ok { + // bytes, err := json.Marshal(val) + // if err != nil { + // return nil, err + // } + // var plan motionplan.Plan + // err = json.Unmarshal(bytes, &plan) + // if err != nil { + // return nil, errors.New("couldn't unmarshal to motionplan.Plan") + // } + // resp[DoExecute] = ms.execute(ctx, plan) != nil + // } return resp, nil } diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 6fcba9b3763..f0af8016b1f 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -11,6 +11,8 @@ import ( "github.com/google/uuid" geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" + commonpb "go.viam.com/api/common/v1" + pb "go.viam.com/api/service/motion/v1" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -24,6 +26,7 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" @@ -35,6 +38,55 @@ var ( testMotionServiceName2 = motion.Named("motion2") ) +func TestDoCommandClient(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + listener1, err := net.Listen("tcp", "localhost:0") + test.That(t, err, test.ShouldBeNil) + rpcServer, err := rpc.NewServer(logger, rpc.WithUnauthenticated()) + test.That(t, err, test.ShouldBeNil) + + ms, err := builtin.NewBuiltIn(ctx, resource.Dependencies{}, resource.Config{ConvertedAttributes: &builtin.Config{}}, logging.NewLogger("")) + test.That(t, err, test.ShouldBeNil) + resources := map[resource.Name]motion.Service{ + testMotionServiceName: ms, + } + + svc, err := resource.NewAPIResourceCollection(motion.API, resources) + test.That(t, err, test.ShouldBeNil) + resourceAPI, ok, err := resource.LookupAPIRegistration[motion.Service](motion.API) + test.That(t, err, test.ShouldBeNil) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, resourceAPI.RegisterRPCService(context.Background(), rpcServer, svc), test.ShouldBeNil) + + go func() { + test.That(t, rpcServer.Serve(listener1), test.ShouldBeNil) + }() + + defer func() { + test.That(t, rpcServer.Stop(), test.ShouldBeNil) + }() + + conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) + + test.That(t, err, test.ShouldBeNil) + + client, err := motion.NewClientFromConn(context.Background(), conn, "", testMotionServiceName, logger) + test.That(t, err, test.ShouldBeNil) + + cmd := make(map[string]interface{}) + cmd[builtin.DoPlan] = pb.MoveRequest{ + Name: "builtin", + Destination: &commonpb.PoseInFrame{ + ReferenceFrame: "world", + Pose: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), + }, + ComponentName: &commonpb.ResourceName{}, + } + + client.DoCommand(ctx, cmd) +} + func TestClient(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index cd3c8493a39..3c4b1ffdde8 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -1,6 +1,7 @@ package motion import ( + "encoding/json" "fmt" "math" "testing" @@ -16,6 +17,7 @@ import ( "google.golang.org/protobuf/types/known/structpb" "google.golang.org/protobuf/types/known/timestamppb" + "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" "go.viam.com/rdk/components/movementsensor" @@ -1424,6 +1426,30 @@ func TestPlanHistoryReq(t *testing.T) { }) } +func TestMoveReq(t *testing.T) { + req := MoveReq{ + ComponentName: arm.Named("fake"), + Destination: *referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), + WorldState: *referenceframe.NewEmptyWorldState(), + Extra: map[string]interface{}{ + "oops": 1, + }, + Constraints: *motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), + } + bytes, err := json.Marshal(req) + test.That(t, err, test.ShouldBeNil) + var i map[string]interface{} + err = json.Unmarshal(bytes, &i) + test.That(t, err, test.ShouldBeNil) + + bytes2, err := json.Marshal(i) + test.That(t, err, test.ShouldBeNil) + var req2 MoveReq + err = json.Unmarshal(bytes2, &req2) + test.That(t, err, test.ShouldBeNil) + test.That(t, req, test.ShouldResemble, req2) +} + func validMoveOnGlobeRequest() MoveOnGlobeReq { dst := geo.NewPoint(1, 2) visionCameraPairs := [][]resource.Name{ diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 1c7072d3127..4cdb1734955 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -13,22 +13,23 @@ import ( commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" "go.viam.com/test" - vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils" ) func newServer(resources map[resource.Name]motion.Service) (pb.MotionServiceServer, error) { @@ -750,25 +751,49 @@ func TestServerGetPlan(t *testing.T) { } func TestServerDoCommand(t *testing.T) { + ctx := context.Background() + + ms, err := builtin.NewBuiltIn(ctx, resource.Dependencies{}, resource.Config{ConvertedAttributes: &builtin.Config{}}, logging.NewLogger("")) + test.That(t, err, test.ShouldBeNil) + resourceMap := map[resource.Name]motion.Service{ - testMotionServiceName: &inject.MotionService{ - DoCommandFunc: testutils.EchoFunc, - }, + testMotionServiceName: ms, } + + // resourceMap := map[resource.Name]motion.Service{ + // testMotionServiceName: &inject.MotionService{ + // DoCommandFunc: testutils.EchoFunc, + // }, + // } server, err := newServer(resourceMap) test.That(t, err, test.ShouldBeNil) - cmd, err := vprotoutils.StructToStructPb(testutils.TestCommand) + motionServer, err := utils.AssertType[motion.Service](server) test.That(t, err, test.ShouldBeNil) - doCommandRequest := &commonpb.DoCommandRequest{ - Name: testMotionServiceName.ShortName(), - Command: cmd, + + // cmd, err := vprotoutils.StructToStructPb(testutils.TestCommand) + // test.That(t, err, test.ShouldBeNil) + // doCommandRequest := &commonpb.DoCommandRequest{ + // Name: testMotionServiceName.ShortName(), + // Command: cmd, + // } + // doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) + // test.That(t, err, test.ShouldBeNil) + + // // Assert that do command response is an echoed request. + // respMap := doCommandResponse.Result.AsMap() + // test.That(t, respMap["command"], test.ShouldResemble, "test") + // test.That(t, respMap["data"], test.ShouldResemble, 500.0) + + cmd := make(map[string]interface{}) + cmd[builtin.DoPlan] = pb.MoveRequest{ + Name: "builtin", + Destination: &commonpb.PoseInFrame{ + ReferenceFrame: "world", + Pose: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), + }, + ComponentName: &commonpb.ResourceName{}, } - doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) - test.That(t, err, test.ShouldBeNil) - // Assert that do command response is an echoed request. - respMap := doCommandResponse.Result.AsMap() - test.That(t, respMap["command"], test.ShouldResemble, "test") - test.That(t, respMap["data"], test.ShouldResemble, 500.0) + motionServer.DoCommand(ctx, cmd) } From 77571f8c6f9d12e9fc115bd90cb718ca202297c3 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 18 Sep 2024 16:30:48 -0400 Subject: [PATCH 05/16] wip --- motionplan/constraint.go | 2 +- services/motion/builtin/builtin.go | 17 +++++--- services/motion/builtin/builtin_test.go | 40 ++++++++++++++---- services/motion/client.go | 2 +- services/motion/client_test.go | 5 +-- services/motion/motion.go | 6 +-- services/motion/motion_test.go | 6 +-- services/motion/pbhelpers.go | 13 +++--- services/motion/server.go | 2 +- services/motion/server_test.go | 55 +++++++------------------ 10 files changed, 74 insertions(+), 74 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index d5e789c1a67..0f9cc4474ea 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -600,7 +600,7 @@ func NewConstraints( // ConstraintsFromProtobuf converts a protobuf object to a Constraints object. func ConstraintsFromProtobuf(pbConstraint *motionpb.Constraints) *Constraints { if pbConstraint == nil { - return nil + return NewEmptyConstraints() } // iterate through all motionpb.LinearConstraint and convert to RDK form diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 98598821af9..280dfa12b66 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -12,6 +12,7 @@ import ( "github.com/google/uuid" "github.com/pkg/errors" + pb "go.viam.com/api/service/motion/v1" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" @@ -342,10 +343,14 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m if err != nil { return nil, err } - var moveReq motion.MoveReq - err = json.Unmarshal(bytes, &moveReq) + var moveReqProto pb.MoveRequest + err = json.Unmarshal(bytes, &moveReqProto) if err != nil { - return nil, errors.New("couldn't unmarshal to motion.MoveReq") + return nil, err + } + moveReq, err := motion.MoveReqFromProto(&moveReqProto) + if err != nil { + return nil, err } plan, err := ms.plan(ctx, moveReq) if err != nil { @@ -390,7 +395,7 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla // re-evaluate goalPose to be in the frame of World solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName - tf, err := frameSys.Transform(fsInputs, &req.Destination, solvingFrame) + tf, err := frameSys.Transform(fsInputs, req.Destination, solvingFrame) if err != nil { return nil, err } @@ -403,8 +408,8 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla Frame: movingFrame, StartConfiguration: fsInputs, FrameSystem: frameSys, - WorldState: &req.WorldState, - Constraints: &req.Constraints, + WorldState: req.WorldState, + Constraints: req.Constraints, Options: req.Extra, }) if err != nil { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 76df415c52a..7ffc8bb70e4 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -130,7 +130,7 @@ func TestMoveFailures(t *testing.T) { ctx := context.Background() t.Run("fail on not finding gripper", func(t *testing.T) { grabPose := referenceframe.NewPoseInFrame("fakeCamera", spatialmath.NewPoseFromPoint(r3.Vector{X: 10.0, Y: 10.0, Z: 10.0})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: camera.Named("fake"), Destination: *grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: camera.Named("fake"), Destination: grabPose}) test.That(t, err, test.ShouldNotBeNil) }) @@ -145,7 +145,7 @@ func TestMoveFailures(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) poseInFrame := referenceframe.NewPoseInFrame("frame2", spatialmath.NewZeroPose()) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1"), Destination: *poseInFrame, WorldState: *worldState}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1"), Destination: poseInFrame, WorldState: worldState}) test.That(t, err, test.ShouldBeError, referenceframe.NewParentFrameMissingError("frame2", "noParent")) }) } @@ -158,7 +158,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -166,7 +166,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceArm", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("pieceArm"), Destination: *grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("pieceArm"), Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -174,7 +174,7 @@ func TestMove(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose}) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose}) test.That(t, err, test.ShouldBeNil) }) @@ -194,7 +194,7 @@ func TestMove(t *testing.T) { worldState, err := referenceframe.NewWorldState(nil, transforms) test.That(t, err, test.ShouldBeNil) grabPose := referenceframe.NewPoseInFrame("testFrame2", spatialmath.NewPoseFromPoint(r3.Vector{X: -20, Y: -130, Z: -40})) - moveReq := motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: *grabPose, WorldState: *worldState} + moveReq := motion.MoveReq{ComponentName: gripper.Named("pieceGripper"), Destination: grabPose, WorldState: worldState} _, err = ms.Move(context.Background(), moveReq) test.That(t, err, test.ShouldBeNil) }) @@ -245,7 +245,7 @@ func TestMoveWithObstacles(t *testing.T) { test.That(t, err, test.ShouldBeNil) _, err = ms.Move( context.Background(), - motion.MoveReq{ComponentName: gripper.Named("pieceArm"), Destination: *grabPose, WorldState: *worldState}, + motion.MoveReq{ComponentName: gripper.Named("pieceArm"), Destination: grabPose, WorldState: worldState}, ) // This fails due to a large obstacle being in the way test.That(t, err, test.ShouldNotBeNil) @@ -420,7 +420,7 @@ func TestMultiplePieces(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/fake_tomato.json") defer teardown() grabPose := referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: -0, Y: -30, Z: -50})) - _, err = ms.Move(context.Background(), motion.MoveReq{ComponentName: gripper.Named("gr"), Destination: *grabPose}) + _, err = ms.Move(context.Background(), motion.MoveReq{ComponentName: gripper.Named("gr"), Destination: grabPose}) test.That(t, err, test.ShouldBeNil) } @@ -572,7 +572,7 @@ func TestStoppableMoveFunctions(t *testing.T) { t.Run("stop during Move(...) call", func(t *testing.T) { calledStopFunc = false - success, err := ms.Move(ctx, motion.MoveReq{ComponentName: injectArmName, Destination: *goal, Extra: extra}) + success, err := ms.Move(ctx, motion.MoveReq{ComponentName: injectArmName, Destination: goal, Extra: extra}) testIfStoppable(t, success, err, failToReachGoalError) }) }) @@ -1237,3 +1237,25 @@ func TestCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) }) } + +func TestDoCommand(t *testing.T) { + ctx := context.Background() + t.Run("DoPlan", func(t *testing.T) { + ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") + defer teardown() + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Destination: referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})), + } + proto, err := moveReq.ToProto(ms.Name().Name) + test.That(t, err, test.ShouldBeNil) + cmd := map[string]interface{}{DoPlan: proto} + // command, err := protoutils.StructToStructPb(cmd) + // if err != nil { + // return nil, err + // } + // need to simulate what its like being converted to a proto struct and back to ensure it serialized/deserialized correctly + _ = cmd + test.That(t, err, test.ShouldBeNil) + }) +} diff --git a/services/motion/client.go b/services/motion/client.go index 1dd597a872f..8a1bbcb02ca 100644 --- a/services/motion/client.go +++ b/services/motion/client.go @@ -43,7 +43,7 @@ func NewClientFromConn( } func (c *client) Move(ctx context.Context, req MoveReq) (bool, error) { - protoReq, err := req.toProto(c.name) + protoReq, err := req.ToProto(c.name) if err != nil { return false, err } diff --git a/services/motion/client_test.go b/services/motion/client_test.go index f0af8016b1f..71d18256a81 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -83,7 +83,6 @@ func TestDoCommandClient(t *testing.T) { }, ComponentName: &commonpb.ResourceName{}, } - client.DoCommand(ctx, cmd) } @@ -159,7 +158,7 @@ func TestClient(t *testing.T) { } // Move - result, err := client.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: *zeroPoseInFrame}) + result, err := client.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: zeroPoseInFrame}) test.That(t, err, test.ShouldBeNil) test.That(t, result, test.ShouldEqual, success) @@ -225,7 +224,7 @@ func TestClient(t *testing.T) { } // Move - resp, err := client2.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: *zeroPoseInFrame}) + resp, err := client2.Move(ctx, motion.MoveReq{ComponentName: gripperName, Destination: zeroPoseInFrame}) test.That(t, err.Error(), test.ShouldContainSubstring, passedErr.Error()) test.That(t, resp, test.ShouldEqual, false) diff --git a/services/motion/motion.go b/services/motion/motion.go index 9f548be5193..9e895b4b661 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -45,11 +45,11 @@ type MoveReq struct { // ComponentName of the component to move ComponentName resource.Name // Goal destination the component should be moved to - Destination referenceframe.PoseInFrame + Destination *referenceframe.PoseInFrame // The external environment to be considered for the duration of the move - WorldState referenceframe.WorldState + WorldState *referenceframe.WorldState // Constraints which need to be satisfied during the movement - Constraints motionplan.Constraints + Constraints *motionplan.Constraints Extra map[string]interface{} } diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index 3c4b1ffdde8..b8bdd1b7afb 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -1429,12 +1429,12 @@ func TestPlanHistoryReq(t *testing.T) { func TestMoveReq(t *testing.T) { req := MoveReq{ ComponentName: arm.Named("fake"), - Destination: *referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), - WorldState: *referenceframe.NewEmptyWorldState(), + Destination: referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), + WorldState: referenceframe.NewEmptyWorldState(), Extra: map[string]interface{}{ "oops": 1, }, - Constraints: *motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), + Constraints: motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), } bytes, err := json.Marshal(req) test.That(t, err, test.ShouldBeNil) diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index ea970082417..f811045faff 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -153,8 +153,7 @@ func planStateFromProto(ps pb.PlanState) PlanState { } } -// toProto converts a MoveRequest to a *pb.MoveRequest. -func (r MoveReq) toProto(name string) (*pb.MoveRequest, error) { +func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { ext, err := vprotoutils.StructToStructPb(r.Extra) if err != nil { return nil, err @@ -166,23 +165,23 @@ func (r MoveReq) toProto(name string) (*pb.MoveRequest, error) { return &pb.MoveRequest{ Name: name, ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), - Destination: referenceframe.PoseInFrameToProtobuf(&r.Destination), + Destination: referenceframe.PoseInFrameToProtobuf(r.Destination), WorldState: worldStateMsg, Constraints: r.Constraints.ToProtobuf(), Extra: ext, }, nil } -func moveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { +func MoveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) if err != nil { return MoveReq{}, err } return MoveReq{ rprotoutils.ResourceNameFromProto(req.GetComponentName()), - *referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - *worldState, - *motionplan.ConstraintsFromProtobuf(req.GetConstraints()), + referenceframe.ProtobufToPoseInFrame(req.GetDestination()), + worldState, + motionplan.ConstraintsFromProtobuf(req.GetConstraints()), req.Extra.AsMap(), }, nil } diff --git a/services/motion/server.go b/services/motion/server.go index 859b8c521ae..e5f89664b04 100644 --- a/services/motion/server.go +++ b/services/motion/server.go @@ -29,7 +29,7 @@ func (server *serviceServer) Move(ctx context.Context, req *pb.MoveRequest) (*pb if err != nil { return nil, err } - r, err := moveReqFromProto(req) + r, err := MoveReqFromProto(req) if err != nil { return nil, err } diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 4cdb1734955..1c7072d3127 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -13,23 +13,22 @@ import ( commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" "go.viam.com/test" + vprotoutils "go.viam.com/utils/protoutils" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils" "go.viam.com/rdk/testutils/inject" - "go.viam.com/rdk/utils" ) func newServer(resources map[resource.Name]motion.Service) (pb.MotionServiceServer, error) { @@ -751,49 +750,25 @@ func TestServerGetPlan(t *testing.T) { } func TestServerDoCommand(t *testing.T) { - ctx := context.Background() - - ms, err := builtin.NewBuiltIn(ctx, resource.Dependencies{}, resource.Config{ConvertedAttributes: &builtin.Config{}}, logging.NewLogger("")) - test.That(t, err, test.ShouldBeNil) - resourceMap := map[resource.Name]motion.Service{ - testMotionServiceName: ms, + testMotionServiceName: &inject.MotionService{ + DoCommandFunc: testutils.EchoFunc, + }, } - - // resourceMap := map[resource.Name]motion.Service{ - // testMotionServiceName: &inject.MotionService{ - // DoCommandFunc: testutils.EchoFunc, - // }, - // } server, err := newServer(resourceMap) test.That(t, err, test.ShouldBeNil) - motionServer, err := utils.AssertType[motion.Service](server) + cmd, err := vprotoutils.StructToStructPb(testutils.TestCommand) test.That(t, err, test.ShouldBeNil) - - // cmd, err := vprotoutils.StructToStructPb(testutils.TestCommand) - // test.That(t, err, test.ShouldBeNil) - // doCommandRequest := &commonpb.DoCommandRequest{ - // Name: testMotionServiceName.ShortName(), - // Command: cmd, - // } - // doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) - // test.That(t, err, test.ShouldBeNil) - - // // Assert that do command response is an echoed request. - // respMap := doCommandResponse.Result.AsMap() - // test.That(t, respMap["command"], test.ShouldResemble, "test") - // test.That(t, respMap["data"], test.ShouldResemble, 500.0) - - cmd := make(map[string]interface{}) - cmd[builtin.DoPlan] = pb.MoveRequest{ - Name: "builtin", - Destination: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - }, - ComponentName: &commonpb.ResourceName{}, + doCommandRequest := &commonpb.DoCommandRequest{ + Name: testMotionServiceName.ShortName(), + Command: cmd, } + doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) + test.That(t, err, test.ShouldBeNil) - motionServer.DoCommand(ctx, cmd) + // Assert that do command response is an echoed request. + respMap := doCommandResponse.Result.AsMap() + test.That(t, respMap["command"], test.ShouldResemble, "test") + test.That(t, respMap["data"], test.ShouldResemble, 500.0) } From 5ecdfbb84f0e299fa27366c3c06dc89c6b31baed Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Wed, 18 Sep 2024 17:04:18 -0400 Subject: [PATCH 06/16] de/serializing requests properly --- services/motion/builtin/builtin_test.go | 26 ++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 7ffc8bb70e4..4c1620b6bd1 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -13,6 +13,7 @@ import ( "github.com/pkg/errors" commonpb "go.viam.com/api/common/v1" "go.viam.com/test" + "go.viam.com/utils/protoutils" "go.viam.com/rdk/components/arm" armFake "go.viam.com/rdk/components/arm/fake" @@ -35,6 +36,7 @@ import ( "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/utils" viz "go.viam.com/rdk/vision" ) @@ -1250,12 +1252,26 @@ func TestDoCommand(t *testing.T) { proto, err := moveReq.ToProto(ms.Name().Name) test.That(t, err, test.ShouldBeNil) cmd := map[string]interface{}{DoPlan: proto} - // command, err := protoutils.StructToStructPb(cmd) - // if err != nil { - // return nil, err - // } + // need to simulate what its like being converted to a proto struct and back to ensure it serialized/deserialized correctly - _ = cmd + command, err := protoutils.StructToStructPb(cmd) + test.That(t, err, test.ShouldBeNil) + arg := command.AsMap() + + // make call to DoCommand + resp, err := ms.DoCommand(ctx, arg) + test.That(t, err, test.ShouldBeNil) + val, ok := resp[DoPlan] + test.That(t, ok, test.ShouldBeTrue) + plan, err := utils.AssertType[motionplan.Plan](val) test.That(t, err, test.ShouldBeNil) + test.That(t, len(plan.Trajectory()), test.ShouldEqual, 2) + + // now simulate what happens to the response as it goes back over the wire + // planProto, err := protoutils.StructToStructPb(plan) + // test.That(t, err, test.ShouldBeNil) + // _ = planProto + // planProtoMap := planProto.AsMap() + // _ = planProtoMap }) } From 562f4ed91b6671684383ab2cec59ccb41f1fbdb3 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 12:11:24 -0400 Subject: [PATCH 07/16] have both functions working --- services/motion/builtin/builtin.go | 39 ++++++++------- services/motion/builtin/builtin_test.go | 63 ++++++++++++++++--------- 2 files changed, 61 insertions(+), 41 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 280dfa12b66..471a0e3d3f6 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -10,6 +10,7 @@ import ( "github.com/golang/geo/r3" "github.com/google/uuid" + "github.com/mitchellh/mapstructure" "github.com/pkg/errors" pb "go.viam.com/api/service/motion/v1" @@ -186,7 +187,7 @@ func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) { if err != nil { return false, err } - err = ms.execute(ctx, plan) + err = ms.execute(ctx, plan.Trajectory()) return err != nil, err } @@ -338,8 +339,8 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m defer ms.mu.RUnlock() resp := make(map[string]interface{}, 0) - if val, ok := cmd[DoPlan]; ok { - bytes, err := json.Marshal(val) + if req, ok := cmd[DoPlan]; ok { + bytes, err := json.Marshal(req) if err != nil { return nil, err } @@ -356,20 +357,18 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m if err != nil { return nil, err } - resp[DoPlan] = plan - } - // if val, ok := cmd[DoExecute]; ok { - // bytes, err := json.Marshal(val) - // if err != nil { - // return nil, err - // } - // var plan motionplan.Plan - // err = json.Unmarshal(bytes, &plan) - // if err != nil { - // return nil, errors.New("couldn't unmarshal to motionplan.Plan") - // } - // resp[DoExecute] = ms.execute(ctx, plan) != nil - // } + resp[DoPlan] = plan.Trajectory() + } + if req, ok := cmd[DoExecute]; ok { + var trajectory motionplan.Trajectory + if err := mapstructure.Decode(req, &trajectory); err != nil { + return nil, err + } + if err := ms.execute(ctx, trajectory); err != nil { + return nil, err + } + resp[DoExecute] = true + } return resp, nil } @@ -418,8 +417,8 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla return plan, nil } -func (ms *builtIn) execute(ctx context.Context, plan motionplan.Plan) error { - // build maps of relevant components and inputs from initial inputs +func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory) error { + // build maps of relevant components from initial inputs _, resources, err := ms.fsService.CurrentInputs(ctx) if err != nil { return err @@ -428,7 +427,7 @@ func (ms *builtIn) execute(ctx context.Context, plan motionplan.Plan) error { // Batch GoToInputs calls if possible; components may want to blend between inputs combinedSteps := []map[string][][]referenceframe.Input{} currStep := map[string][][]referenceframe.Input{} - for i, step := range plan.Trajectory() { + for i, step := range trajectory { if i == 0 { for name, inputs := range step { if len(inputs) == 0 { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 4c1620b6bd1..e53970919f0 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -8,6 +8,7 @@ import ( "testing" "time" + "github.com/go-viper/mapstructure/v2" "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" @@ -36,7 +37,6 @@ import ( "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" - "go.viam.com/rdk/utils" viz "go.viam.com/rdk/vision" ) @@ -1242,36 +1242,57 @@ func TestCheckPlan(t *testing.T) { func TestDoCommand(t *testing.T) { ctx := context.Background() + + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Destination: referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})), + } + + // need to simulate what happens when the DoCommand message is serialized/deserialized into proto + doOverWire := func(ms motion.Service, cmd map[string]interface{}) map[string]interface{} { + command, err := protoutils.StructToStructPb(cmd) + test.That(t, err, test.ShouldBeNil) + resp, err := ms.DoCommand(ctx, command.AsMap()) + test.That(t, err, test.ShouldBeNil) + respProto, err := protoutils.StructToStructPb(resp) + test.That(t, err, test.ShouldBeNil) + return respProto.AsMap() + } + t.Run("DoPlan", func(t *testing.T) { ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") defer teardown() - moveReq := motion.MoveReq{ - ComponentName: gripper.Named("pieceGripper"), - Destination: referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})), - } + + // format the command to send DoCommand proto, err := moveReq.ToProto(ms.Name().Name) test.That(t, err, test.ShouldBeNil) cmd := map[string]interface{}{DoPlan: proto} - // need to simulate what its like being converted to a proto struct and back to ensure it serialized/deserialized correctly - command, err := protoutils.StructToStructPb(cmd) + // simulate going over the wire + resp, ok := doOverWire(ms, cmd)[DoPlan] + test.That(t, ok, test.ShouldBeTrue) + + // the client will need to decode the response still + var trajectory motionplan.Trajectory + err = mapstructure.Decode(resp, &trajectory) test.That(t, err, test.ShouldBeNil) - arg := command.AsMap() + test.That(t, len(trajectory), test.ShouldEqual, 2) + }) + t.Run("DoExectute", func(t *testing.T) { + ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") + defer teardown() - // make call to DoCommand - resp, err := ms.DoCommand(ctx, arg) + plan, err := ms.(*builtIn).plan(ctx, moveReq) test.That(t, err, test.ShouldBeNil) - val, ok := resp[DoPlan] + + // format the command to sent DoCommand + cmd := map[string]interface{}{DoExecute: plan.Trajectory()} + + // simulate going over the wire + resp, ok := doOverWire(ms, cmd)[DoExecute] test.That(t, ok, test.ShouldBeTrue) - plan, err := utils.AssertType[motionplan.Plan](val) - test.That(t, err, test.ShouldBeNil) - test.That(t, len(plan.Trajectory()), test.ShouldEqual, 2) - - // now simulate what happens to the response as it goes back over the wire - // planProto, err := protoutils.StructToStructPb(plan) - // test.That(t, err, test.ShouldBeNil) - // _ = planProto - // planProtoMap := planProto.AsMap() - // _ = planProtoMap + + // the client will need to decode the response still + test.That(t, resp, test.ShouldBeTrue) }) } From 886f856d60c1b4781032d552274a9156e37f1e0a Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 13:14:11 -0400 Subject: [PATCH 08/16] wip --- services/motion/motion_test.go | 48 ++++++++++++++++------------------ 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index b8bdd1b7afb..384873d1ee8 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -1,7 +1,6 @@ package motion import ( - "encoding/json" "fmt" "math" "testing" @@ -17,7 +16,6 @@ import ( "google.golang.org/protobuf/types/known/structpb" "google.golang.org/protobuf/types/known/timestamppb" - "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/base" "go.viam.com/rdk/components/camera" "go.viam.com/rdk/components/movementsensor" @@ -1426,29 +1424,29 @@ func TestPlanHistoryReq(t *testing.T) { }) } -func TestMoveReq(t *testing.T) { - req := MoveReq{ - ComponentName: arm.Named("fake"), - Destination: referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), - WorldState: referenceframe.NewEmptyWorldState(), - Extra: map[string]interface{}{ - "oops": 1, - }, - Constraints: motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), - } - bytes, err := json.Marshal(req) - test.That(t, err, test.ShouldBeNil) - var i map[string]interface{} - err = json.Unmarshal(bytes, &i) - test.That(t, err, test.ShouldBeNil) - - bytes2, err := json.Marshal(i) - test.That(t, err, test.ShouldBeNil) - var req2 MoveReq - err = json.Unmarshal(bytes2, &req2) - test.That(t, err, test.ShouldBeNil) - test.That(t, req, test.ShouldResemble, req2) -} +// func TestMoveReq(t *testing.T) { +// req := MoveReq{ +// ComponentName: arm.Named("fake"), +// Destination: referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), +// WorldState: referenceframe.NewEmptyWorldState(), +// Extra: map[string]interface{}{ +// "oops": 1, +// }, +// Constraints: motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), +// } +// bytes, err := json.Marshal(req) +// test.That(t, err, test.ShouldBeNil) +// var i map[string]interface{} +// err = json.Unmarshal(bytes, &i) +// test.That(t, err, test.ShouldBeNil) + +// bytes2, err := json.Marshal(i) +// test.That(t, err, test.ShouldBeNil) +// var req2 MoveReq +// err = json.Unmarshal(bytes2, &req2) +// test.That(t, err, test.ShouldBeNil) +// test.That(t, req, test.ShouldResemble, req2) +// } func validMoveOnGlobeRequest() MoveOnGlobeReq { dst := geo.NewPoint(1, 2) From 34a274b236705ec853cb2fcb78242c0ee69439ed Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 13:48:14 -0400 Subject: [PATCH 09/16] add explore back in --- services/motion/builtin/builtin.go | 6 +-- services/motion/builtin/builtin_test.go | 1 - services/motion/client_test.go | 51 --------------------- services/navigation/builtin/builtin.go | 14 +++--- services/navigation/builtin/explore.go | 46 +++++++++++++++++++ services/navigation/builtin/explore_test.go | 42 +++++++++++++++++ 6 files changed, 98 insertions(+), 62 deletions(-) create mode 100644 services/navigation/builtin/explore.go create mode 100644 services/navigation/builtin/explore_test.go diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 471a0e3d3f6..e1eb89270c0 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -182,13 +182,14 @@ func (ms *builtIn) Close(ctx context.Context) error { func (ms *builtIn) Move(ctx context.Context, req motion.MoveReq) (bool, error) { ms.mu.RLock() defer ms.mu.RUnlock() + operation.CancelOtherWithLabel(ctx, builtinOpLabel) plan, err := ms.plan(ctx, req) if err != nil { return false, err } err = ms.execute(ctx, plan.Trajectory()) - return err != nil, err + return err == nil, err } func (ms *builtIn) MoveOnMap(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { @@ -337,6 +338,7 @@ func (ms *builtIn) PlanHistory( func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { ms.mu.RLock() defer ms.mu.RUnlock() + operation.CancelOtherWithLabel(ctx, builtinOpLabel) resp := make(map[string]interface{}, 0) if req, ok := cmd[DoPlan]; ok { @@ -373,8 +375,6 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m } func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Plan, error) { - operation.CancelOtherWithLabel(ctx, builtinOpLabel) - frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) if err != nil { return nil, err diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index e53970919f0..3e907b65920 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1242,7 +1242,6 @@ func TestCheckPlan(t *testing.T) { func TestDoCommand(t *testing.T) { ctx := context.Background() - moveReq := motion.MoveReq{ ComponentName: gripper.Named("pieceGripper"), Destination: referenceframe.NewPoseInFrame("c", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -30, Z: -50})), diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 71d18256a81..47dbc4975e8 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -11,8 +11,6 @@ import ( "github.com/google/uuid" geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" - pb "go.viam.com/api/service/motion/v1" "go.viam.com/test" "go.viam.com/utils/rpc" @@ -26,7 +24,6 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/motion/builtin" "go.viam.com/rdk/services/slam" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils" @@ -38,54 +35,6 @@ var ( testMotionServiceName2 = motion.Named("motion2") ) -func TestDoCommandClient(t *testing.T) { - ctx := context.Background() - logger := logging.NewTestLogger(t) - listener1, err := net.Listen("tcp", "localhost:0") - test.That(t, err, test.ShouldBeNil) - rpcServer, err := rpc.NewServer(logger, rpc.WithUnauthenticated()) - test.That(t, err, test.ShouldBeNil) - - ms, err := builtin.NewBuiltIn(ctx, resource.Dependencies{}, resource.Config{ConvertedAttributes: &builtin.Config{}}, logging.NewLogger("")) - test.That(t, err, test.ShouldBeNil) - resources := map[resource.Name]motion.Service{ - testMotionServiceName: ms, - } - - svc, err := resource.NewAPIResourceCollection(motion.API, resources) - test.That(t, err, test.ShouldBeNil) - resourceAPI, ok, err := resource.LookupAPIRegistration[motion.Service](motion.API) - test.That(t, err, test.ShouldBeNil) - test.That(t, ok, test.ShouldBeTrue) - test.That(t, resourceAPI.RegisterRPCService(context.Background(), rpcServer, svc), test.ShouldBeNil) - - go func() { - test.That(t, rpcServer.Serve(listener1), test.ShouldBeNil) - }() - - defer func() { - test.That(t, rpcServer.Stop(), test.ShouldBeNil) - }() - - conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) - - test.That(t, err, test.ShouldBeNil) - - client, err := motion.NewClientFromConn(context.Background(), conn, "", testMotionServiceName, logger) - test.That(t, err, test.ShouldBeNil) - - cmd := make(map[string]interface{}) - cmd[builtin.DoPlan] = pb.MoveRequest{ - Name: "builtin", - Destination: &commonpb.PoseInFrame{ - ReferenceFrame: "world", - Pose: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), - }, - ComponentName: &commonpb.ResourceName{}, - } - client.DoCommand(ctx, cmd) -} - func TestClient(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) diff --git a/services/navigation/builtin/builtin.go b/services/navigation/builtin/builtin.go index 363f61bd293..0ddfcb7bcb2 100644 --- a/services/navigation/builtin/builtin.go +++ b/services/navigation/builtin/builtin.go @@ -25,6 +25,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/services/motion/explore" "go.viam.com/rdk/services/navigation" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" @@ -391,11 +392,11 @@ func (svc *builtIn) Reconfigure(ctx context.Context, deps resource.Dependencies, // Create explore motion service // Note: this service will disappear after the explore motion model is integrated into builtIn - // exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}} - // svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger) - // if err != nil { - // return err - // } + exploreMotionConf := resource.Config{ConvertedAttributes: &explore.Config{}} + svc.exploreMotionService, err = explore.NewExplore(ctx, deps, exploreMotionConf, svc.logger) + if err != nil { + return err + } svc.mode = navigation.ModeManual svc.base = baseComponent @@ -458,8 +459,7 @@ func (svc *builtIn) SetMode(ctx context.Context, mode navigation.Mode, extra map if len(svc.motionCfg.ObstacleDetectors) == 0 { return errors.New("explore mode requires at least one vision service") } - panic("no way to explore") - // svc.startExploreMode(cancelCtx) + svc.startExploreMode(cancelCtx) } return nil diff --git a/services/navigation/builtin/explore.go b/services/navigation/builtin/explore.go new file mode 100644 index 00000000000..ce8cd208489 --- /dev/null +++ b/services/navigation/builtin/explore.go @@ -0,0 +1,46 @@ +package builtin + +import ( + "context" + "math" + "math/rand" + + "github.com/golang/geo/r3" + "go.viam.com/utils" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const defaultDistanceMM = 50 * 1000 + +func (svc *builtIn) startExploreMode(ctx context.Context) { + svc.logger.CDebug(ctx, "startExploreMode called") + + svc.activeBackgroundWorkers.Add(1) + + utils.ManagedGo(func() { + // Send motionCfg parameters through extra until motionCfg can be added to Move() + extra := map[string]interface{}{"motionCfg": *svc.motionCfg} + + for { + if ctx.Err() != nil { + return + } + + // Choose a new random point using a normal distribution centered on the position directly the robot + randAngle := rand.NormFloat64() + math.Pi + destination := referenceframe.NewPoseInFrame(svc.base.Name().Name, spatialmath.NewPose( + r3.Vector{ + X: math.Sin(randAngle), + Y: math.Cos(randAngle), + Z: 0., + }.Normalize().Mul(defaultDistanceMM), spatialmath.NewOrientationVector())) + + _, err := svc.exploreMotionService.Move(ctx, svc.base.Name(), destination, nil, nil, extra) + if err != nil { + svc.logger.CDebugf(ctx, "error occurred when moving to point %v: %v", destination, err) + } + } + }, svc.activeBackgroundWorkers.Done) +} diff --git a/services/navigation/builtin/explore_test.go b/services/navigation/builtin/explore_test.go new file mode 100644 index 00000000000..579f0532149 --- /dev/null +++ b/services/navigation/builtin/explore_test.go @@ -0,0 +1,42 @@ +package builtin + +import ( + "context" + "testing" + "time" + + "github.com/golang/geo/r3" + "github.com/pkg/errors" + "go.viam.com/test" + + "go.viam.com/rdk/motionplan" + frame "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils/inject" +) + +func TestExploreMode(t *testing.T) { + cancelCtx, cancel := context.WithCancel(context.Background()) + defer cancel() + ns, teardown := setupNavigationServiceFromConfig(t, "../data/nav_no_map_cfg.json") + + var points []r3.Vector + mockExploreMotionService := &inject.MotionService{} + mockExploreMotionService.MoveFunc = func(ctx context.Context, componentName resource.Name, + destination *frame.PoseInFrame, worldState *frame.WorldState, constraints *motionplan.Constraints, + extra map[string]interface{}, + ) (bool, error) { + points = append(points, destination.Pose().Point()) + return false, errors.New("expected error") + } + + nsStruct := ns.(*builtIn) + nsStruct.exploreMotionService = mockExploreMotionService + + ctxTimeout, cancelFunc := context.WithTimeout(cancelCtx, 50*time.Millisecond) + defer cancelFunc() + nsStruct.startExploreMode(ctxTimeout) + <-ctxTimeout.Done() + teardown() + test.That(t, len(points), test.ShouldBeGreaterThan, 2) +} From b88440a251b99fcfab6d8c87115bc9b26dd0ff3b Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 13:51:01 -0400 Subject: [PATCH 10/16] add back explore --- services/motion/explore/explore.go | 632 ++++++++++++++++++ services/motion/explore/explore_test.go | 386 +++++++++++ services/motion/explore/explore_utils_test.go | 187 ++++++ 3 files changed, 1205 insertions(+) create mode 100644 services/motion/explore/explore.go create mode 100644 services/motion/explore/explore_test.go create mode 100644 services/motion/explore/explore_utils_test.go diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go new file mode 100644 index 00000000000..03a89a393ae --- /dev/null +++ b/services/motion/explore/explore.go @@ -0,0 +1,632 @@ +// Package explore implements a motion service for exploration. This motion service model is a temporary model +// that will be incorporated into the rdk:builtin:motion service in the future. +package explore + +import ( + "context" + "fmt" + "math" + "strconv" + "strings" + "sync" + "time" + + "github.com/golang/geo/r3" + "github.com/google/uuid" + "github.com/pkg/errors" + goutils "go.viam.com/utils" + + "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/base/kinematicbase" + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/operation" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" + "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +var ( + model = resource.DefaultModelFamily.WithModel("explore") + errUnimplemented = errors.New("unimplemented") + // The distance a detected obstacle can be from a base to trigger the Move command to stop. + lookAheadDistanceMM = 500. + // successiveErrorLimit places a limit on the number of errors that can occur in a row when running + // checkForObstacles. + successiveErrorLimit = 5 + // Defines the limit on how far a potential kinematic base move action can be. For explore there is none. + defaultMoveLimitMM = math.Inf(1) + // The timeout for any individual move action on the kinematic base. + defaultExploreTimeout = 100 * time.Second + // The max angle the kinematic base spin action can be. + defaultMaxSpinAngleDegs = 180. +) + +func init() { + resource.RegisterService( + motion.API, + model, + resource.Registration[motion.Service, *Config]{ + Constructor: NewExplore, + WeakDependencies: []resource.Matcher{ + resource.TypeMatcher{Type: resource.APITypeComponentName}, + resource.SubtypeMatcher{Subtype: vision.SubtypeName}, + }, + }, + ) +} + +const ( + exploreOpLabel = "explore-motion-service" +) + +// moveResponse is the message type returned by both channels: obstacle detection and plan execution. +// It contains a bool stating if an obstacle was found or the execution was complete and/or any +// associated error. +type moveResponse struct { + err error + success bool +} + +// inputEnabledActuator is an actuator that interacts with the frame system. +// This allows us to figure out where the actuator currently is and then +// move it. Input units are always in meters or radians. +type inputEnabledActuator interface { + resource.Actuator + referenceframe.InputEnabled +} + +// obstacleDetectorObject provides a map for matching vision services to any and all cameras names they use. +type obstacleDetectorObject map[vision.Service]resource.Name + +// Config describes how to configure the service. As the explore motion service is not being surfaced to users, this is blank. +type Config struct{} + +// Validate here adds a dependency on the internal framesystem service. +func (c *Config) Validate(path string) ([]string, error) { + return []string{framesystem.InternalServiceName.String()}, nil +} + +// NewExplore returns a new explore motion service for the given robot. +func NewExplore(ctx context.Context, deps resource.Dependencies, conf resource.Config, logger logging.Logger) (motion.Service, error) { + ms := &explore{ + Named: conf.ResourceName().AsNamed(), + logger: logger, + obstacleResponseChan: make(chan moveResponse), + executionResponseChan: make(chan moveResponse), + backgroundWorkers: &sync.WaitGroup{}, + } + + if err := ms.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + return ms, nil +} + +// Reconfigure updates the explore motion service when the config has changed. +func (ms *explore) Reconfigure( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, +) (err error) { + ms.resourceMutex.Lock() + defer ms.resourceMutex.Unlock() + // Iterate over dependencies and store components and services along with the frame service directly + components := make(map[resource.Name]resource.Resource) + services := make(map[resource.Name]resource.Resource) + for name, dep := range deps { + switch dep := dep.(type) { + case framesystem.Service: + ms.fsService = dep + case vision.Service: + services[name] = dep + default: + components[name] = dep + } + } + + ms.components = components + ms.services = services + return nil +} + +type explore struct { + resource.Named + logger logging.Logger + + processCancelFunc context.CancelFunc + obstacleResponseChan chan moveResponse + executionResponseChan chan moveResponse + backgroundWorkers *sync.WaitGroup + + // resourceMutex protects the connect to other resources, the frame service/system and prevent multiple + // Move and/or Reconfigure actions from being performed simultaneously. + resourceMutex sync.Mutex + components map[resource.Name]resource.Resource + services map[resource.Name]resource.Resource + fsService framesystem.Service + frameSystem referenceframe.FrameSystem +} + +func (ms *explore) MoveOnMap(ctx context.Context, req motion.MoveOnMapReq) (motion.ExecutionID, error) { + return uuid.Nil, errUnimplemented +} + +func (ms *explore) MoveOnGlobe( + ctx context.Context, + req motion.MoveOnGlobeReq, +) (motion.ExecutionID, error) { + return uuid.Nil, errUnimplemented +} + +func (ms *explore) GetPose( + ctx context.Context, + componentName resource.Name, + destinationFrame string, + supplementalTransforms []*referenceframe.LinkInFrame, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, error) { + return nil, errUnimplemented +} + +func (ms *explore) StopPlan( + ctx context.Context, + req motion.StopPlanReq, +) error { + return errUnimplemented +} + +func (ms *explore) ListPlanStatuses( + ctx context.Context, + req motion.ListPlanStatusesReq, +) ([]motion.PlanStatusWithID, error) { + return nil, errUnimplemented +} + +func (ms *explore) PlanHistory( + ctx context.Context, + req motion.PlanHistoryReq, +) ([]motion.PlanWithStatus, error) { + return nil, errUnimplemented +} + +func (ms *explore) Close(ctx context.Context) error { + ms.resourceMutex.Lock() + defer ms.resourceMutex.Unlock() + + if ms.processCancelFunc != nil { + ms.processCancelFunc() + } + utils.FlushChan(ms.obstacleResponseChan) + utils.FlushChan(ms.executionResponseChan) + ms.backgroundWorkers.Wait() + return nil +} + +// Move takes a goal location and will plan and execute a movement to move a component specified by its name +// to that destination. +func (ms *explore) Move( + ctx context.Context, + componentName resource.Name, + destination *referenceframe.PoseInFrame, + worldState *referenceframe.WorldState, + constraints *motionplan.Constraints, + extra map[string]interface{}, +) (bool, error) { + ms.resourceMutex.Lock() + defer ms.resourceMutex.Unlock() + + operation.CancelOtherWithLabel(ctx, exploreOpLabel) + + // Parse extras + motionCfg, err := parseMotionConfig(extra) + if err != nil { + return false, err + } + + // obstacleDetectors + obstacleDetectors, err := ms.createObstacleDetectors(motionCfg) + if err != nil { + return false, err + } + + // Create kinematic base + kb, err := ms.createKinematicBase(ctx, componentName, motionCfg) + if err != nil { + return false, err + } + + // Create motionplan plan + plan, err := ms.createMotionPlan(ctx, kb, destination, extra) + if err != nil { + return false, err + } + + // Start background processes + cancelCtx, cancel := context.WithCancel(ctx) + ms.processCancelFunc = cancel + defer ms.processCancelFunc() + + // Start polling for obstacles + ms.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + ms.checkForObstacles(cancelCtx, obstacleDetectors, kb, plan, *motionCfg.ObstaclePollingFreqHz) + }, ms.backgroundWorkers.Done) + + // Start executing plan + ms.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + ms.executePlan(cancelCtx, kb, plan) + }, ms.backgroundWorkers.Done) + + // this ensures that if the context is cancelled we always return early at the top of the loop + if err := ctx.Err(); err != nil { + return false, err + } + + select { + // if context was cancelled by the calling function, error out + case <-ctx.Done(): + return false, ctx.Err() + + // once execution responds: return the result to the caller + case resp := <-ms.executionResponseChan: + ms.logger.CDebugf(ctx, "execution completed: %v", resp) + if resp.err != nil { + return resp.success, resp.err + } + if resp.success { + return true, nil + } + + // if the checkPartialPlan process hit an error return it, otherwise exit + case resp := <-ms.obstacleResponseChan: + ms.logger.CDebugf(ctx, "obstacle response: %v", resp) + if resp.err != nil { + return resp.success, resp.err + } + if resp.success { + return false, nil + } + } + return false, errors.New("no obstacle was seen during movement to goal and goal was not reached") +} + +// checkForObstacles will continuously monitor the generated transient worldState for obstacles in the given +// motionplan plan. A response will be sent through the channel if an error occurs, the motionplan plan +// completes or an obstacle is detected in the given range. +func (ms *explore) checkForObstacles( + ctx context.Context, + obstacleDetectors []obstacleDetectorObject, + kb kinematicbase.KinematicBase, + plan motionplan.Plan, + obstaclePollingFrequencyHz float64, +) { + ms.logger.Debug("Current Plan") + ms.logger.Debug(plan) + + // Constantly check for obstacles in path at desired obstacle polling frequency + ticker := time.NewTicker(time.Duration(int(1000/obstaclePollingFrequencyHz)) * time.Millisecond) + defer ticker.Stop() + + var errCounterCurrentInputs, errCounterGenerateTransientWorldState int + for { + select { + case <-ctx.Done(): + return + case <-ticker.C: + + currentInputs, err := kb.CurrentInputs(ctx) + if err != nil { + ms.logger.Debugf("issue occurred getting current inputs from kinematic base: %v", err) + if errCounterCurrentInputs > successiveErrorLimit { + ms.obstacleResponseChan <- moveResponse{success: false} + return + } + errCounterCurrentInputs++ + } else { + errCounterCurrentInputs = 0 + } + + currentPose := spatialmath.NewPose( + r3.Vector{X: currentInputs[0].Value, Y: currentInputs[1].Value, Z: 0}, + &spatialmath.OrientationVector{OX: 0, OY: 0, OZ: 1, Theta: currentInputs[2].Value}, + ) + + // Look for new transient obstacles and add to worldState + worldState, err := ms.generateTransientWorldState(ctx, obstacleDetectors) + if err != nil { + ms.logger.CDebugf(ctx, "issue occurred generating transient worldState: %v", err) + if errCounterGenerateTransientWorldState > successiveErrorLimit { + ms.obstacleResponseChan <- moveResponse{success: false} + return + } + errCounterGenerateTransientWorldState++ + } else { + errCounterGenerateTransientWorldState = 0 + } + + // TODO: Generalize this fix to work for maps with non-transient obstacles. This current implementation + // relies on the plan being two steps: a start position and a goal position. + // JIRA Ticket: https://viam.atlassian.net/browse/RSDK-5964 + planTraj := plan.Trajectory() + planTraj[0][kb.Name().ShortName()] = currentInputs + ms.logger.Debugf("Current transient worldState: ", worldState.String()) + + executionState, err := motionplan.NewExecutionState( + plan, + 0, + plan.Trajectory()[0], + map[string]*referenceframe.PoseInFrame{ + kb.Kinematics().Name(): referenceframe.NewPoseInFrame(referenceframe.World, currentPose), + }, + ) + if err != nil { + ms.logger.Debugf("issue occurred getting execution state from kinematic base: %v", err) + if errCounterCurrentInputs > successiveErrorLimit { + ms.obstacleResponseChan <- moveResponse{success: false} + return + } + errCounterCurrentInputs++ + } + + // Check plan for transient obstacles + err = motionplan.CheckPlan( + kb.Kinematics(), + executionState, + worldState, + ms.frameSystem, + lookAheadDistanceMM, + ms.logger, + ) + if err != nil { + if strings.Contains(err.Error(), "found constraint violation or collision") { + ms.logger.CDebug(ctx, "collision found in given range") + ms.obstacleResponseChan <- moveResponse{success: true} + return + } + } + } + } +} + +// executePlan will carry out the desired motionplan plan. +func (ms *explore) executePlan(ctx context.Context, kb kinematicbase.KinematicBase, plan motionplan.Plan) { + steps, err := plan.Trajectory().GetFrameInputs(kb.Name().Name) + if err != nil { + ms.logger.Debugf("error in executePlan: %s", err) + return + } + for _, inputs := range steps { + select { + case <-ctx.Done(): + return + default: + if inputEnabledKb, ok := kb.(inputEnabledActuator); ok { + if err := inputEnabledKb.GoToInputs(ctx, inputs); err != nil { + // If there is an error on GoToInputs, stop the component if possible before returning the error + if stopErr := kb.Stop(ctx, nil); stopErr != nil { + ms.executionResponseChan <- moveResponse{err: err} + return + } + // If the error was simply a cancellation of context return without erroring out + if errors.Is(err, context.Canceled) { + return + } + ms.executionResponseChan <- moveResponse{err: err} + return + } + } else { + ms.executionResponseChan <- moveResponse{err: errors.New("unable to cast kinematic base to inputEnabledActuator")} + return + } + } + } + ms.executionResponseChan <- moveResponse{success: true} +} + +// generateTransientWorldState will create a new worldState with transient obstacles generated by the provided +// obstacleDetectors. +func (ms *explore) generateTransientWorldState( + ctx context.Context, + obstacleDetectors []obstacleDetectorObject, +) (*referenceframe.WorldState, error) { + geometriesInFrame := []*referenceframe.GeometriesInFrame{} + + // Iterate through provided obstacle detectors and their associated vision service and cameras + for _, obstacleDetector := range obstacleDetectors { + for visionService, cameraName := range obstacleDetector { + // Get detections as vision objects + detections, err := visionService.GetObjectPointClouds(ctx, cameraName.Name, nil) + if err != nil && strings.Contains(err.Error(), "does not implement a 3D segmenter") { + ms.logger.CInfof(ctx, "cannot call GetObjectPointClouds on %q as it does not implement a 3D segmenter", + visionService.Name()) + } else if err != nil { + return nil, err + } + + // Extract geometries from vision objects + geometries := []spatialmath.Geometry{} + for i, detection := range detections { + geometry := detection.Geometry + label := cameraName.Name + "_transientObstacle_" + strconv.Itoa(i) + if geometry.Label() != "" { + label += "_" + geometry.Label() + } + geometry.SetLabel(label) + geometries = append(geometries, geometry) + } + geometriesInFrame = append(geometriesInFrame, + referenceframe.NewGeometriesInFrame(cameraName.Name, geometries), + ) + } + } + + // Add geometries to worldState + worldState, err := referenceframe.NewWorldState(geometriesInFrame, nil) + if err != nil { + return nil, err + } + return worldState, nil +} + +// createKinematicBase will instantiate a kinematic base from the provided base name and motionCfg with +// associated kinematic base options. This will be a differential drive kinematic base. +func (ms *explore) createKinematicBase( + ctx context.Context, + baseName resource.Name, + motionCfg motion.MotionConfiguration, +) (kinematicbase.KinematicBase, error) { + // Select the base from the component list using the baseName + component, ok := ms.components[baseName] + if !ok { + return nil, resource.DependencyNotFoundError(baseName) + } + + b, ok := component.(base.Base) + if !ok { + return nil, fmt.Errorf("cannot get component of type %T because it is not a Case", component) + } + + // Generate kinematic base options from motionCfg + kinematicsOptions := kinematicbase.NewKinematicBaseOptions() + kinematicsOptions.NoSkidSteer = true + kinematicsOptions.UsePTGs = false + // PositionOnlyMode needs to be false to allow orientation info to be available in CurrentInputs as + // we need that to correctly place obstacles in world's reference frame. + kinematicsOptions.PositionOnlyMode = false + kinematicsOptions.AngularVelocityDegsPerSec = motionCfg.AngularDegsPerSec + kinematicsOptions.LinearVelocityMMPerSec = motionCfg.LinearMPerSec * 1000 + kinematicsOptions.Timeout = defaultExploreTimeout + kinematicsOptions.MaxSpinAngleDeg = defaultMaxSpinAngleDegs + kinematicsOptions.MaxMoveStraightMM = defaultMoveLimitMM + + // Create new kinematic base (differential drive) + kb, err := kinematicbase.WrapWithKinematics( + ctx, + b, + ms.logger, + nil, + []referenceframe.Limit{ + {Min: -defaultMoveLimitMM, Max: defaultMoveLimitMM}, + {Min: -defaultMoveLimitMM, Max: defaultMoveLimitMM}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, + }, + kinematicsOptions, + ) + if err != nil { + return nil, err + } + + return kb, nil +} + +// createObstacleDetectors will generate the list of obstacle detectors from the camera and vision services +// names provided in them motionCfg. +func (ms *explore) createObstacleDetectors(motionCfg motion.MotionConfiguration) ([]obstacleDetectorObject, error) { + var obstacleDetectors []obstacleDetectorObject + // Iterate through obstacleDetectorsNames + for _, obstacleDetectorsName := range motionCfg.ObstacleDetectors { + // Select the vision service from the service list using the vision service name in obstacleDetectorsNames + visionServiceResource, ok := ms.services[obstacleDetectorsName.VisionServiceName] + if !ok { + return nil, resource.DependencyNotFoundError(obstacleDetectorsName.VisionServiceName) + } + visionService, ok := visionServiceResource.(vision.Service) + if !ok { + return nil, fmt.Errorf("cannot get service of type %T because it is not a vision service", + visionServiceResource, + ) + } + + // Select the camera from the component list using the camera name in obstacleDetectorsNames + // Note: May need to be converted to a for loop if we accept multiple cameras for each vision service + cameraResource, ok := ms.components[obstacleDetectorsName.CameraName] + if !ok { + return nil, resource.DependencyNotFoundError(obstacleDetectorsName.CameraName) + } + cam, ok := cameraResource.(camera.Camera) + if !ok { + return nil, fmt.Errorf("cannot get component of type %T because it is not a camera", cameraResource) + } + obstacleDetectors = append(obstacleDetectors, obstacleDetectorObject{visionService: cam.Name()}) + } + + return obstacleDetectors, nil +} + +// createMotionPlan will construct a motion plan towards a specified destination using the given kinematic base. +// No position knowledge is assumed so every plan uses the base, and therefore world, origin as the starting location. +func (ms *explore) createMotionPlan( + ctx context.Context, + kb kinematicbase.KinematicBase, + destination *referenceframe.PoseInFrame, + extra map[string]interface{}, +) (motionplan.Plan, error) { + if destination.Pose().Point().Norm() >= defaultMoveLimitMM { + return nil, errors.Errorf("destination %v is above the defined limit of %v", destination.Pose().Point().String(), defaultMoveLimitMM) + } + + worldState, err := referenceframe.NewWorldState(nil, nil) + if err != nil { + return nil, err + } + + ms.frameSystem, err = ms.fsService.FrameSystem(ctx, worldState.Transforms()) + if err != nil { + return nil, err + } + + // replace origin base frame to remove original differential drive kinematic base geometry as it is overwritten by a bounding sphere + // during kinematic base creation + if err := ms.frameSystem.ReplaceFrame(referenceframe.NewZeroStaticFrame(kb.Kinematics().Name() + "_origin")); err != nil { + return nil, err + } + + // replace original base frame with one that knows how to move itself and allow planning for + if err := ms.frameSystem.ReplaceFrame(kb.Kinematics()); err != nil { + return nil, err + } + + inputs := make([]referenceframe.Input, len(kb.Kinematics().DoF())) + + f := kb.Kinematics() + + seedMap := map[string][]referenceframe.Input{f.Name(): inputs} + + goalPose, err := ms.fsService.TransformPose(ctx, destination, ms.frameSystem.World().Name(), nil) + if err != nil { + return nil, err + } + + ms.logger.CDebugf(ctx, "goal position: %v", goalPose.Pose().Point()) + return motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + Logger: ms.logger, + Goal: goalPose, + Frame: f, + StartConfiguration: seedMap, + FrameSystem: ms.frameSystem, + WorldState: worldState, + Constraints: nil, + Options: extra, + }) +} + +// parseMotionConfig extracts the MotionConfiguration from extra's. +func parseMotionConfig(extra map[string]interface{}) (motion.MotionConfiguration, error) { + motionCfgInterface, ok := extra["motionCfg"] + if !ok { + return motion.MotionConfiguration{}, errors.New("no motionCfg provided") + } + + motionCfg, ok := motionCfgInterface.(motion.MotionConfiguration) + if !ok { + return motion.MotionConfiguration{}, errors.New("could not interpret motionCfg field as an MotionConfiguration") + } + return motionCfg, nil +} diff --git a/services/motion/explore/explore_test.go b/services/motion/explore/explore_test.go new file mode 100644 index 00000000000..658100c6193 --- /dev/null +++ b/services/motion/explore/explore_test.go @@ -0,0 +1,386 @@ +package explore + +import ( + "context" + "fmt" + "testing" + "time" + + "github.com/golang/geo/r3" + "github.com/pkg/errors" + "go.viam.com/test" + goutils "go.viam.com/utils" + + "go.viam.com/rdk/components/base" + "go.viam.com/rdk/components/camera" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" + "go.viam.com/rdk/services/motion" + "go.viam.com/rdk/spatialmath" +) + +var ( + testBaseName = resource.NewName(base.API, "test_base") + testCameraName1 = resource.NewName(camera.API, "test_camera1") + testCameraName2 = resource.NewName(camera.API, "test_camera2") + testFrameServiceName = resource.NewName(framesystem.API, "test_fs") + defaultCollisionBufferMM = 1e-8 + defaultObstaclePollingHz = 2. + defaultMotionCfg = motion.MotionConfiguration{ + AngularDegsPerSec: 0.25, + LinearMPerSec: 0.1, + ObstaclePollingFreqHz: &defaultObstaclePollingHz, + } +) + +type obstacleMetadata struct { + position r3.Vector + data int + label string +} + +func TestExplorePlanMove(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + + fakeBase, err := createFakeBase(ctx, logger) + test.That(t, err, test.ShouldBeNil) + + ms, err := createNewExploreMotionService(ctx, logger, fakeBase, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, ms, test.ShouldNotBeNil) + defer ms.Close(ctx) + + msStruct := ms.(*explore) + + // Create kinematic base + kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) + + cases := []struct { + description string + destination spatialmath.Pose + expectedMotionPlanLength int + }{ + { + description: "destination directly in front of base", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + expectedMotionPlanLength: 2, + }, + { + description: "destination directly behind the base", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -1000, Z: 0}), + expectedMotionPlanLength: 2, + }, + { + description: "destination off axis of base", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 1000, Y: 1000, Z: 0}), + expectedMotionPlanLength: 2, + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) + plan, err := msStruct.createMotionPlan(ctx, kb, dest, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(plan.Path()), test.ShouldEqual, tt.expectedMotionPlanLength) + }) + } +} + +func TestExploreCheckForObstacles(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + + // Create fake camera + fakeCamera, err := createFakeCamera(ctx, logger, testCameraName1.Name) + test.That(t, err, test.ShouldBeNil) + + // Create fake base + fakeBase, err := createFakeBase(ctx, logger) + test.That(t, err, test.ShouldBeNil) + + // Create explore motion service + ms, err := createNewExploreMotionService(ctx, logger, fakeBase, []camera.Camera{fakeCamera}) + test.That(t, err, test.ShouldBeNil) + test.That(t, ms, test.ShouldNotBeNil) + defer ms.Close(ctx) + + msStruct := ms.(*explore) + + // Create kinematic base + kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) + + cases := []struct { + description string + destination spatialmath.Pose + obstacle []obstacleMetadata + detection bool + detectionErr error + }{ + { + description: "no obstacles in view", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + detection: false, + }, + { + description: "obstacle in path nearby", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacle: []obstacleMetadata{ + { + position: r3.Vector{X: 0, Y: 300, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + }, + detection: true, + }, + { + description: "obstacle in path farther away", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacle: []obstacleMetadata{ + { + position: r3.Vector{X: 0, Y: 800, Z: 0}, + data: 100, + label: "far_obstacle_in_path", + }, + }, + detection: false, + }, + { + description: "obstacle in path too far", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 9000, Z: 0}), + obstacle: []obstacleMetadata{ + { + position: r3.Vector{X: 0, Y: 1500, Z: 0}, + data: 100, + label: "far_obstacle_not_in_path", + }, + }, + detection: false, + detectionErr: errors.New("found collision between positions"), + }, + { + description: "obstacle off axis in path", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 1000, Y: 1000, Z: 0}), + obstacle: []obstacleMetadata{ + { + position: r3.Vector{X: 500, Y: 500, Z: 0}, + data: 100, + label: "obstacle on diagonal", + }, + }, + detection: true, + }, + { + description: "obstacle off axis not in path", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacle: []obstacleMetadata{ + { + position: r3.Vector{X: 500, Y: 500, Z: 0}, + data: 100, + label: "close_obstacle_not_in_path", + }, + }, + detection: false, + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + // Create motionplan plan + dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) + plan, err := msStruct.createMotionPlan( + ctx, + kb, + dest, + nil, + ) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) + test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 0) + + // Create a vision service using provided obstacles and place it in an obstacle DetectorPair object + visionService := createMockVisionService("", tt.obstacle) + + obstacleDetectors := []obstacleDetectorObject{ + {visionService: fakeCamera.Name()}, + } + + // Update and check worldState + worldState, err := msStruct.generateTransientWorldState(ctx, obstacleDetectors) + test.That(t, err, test.ShouldBeNil) + + if len(tt.obstacle) == 0 { + fs, err := msStruct.fsService.FrameSystem(ctx, nil) + test.That(t, err, test.ShouldBeNil) + + obstacles, err := worldState.ObstaclesInWorldFrame(fs, nil) + test.That(t, err, test.ShouldBeNil) + + for _, obs := range tt.obstacle { + collisionDetected, err := geometriesContainsPoint(obstacles.Geometries(), obs.position, defaultCollisionBufferMM) + test.That(t, err, test.ShouldBeNil) + test.That(t, collisionDetected, test.ShouldBeTrue) + } + } + + // Run check obstacles in of plan path + ctxTimeout, cancelFunc := context.WithTimeout(ctx, 1*time.Second) + defer cancelFunc() + + msStruct.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + msStruct.checkForObstacles(ctxTimeout, obstacleDetectors, kb, plan, *defaultMotionCfg.ObstaclePollingFreqHz) + }, msStruct.backgroundWorkers.Done) + + time.Sleep(20 * time.Millisecond) + + if tt.detection { + resp := <-msStruct.obstacleResponseChan + test.That(t, resp.success, test.ShouldEqual, tt.detection) + if tt.detectionErr == nil { + test.That(t, resp.err, test.ShouldBeNil) + } else { + test.That(t, resp.err, test.ShouldNotBeNil) + test.That(t, resp.err.Error(), test.ShouldContainSubstring, tt.detectionErr.Error()) + } + } + }) + } +} + +func TestMultipleObstacleDetectors(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + + // Create fake cameras + fakeCamera1, err := createFakeCamera(ctx, logger, testCameraName1.Name) + test.That(t, err, test.ShouldBeNil) + + fakeCamera2, err := createFakeCamera(ctx, logger, testCameraName2.Name) + test.That(t, err, test.ShouldBeNil) + + // Create fake base + fakeBase, err := createFakeBase(ctx, logger) + test.That(t, err, test.ShouldBeNil) + + // Create explore motion service + ms, err := createNewExploreMotionService(ctx, logger, fakeBase, []camera.Camera{fakeCamera1, fakeCamera2}) + test.That(t, err, test.ShouldBeNil) + test.That(t, ms, test.ShouldNotBeNil) + defer ms.Close(ctx) + + msStruct := ms.(*explore) + + // Create kinematic base + kb, err := msStruct.createKinematicBase(ctx, fakeBase.Name(), defaultMotionCfg) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) + + cases := []struct { + description string + destination spatialmath.Pose + obstacleCamera1 obstacleMetadata + obstacleCamera2 obstacleMetadata + visionServiceCamLink []camera.Camera + }{ + { + description: "two independent vision services both detecting obstacles", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacleCamera1: obstacleMetadata{ + position: r3.Vector{X: 0, Y: 300, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + obstacleCamera2: obstacleMetadata{ + position: r3.Vector{X: 0, Y: 500, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, + }, + { + description: "two independent vision services only first detecting obstacle", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacleCamera1: obstacleMetadata{ + position: r3.Vector{X: 0, Y: 300, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + obstacleCamera2: obstacleMetadata{}, + visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, + }, + { + description: "two independent vision services only second detecting obstacle", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacleCamera1: obstacleMetadata{}, + obstacleCamera2: obstacleMetadata{ + position: r3.Vector{X: 0, Y: 300, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, + }, + { + description: "two vision services depending on same camera", + destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 1000, Z: 0}), + obstacleCamera1: obstacleMetadata{ + position: r3.Vector{X: 0, Y: 300, Z: 0}, + data: 100, + label: "close_obstacle_in_path", + }, + obstacleCamera2: obstacleMetadata{}, + visionServiceCamLink: []camera.Camera{fakeCamera1, fakeCamera2}, + }, + } + + for _, tt := range cases { + t.Run(tt.description, func(t *testing.T) { + // Create motionplan plan + dest := referenceframe.NewPoseInFrame(testBaseName.Name, tt.destination) + plan, err := msStruct.createMotionPlan( + ctx, + kb, + dest, + nil, + ) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb.Name().Name, test.ShouldEqual, testBaseName.Name) + test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 0) + + // Create a vision service using provided obstacles and place it in an obstacle DetectorPair object + var obstacleDetectors []obstacleDetectorObject + for i, cam := range tt.visionServiceCamLink { + var obstacles []obstacleMetadata + if cam.Name().Name == testCameraName1.Name { + obstacles = append(obstacles, tt.obstacleCamera1) + } + if cam.Name().Name == testCameraName2.Name { + obstacles = append(obstacles, tt.obstacleCamera2) + } + visionService := createMockVisionService(fmt.Sprint(i), obstacles) + obstacleDetectors = append(obstacleDetectors, obstacleDetectorObject{visionService: cam.Name()}) + } + + // Run check obstacles in of plan path + ctxTimeout, cancelFunc := context.WithTimeout(ctx, 1*time.Second) + defer cancelFunc() + + msStruct.backgroundWorkers.Add(1) + goutils.ManagedGo(func() { + msStruct.checkForObstacles(ctxTimeout, obstacleDetectors, kb, plan, *defaultMotionCfg.ObstaclePollingFreqHz) + }, msStruct.backgroundWorkers.Done) + + resp := <-msStruct.obstacleResponseChan + test.That(t, resp.success, test.ShouldBeTrue) + test.That(t, resp.err, test.ShouldBeNil) + }) + } +} diff --git a/services/motion/explore/explore_utils_test.go b/services/motion/explore/explore_utils_test.go new file mode 100644 index 00000000000..3cb02ab94fa --- /dev/null +++ b/services/motion/explore/explore_utils_test.go @@ -0,0 +1,187 @@ +package explore + +import ( + "context" + + "github.com/golang/geo/r3" + + "go.viam.com/rdk/components/base" + baseFake "go.viam.com/rdk/components/base/fake" + "go.viam.com/rdk/components/camera" + cameraFake "go.viam.com/rdk/components/camera/fake" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/robot/framesystem" + "go.viam.com/rdk/services/motion" + vSvc "go.viam.com/rdk/services/vision" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/testutils/inject" + "go.viam.com/rdk/vision" +) + +// createNewExploreMotionService creates a new motion service complete with base, camera (optional) and frame system. +// Note: The required vision service can/is added later as it will not affect the building of the frame system. +func createNewExploreMotionService(ctx context.Context, logger logging.Logger, fakeBase base.Base, cameras []camera.Camera, +) (motion.Service, error) { + var fsParts []*referenceframe.FrameSystemPart + deps := make(resource.Dependencies) + + // create base link + baseLink, err := createBaseLink(testBaseName.Name) + if err != nil { + return nil, err + } + fsParts = append(fsParts, &referenceframe.FrameSystemPart{FrameConfig: baseLink}) + deps[testBaseName] = fakeBase + + // create camera link + for _, cam := range cameras { + cameraLink, err := createCameraLink(cam.Name().Name, fakeBase.Name().Name) + if err != nil { + return nil, err + } + fsParts = append(fsParts, &referenceframe.FrameSystemPart{FrameConfig: cameraLink}) + deps[cam.Name()] = cam + } + + // create frame service + fs, err := createFrameSystemService(ctx, deps, fsParts, logger) + if err != nil { + return nil, err + } + deps[testFrameServiceName] = fs + + // create explore motion service + exploreMotionConf := resource.Config{ConvertedAttributes: &Config{}} + return NewExplore(ctx, deps, exploreMotionConf, logger) +} + +// createFakeBase instantiates a fake base. +func createFakeBase(ctx context.Context, logger logging.Logger) (base.Base, error) { + fakeBaseCfg := resource.Config{ + Name: testBaseName.Name, + API: base.API, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{X: 300, Y: 200, Z: 100}}, + } + return baseFake.NewBase(ctx, nil, fakeBaseCfg, logger) +} + +// createFakeCamera instantiates a fake camera. +func createFakeCamera(ctx context.Context, logger logging.Logger, name string) (camera.Camera, error) { + fakeCameraCfg := resource.Config{ + Name: name, + API: camera.API, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 5}}, + ConvertedAttributes: &cameraFake.Config{ + Width: 1000, + Height: 1000, + }, + } + return cameraFake.NewCamera(ctx, resource.Dependencies{}, fakeCameraCfg, logger) +} + +// createMockVisionService instantiates a mock vision service with a custom version of GetObjectPointCloud that returns +// vision objects from a given set of points. +func createMockVisionService(visionSvcNum string, obstacles []obstacleMetadata) vSvc.Service { + mockVisionService := &inject.VisionService{} + + mockVisionService.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}, + ) ([]*vision.Object, error) { + var vObjects []*vision.Object + for _, obs := range obstacles { + obstaclePosition := spatialmath.NewPoseFromPoint(obs.position) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 100, Y: 100, Z: 100}, "test-case-2") + if err != nil { + return nil, err + } + + detection, err := vision.NewObjectWithLabel(pointcloud.New(), obs.label+visionSvcNum, box.ToProtobuf()) + if err != nil { + return nil, err + } + vObjects = append(vObjects, detection) + } + return vObjects, nil + } + return mockVisionService +} + +// createFrameSystemService will create a basic frame service from the list of parts. +func createFrameSystemService( + ctx context.Context, + deps resource.Dependencies, + fsParts []*referenceframe.FrameSystemPart, + logger logging.Logger, +) (framesystem.Service, error) { + fsSvc, err := framesystem.New(ctx, deps, logger) + if err != nil { + return nil, err + } + conf := resource.Config{ + ConvertedAttributes: &framesystem.Config{Parts: fsParts}, + } + if err := fsSvc.Reconfigure(ctx, deps, conf); err != nil { + return nil, err + } + + return fsSvc, nil +} + +// createBaseLink instantiates the base to world link for the frame system. +func createBaseLink(baseName string) (*referenceframe.LinkInFrame, error) { + basePose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) + baseSphere, err := spatialmath.NewSphere(basePose, 10, "base-box") + if err != nil { + return nil, err + } + + baseLink := referenceframe.NewLinkInFrame( + referenceframe.World, + spatialmath.NewZeroPose(), + baseName, + baseSphere, + ) + return baseLink, nil +} + +// createCameraLink instantiates the camera to base link for the frame system. +func createCameraLink(camName, baseFrame string) (*referenceframe.LinkInFrame, error) { + camPose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) + camSphere, err := spatialmath.NewSphere(camPose, 5, "cam-sphere") + if err != nil { + return nil, err + } + + camLink := referenceframe.NewLinkInFrame( + baseFrame, + spatialmath.NewZeroPose(), + camName, + camSphere, + ) + return camLink, nil +} + +// geometriesContainsPoint is a helper function to test if a point is in a given geometry. +func geometriesContainsPoint(geometries []spatialmath.Geometry, point r3.Vector, collisionBufferMM float64) (bool, error) { + var collisionDetected bool + for _, geo := range geometries { + pointGeoCfg := spatialmath.GeometryConfig{ + Type: spatialmath.BoxType, + TranslationOffset: point, + } + pointGeo, err := pointGeoCfg.ParseConfig() + if err != nil { + return false, err + } + collides, err := geo.CollidesWith(pointGeo, collisionBufferMM) + if err != nil { + return false, err + } + if collides { + collisionDetected = true + } + } + return collisionDetected, nil +} From a6890e83794681abc6c4a8c1904527e917247398 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 13:58:36 -0400 Subject: [PATCH 11/16] make explore tests work --- services/motion/explore/explore.go | 15 ++++----------- services/navigation/builtin/explore.go | 8 ++++++-- services/navigation/builtin/explore_test.go | 11 +++-------- testutils/inject/motion_service.go | 6 ++++-- 4 files changed, 17 insertions(+), 23 deletions(-) diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index 03a89a393ae..bed7f8674c6 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -210,21 +210,14 @@ func (ms *explore) Close(ctx context.Context) error { // Move takes a goal location and will plan and execute a movement to move a component specified by its name // to that destination. -func (ms *explore) Move( - ctx context.Context, - componentName resource.Name, - destination *referenceframe.PoseInFrame, - worldState *referenceframe.WorldState, - constraints *motionplan.Constraints, - extra map[string]interface{}, -) (bool, error) { +func (ms *explore) Move(ctx context.Context, req motion.MoveReq) (bool, error) { ms.resourceMutex.Lock() defer ms.resourceMutex.Unlock() operation.CancelOtherWithLabel(ctx, exploreOpLabel) // Parse extras - motionCfg, err := parseMotionConfig(extra) + motionCfg, err := parseMotionConfig(req.Extra) if err != nil { return false, err } @@ -236,13 +229,13 @@ func (ms *explore) Move( } // Create kinematic base - kb, err := ms.createKinematicBase(ctx, componentName, motionCfg) + kb, err := ms.createKinematicBase(ctx, req.ComponentName, motionCfg) if err != nil { return false, err } // Create motionplan plan - plan, err := ms.createMotionPlan(ctx, kb, destination, extra) + plan, err := ms.createMotionPlan(ctx, kb, req.Destination, req.Extra) if err != nil { return false, err } diff --git a/services/navigation/builtin/explore.go b/services/navigation/builtin/explore.go index ce8cd208489..594f9c3d8e9 100644 --- a/services/navigation/builtin/explore.go +++ b/services/navigation/builtin/explore.go @@ -9,6 +9,7 @@ import ( "go.viam.com/utils" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" ) @@ -37,8 +38,11 @@ func (svc *builtIn) startExploreMode(ctx context.Context) { Z: 0., }.Normalize().Mul(defaultDistanceMM), spatialmath.NewOrientationVector())) - _, err := svc.exploreMotionService.Move(ctx, svc.base.Name(), destination, nil, nil, extra) - if err != nil { + if _, err := svc.exploreMotionService.Move(ctx, motion.MoveReq{ + ComponentName: svc.base.Name(), + Destination: destination, + Extra: extra, + }); err != nil { svc.logger.CDebugf(ctx, "error occurred when moving to point %v: %v", destination, err) } } diff --git a/services/navigation/builtin/explore_test.go b/services/navigation/builtin/explore_test.go index 579f0532149..606c26d8164 100644 --- a/services/navigation/builtin/explore_test.go +++ b/services/navigation/builtin/explore_test.go @@ -9,9 +9,7 @@ import ( "github.com/pkg/errors" "go.viam.com/test" - "go.viam.com/rdk/motionplan" - frame "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" + "go.viam.com/rdk/services/motion" "go.viam.com/rdk/testutils/inject" ) @@ -22,11 +20,8 @@ func TestExploreMode(t *testing.T) { var points []r3.Vector mockExploreMotionService := &inject.MotionService{} - mockExploreMotionService.MoveFunc = func(ctx context.Context, componentName resource.Name, - destination *frame.PoseInFrame, worldState *frame.WorldState, constraints *motionplan.Constraints, - extra map[string]interface{}, - ) (bool, error) { - points = append(points, destination.Pose().Point()) + mockExploreMotionService.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { + points = append(points, req.Destination.Pose().Point()) return false, errors.New("expected error") } diff --git a/testutils/inject/motion_service.go b/testutils/inject/motion_service.go index 50bf3f0564f..66a8cc2f7e0 100644 --- a/testutils/inject/motion_service.go +++ b/testutils/inject/motion_service.go @@ -44,8 +44,10 @@ type MotionService struct { ctx context.Context, req motion.PlanHistoryReq, ) ([]motion.PlanWithStatus, error) - DoCommandFunc func(ctx context.Context, - cmd map[string]interface{}) (map[string]interface{}, error) + DoCommandFunc func( + ctx context.Context, + cmd map[string]interface{}) (map[string]interface{}, error, + ) CloseFunc func(ctx context.Context) error } From 8667ccbfa2fe6a299dafd53ad6b2165509fcece0 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 15:27:48 -0400 Subject: [PATCH 12/16] add case for nil destination --- go.mod | 2 +- services/motion/builtin/builtin.go | 6 +++++- services/motion/builtin/builtin_test.go | 9 +++++++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/go.mod b/go.mod index c3dcaa00077..6d1c1823acd 100644 --- a/go.mod +++ b/go.mod @@ -58,6 +58,7 @@ require ( github.com/lucasb-eyer/go-colorful v1.2.0 github.com/mattn/go-tflite v1.0.4 github.com/matttproud/golang_protobuf_extensions v1.0.4 + github.com/mitchellh/mapstructure v1.5.0 github.com/mkch/gpio v0.0.0-20190919032813-8327cd97d95e github.com/montanaflynn/stats v0.7.0 github.com/muesli/clusters v0.0.0-20200529215643-2700303c1762 @@ -285,7 +286,6 @@ require ( github.com/miekg/dns v1.1.53 // indirect github.com/mitchellh/copystructure v1.2.0 // indirect github.com/mitchellh/go-homedir v1.1.0 // indirect - github.com/mitchellh/mapstructure v1.5.0 // indirect github.com/mitchellh/reflectwalk v1.0.2 // indirect github.com/moby/term v0.5.0 // indirect github.com/moricho/tparallel v0.3.1 // indirect diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index e1eb89270c0..f1b1d19626c 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -12,8 +12,8 @@ import ( "github.com/google/uuid" "github.com/mitchellh/mapstructure" "github.com/pkg/errors" - pb "go.viam.com/api/service/motion/v1" + "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" @@ -375,6 +375,10 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m } func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Plan, error) { + if req.Destination == nil { + return nil, errors.New("cannot specify a nil destination in motion.MoveReq") + } + frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) if err != nil { return nil, err diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 3e907b65920..343d2277224 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -131,8 +131,13 @@ func TestMoveFailures(t *testing.T) { defer teardown() ctx := context.Background() t.Run("fail on not finding gripper", func(t *testing.T) { - grabPose := referenceframe.NewPoseInFrame("fakeCamera", spatialmath.NewPoseFromPoint(r3.Vector{X: 10.0, Y: 10.0, Z: 10.0})) - _, err = ms.Move(ctx, motion.MoveReq{ComponentName: camera.Named("fake"), Destination: grabPose}) + grabPose := referenceframe.NewPoseInFrame("fakeGripper", spatialmath.NewPoseFromPoint(r3.Vector{X: 10.0, Y: 10.0, Z: 10.0})) + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: gripper.Named("fake"), Destination: grabPose}) + test.That(t, err, test.ShouldNotBeNil) + }) + + t.Run("fail on nil destination", func(t *testing.T) { + _, err = ms.Move(ctx, motion.MoveReq{ComponentName: arm.Named("arm1")}) test.That(t, err, test.ShouldNotBeNil) }) From 4358fea7d6291b9bb03896124f78d3f480e9e6ee Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 15:31:53 -0400 Subject: [PATCH 13/16] a little lint --- go.mod | 2 +- services/motion/builtin/builtin.go | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/go.mod b/go.mod index 6d1c1823acd..c3dcaa00077 100644 --- a/go.mod +++ b/go.mod @@ -58,7 +58,6 @@ require ( github.com/lucasb-eyer/go-colorful v1.2.0 github.com/mattn/go-tflite v1.0.4 github.com/matttproud/golang_protobuf_extensions v1.0.4 - github.com/mitchellh/mapstructure v1.5.0 github.com/mkch/gpio v0.0.0-20190919032813-8327cd97d95e github.com/montanaflynn/stats v0.7.0 github.com/muesli/clusters v0.0.0-20200529215643-2700303c1762 @@ -286,6 +285,7 @@ require ( github.com/miekg/dns v1.1.53 // indirect github.com/mitchellh/copystructure v1.2.0 // indirect github.com/mitchellh/go-homedir v1.1.0 // indirect + github.com/mitchellh/mapstructure v1.5.0 // indirect github.com/mitchellh/reflectwalk v1.0.2 // indirect github.com/moby/term v0.5.0 // indirect github.com/moricho/tparallel v0.3.1 // indirect diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index f1b1d19626c..241b36bf56e 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -8,9 +8,9 @@ import ( "sync" "time" + "github.com/go-viper/mapstructure/v2" "github.com/golang/geo/r3" "github.com/google/uuid" - "github.com/mitchellh/mapstructure" "github.com/pkg/errors" pb "go.viam.com/api/service/motion/v1" From 88158e26f96aa0b3dff07ebcbe75b4363f03859f Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 16:18:28 -0400 Subject: [PATCH 14/16] comments --- services/motion/builtin/builtin.go | 9 ++++ services/motion/motion.go | 13 ++++-- services/motion/motion_test.go | 24 ----------- services/motion/pbhelpers.go | 69 ++++++++++++++++-------------- 4 files changed, 54 insertions(+), 61 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 241b36bf56e..052dad9b42a 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -335,6 +335,15 @@ func (ms *builtIn) PlanHistory( return ms.state.PlanHistory(req) } +// DoCommand supports two commands which are specified through the command map +// - DoPlan generates and returns a Trajectory for a given motionpb.MoveRequest without executing it +// required key: DoPlan +// input value: a motionpb.MoveRequest which will be used to create a Trajectory +// output value: a motionplan.Trajectory specified as a map (the mapstructure.Decode function is useful for decoding this) +// - DoExecute takes a Trajectory and executes it +// required key: DoExecute +// input value: a motionplan.Trajectory +// output value: a bool func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { ms.mu.RLock() defer ms.mu.RUnlock() diff --git a/services/motion/motion.go b/services/motion/motion.go index 153e523d6a3..98eadaa4a9e 100644 --- a/services/motion/motion.go +++ b/services/motion/motion.go @@ -204,7 +204,7 @@ type PlanWithStatus struct { // A Service controls the flow of moving components. // For more information, see the [motion service docs]. // -// Move example: // this has to be changed +// Move example: // // motionService, err := motion.FromRobot(machine, "builtin") // @@ -232,7 +232,12 @@ type PlanWithStatus struct { // worldState, err := referenceframe.NewWorldState(obstacles, transforms) // // // Move gripper component -// moved, err := motionService.Move(context.Background(), gripperName, destination, worldState, nil, nil) +// +// moved, err := motionService.Move(context.Background(), motion.MoveReq{ +// ComponentName: gripperName, +// Destination: destination, +// WorldState: WorldState +// }) // // MoveOnMap example: // @@ -346,10 +351,10 @@ type PlanWithStatus struct { type Service interface { resource.Resource - // Move is the primary method to move multiple components or any object to a specified location. // TODO change comment + // Move is the primary method to move multiple components or any object to a specified location. // Given a destination pose and a component, Move constructs a kinematic chain from goal to destination, // solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself - // and other known objects. + // and other known objects. The above arguments are all grouped together in the MoveReq struct. Move(ctx context.Context, req MoveReq) (bool, error) // MoveOnMap moves a base component to a destination Pose on a SLAM map and returns a unique ExecutionID. diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index 384873d1ee8..cd3c8493a39 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -1424,30 +1424,6 @@ func TestPlanHistoryReq(t *testing.T) { }) } -// func TestMoveReq(t *testing.T) { -// req := MoveReq{ -// ComponentName: arm.Named("fake"), -// Destination: referenceframe.NewPoseInFrame("world", spatialmath.NewZeroPose()), -// WorldState: referenceframe.NewEmptyWorldState(), -// Extra: map[string]interface{}{ -// "oops": 1, -// }, -// Constraints: motionplan.NewConstraints([]motionplan.LinearConstraint{{LineToleranceMm: 10}}, nil, nil), -// } -// bytes, err := json.Marshal(req) -// test.That(t, err, test.ShouldBeNil) -// var i map[string]interface{} -// err = json.Unmarshal(bytes, &i) -// test.That(t, err, test.ShouldBeNil) - -// bytes2, err := json.Marshal(i) -// test.That(t, err, test.ShouldBeNil) -// var req2 MoveReq -// err = json.Unmarshal(bytes2, &req2) -// test.That(t, err, test.ShouldBeNil) -// test.That(t, req, test.ShouldResemble, req2) -// } - func validMoveOnGlobeRequest() MoveOnGlobeReq { dst := geo.NewPoint(1, 2) visionCameraPairs := [][]resource.Name{ diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index f811045faff..5c1a0b7bcd2 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -16,6 +16,42 @@ import ( "go.viam.com/rdk/spatialmath" ) +// ToProto converts a MoveReq to a pb.MoveRequest +// the name argument should correspond to the name of the motion service the request will be used with. +func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { + ext, err := vprotoutils.StructToStructPb(r.Extra) + if err != nil { + return nil, err + } + worldStateMsg, err := r.WorldState.ToProtobuf() + if err != nil { + return nil, err + } + return &pb.MoveRequest{ + Name: name, + ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), + Destination: referenceframe.PoseInFrameToProtobuf(r.Destination), + WorldState: worldStateMsg, + Constraints: r.Constraints.ToProtobuf(), + Extra: ext, + }, nil +} + +// MoveReqFromProto converts a pb.MoveRequest to a MoveReq struct. +func MoveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { + worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) + if err != nil { + return MoveReq{}, err + } + return MoveReq{ + rprotoutils.ResourceNameFromProto(req.GetComponentName()), + referenceframe.ProtobufToPoseInFrame(req.GetDestination()), + worldState, + motionplan.ConstraintsFromProtobuf(req.GetConstraints()), + req.Extra.AsMap(), + }, nil +} + // planWithStatusFromProto converts a *pb.PlanWithStatus to a PlanWithStatus. func planWithStatusFromProto(pws *pb.PlanWithStatus) (PlanWithStatus, error) { if pws == nil { @@ -153,39 +189,6 @@ func planStateFromProto(ps pb.PlanState) PlanState { } } -func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { - ext, err := vprotoutils.StructToStructPb(r.Extra) - if err != nil { - return nil, err - } - worldStateMsg, err := r.WorldState.ToProtobuf() - if err != nil { - return nil, err - } - return &pb.MoveRequest{ - Name: name, - ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), - Destination: referenceframe.PoseInFrameToProtobuf(r.Destination), - WorldState: worldStateMsg, - Constraints: r.Constraints.ToProtobuf(), - Extra: ext, - }, nil -} - -func MoveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { - worldState, err := referenceframe.WorldStateFromProtobuf(req.GetWorldState()) - if err != nil { - return MoveReq{}, err - } - return MoveReq{ - rprotoutils.ResourceNameFromProto(req.GetComponentName()), - referenceframe.ProtobufToPoseInFrame(req.GetDestination()), - worldState, - motionplan.ConstraintsFromProtobuf(req.GetConstraints()), - req.Extra.AsMap(), - }, nil -} - // toProto converts a MoveOnGlobeRequest to a *pb.MoveOnGlobeRequest. func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) { ext, err := vprotoutils.StructToStructPb(r.Extra) From 8bb224392fce086926274eb6d1bc83d394317e87 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Thu, 19 Sep 2024 16:22:55 -0400 Subject: [PATCH 15/16] return early --- services/motion/builtin/builtin.go | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 052dad9b42a..c532a550d92 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -414,7 +414,7 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla goalPose, _ := tf.(*referenceframe.PoseInFrame) // the goal is to move the component to goalPose which is specified in coordinates of goalFrameName - plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ + return motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ Logger: ms.logger, Goal: goalPose, Frame: movingFrame, @@ -424,10 +424,6 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla Constraints: req.Constraints, Options: req.Extra, }) - if err != nil { - return nil, err - } - return plan, nil } func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory) error { From 71c912a38286d14e4e2bb4fe2042cf990bf5ae5a Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Mon, 23 Sep 2024 09:54:11 -0400 Subject: [PATCH 16/16] move inputEnabled to framesystem --- components/arm/arm.go | 3 ++- components/base/kinematicbase/kinematics.go | 3 ++- components/gantry/gantry.go | 3 ++- referenceframe/primitives.go | 10 ---------- robot/framesystem/framesystem.go | 18 +++++++++++++----- services/motion/builtin/builtin.go | 2 +- services/motion/explore/explore.go | 2 +- testutils/inject/framesystem_service.go | 4 ++-- 8 files changed, 23 insertions(+), 22 deletions(-) diff --git a/components/arm/arm.go b/components/arm/arm.go index d01d6502a3f..d315d07bc8f 100644 --- a/components/arm/arm.go +++ b/components/arm/arm.go @@ -17,6 +17,7 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" + "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/spatialmath" ) @@ -98,7 +99,7 @@ type Arm interface { referenceframe.ModelFramer resource.Shaped resource.Actuator - referenceframe.InputEnabled + framesystem.InputEnabled // EndPosition returns the current position of the arm. EndPosition(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index fef8c62357b..755033d40d3 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -13,6 +13,7 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/robot/framesystem" "go.viam.com/rdk/services/motion" ) @@ -20,7 +21,7 @@ import ( type KinematicBase interface { base.Base motion.Localizer - referenceframe.InputEnabled + framesystem.InputEnabled Kinematics() referenceframe.Frame LocalizationFrame() referenceframe.Frame diff --git a/components/gantry/gantry.go b/components/gantry/gantry.go index 5d3e1a8ba29..df23197386d 100644 --- a/components/gantry/gantry.go +++ b/components/gantry/gantry.go @@ -13,6 +13,7 @@ import ( "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/robot" + "go.viam.com/rdk/robot/framesystem" ) func init() { @@ -85,7 +86,7 @@ type Gantry interface { resource.Resource resource.Actuator referenceframe.ModelFramer - referenceframe.InputEnabled + framesystem.InputEnabled // Position returns the position in meters. Position(ctx context.Context, extra map[string]interface{}) ([]float64, error) diff --git a/referenceframe/primitives.go b/referenceframe/primitives.go index bf8851f4932..c5ced88a77b 100644 --- a/referenceframe/primitives.go +++ b/referenceframe/primitives.go @@ -1,7 +1,6 @@ package referenceframe import ( - "context" "fmt" "math" @@ -56,15 +55,6 @@ func JointPositionsFromRadians(radians []float64) *pb.JointPositions { return &pb.JointPositions{Values: n} } -// InputEnabled is a standard interface for all things that interact with the frame system -// This allows us to figure out where they currently are, and then move them. -// Input units are always in meters or radians. -// TODO: this really belongs in the motion service theres nothing stateful about the referenceframe package. -type InputEnabled interface { - CurrentInputs(ctx context.Context) ([]Input, error) - GoToInputs(context.Context, ...[]Input) error -} - // interpolateInputs will return a set of inputs that are the specified percent between the two given sets of // inputs. For example, setting by to 0.5 will return the inputs halfway between the from/to values, and 0.25 would // return one quarter of the way from "from" to "to". diff --git a/robot/framesystem/framesystem.go b/robot/framesystem/framesystem.go index a432fb9686c..62a0583f23b 100644 --- a/robot/framesystem/framesystem.go +++ b/robot/framesystem/framesystem.go @@ -30,6 +30,14 @@ var API = resource.APINamespaceRDKInternal.WithServiceType(SubtypeName) // InternalServiceName is used to refer to/depend on this service internally. var InternalServiceName = resource.NewName(API, "builtin") +// InputEnabled is a standard interface for all things that interact with the frame system +// This allows us to figure out where they currently are, and then move them. +// Input units are always in meters or radians. +type InputEnabled interface { + CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) + GoToInputs(context.Context, ...[]referenceframe.Input) error +} + // A Service that returns the frame system for a robot. // // TransformPose example: @@ -73,7 +81,7 @@ type Service interface { // CurrentInputs returns a map of the current inputs for each component of a machine's frame system // and a map of statuses indicating which of the machine's components may be actuated through input values. - CurrentInputs(ctx context.Context) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error) + CurrentInputs(ctx context.Context) (map[string][]referenceframe.Input, map[string]InputEnabled, error) // FrameSystem returns the frame system of the machine and incorporates any specified additional transformations. FrameSystem(ctx context.Context, additionalTransforms []*referenceframe.LinkInFrame) (referenceframe.FrameSystem, error) @@ -222,7 +230,7 @@ func (svc *frameSystemService) TransformPose( if !ok { return nil, DependencyNotFoundError(name) } - inputEnabled, ok := component.(referenceframe.InputEnabled) + inputEnabled, ok := component.(InputEnabled) if !ok { return nil, NotInputEnabledError(component) } @@ -247,7 +255,7 @@ func (svc *frameSystemService) TransformPose( // InputEnabled resources that those inputs came from. func (svc *frameSystemService) CurrentInputs( ctx context.Context, -) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error) { +) (map[string][]referenceframe.Input, map[string]InputEnabled, error) { fs, err := svc.FrameSystem(ctx, []*referenceframe.LinkInFrame{}) if err != nil { return nil, nil, err @@ -255,7 +263,7 @@ func (svc *frameSystemService) CurrentInputs( input := referenceframe.StartPositions(fs) // build maps of relevant components and inputs from initial inputs - resources := map[string]referenceframe.InputEnabled{} + resources := map[string]InputEnabled{} for name, original := range input { // skip frames with no input if len(original) == 0 { @@ -267,7 +275,7 @@ func (svc *frameSystemService) CurrentInputs( if !ok { return nil, nil, DependencyNotFoundError(name) } - inputEnabled, ok := component.(referenceframe.InputEnabled) + inputEnabled, ok := component.(InputEnabled) if !ok { return nil, nil, NotInputEnabledError(component) } diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index c532a550d92..1aca849e8b4 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -76,7 +76,7 @@ var ( // move it. Input units are always in meters or radians. type inputEnabledActuator interface { resource.Actuator - referenceframe.InputEnabled + framesystem.InputEnabled } // Config describes how to configure the service; currently only used for specifying dependency on framesystem service. diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index bed7f8674c6..d00b326ed72 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -78,7 +78,7 @@ type moveResponse struct { // move it. Input units are always in meters or radians. type inputEnabledActuator interface { resource.Actuator - referenceframe.InputEnabled + framesystem.InputEnabled } // obstacleDetectorObject provides a map for matching vision services to any and all cameras names they use. diff --git a/testutils/inject/framesystem_service.go b/testutils/inject/framesystem_service.go index 0ebca7c0977..486a1dbaddc 100644 --- a/testutils/inject/framesystem_service.go +++ b/testutils/inject/framesystem_service.go @@ -26,7 +26,7 @@ type FrameSystemService struct { srcpc pointcloud.PointCloud, srcName, dstName string, ) (pointcloud.PointCloud, error) - CurrentInputsFunc func(ctx context.Context) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error) + CurrentInputsFunc func(ctx context.Context) (map[string][]referenceframe.Input, map[string]framesystem.InputEnabled, error) FrameSystemFunc func( ctx context.Context, additionalTransforms []*referenceframe.LinkInFrame, @@ -80,7 +80,7 @@ func (fs *FrameSystemService) TransformPointCloud( // CurrentInputs calls the injected method or the real variant. func (fs *FrameSystemService) CurrentInputs( ctx context.Context, -) (map[string][]referenceframe.Input, map[string]referenceframe.InputEnabled, error) { +) (map[string][]referenceframe.Input, map[string]framesystem.InputEnabled, error) { if fs.CurrentInputsFunc == nil { return fs.Service.CurrentInputs(ctx) }