Publish Poses with Covariance

[tappan-at-git]

Addressing issue #43 by adding parameters to publish pose_with_covariance and pose_with_covariance_stamped messages.

• If no other parameters are set, the covariance array will default to 1e-9 across the diagonal.
• If the position_covariance or orientation_covariance parameters are set, those values will be used for the relevant fields.
• If the do_compute_covariance parameter is set, these messages will use CSM's covariance values for x, y, and yaw covariance, with 1e-9 or the parameterized covariance filling in the fields CSM does not calculate as appropriate Let me know if there are any concerns..

[130s]

Great!

Can you think of any unit test cases? Small ones should be fine (Ref. https://github.com/k-okada/scan_tools/pull/1).

[tappan-at-git]

Added very simple hztests for pose_with_covariance and pose_with_covariance stamped.

[130s]

Maybe 1 last thing; can you add the .test file to add_rostest like another file so that the test gets run with Catkin's run_tests?

[tappan-at-git]

Whoops, my bad. That's added.

[130s]

+1

[tappan-at-git]

A small commit to fix a big problem. Because LaserScanMatcher persists a single sm_output object, enabling do_compute_covariance in CSM leads to a really dramatic memory leak. This is easily fixed by calling gsl_matrixfree() on the relevant fields of LaserScanMatcher.output.

I'm calling gsl_matrix_free() immediately before smicp() is run to support hypothetical usage of output outside of processScan(), but I'd argue that it might be better to destroy it after processScan() is run and not persist a member variable at all. Unfortunately, since CSM didn't implement a destructor for its smoutput struct you would still need to manually free the GSL matrices before getting rid of output.

[130s]

@tappan-at-git sounds great. Sorry to ask again, but can you add tests to this PR for the last changes you mentioned? If not, can you think of test scenario that would validate your changes (so that someone else can implement the tests)?

[tappan-at-git]

Sorry for the long delay. I haven't been able to devote a lot of time to this branch, but I've been trying to figure out whether freeing the GSL matrices is even something that can be tested beyond "yep, that still runs." (In other words, the current test.) If that's alright with you, great, otherwise I'll keep thinking.

I'm probably going to give the precise clearing logic another look, with an eye to when ICP considers an output invalid, and whether it makes sense to store the output_ struct in light of that. I'm increasingly lean towards "no", but I don't think making that change is pertinent to this feature.

Also, I discovered while enabling proper diagnostics support that ICP is frequently spitting out negative covariances. I need to review the implementation to figure out if that just means they're unsquared or what. In any case things have been working surprisingly well with robot_localization silently replacing negative covariances with a small positive value.

[130s]

Sounds good to me. Thanks!