Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RSDK-2821 Update Frontend SLAM 2D View to XY Plane #2269

Merged
merged 15 commits into from
Apr 26, 2023
Merged

RSDK-2821 Update Frontend SLAM 2D View to XY Plane #2269

Show file tree
Hide file tree
Changes from 11 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
799df92
comments for where to update
jeremyrhyde Apr 24, 2023
3744116
comments for where to update
jeremyrhyde Apr 24, 2023
f324a15
update 2d view
jeremyrhyde Apr 24, 2023
e301f0e
remove comments
jeremyrhyde Apr 24, 2023
e082468
push fake slam update
jeremyrhyde Apr 25, 2023
743efeb
add artifacts
jeremyrhyde Apr 25, 2023
0578484
fix dataset
jeremyrhyde Apr 25, 2023
d063dfe
udpate marker
jeremyrhyde Apr 25, 2023
245052c
Merge branch 'main' of github.com:viamrobotics/rdk into RSDK_2823_Upd…
jeremyrhyde Apr 25, 2023
e90d9e0
update kinematic tests
jeremyrhyde Apr 25, 2023
14eac46
fix fake base test
jeremyrhyde Apr 25, 2023
03e85d6
fix kinematic tests
jeremyrhyde Apr 25, 2023
9b1a76b
update artifacts
jeremyrhyde Apr 26, 2023
0ad97cc
update int tests
jeremyrhyde Apr 26, 2023
d884197
lint
jeremyrhyde Apr 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
224 changes: 224 additions & 0 deletions .artifact/tree.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50843,6 +50843,230 @@
}
},
"position_new": {}
},
"viam-office-02-22-2": {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[note] I've left the old artifacts in for the present. I am planning to remove them after we've stabilized the frontend view but can delete them now if desired. Comment here if you'd like me to remove them now.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm trusting that this dataset will be regenerated from the final version of viam-modules/viam-cartographer#59 once that viam-cartographer PR is is merged, before this PR is merged.

"internal_state": {
"internal_state_0.pbstream": {
"hash": "c45a129e3bb88449c17591aa28877aed",
"size": 15854
},
"internal_state_1.pbstream": {
"hash": "64f40901261a4a368fa5fe1cb197fe1c",
"size": 2812144
},
"internal_state_10.pbstream": {
"hash": "3f668318747a004799caead607bf6065",
"size": 22207344
},
"internal_state_11.pbstream": {
"hash": "cc713308a2766bbe0feaa6efb210d2e0",
"size": 23572480
},
"internal_state_12.pbstream": {
"hash": "a153506f7cd833c341e5b015300a418c",
"size": 23381025
},
"internal_state_13.pbstream": {
"hash": "75706da112fc4e23dd17f7b6c326e12e",
"size": 23414263
},
"internal_state_14.pbstream": {
"hash": "7a6e2ef411a3642a930cc7d9c56839bb",
"size": 26705504
},
"internal_state_15.pbstream": {
"hash": "a1e45c8ead61b48a5a208dccf30e5d5f",
"size": 28853515
},
"internal_state_16.pbstream": {
"hash": "b2d8d427a1bbd9f6af6a5211037bb6a3",
"size": 30836932
},
"internal_state_17.pbstream": {
"hash": "ee36674c26df9fe6a48db7d1ee24963e",
"size": 32057093
},
"internal_state_2.pbstream": {
"hash": "45a4af36429216d4c51eee1a123975a4",
"size": 5435611
},
"internal_state_3.pbstream": {
"hash": "99091c95873b82a90db4c16d94e572ba",
"size": 7424545
},
"internal_state_4.pbstream": {
"hash": "8e568f2bd65bc7cd2360491d5e7eddaf",
"size": 7460762
},
"internal_state_5.pbstream": {
"hash": "0ce26ce2f76514823bb73978d7048acd",
"size": 10297244
},
"internal_state_6.pbstream": {
"hash": "70e7810bbd899768fc64bc791ef033c1",
"size": 13147532
},
"internal_state_7.pbstream": {
"hash": "4aecfc067bb81c7e916b62774c6405aa",
"size": 14800927
},
"internal_state_8.pbstream": {
"hash": "0dd2e6d75ffef228a419015097ce84ad",
"size": 16954328
},
"internal_state_9.pbstream": {
"hash": "b094d749b031af7a91f01d6e4e2ae6e6",
"size": 19560436
}
},
"pointcloud": {
"pointcloud_0.pcd": {
"hash": "b5f6cd31b36597a37dcf8b8ffdbbaee6",
"size": 170746
},
"pointcloud_1.pcd": {
"hash": "b971600b0db1f88f378968b989509f06",
"size": 4524796
},
"pointcloud_10.pcd": {
"hash": "dbe6d8c90bc36c329acb9519e5cca2cd",
"size": 18793518
},
"pointcloud_11.pcd": {
"hash": "ba35c9781182f37d474f753ff6444ef2",
"size": 19325550
},
"pointcloud_12.pcd": {
"hash": "2774e355263ddc4237648c0225fc9322",
"size": 19972750
},
"pointcloud_13.pcd": {
"hash": "0dfdcc6206fef6bb6ce133746a871cd7",
"size": 20164142
},
"pointcloud_14.pcd": {
"hash": "8748ed6057e4958cc8bed7c77e4e2175",
"size": 19000622
},
"pointcloud_15.pcd": {
"hash": "596031d97ad94ed050a402591ffc25f0",
"size": 18800654
},
"pointcloud_16.pcd": {
"hash": "c2c4d2c5a264a1b158f7c153a8f9283a",
"size": 18336750
},
"pointcloud_17.pcd": {
"hash": "c80a7d6cfcc0f1af6813d286ed5350fe",
"size": 18318142
},
"pointcloud_2.pcd": {
"hash": "47d4212ab6f454cc54bd883a04fecbc6",
"size": 7536428
},
"pointcloud_3.pcd": {
"hash": "67701eea7cb97b9d823dea5ef3cb274e",
"size": 10192316
},
"pointcloud_4.pcd": {
"hash": "4d951f6352b77ed51f1bc2f325d0d5c6",
"size": 10213964
},
"pointcloud_5.pcd": {
"hash": "13951269e3033145d7a2df14bdf18354",
"size": 12536268
},
"pointcloud_6.pcd": {
"hash": "c478df84010df9e810597d3558c41853",
"size": 13721468
},
"pointcloud_7.pcd": {
"hash": "a68fef728011b459255f7e9780e28af1",
"size": 14798556
},
"pointcloud_8.pcd": {
"hash": "fcaf0e840a3ef86cc5e69a6acd2d7ddb",
"size": 17032190
},
"pointcloud_9.pcd": {
"hash": "e6b57aa563114da93c34c653ecbf8903",
"size": 18981454
}
},
"position": {
"position_0.json": {
"hash": "5190ef17a7875549f52d38fd2176035e",
"size": 258
},
"position_1.json": {
"hash": "8858a23f8cf0e0b2d9642b9c8d5abc92",
"size": 250
},
"position_10.json": {
"hash": "fc8ff978c7a616059e7968c06e2460cb",
"size": 250
},
"position_11.json": {
"hash": "4e3a82bc5a268b5e3f0e66104094ed17",
"size": 248
},
"position_12.json": {
"hash": "0171f3ff42af89e0836a7e2b70379e95",
"size": 247
},
"position_13.json": {
"hash": "c18f2abec5b4d8960beb76548398fcce",
"size": 259
},
"position_14.json": {
"hash": "8131c2a087ff7d88ba6b1aaca50ad410",
"size": 250
},
"position_15.json": {
"hash": "f3c73d230cf51f633821f9b87474ba2e",
"size": 252
},
"position_16.json": {
"hash": "a8fe3783605e8ad38851d0f4b4049b94",
"size": 251
},
"position_17.json": {
"hash": "429c2b80ecc9fe1ca9eed8208ae5a79f",
"size": 250
},
"position_2.json": {
"hash": "0354832bab2abc0fe8e7b45ef168fec9",
"size": 250
},
"position_3.json": {
"hash": "e7639ced49f8ec800cc2de2c81bab6fd",
"size": 250
},
"position_4.json": {
"hash": "110764b72240827cfb17e3512dc3f589",
"size": 259
},
"position_5.json": {
"hash": "12c957ab618dcabb1fe010b06026a70f",
"size": 251
},
"position_6.json": {
"hash": "8614db34685de0036be1e86bd0dd1ca3",
"size": 248
},
"position_7.json": {
"hash": "300aec716968d85c27546c29961979e6",
"size": 248
},
"position_8.json": {
"hash": "8d608f95d2fb39838daf05535b4b5a9c",
"size": 250
},
"position_9.json": {
"hash": "0fb8612ef11681660ebec33ceed96d79",
"size": 252
}
}
}
},
"locating_in_map.lua": {
Expand Down
2 changes: 1 addition & 1 deletion components/base/fake/fake.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ func (b *Base) WrapWithKinematics(ctx context.Context, slamSvc slam.Service) (ba
if err != nil {
return nil, err
}
limits := []referenceframe.Limit{{Min: dims.MinX, Max: dims.MaxX}, {Min: dims.MinZ, Max: dims.MaxZ}}
limits := []referenceframe.Limit{{Min: dims.MinX, Max: dims.MaxX}, {Min: dims.MinY, Max: dims.MaxY}}
model, err := wheeled.MakeModelFrame(b.Name().ShortName(), geometry, limits)
if err != nil {
return nil, errors.Wrap(err, "fake base cannot be created")
Expand Down
8 changes: 4 additions & 4 deletions components/base/wheeled/kinematics_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,10 +65,10 @@ func TestWrapWithKinematics(t *testing.T) {
kwb, ok := wb.(*kinematicWheeledBase)
test.That(t, ok, test.ShouldBeTrue)
limits := kwb.model.DoF()
test.That(t, limits[0].Min, test.ShouldBeLessThan, 0)
test.That(t, limits[1].Min, test.ShouldBeLessThan, 0)
test.That(t, limits[1].Max, test.ShouldBeGreaterThan, 0)
test.That(t, limits[1].Max, test.ShouldBeGreaterThan, 0)
test.That(t, limits[0].Min, test.ShouldBeLessThanOrEqualTo, 0)
test.That(t, limits[1].Min, test.ShouldBeLessThanOrEqualTo, 0)
test.That(t, limits[0].Max, test.ShouldBeGreaterThanOrEqualTo, 0)
test.That(t, limits[1].Max, test.ShouldBeGreaterThanOrEqualTo, 0)
geometry, err := kwb.model.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits)))
test.That(t, err, test.ShouldBeNil)
test.That(t, geometry.GeometryByName(testCfg.Name+":"+label).AlmostEqual(expectedSphere), test.ShouldBeTrue)
Expand Down
2 changes: 1 addition & 1 deletion services/slam/fake/data_loader.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ type position struct {
}

const (
maxDataCount = 16
maxDataCount = 18
internalStateTemplate = "%s/internal_state/internal_state_%d.pbstream"
pcdTemplate = "%s/pointcloud/pointcloud_%d.pcd"
positionTemplate = "%s/position/position_%d.json"
Expand Down
2 changes: 1 addition & 1 deletion services/slam/fake/slam.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ import (

var model = resource.DefaultModelFamily.WithModel("fake")

const datasetDirectory = "slam/example_cartographer_outputs/viam-office-02-22-1"
const datasetDirectory = "slam/example_cartographer_outputs/viam-office-02-22-2"

func init() {
resource.RegisterService(
Expand Down
4 changes: 2 additions & 2 deletions services/slam/fake/slam_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ func TestFakeSLAMGetPosition(t *testing.T) {
// in floating point values between M1 mac & arm64 linux which
// were causing tests to pass on M1 mac but fail on ci.
expectedPose := spatialmath.NewPose(
r3.Vector{X: -0.005666600181385561, Y: -6.933830159344678e-10, Z: -0.013030459250151614},
&spatialmath.Quaternion{Real: 0.9999999087728241, Imag: 0, Jmag: 0.0005374749356603168, Kmag: 0})
r3.Vector{X: -0.005839621552532072747133, Y: 0.012640521020122929413132, Z: 0.000000000000000000},
&spatialmath.Quaternion{Real: 0.9999998930633826, Imag: 0, Jmag: 0, Kmag: -0.000462464294427742})
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this definitely the actual orientation you want? It is sooooo close to (1,0,0,0) in which case you could just use NewPoseFromPoint and leave off the quaternion entirely. My guess is that PoseAlmostEqual will still be true

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd prefer to keep the explicit definitions because the test is loading the first pointcloud and in future datasets this first pointcloud may include some small degree of rotation and this will be easier to update with new datasets if we leave these written out in longhand.

@nicksanford @EshaMaharishi I'd like to hear your thoughts on this.
Peter is correct we can use the default orientation here. Although, we can't use the full: spatialmath.NewZeroPose() as the PoseAlmostEqual function still fails when comparing the r3.vector from the first fake position dataset defined above with (0,0,0).

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

NewPoseFromPoint is a convenience function that will accept your r3.Vector and fill in the zero orientation automatically

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO this test should break if we change the test dataset. I think it should reflect the dataset's actual value, i.e. what you already have @jeremyrhyde.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To be clear; NewPoseFromPoint doesn't ignore orientation, it simply auto-fills (1,0,0,0), which is functionally the same as what's currently used. So this would still break if the tests dataset does.

Totally optional change though.

test.That(t, spatialmath.PoseAlmostEqual(p, expectedPose), test.ShouldBeTrue)

p2, componentReference, err := slamSvc.GetPosition(context.Background())
Expand Down
18 changes: 9 additions & 9 deletions web/frontend/src/components/slam-2d-render.vue
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ raycaster.on('click', (event) => {

const markerSize = 0.5;
const marker = new THREE.Mesh(
new THREE.PlaneGeometry(markerSize, markerSize).rotateX(-Math.PI / 2),
Copy link
Contributor Author

@jeremyrhyde jeremyrhyde Apr 25, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[note] default plane is XY so a rotation here is no longer necessary

new THREE.PlaneGeometry(markerSize, markerSize),
new THREE.MeshBasicMaterial({ color: 'red' })
);
marker.name = 'Marker';
Expand Down Expand Up @@ -124,9 +124,9 @@ const updateCloud = (pointcloud: Uint8Array) => {
const points = loader.parse(pointcloud.buffer);
points.geometry.computeBoundingSphere();

const { radius = 1, center = { x: 0, z: 0 } } = points.geometry.boundingSphere ?? {};
camera.position.set(center.x, 100, center.z);
camera.lookAt(center.x, 0, center.z);
const { radius = 1, center = { x: 0, y: 0 } } = points.geometry.boundingSphere ?? {};
camera.position.set(center.x, center.y, 100);
camera.lookAt(center.x, center.y, 0);

const aspect = canvas.clientHeight / canvas.clientWidth;
camera.zoom = aspect > 1
Expand All @@ -135,16 +135,16 @@ const updateCloud = (pointcloud: Uint8Array) => {

camera.updateProjectionMatrix();

controls.target.set(center.x, 0, center.z);
controls.target.set(center.x, center.y, 0);
controls.maxZoom = radius * 2;

const intersectionPlane = new THREE.Mesh(
new THREE.PlaneGeometry(radius * 2, radius * 2, 1, 1).rotateX(-Math.PI / 2),
new MeshDiscardMaterial()
);
intersectionPlane.name = 'Intersection Plane';
intersectionPlane.position.y = -1;
intersectionPlane.position.set(center.x, 0, center.z);
intersectionPlane.position.z = -1;
intersectionPlane.position.set(center.x, center.y, 0);
raycaster.objects = [intersectionPlane];

const colors = points.geometry.attributes.color;
Expand All @@ -168,9 +168,9 @@ const updateCloud = (pointcloud: Uint8Array) => {

const updatePose = (newPose: commonApi.Pose) => {
const x = newPose.getX();
const z = newPose.getZ();
const y = newPose.getY();
marker.position.setX(x);
marker.position.setZ(z);
marker.position.setY(y);
};

onMounted(() => {
Expand Down