diff --git a/components/camera/fake/camera.go b/components/camera/fake/camera.go index a888dcfd994..4e4b7370f7c 100644 --- a/components/camera/fake/camera.go +++ b/components/camera/fake/camera.go @@ -28,7 +28,6 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/rimage" "go.viam.com/rdk/rimage/transform" - "go.viam.com/rdk/spatialmath" ) var ( @@ -191,12 +190,6 @@ type Camera struct { logger logging.Logger } -// Geometries returns Geometries. -func (c *Camera) Geometries(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error) { - box, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{X: 40, Y: 40, Z: 10}, "box") - return []spatialmath.Geometry{box}, err -} - // Read always returns the same image of a yellow to blue gradient. func (c *Camera) Read(ctx context.Context) (image.Image, func(), error) { if c.cacheImage != nil { diff --git a/referenceframe/errors.go b/referenceframe/errors.go index ef2d88e95af..0a7136e1870 100644 --- a/referenceframe/errors.go +++ b/referenceframe/errors.go @@ -22,6 +22,9 @@ var ErrNilPose = errors.New("pose was nil") // ErrMarshalingHighDOFFrame describes the error when attempting to marshal a frame with multiple degrees of freedom. var ErrMarshalingHighDOFFrame = errors.New("cannot marshal frame with >1 DOF, use a Model instead") +// ErrNoWorldConnection describes the error when a frame system is built but nothing is connected to the world node. +var ErrNoWorldConnection = errors.New("there are no robot parts that connect to a 'world' node. Root node must be named 'world'") + // NewParentFrameMissingError returns an error for when a part has named a parent whose part is missing from the collection of Parts // that are becoming a FrameSystem object. func NewParentFrameMissingError(partName, parentName string) error { diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 79c94a9c8fa..7b5d4512d4d 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -117,6 +117,19 @@ func NewFrameSystem(name string, parts []*FrameSystemPart, additionalTransforms allParts = append(allParts, transformPart) } + if len(allParts) != 0 { + hasWorld := false + for _, part := range allParts { + if part.FrameConfig.Parent() == World { + hasWorld = true + break + } + } + if !hasWorld { + return nil, ErrNoWorldConnection + } + } + // Topologically sort parts. After sorting, unlinked parts may reference frames // that will only exist after model flattening (e.g., "arm1:joint1"). Those will // be processed in a second pass after flattening. diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go index 6902ab42c32..d92de7280fa 100644 --- a/referenceframe/transformable.go +++ b/referenceframe/transformable.go @@ -95,7 +95,7 @@ func (pF *PoseInFrame) TransformOpt(tf *PoseInFrame) { // String returns the string representation of the PoseInFrame. func (pF *PoseInFrame) String() string { - return fmt.Sprintf("name: %v parent: %v, pose: %v", pF.name, pF.parent, pF.pose) + return fmt.Sprintf("parent: %s, pose: %v", pF.parent, pF.pose) } // MarshalJSON converts a PoseInFrame to JSON through its protobuf representation. @@ -133,14 +133,6 @@ func NewLinkInFrame(frame string, pose spatialmath.Pose, name string, geometry s } } -// SetGeometry replaces the existing geometry with the input. This only exists to deal with the -// clunkiness of the `LinkConfig` type that only speaks `GeometryConfig`s while we also allow for -// resources to simply declare their `[]Geometry` objects. Which is a different kind of impedance at -// the moment. -func (lF *LinkInFrame) SetGeometry(geom spatialmath.Geometry) { - lF.geometry = geom -} - // Geometry returns the Geometry of the LinkInFrame. func (lF *LinkInFrame) Geometry() spatialmath.Geometry { return lF.geometry diff --git a/robot/framesystem/framesystem_test.go b/robot/framesystem/framesystem_test.go index 2862d77fe2d..cc5c6040363 100644 --- a/robot/framesystem/framesystem_test.go +++ b/robot/framesystem/framesystem_test.go @@ -6,6 +6,7 @@ import ( "testing" "github.com/golang/geo/r3" + "github.com/pkg/errors" "go.viam.com/test" _ "go.viam.com/rdk/components/arm/fake" @@ -212,6 +213,33 @@ func TestNewFrameSystemFromBadConfig(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) + testCases := []struct { + name string + num string + err error + }{ + {"no world node", "2", referenceframe.ErrNoWorldConnection}, + {"frame named world", "3", errors.Errorf("cannot give frame system part the name %s", referenceframe.World)}, + {"parent field empty", "4", errors.New("parent field in frame config for part \"cameraOver\" is empty")}, + } + + for _, tc := range testCases { + t.Run(tc.name, func(t *testing.T) { + cfg, err := config.Read(ctx, rdkutils.ResolveFile("robot/impl/data/fake_wrongconfig"+tc.num+".json"), logger, nil) + test.That(t, err, test.ShouldBeNil) + r, err := robotimpl.New(ctx, cfg, nil, logger) + test.That(t, err, test.ShouldBeNil) + defer r.Close(ctx) + fsCfg, err := r.FrameSystemConfig(ctx) + if err != nil { + test.That(t, err, test.ShouldBeError, tc.err) + return + } + _, err = referenceframe.NewFrameSystem(tc.num, fsCfg.Parts, nil) + test.That(t, err, test.ShouldBeError, tc.err) + }) + } + cfg, err := config.Read(ctx, rdkutils.ResolveFile("robot/impl/data/fake.json"), logger, nil) test.That(t, err, test.ShouldBeNil) r, err := robotimpl.New(ctx, cfg, nil, logger) diff --git a/robot/impl/data/fake_wrongconfig1.json b/robot/impl/data/fake_wrongconfig1.json new file mode 100644 index 00000000000..00a8babbcf3 --- /dev/null +++ b/robot/impl/data/fake_wrongconfig1.json @@ -0,0 +1,71 @@ +{ + "components": [ + { + "name": "pieceGripper", + "type": "gripper", + "model": "fake", + "frame": { + "parent": "pieceArm" + } + }, + { + "name": "cameraOver", + "type": "camera", + "model": "file", + "attributes": { + "color": "artifact_data/vision/chess/board3.png", + "depth": "artifact_data/vision/chess/board3.dat.gz", + "aligned": true + }, + "frame": { + "parent": "world", + "translation": { + "x": 2000, + "y": 500, + "z": 1300 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 180 + } + } + } + }, + { + "name": "pieceArm", + "type": "arm", + "model": "fake", + "attributes": { + "model-path": "../../components/arm/fake/kinematics/fake.json" + }, + "frame": { + "parent": "world", + "translation": { + "x": 500, + "y": 500, + "z": 1000 + } + } + }, + { + "name": "movement_sensor1", + "type": "movement_sensor", + "model": "fake" + }, + { + "name": "movement_sensor2", + "type": "movement_sensor", + "model": "fake", + "frame": { + "parent": "gripperPiece" + }, + "attributes": { + "relative": true + } + } + ] +} diff --git a/robot/impl/data/fake_wrongconfig2.json b/robot/impl/data/fake_wrongconfig2.json new file mode 100644 index 00000000000..21111a3fc22 --- /dev/null +++ b/robot/impl/data/fake_wrongconfig2.json @@ -0,0 +1,71 @@ +{ + "components": [ + { + "name": "pieceGripper", + "type": "gripper", + "model": "fake", + "frame": { + "parent": "pieceArm" + } + }, + { + "name": "cameraOver", + "type": "camera", + "model": "file", + "attributes": { + "color": "artifact_data/vision/chess/board3.png", + "depth": "artifact_data/vision/chess/board3.dat.gz", + "aligned": true + }, + "frame": { + "parent": "pieceArm", + "translation": { + "x": 2000, + "y": 500, + "z": 1300 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 180 + } + } + } + }, + { + "name": "pieceArm", + "type": "arm", + "model": "fake", + "attributes": { + "model-path": "../../components/arm/fake/kinematics/fake.json" + }, + "frame": { + "parent": "base", + "translation": { + "x": 500, + "y": 500, + "z": 1000 + } + } + }, + { + "name": "movement_sensor1", + "type": "movement_sensor", + "model": "fake" + }, + { + "name": "movement_sensor2", + "type": "movement_sensor", + "model": "fake", + "frame": { + "parent": "pieceArm" + }, + "attributes": { + "relative": true + } + } + ] +} diff --git a/robot/impl/data/fake_wrongconfig3.json b/robot/impl/data/fake_wrongconfig3.json new file mode 100644 index 00000000000..43b098e3978 --- /dev/null +++ b/robot/impl/data/fake_wrongconfig3.json @@ -0,0 +1,71 @@ +{ + "components": [ + { + "name": "pieceGripper", + "type": "gripper", + "model": "fake", + "frame": { + "parent": "pieceArm" + } + }, + { + "name": "cameraOver", + "type": "camera", + "model": "file", + "attributes": { + "color": "artifact_data/vision/chess/board3.png", + "depth": "artifact_data/vision/chess/board3.dat.gz", + "aligned": true + }, + "frame": { + "parent": "world", + "translation": { + "x": 2000, + "y": 500, + "z": 1300 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 180 + } + } + } + }, + { + "name": "pieceArm", + "type": "arm", + "model": "fake", + "attributes": { + "model-path": "../../components/arm/fake/kinematics/fake.json" + }, + "frame": { + "parent": "world", + "translation": { + "x": 500, + "y": 500, + "z": 1000 + } + } + }, + { + "name": "movement_sensor1", + "type": "movement_sensor", + "model": "fake" + }, + { + "model": "fake", + "name": "world", + "type": "movement_sensor", + "frame": { + "parent": "pieceGripper" + }, + "attributes": { + "relative": true + } + } + ] +} diff --git a/robot/impl/data/fake_wrongconfig4.json b/robot/impl/data/fake_wrongconfig4.json new file mode 100644 index 00000000000..71af516b1d9 --- /dev/null +++ b/robot/impl/data/fake_wrongconfig4.json @@ -0,0 +1,71 @@ +{ + "components": [ + { + "name": "pieceGripper", + "type": "gripper", + "model": "fake", + "frame": { + "parent": "pieceArm" + } + }, + { + "name": "cameraOver", + "type": "camera", + "model": "file", + "attributes": { + "color": "artifact_data/vision/chess/board3.png", + "depth": "artifact_data/vision/chess/board3.dat.gz", + "aligned": true + }, + "frame": { + "parent": "", + "translation": { + "x": 2000, + "y": 500, + "z": 1300 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "x": 0, + "y": 0, + "z": 1, + "th": 180 + } + } + } + }, + { + "name": "pieceArm", + "type": "arm", + "model": "fake", + "attributes": { + "model-path": "../../components/arm/fake/kinematics/fake.json" + }, + "frame": { + "parent": "world", + "translation": { + "x": 500, + "y": 500, + "z": 1000 + } + } + }, + { + "name": "movement_sensor1", + "type": "movement_sensor", + "model": "fake" + }, + { + "name": "movement_sensor2", + "type": "movement_sensor", + "model": "fake", + "frame": { + "parent": "pieceArm" + }, + "attributes": { + "relative": true + } + } + ] +} diff --git a/robot/impl/local_robot.go b/robot/impl/local_robot.go index 3b77213abb9..3a78c7d71ad 100644 --- a/robot/impl/local_robot.go +++ b/robot/impl/local_robot.go @@ -1177,148 +1177,55 @@ func (r *localRobot) getLocalFrameSystemParts(ctx context.Context) ([]*reference cfg := r.Config() parts := make([]*referenceframe.FrameSystemPart, 0) - // For each part we will see if there's a corollary frame configuration. For those that have - // one, we'll craft a `FrameSystemPart` containing that information. Furthermore, the - // FrameSystemPart may include geometry or model/kinematic information. Kinematics are always - // fetched from `InputEnabled` resources. Geometries can be specified in the robot config. If - // none exists, we will perform a `Geometries` query on the resource. - for _, resConfig := range cfg.Components { - if resConfig.Frame == nil { // no Frame means dont include in frame system. + for _, component := range cfg.Components { + if component.Frame == nil { // no Frame means dont include in frame system. continue } - logger := r.logger.Sublogger("framesystem").WithFields("ResName", resConfig.Name) - // Dan: Consider not doing frame validation at all in the robot impl code. Should probably - // live entirely in the `FrameSystemService.Reconfigure` method. The consequence of the code - // as it stands is that the frame system service does not know the difference of a frame - // that doesn't exist versus a frame that's misconfigured. Hence, when a motion request (or - // other thing that consumes a FrameSystem) comes in identifying a part/frame that the frame - // system service was not informed of, we cannot give back an error message better than - // " doesn't exist". A user must sift through robot configuration logs to know why a - // frame might be missing. - frameName := resConfig.Frame.ID - if frameName == "" { - frameName = resConfig.Name - } - if frameName == referenceframe.World { - logger.Warnw("Refusing to create frame named `world` for resource.", - "FrameID", resConfig.Frame.ID) - continue + if component.Name == referenceframe.World { + return nil, errors.Errorf("cannot give frame system part the name %s", referenceframe.World) } - - if resConfig.Frame.Parent == "" { - logger.Warn("Frame config for resource is missing a parent.") - continue + if component.Frame.Parent == "" { + return nil, errors.Errorf("parent field in frame config for part %q is empty", component.Name) + } + cfgCopy := &referenceframe.LinkConfig{ + ID: component.Frame.ID, + Translation: component.Frame.Translation, + Orientation: component.Frame.Orientation, + Geometry: component.Frame.Geometry, + Parent: component.Frame.Parent, + } + if cfgCopy.ID == "" { + cfgCopy.ID = component.Name } - res, resErr := r.ResourceByName(resConfig.ResourceName()) - isAvailable := resErr == nil - resType := resConfig.ResourceName().API.SubtypeName - if resType == arm.SubtypeName || resType == gantry.SubtypeName || resType == gripper.SubtypeName { - // Components that have multiple degrees of freedom are required to be available and - // implement the `Kinematics` method to be used in the frame system. - if !isAvailable { - logger.Warnw("InputEnabled component is not available. Omitting from FrameSystem.", "err", resErr) - continue - } - - ie, ok := res.(framesystem.InputEnabled) - if !ok { - logger.Warnw("Resource type expected to have kinematics, but resource was not InputEnabled.", - "APISubtype", resType, "ResObjectType", fmt.Sprintf("%T", res)) - continue - } - - model, err := ie.Kinematics(ctx) - if err != nil { - // Dan: I've introduced a change in behavior here. Before, unavailable/not found - // errors, as this code does, would not add an item to the FrameSystem. But errors - // from the `Kinematics` call, or a resource that does not implement the - // `InputEnabled` interface would be added to the frame system without a model. - // - // I've chosen to not include the latter to the frame system. It's unclear if that - // distinction was meaningful. - logger.Warnw("Error getting kinematics for resource.", "err", err) + var model referenceframe.Model + var err error + switch component.ResourceName().API.SubtypeName { + case arm.SubtypeName, gantry.SubtypeName, gripper.SubtypeName: // catch the case for all the ModelFramers + model, err = r.extractModelFrameJSON(ctx, component.ResourceName()) + if resource.IsNotAvailableError(err) || resource.IsNotFoundError(err) { + // When we have non-nil errors here, it is because the resource is not yet available. + // In this case, we will exclude it from the FS. When it becomes available, it will be included. continue } - if resConfig.Frame.Geometry != nil { - logger.Warn("An input enabled component erroneously included a geometry. Ignoring the geometry.") - } - - linkInFrame, err := (&referenceframe.LinkConfig{ - ID: frameName, - Translation: resConfig.Frame.Translation, - Orientation: resConfig.Frame.Orientation, - Parent: resConfig.Frame.Parent, - }).ParseConfig() if err != nil { - logger.Warnw("Failed to create LinkInFrame.", "err", err) - continue + // If there is an error getting kinematics unrelated to resource availability, log a + // warning. It probably impacts correct operation of the application. + r.logger.Warnw( + "Error getting kinematics. Resource is added to the frame system, but modeling may not work correctly.", + "res", component, "err", err) } - - parts = append(parts, &referenceframe.FrameSystemPart{FrameConfig: linkInFrame, ModelFrame: model}) - continue + default: } - - // Dan: Consider changing `LinkConfig.ParseConfig()` to `LinkInFrameFromConfig(LinkConfig)`. - linkInFrame, err := (&referenceframe.LinkConfig{ - ID: frameName, - Translation: resConfig.Frame.Translation, - Orientation: resConfig.Frame.Orientation, - Geometry: resConfig.Frame.Geometry, - Parent: resConfig.Frame.Parent, - }).ParseConfig() + lif, err := cfgCopy.ParseConfig() if err != nil { - logger.Warnw("Failed to create LinkInFrame.", "err", err) - continue - } - - // If the frame config included a geometry, prefer that to asking the resource. If the frame - // config does not include a geometry and the resource happens to be unavailable we won't be - // able to ask for a geometry, create a frame with what we have. - if linkInFrame.Geometry() != nil || !isAvailable { - parts = append(parts, &referenceframe.FrameSystemPart{FrameConfig: linkInFrame, ModelFrame: nil}) - continue - } - - // If the resource is available and the config didn't explicitly give a geometry, ask the - // resource if it has one. - shaper, isShaped := res.(resource.Shaped) - if isShaped { - resGeometries, err := shaper.Geometries(ctx, nil) - // Dan: I'm concerned that `Geometries` will return unimplemented errors? Leaving as - // Debug due to FUD. - if err != nil { - logger.Debugw("`Geometries` method returned error.", "err", err) - } else { - //nolint - switch len(resGeometries) { - case 0: - default: // > 1 - logger.Warnw( - "`Geometries` returned more than one geometry, but the LinkInFrame does not support that."+ - "Keeping the first one.", "Size", len(resGeometries)) - fallthrough - case 1: - geom := resGeometries[0] - // Dan: I feel it's appropriate to re-label the geometry here by concatenating - // the resource name with the geometry label. But the FrameSystem construction - // is going to copy and re-label the resulting geometry anyways. - linkInFrame.SetGeometry(geom) - } - } - } else { - // All resources should have a `Geometries` method. Naturally, they're allowed to leave - // it unimplemented. This log implies programmer error within the viam-server. For - // example, sensors do not seem to be `Shaped`. - logger.Debugw("Resource missing `Geometries` method.", - "ResType", resType, "ResObjectType", fmt.Sprintf("%T", res)) + return nil, err } - parts = append(parts, &referenceframe.FrameSystemPart{FrameConfig: linkInFrame, ModelFrame: nil}) + parts = append(parts, &referenceframe.FrameSystemPart{FrameConfig: lif, ModelFrame: model}) } - return parts, nil } @@ -1378,6 +1285,19 @@ func (r *localRobot) getRemoteFrameSystemParts(ctx context.Context) ([]*referenc return remoteParts, nil } +// extractModelFrameJSON finds the robot part with a given name, checks to see if it implements ModelFrame, and returns the +// JSON []byte if it does, or nil if it doesn't. +func (r *localRobot) extractModelFrameJSON(ctx context.Context, name resource.Name) (referenceframe.Model, error) { + part, err := r.ResourceByName(name) + if err != nil { + return nil, err + } + if k, ok := part.(framesystem.InputEnabled); ok { + return k.Kinematics(ctx) + } + return nil, referenceframe.ErrNoModelInformation +} + // GetPose returns the pose of the specified component in the given destination frame. func (r *localRobot) GetPose( ctx context.Context, @@ -2213,9 +2133,5 @@ func (r *localRobot) ListTunnels(_ context.Context) ([]config.TrafficTunnelEndpo // GetResource implements resource.Provider for a localRobot by looking up a resource by name. func (r *localRobot) GetResource(name resource.Name) (resource.Resource, error) { - if name == framesystem.PublicServiceName { - return r.ResourceByName(framesystem.InternalServiceName) - } - return r.ResourceByName(name) } diff --git a/robot/impl/robot_framesystem_test.go b/robot/impl/robot_framesystem_test.go index 516a428b9e4..380d57adf64 100644 --- a/robot/impl/robot_framesystem_test.go +++ b/robot/impl/robot_framesystem_test.go @@ -12,15 +12,12 @@ import ( "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/base" - "go.viam.com/rdk/components/camera" - fakecamera "go.viam.com/rdk/components/camera/fake" "go.viam.com/rdk/components/generic" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/config" "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/datamanager/builtin" "go.viam.com/rdk/spatialmath" rtestutils "go.viam.com/rdk/testutils" @@ -487,55 +484,93 @@ func TestObserveArmKinematicReconfiguration(t *testing.T) { test.That(t, postFSLabels, test.ShouldResemble, postArmLabels) } -func TestResourcesImplementingGeometriesInFrameSystem(t *testing.T) { +type staticObstacleGripper struct { + resource.Named + resource.TriviallyCloseable + resource.TriviallyReconfigurable +} + +func (g *staticObstacleGripper) Open(context.Context, map[string]interface{}) error { + return nil +} + +func (g *staticObstacleGripper) Grab(context.Context, map[string]interface{}) (bool, error) { + return false, nil +} + +func (g *staticObstacleGripper) IsHoldingSomething(context.Context, map[string]interface{}) (gripper.HoldingStatus, error) { + return gripper.HoldingStatus{}, nil +} + +func (g *staticObstacleGripper) Stop(context.Context, map[string]interface{}) error { + return nil +} + +func (g *staticObstacleGripper) IsMoving(context.Context) (bool, error) { + return false, nil +} + +func (g *staticObstacleGripper) Geometries(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error) { + return nil, nil +} + +func (g *staticObstacleGripper) Kinematics(context.Context) (referenceframe.Model, error) { + return referenceframe.NewSimpleModel(g.Name().ShortName()), nil +} + +func (g *staticObstacleGripper) CurrentInputs(context.Context) ([]referenceframe.Input, error) { + return nil, nil +} + +func (g *staticObstacleGripper) GoToInputs(context.Context, ...[]referenceframe.Input) error { + return nil +} + +func TestGripperAPIStaticObstacleInFrameSystem(t *testing.T) { + staticObstacleModel := resource.NewModel("test", "test", "static-obstacle-gripper") + resource.RegisterComponent(gripper.API, staticObstacleModel, resource.Registration[gripper.Gripper, resource.NoNativeConfig]{ + Constructor: func(_ context.Context, _ resource.Dependencies, conf resource.Config, _ logging.Logger) (gripper.Gripper, error) { + return &staticObstacleGripper{Named: conf.ResourceName().AsNamed()}, nil + }, + }) + defer resource.Deregister(gripper.API, staticObstacleModel) + ctx := context.Background() logger := logging.NewTestLogger(t) - // RSDK-14034: The fake camera implements a `Geometries` method. We want to assert that - // component's geometry shows up when constructing a frame system from the robot's frame system - // service. Such that it would be used as an obstacle for motion planning. cfg := config.Config{ Components: []resource.Config{ { - Name: "camera", - API: camera.API, - Model: fakecamera.Model, + Name: "obstacle", + API: gripper.API, + Model: staticObstacleModel, Frame: &referenceframe.LinkConfig{ - ID: "special-frame-name", + ID: "obstacle-frame", Parent: "world", + Geometry: &spatialmath.GeometryConfig{ + Type: "box", + X: 100, + Y: 200, + Z: 300, + Label: "obstacle-geom", + }, }, - ConvertedAttributes: &fakecamera.Config{}, }, }, } robot := setupLocalRobot(t, ctx, &cfg, logger.Sublogger("robot")) - fss, err := framesystem.FromProvider(robot) - test.That(t, err, test.ShouldBeNil) - fs, err := framesystem.NewFromService(ctx, fss, nil) + fsCfg, err := robot.FrameSystemConfig(ctx) test.That(t, err, test.ShouldBeNil) - frame := fs.Frame("special-frame-name_origin") - test.That(t, frame, test.ShouldNotBeNil) - - geomsInFrame, err := frame.Geometries([]referenceframe.Input{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, geomsInFrame.Geometries(), test.ShouldHaveLength, 1) - - camGeomFromFS := geomsInFrame.Geometries()[0] - test.That(t, camGeomFromFS.Label(), test.ShouldEqual, "special-frame-name_origin") - - cam, err := camera.FromProvider(robot, "camera") - test.That(t, err, test.ShouldBeNil) - geoms, err := cam.Geometries(ctx, nil) - test.That(t, err, test.ShouldBeNil) - test.That(t, geoms, test.ShouldHaveLength, 1) - - camGeomFromCam := geoms[0] - test.That(t, camGeomFromCam.Label(), test.ShouldEqual, "box") - // Assert geometry identify by using `ToPoints` with the same resolution. The camera is parented - // with no translation to the world frame. Hence we can expect the X/Y/Z points of the frame - // system geometry to match the camera's. - test.That(t, camGeomFromCam.ToPoints(1), test.ShouldResemble, camGeomFromFS.ToPoints(1)) + var obstaclePart *referenceframe.FrameSystemPart + for _, part := range fsCfg.Parts { + if part.FrameConfig != nil && part.FrameConfig.Name() == "obstacle-frame" { + obstaclePart = part + break + } + } + test.That(t, obstaclePart, test.ShouldNotBeNil) + test.That(t, obstaclePart.FrameConfig.Geometry(), test.ShouldNotBeNil) }