Skip to content

Commit

Permalink
[RSDK-7904] Remove status and streaming status from robot interface (#…
Browse files Browse the repository at this point in the history 
…4461)
  • Loading branch information
@randhid
randhid authored Dec 11, 2024
1 parent cd49068 commit 3f1a3b6
Show file tree
Hide file tree
Showing 32 changed files with 89 additions and 2,061 deletions.
28 changes: 0 additions & 28 deletions components/arm/arm.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@ import (
"context"
"fmt"

v1 "go.viam.com/api/common/v1"
pb "go.viam.com/api/component/arm/v1"

"go.viam.com/rdk/data"
Expand All @@ -24,7 +23,6 @@ import (

func init() {
resource.RegisterAPI(API, resource.APIRegistration[Arm]{
Status: resource.StatusFunc(CreateStatus),
RPCServiceServerConstructor: NewRPCServiceServer,
RPCServiceHandler: pb.RegisterArmServiceHandlerFromEndpoint,
RPCServiceDesc: &pb.ArmService_ServiceDesc,
Expand Down Expand Up @@ -146,32 +144,6 @@ func NamesFromRobot(r robot.Robot) []string {
return robot.NamesByAPI(r, API)
}

// CreateStatus creates a status from the arm. This will report calculated end effector positions even if the given
// arm is perceived to be out of bounds.
func CreateStatus(ctx context.Context, a Arm) (*pb.Status, error) {
model := a.ModelFrame()
joints, err := a.JointPositions(ctx, nil)
if err != nil {
return nil, err
}

var endPosition *v1.Pose
if endPose, err := referenceframe.ComputeOOBPosition(model, joints); err == nil {
endPosition = spatialmath.PoseToProtobuf(endPose)
}

var jointPositions *pb.JointPositions
if jp, err := referenceframe.JointPositionsFromInputs(model, joints); err == nil {
jointPositions = jp
}

isMoving, err := a.IsMoving(ctx)
if err != nil {
return nil, err
}
return &pb.Status{EndPosition: endPosition, JointPositions: jointPositions, IsMoving: isMoving}, nil
}

// CheckDesiredJointPositions validates that the desired joint positions either bring the joint back
// in bounds or do not move the joint more out of bounds.
func CheckDesiredJointPositions(ctx context.Context, a Arm, desiredInputs []referenceframe.Input) error {
Expand Down
166 changes: 0 additions & 166 deletions components/arm/arm_test.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,18 +2,13 @@ package arm_test

import (
"context"
"errors"
"testing"

"github.com/go-viper/mapstructure/v2"
"github.com/golang/geo/r3"
pb "go.viam.com/api/component/arm/v1"
"go.viam.com/test"
"go.viam.com/utils/protoutils"

"go.viam.com/rdk/components/arm"
"go.viam.com/rdk/components/arm/fake"
ur "go.viam.com/rdk/components/arm/universalrobots"
"go.viam.com/rdk/components/generic"
"go.viam.com/rdk/logging"
"go.viam.com/rdk/referenceframe"
Expand All @@ -30,167 +25,6 @@ const (
missingArmName = "arm4"
)

var pose = spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})

func TestStatusValid(t *testing.T) {
status := &pb.Status{
EndPosition: spatialmath.PoseToProtobuf(pose),
JointPositions: &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3}},
IsMoving: true,
}
newStruct, err := protoutils.StructToStructPb(status)
test.That(t, err, test.ShouldBeNil)
test.That(
t,
newStruct.AsMap(),
test.ShouldResemble,
map[string]interface{}{
"end_position": map[string]interface{}{"o_z": 1.0, "x": 1.0, "y": 2.0, "z": 3.0},
"joint_positions": map[string]interface{}{"values": []interface{}{1.1, 2.2, 3.3}},
"is_moving": true,
},
)

convMap := &pb.Status{}
decoder, err := mapstructure.NewDecoder(&mapstructure.DecoderConfig{TagName: "json", Result: &convMap})
test.That(t, err, test.ShouldBeNil)
err = decoder.Decode(newStruct.AsMap())
test.That(t, err, test.ShouldBeNil)
test.That(t, convMap, test.ShouldResemble, status)
}

func TestCreateStatus(t *testing.T) {
successfulPose := spatialmath.NewPose(
r3.Vector{-802.801508917897990613710135, -248.284077946287368376943050, 9.115758604150467903082244},
&spatialmath.R4AA{1.5810814917942602, 0.992515011486776, -0.0953988491934626, 0.07624310818669232},
)
successfulStatus := &pb.Status{
EndPosition: spatialmath.PoseToProtobuf(successfulPose),
JointPositions: &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}},
IsMoving: true,
}

injectArm := &inject.Arm{}

//nolint:unparam
successfulJointPositionsFunc := func(context.Context, map[string]interface{}) ([]referenceframe.Input, error) {
return referenceframe.FloatsToInputs(referenceframe.JointPositionsToRadians(successfulStatus.JointPositions)), nil
}

successfulIsMovingFunc := func(context.Context) (bool, error) {
return true, nil
}

successfulModelFrameFunc := func() referenceframe.Model {
model, _ := ur.MakeModelFrame("ur5e")
return model
}

t.Run("working", func(t *testing.T) {
injectArm.JointPositionsFunc = successfulJointPositionsFunc
injectArm.IsMovingFunc = successfulIsMovingFunc
injectArm.ModelFrameFunc = successfulModelFrameFunc

expectedPose := successfulPose
expectedStatus := successfulStatus

actualStatus, err := arm.CreateStatus(context.Background(), injectArm)
test.That(t, err, test.ShouldBeNil)
test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving)
test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions)

actualPose := spatialmath.NewPoseFromProtobuf(actualStatus.EndPosition)
test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue)

resourceAPI, ok, err := resource.LookupAPIRegistration[arm.Arm](arm.API)
test.That(t, err, test.ShouldBeNil)
test.That(t, ok, test.ShouldBeTrue)
statusInterface, err := resourceAPI.Status(context.Background(), injectArm)
test.That(t, err, test.ShouldBeNil)

statusMap, err := protoutils.InterfaceToMap(statusInterface)
test.That(t, err, test.ShouldBeNil)

endPosMap, err := protoutils.InterfaceToMap(statusMap["end_position"])
test.That(t, err, test.ShouldBeNil)
actualPose = spatialmath.NewPose(
r3.Vector{endPosMap["x"].(float64), endPosMap["y"].(float64), endPosMap["z"].(float64)},
&spatialmath.OrientationVectorDegrees{
endPosMap["theta"].(float64), endPosMap["o_x"].(float64),
endPosMap["o_y"].(float64), endPosMap["o_z"].(float64),
},
)
test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue)

