[RSDK-1644] Convert all elements of a frame system to with respect to the world #1741
Conversation
…ted and can be used by the referenceframe package
viambot
added
the
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
referenceframe/frame_system.go
Outdated
| return nil, err | ||
| } | ||
|
|
||
| geosInFrame, err := currentFrame.Geometries(inputs[name]) |
There was a problem hiding this comment.
We can't guarantee the contents of inputs and whether information for any particular frame is in there.
We should check whether name is in inputs, and if it's not, then let's pass make([]Input, len(currentFrame.DoF())
referenceframe/frame_system.go
Outdated
| aggregatePoints = append(aggregatePoints, g.ToPoints(1.)...) | ||
| } | ||
|
|
||
| parentFrame := system.Frame(parent.Name()) |
There was a problem hiding this comment.
I'm not clear on what this code with the Parent (L338-L347) is doing/why it's needed. Could you elaborate?
There was a problem hiding this comment.
the geometries within 'transformedGeo' have been transformed to be with respect to the world. however since their parent is not world they need to be translated to be located at their parent. I construct parentFrame so i can get the needed pose and then translate the geometries of transformedGeo accordingly.
There was a problem hiding this comment.
The parent's pose should be accounted for in the Transform call to world, no?
Also- it looks like what's happening here is we're taking the pose of just one geometry emitted by the parent frame, at the parent frame's zero position- regardless of how many geometries the parent emits, or what the actual pose of the Parent is.
Were you observing points being in the wrong place before implementing the parent functionality?
There was a problem hiding this comment.
Yeah the Transform call should be working the way Peter is suggesting (and if its not thats a problem), so most of this can probably go away
viambot
added
safe to test
viambot
added
safe to test
viambot
added
the
safe to test
viambot
added
safe to test
viambot
added
safe to test
referenceframe/frame_system.go
Outdated
| @@ -15,6 +18,9 @@ import ( | |||
| // World is the string "world", but made into an exported constant. | |||
| const World = "world" | |||
|
|
|||
| // defaultPointDensity is the default value for the spatialmath.ToPoints conversion method. | |||
There was a problem hiding this comment.
Comment on this should specify that this value will cause the default value defined in spatialmath to be used. The variable name should probably also do the same.
There was a problem hiding this comment.
ok, I updated the comment. I am unsure what to change the constant's name to. defaultSpatialMathPointDensity seems a bit lengthy to me.
referenceframe/frame_system.go
Outdated
| @@ -308,6 +314,74 @@ func (sfs *simpleFrameSystem) FrameSystemSubset(newRoot Frame) (FrameSystem, err | |||
| return newFS, nil | |||
| } | |||
|
|
|||
| // FrameSystemToPCD takes in a framesystem and returns a map where all elements are | |||
| // the point representation of their geometry type with respect to the world. | |||
| func FrameSystemToPCD(system FrameSystem, inputs map[string][]Input, logger *zap.SugaredLogger) (map[string][]r3.Vector, error) { | |||
There was a problem hiding this comment.
Eric maintains a fork of SugaredLogger for Viam's internal use that should be used here. Usage does not change, only typing.
import "github.com/edaniels/golog"
logger golog.Logger
referenceframe/frame_system.go
Outdated
| return nil, err | ||
| } | ||
| for name, geosInFrame := range geoMap { | ||
| geos := geosInFrame.geometries |
There was a problem hiding this comment.
[nit] Use Geometries() rather than geometries, this will ensure that geos is not nil, even if it is empty.
referenceframe/frame_system.go
Outdated
| currentInput = []Input{} | ||
| } | ||
| if currentInput == nil { | ||
| logger.Debugf("will not transform %v to be with respect to the world", name) |
There was a problem hiding this comment.
This should say why, e.g. "will not transform %v to be with respect to the world as it had no inputs provided"
referenceframe/frame_system.go
Outdated
| // the parent of the frame is handled by the Transform method. | ||
| transformed, err := system.Transform(inputs, geosInFrame, World) | ||
| if err != nil && strings.Contains(err.Error(), "no positions provided for frame with name") { | ||
| logger.Debugf("unable to handle the transform for %v", name) |
There was a problem hiding this comment.
The various debug statements below here are complex and also unneeded- it may not be the direct parent that had no inputs, rather the grandparent, or great-grandparent, etc.
Something that will capture the needed information and also will be much more succinct would be to append the text of err.Error() to this debug statement.
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
|
Note: merge base coverage results not available, comparing against closest aa1914d~2=(9e2f63d) instead
|
No description provided.