correctly accumulating lidar

[mauricefallon]

Current approach:

1. check if the bot frame is correct (pronto_frame_check_tools), if true
2. project scan using laser_utils

PROBLEMS
1 contains hard coded checks of multisense joints. (but the lidar could be from a another lidar e.g. SICK)
2 scan could arrive quite late

Challenges:

• some frames change (multisense, body-to-head on valk), some don't (body-to-head on hyq, fixed lidar)
• some frames are updated after the lidar is received e.g. state estimate on husky

HyQ issue:

• there is no neck, so the pronto_frame_check_tools check just fails
• To solve HyQ issue:
  • properly check that the frame is correct

Husky issue:

• scan received before frame
• To solve Husky issue:
  • only lidar which has a valid frame should be projected
  • try to project when lidar is first recieved
  • where this fails, put in a buffer and try again when the next lidar message is received, until the buffer reaches a large size, then drop the lidar scan

[mauricefallon]

proper frame checking added here: #95

the issue that the husky was having has not been resolved within Pronto

[patmarion]

I'm trying to follow along, let me try to summarize briefly in my words to see if we agree:

We want to project a planar lidar scan into the local (world) coordinate frame, so we ask bot frames for SCAN_to_local transform using the utime of the latest scan message. Problem: state estimate may lag behind, so one or more frames in the SCAN_to_local chain are too old to compute an accurate SCAN_to_local. Fix: identify which frames in the chain are relevant, and do not project the scan until those frames have been updated with timestamps newer[1] than the scan utime.

I think we decided that the behavior of bot-viewer is desirable: it renders all scans, including new ones with inaccurate projections, but it corrects the projections when the relevant frames eventually arrive.

[1] actually, we want the closest in time, bot frames handles this automatically with buffers

[mauricefallon]

Your interpretation of the problem is correct. The root of the issue is that bot_frames does not distinguish between fixed and moving frames (spinning lidar, body-to-head or body frame).

In the above use case we want to collect LIDAR into a point cloud in world frame - not just render it. So we don't want to correct anything at a later date.

bot-viewer is also not quite perfect. It only checks if ONE of the links from SCAN to local is newer than the SCAN timestamp.

I have implemented some thing, which does the correct checking. It could be done better, but at least it removes hardcoded strings like "MULTISENSE_SCAN" AND "HEAD" from the check.