moving := statusMap["is_moving"].(bool)
test.That(t, moving, test.ShouldEqual, expectedStatus.IsMoving)

jPosFace := statusMap["joint_positions"].(map[string]interface{})["values"].([]interface{})
actualJointPositions := []float64{
jPosFace[0].(float64), jPosFace[1].(float64), jPosFace[2].(float64),
jPosFace[3].(float64), jPosFace[4].(float64), jPosFace[5].(float64),
}
test.That(t, actualJointPositions, test.ShouldResemble, expectedStatus.JointPositions.Values)
})

t.Run("not moving", func(t *testing.T) {
injectArm.JointPositionsFunc = successfulJointPositionsFunc
injectArm.ModelFrameFunc = successfulModelFrameFunc

injectArm.IsMovingFunc = func(context.Context) (bool, error) {
return false, nil
}

expectedPose := successfulPose
expectedStatus := &pb.Status{
EndPosition: successfulStatus.EndPosition, //nolint:govet
JointPositions: successfulStatus.JointPositions,
IsMoving: false,
}

actualStatus, err := arm.CreateStatus(context.Background(), injectArm)
test.That(t, err, test.ShouldBeNil)
test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving)
test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions)
actualPose := spatialmath.NewPoseFromProtobuf(actualStatus.EndPosition)
test.That(t, spatialmath.PoseAlmostEqualEps(actualPose, expectedPose, 0.01), test.ShouldBeTrue)
})

t.Run("fail on JointPositions", func(t *testing.T) {
injectArm.IsMovingFunc = successfulIsMovingFunc
injectArm.ModelFrameFunc = successfulModelFrameFunc

errFail := errors.New("can't get joint positions")
injectArm.JointPositionsFunc = func(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
return nil, errFail
}

actualStatus, err := arm.CreateStatus(context.Background(), injectArm)
test.That(t, err, test.ShouldBeError, errFail)
test.That(t, actualStatus, test.ShouldBeNil)
})

t.Run("nil model frame", func(t *testing.T) {
injectArm.IsMovingFunc = successfulIsMovingFunc
injectArm.JointPositionsFunc = successfulJointPositionsFunc
injectArm.ModelFrameFunc = func() referenceframe.Model {
return nil
}

expectedStatus := &pb.Status{
EndPosition: nil,
JointPositions: successfulStatus.JointPositions,
IsMoving: successfulStatus.IsMoving,
}

actualStatus, err := arm.CreateStatus(context.Background(), injectArm)
test.That(t, err, test.ShouldBeNil)
test.That(t, actualStatus.EndPosition, test.ShouldEqual, expectedStatus.EndPosition)
test.That(t, actualStatus.JointPositions, test.ShouldResemble, expectedStatus.JointPositions)
test.That(t, actualStatus.IsMoving, test.ShouldEqual, expectedStatus.IsMoving)
})
}

