RSDK-4862 - Add odometer sensor process #307

kkufieta · 2023-12-11T15:15:49Z

Ticket: https://viam.atlassian.net/browse/RSDK-4862

Tested on:

Intel Mac w/ lidar
Cloud slam (live mode) results show an overall improvement in mapping quality based on this PR. They also show a similar map quality for both IMU and IMU + odometer mapping, though the latter leads to higher confidence sooner than just using an IMU. I could not test mapping using just an odometer because cloud slam won't start unless the data manager is capturing IMU data.

Cloud slam - live mode:
NOTE: Unbeknownst to me, the IMU was providing only zero values. The map quality in the results below is therefore not representative of what the actual performance likely looks like. But the maps do still serve as a proof that adding IMU and odometer data is successful.

	main branch - w/ imu	this PR - w/ imu	this PR - w/ imu & odometer
final result
1st position
2nd position
3rd position
4th position

kkufieta · 2023-12-19T17:09:00Z

sensorprocess/lidarsensorprocess_test.go

@@ -0,0 +1,303 @@
+package sensorprocess


I moved lidar related tests from sensorprocess_test.go out into this file to make them easier to read.

kkufieta · 2023-12-19T17:09:11Z

sensorprocess/movementsensorprocess_test.go

@@ -0,0 +1,344 @@
+package sensorprocess


I moved movement sensor related tests from sensorprocess_test.go out into this file to make them easier to read.

kkufieta · 2023-12-19T17:25:48Z

sensorprocess/lidarsensorprocess_test.go