func TestXArm6Locations(t *testing.T) {
// check the exact values/locations of arm geometries at a couple different poses
logger := logging.NewTestLogger(t)
Expand Down
11 changes: 0 additions & 11 deletions components/base/base.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@ import (
"context"

"github.com/golang/geo/r3"
commonpb "go.viam.com/api/common/v1"
pb "go.viam.com/api/component/base/v1"

"go.viam.com/rdk/resource"
Expand All @@ -17,7 +16,6 @@ import (

func init() {
resource.RegisterAPI(API, resource.APIRegistration[Base]{
Status: resource.StatusFunc(CreateStatus),
RPCServiceServerConstructor: NewRPCServiceServer,
RPCServiceHandler: pb.RegisterBaseServiceHandlerFromEndpoint,
RPCServiceDesc: &pb.BaseService_ServiceDesc,
Expand Down Expand Up @@ -144,12 +142,3 @@ func FromRobot(r robot.Robot, name string) (Base, error) {
func NamesFromRobot(r robot.Robot) []string {
return robot.NamesByAPI(r, API)
}

// CreateStatus creates a status from the base.
func CreateStatus(ctx context.Context, b Base) (*commonpb.ActuatorStatus, error) {
isMoving, err := b.IsMoving(ctx)
if err != nil {
return nil, err
}
return &commonpb.ActuatorStatus{IsMoving: isMoving}, nil
}
64 changes: 0 additions & 64 deletions components/base/base_test.go
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,9 @@
package base_test

import (
"context"
"testing"

"github.com/go-viper/mapstructure/v2"
commonpb "go.viam.com/api/common/v1"
"go.viam.com/test"
"go.viam.com/utils/protoutils"

"go.viam.com/rdk/components/base"
"go.viam.com/rdk/components/generic"
Expand All @@ -21,66 +17,6 @@ const (
failBaseName = "base2"
)

func TestStatusValid(t *testing.T) {
status := &commonpb.ActuatorStatus{
IsMoving: true,
}
newStruct, err := protoutils.StructToStructPb(status)
test.That(t, err, test.ShouldBeNil)
test.That(
t,
newStruct.AsMap(),
test.ShouldResemble,
map[string]interface{}{
"is_moving": true,
},
)

convMap := &commonpb.ActuatorStatus{}
decoder, err := mapstructure.NewDecoder(&mapstructure.DecoderConfig{TagName: "json", Result: &convMap})
test.That(t, err, test.ShouldBeNil)
err = decoder.Decode(newStruct.AsMap())
test.That(t, err, test.ShouldBeNil)
test.That(t, convMap, test.ShouldResemble, status)
}

func TestCreateStatus(t *testing.T) {
t.Run("is moving", func(t *testing.T) {
status := &commonpb.ActuatorStatus{
IsMoving: true,
}

injectBase := &inject.Base{}
injectBase.IsMovingFunc = func(context.Context) (bool, error) {
return true, nil
}
status1, err := base.CreateStatus(context.Background(), injectBase)
test.That(t, err, test.ShouldBeNil)
test.That(t, status1, test.ShouldResemble, status)

resourceAPI, ok, err := resource.LookupAPIRegistration[base.Base](base.API)
test.That(t, err, test.ShouldBeNil)
test.That(t, ok, test.ShouldBeTrue)
status2, err := resourceAPI.Status(context.Background(), injectBase)
test.That(t, err, test.ShouldBeNil)
test.That(t, status2, test.ShouldResemble, status)
})

t.Run("is not moving", func(t *testing.T) {
status := &commonpb.ActuatorStatus{
IsMoving: false,
}

injectBase := &inject.Base{}
injectBase.IsMovingFunc = func(context.Context) (bool, error) {
return false, nil
}
status1, err := base.CreateStatus(context.Background(), injectBase)
test.That(t, err, test.ShouldBeNil)
test.That(t, status1, test.ShouldResemble, status)
})
}

func TestFromRobot(t *testing.T) {
r := &inject.Robot{}
rs := map[resource.Name]resource.Resource{
Expand Down
1 change: 0 additions & 1 deletion components/board/board.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@ import (

func init() {
resource.RegisterAPI(API, resource.APIRegistration[Board]{
Status: resource.StatusFunc(CreateStatus),
RPCServiceServerConstructor: NewRPCServiceServer,
RPCServiceHandler: pb.RegisterBoardServiceHandlerFromEndpoint,
RPCServiceDesc: &pb.BoardService_ServiceDesc,
Expand Down
Loading

0 comments on commit 3f1a3b6

Please sign in to comment.