+	"github.com/viamrobotics/viam-cartographer/sensors/inject"
+)
+
+func TestStartLidar(t *testing.T) {


TestStartLidar was copy/pasted from sensorprocess_test.go

kkufieta · 2023-12-19T17:27:53Z

sensorprocess/lidarsensorprocess_test.go

+	})
+}
+
+func TestAddLidarReadingInOnline(t *testing.T) {


TestAddLidarReadingInOnline was copy/pasted from sensorprocess_test.go, except for the helper functions that are called: I renamed them.

kkufieta · 2023-12-19T17:28:32Z

sensorprocess/lidarsensorprocess_test.go

+	}
+
+	t.Run("returns error when lidar GetData returns error, doesn't try to add lidar data", func(t *testing.T) {
+		invalidAddLidarReadingInOnlineTestHelper(context.Background(), t, cf, config, s.LidarWithErroringFunctions, 10)


invalidAddLidarReadingInOnlineTestHelper used to be invalidOnlineModeLidarTestHelper

kkufieta · 2023-12-19T19:58:46Z

sensorprocess/movementsensorprocess_test.go

+	})
+}
+
+func TestTryAddMovementSensorReadingOnce(t *testing.T) {


TestTryAddMovementSensorReadingOnce is copy/pasted from sensorprocess_test.go but was previously named TestTryAddIMUReading. It was altered and augmented.

kkufieta · 2023-12-19T19:59:06Z

sensorprocess/movementsensorprocess_test.go

+	})
+}
+
+func TestTryAddIMUReading(t *testing.T) {


TestTryAddIMUReading was added.

kkufieta · 2023-12-19T19:59:20Z

sensorprocess/movementsensorprocess_test.go

+	})
+}
+
+func TestTryAddOdometerReading(t *testing.T) {


TestTryAddOdometerReading was added.

kkufieta · 2023-12-19T20:11:18Z

sensors/movementsensor.go

@@ -107,36 +108,36 @@ func (ms *MovementSensor) TimedMovementSensorReading(ctx context.Context) (Timed

 	timeoutCtx, cancel := context.WithTimeout(ctx, timedMovementSensorReadingTimeout)
 	defer cancel()
-	if ms.imuSupported {
-	imuLoop:
+	if ms.odometerSupported {


Adding odometer first, since we want to prioritize it over the imu

kkufieta · 2023-12-19T20:12:20Z

sensors/movementsensor.go

@@ -24,7 +24,7 @@ const (
 )

 var (
-	defaultTime = time.Time{}
+	undefinedTime = time.Time{}


Renamed this since I kept getting confused with defaultTime, assuming it was set to a specific time, when really it's just used to denote that it was not set to any meaningful value yet.

jeremyrhyde

Made a first (if detailed) run through of the PR. Looks good in terms of logic and testing. Some nits/opinions/optimizations as well as several questions to be answered.

I also keep coming back to the array sort done in offline mode, I feel like there should be a better wy to do this but I haven't come up with a better solution yet

viam_cartographer.go

sensors/test_deps.go

jeremyrhyde · 2023-12-29T17:59:28Z

sensors/test_deps.go

+func getFinishedReplayOdometer() *inject.MovementSensor {
+	odometer := &inject.MovementSensor{}
+	odometer.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) {
+		return geo.NewPoint(0, 0), 0.0, replay.ErrEndOfDataset


I believe geo.NewPoint(0,0) be nil for an erroring function?

I prefer math.Nan for things that actually mean something physical.

So you would want geo.NewPoint(math.Nan, math.Nan)? I suggested nil because the return of Position is a geo.Point pointer

Note: I also just checked RDK and movement sensors that dont implement Position (such as imuwit) returns geo.NewPoint(0,0). This isn't my favorite pattern to support as if an error is returned I'd like the result to be inaccessible and not take up any memory (so nil)

^ No, not all; our preferred pattern with an error is math.Nan. We just haven't gotten around to changing them yet. So you're looking at a bad example. Also the ones that truly don't have out ErrUnimplemented returned, so you can't actually get info out of that client call at all.

The one that does implement Position and has an error associated with it is GPS, where if it is erroring, we return math.NaN.

Also the problem here is if you return it as nil, and someone else uses the geoPoint library that we're wrapping on that nil pointer - like position.MidPointTo you get panics all over the place. I'd rather give NaNs.

The difference here is - the witimu can never return position - it doesn't have that data or a chance at returning that data at all, so we use the movement sensor error unimplemented and a PositionReporting: false Property to make sure that doesn't show up in the sdks or the rc card. So you just get an error unimplemented if you try to get any of that information for business level logic out of the wit imu.

Changed the return values to be return geo.NewPoint(math.NaN(), math.NaN()), math.NaN(), replay.ErrEndOfDataset. Does that look good?

If yes, would this also mean we'd want to return structs populated with NaN values instead of empty structs? That is, instead of this:

return r3.Vector{}, replay.ErrEndOfDataset

we'd want to do this?

return r3.Vector{math.NaN, math.NaN, math.NaN}, replay.ErrEndOfDataset

^ Since those aren't returned pointers like the *geo.Point return from Position: I'm not as concerned about the r3 library accessing them after they're returned. So it is up to you in this pr.

Is cartographer seeing zero values and taking them as meaningful? That's a question for another day, maybe it has a safeguard against a constant stream of zero data, some algorithms do.

TBR (To be researched) about carto's handling of constant streams of zeros

sensorprocess/sensorprocess_test.go

jeremyrhyde · 2023-12-29T18:50:34Z

sensorprocess/testhelper.go

@@ -120,19 +123,55 @@ func onlineModeLidarTestHelper(
 	}
 }

-func onlineModeIMUTestHelper(
+func invalidAddLidarReadingInOnlineTestHelper(


[q] what makes this function invalid? The addLidarReadingFunc returns a nil err every time

This is just a test helper, which means that it is being called with "invalid" lidar sensors that will cause addLidarReadingInOnline to fail. addLidarReadingFunc is just there to count the number of times it is being called so we can test at the end that it has not been called at all.

Note: The function was previously named invalidOnlineModeLidarTestHelper

I'd prefer to remove the invalid if a good lidar COULD be passed into the function and result in valid information. That reduces the confusion for:

invalidAddLidarReadingInOnlineTestHelper(invalidLiDAR) -> invalid result

invalidAddLidarReadingInOnlineTestHelper(validLiDAR) -> valid result <-------

to

addLidarReadingInOnlineTestHelper(invalidLiDAR) -> invalid result

addLidarReadingInOnlineTestHelper(validLiDAR) -> valid result

It has to be "invalid" because it actually ensures that things fail. There's another, similar function, where we check that it's valid. Won't change anything now since refactoring this test is not part of the scope for this ticket, but will keep it in mind when I work on integration tests. (note: this test already existed the same way, I just renamed the name function to be more correct)

sensorprocess/testhelper.go

sensorprocess/movementsensorprocess.go

randhid

Some comments would be good in the places I note, also I'd like the case selector to be a tad bit neater, and why do we need that closed bool?

Edit: I'm not looking at the tests at all, but I trust they're doing good things! Lots of them!

sensorprocess/movementsensorprocess.go

sensorprocess/sensorprocess.go

randhid · 2023-12-29T21:04:05Z

viam_cartographer.go

+	subAlgo        SubAlgo
+
+	muClose sync.Mutex
+	closed  bool


what is this doing?

do you have to tell the C++ layer that the carto facade is closed?

No, it is used in the endpoints to indicate that they're called after the Close function has already been initiated, e.g.:

cartoSvc.muClose.Lock() cartoSvcClosed := cartoSvc.closed cartoSvc.muClose.Unlock() if cartoSvcClosed { cartoSvc.logger.Warn("Position called after closed") return nil, "", ErrClosed }

muClose locks while Close is running and thus blocks all endpoints from attempting to get data from the C++ layer as it's being shut down.

Note Rand, This came up in an eng talk by Tess six months back. Someone told me that the mod manager should be handling this now, but when I talked to Benji, he had no idea what I was talking about and didn't know if it was being handled so we decided to keep this closed check in, until we know for certain.

There was a slakc thread about this, it's not mod manager, it's the resource graph that handles removing the resource nodes from the graph (modules included). Its not something other modules do, since if it's closed the client only returns an error for a resource that's not there anymore.

Like if you try to import the resource from the robot after it's closed and not built again, the FromRobot, FromDep call in your code just fails. If it doesn't, that's a netcode bug.

So the only case to keep this is if cloud slam is directly importing cartographer methods outside of the resource graph.

For example, gpsnmea has public functions that are imported directly from within rdk by gpstrk (this is something we want to fix but never have bandwidth to), so we have a closed bool there to account for one structure being imported directly outside of the resource graph logic.

If nothing is doing so with this module then it's unnecessary, but I want to be exhaustive about the potential gotchas.

Kat, have you seen any of these errors or logs surface at all? Is the C++ layer taking a long time to shut down and thus we are in danger of calling an endpoint directly before the resource graph removes this resource node?

I have neither paid attention nor noticed it. Can we create a separate ticket to look into this?

Then let's not add code that might not do anything at all and wasn't hurting the implementation, but does seem to be important for anyone reading the code.

I was referring to your mutexes, okay to leave removing the closed checks to another pr.

viam_cartographer.go

config/config.go

jeremyrhyde

LGTM! see one or two last comments. @randhid can decide if we want to remove the closed checks in each endpoints in this PR or a future one.

Co-authored-by: jeremyrhyde <34897732+jeremyrhyde@users.noreply.github.com>

github-actions · 2024-01-08T11:39:32Z

AppImages ready!
https://storage.googleapis.com/packages.viam.com/apps/slam-servers/cartographer-module-pr-307-aarch64.AppImage
https://storage.googleapis.com/packages.viam.com/apps/slam-servers/cartographer-module-pr-307-x86_64.AppImage

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 11, 2023

kkufieta added the appimage Build AppImage of PR label Dec 12, 2023

kkufieta requested a review from jeremyrhyde December 19, 2023 17:07

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 19, 2023

kkufieta commented Dec 19, 2023

View reviewed changes

kkufieta requested a review from randhid December 19, 2023 20:13

kkufieta force-pushed the kk/rsdk-4862-sensorprocess-2 branch from dd277d4 to e25e0a0 Compare December 28, 2023 17:40

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 28, 2023

jeremyrhyde suggested changes Dec 29, 2023

View reviewed changes

randhid requested changes Dec 29, 2023

View reviewed changes

kkufieta added 4 commits December 30, 2023 07:56

Change defaultTime to undefinedTime

4c3e62f

Change defaultTime to undefinedTime

0324331

Add odometer to sensor process

6c4df54

Update comment

bb0f9cc

Fix comment

6419e70

kkufieta force-pushed the kk/rsdk-4862-sensorprocess-2 branch from e25e0a0 to 6419e70 Compare December 30, 2023 07:00

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 30, 2023

Fix PR comments

0af8396

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 3, 2024

Appease linter

3878c1d

kkufieta requested review from jeremyrhyde and randhid January 3, 2024 11:13

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 3, 2024

jeremyrhyde reviewed Jan 4, 2024

View reviewed changes

config/config.go Outdated Show resolved Hide resolved

jeremyrhyde approved these changes Jan 4, 2024

View reviewed changes

Update config/config.go

043ff67

Co-authored-by: jeremyrhyde <34897732+jeremyrhyde@users.noreply.github.com>

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 5, 2024

Appease linter

eb12c24

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 5, 2024

Revert mutex changes

dc9d8fd

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 8, 2024

Fix order of vars

c00990a

viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Jan 8, 2024

randhid approved these changes Jan 8, 2024

View reviewed changes

kkufieta merged commit 2f92320 into viam-modules:main Jan 8, 2024
7 checks passed

kkufieta deleted the kk/rsdk-4862-sensorprocess-2 branch January 8, 2024 15:00

RSDK-4862 - Add odometer sensor process #307

RSDK-4862 - Add odometer sensor process #307

Conversation

kkufieta commented Dec 11, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jeremyrhyde left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

randhid Dec 29, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kkufieta Jan 5, 2024 • edited Loading

Choose a reason for hiding this comment

randhid left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

randhid Jan 5, 2024 • edited Loading

Choose a reason for hiding this comment

jeremyrhyde left a comment

Choose a reason for hiding this comment

github-actions bot commented Jan 8, 2024

kkufieta commented Dec 11, 2023 •

edited

Loading

randhid Dec 29, 2023 •

edited

Loading

kkufieta Jan 5, 2024 •

edited

Loading

randhid left a comment •

edited

Loading

randhid Jan 5, 2024 •

edited

Loading