search

SICKAG / sick_scan_xd

Based on the sick_scan drivers for ROS1, sick_scan_xd merges sick_scan, sick_scan2 and sick_scan_base repositories. The driver supports both Linux (native, ROS1, ROS2) and Windows (native and ROS2).
Apache License 2.0
101 stars 84 forks source link

Python API - How can I combine multiple message/segments/objects into one scan? #271

Closed gschwsiSICKAG closed 6 months ago

gschwsiSICKAG commented 8 months ago

Hello,

We do not work with ROS, but use the sensor via the driver or the Python API.

During the first tests, we read the SickScanPointCloudMsg object and saved the x,y,z coordinates. What we have not yet understood or have not found in the documentation is that such a received object does not contain a complete scan in the data object (msg.data). This means that several message objects must be processed/combined into one scan. How can we make this happen? We suspect that there is some object meta info that we can use for this (e.g. frame_id from the message header?).

I have attributes in the header of each PointCloudMsg, but unfortunately, I can't do much with them. The attributes are:

Sequence ID (seq): is always 0 timestamp_sec: I get this timestamp, but timestamp_nsec: is always 0, so I have several messages within a second, but I can't assign them to each other frame_id: this string is also constant 'world'

In the PointCloudMsg there is still the segment ID, which changes. But I have the problem that several messages with the same segment ID arrive within a second and I cannot link them correctly (without the timestamp_nsec - see above).

How can I combine multiple message objects into one scan?

rostest commented 8 months ago

Thanks for your feedback. You can use the segment index of a pointcloud message (i.e. msg.segment_idx) to separate segment and fullframe pointclouds. A fullframe pointcloud has segment index -1 and contains all scan points of one 360 degree fullframe. Segment pointclouds have segment_idx >= 0, e.g. 0 to 11 for multiScan lidars.

An example for a typical sequence of multiScan pointcloud messages looks like this:

pySickScanCartesianPointCloudMsgCallback: 9912x1 pointcloud, segment -1
pySickScanCartesianPointCloudMsgCallback:  888x1 pointcloud, segment  0
pySickScanCartesianPointCloudMsgCallback:  897x1 pointcloud, segment  1
pySickScanCartesianPointCloudMsgCallback:  889x1 pointcloud, segment  2
pySickScanCartesianPointCloudMsgCallback:  884x1 pointcloud, segment  3
pySickScanCartesianPointCloudMsgCallback:  753x1 pointcloud, segment  4
pySickScanCartesianPointCloudMsgCallback:  712x1 pointcloud, segment  5
pySickScanCartesianPointCloudMsgCallback:  768x1 pointcloud, segment  6
pySickScanCartesianPointCloudMsgCallback:  861x1 pointcloud, segment  7
pySickScanCartesianPointCloudMsgCallback:  849x1 pointcloud, segment  8
pySickScanCartesianPointCloudMsgCallback:  749x1 pointcloud, segment  9
pySickScanCartesianPointCloudMsgCallback:  810x1 pointcloud, segment 10
pySickScanCartesianPointCloudMsgCallback:  868x1 pointcloud, segment 11

The first line (segment -1) is displayed in the callback for a fullframe pointcloud, the other messages are from segment pointclouds (segment >= 0). This sequence repeats for each full scan.

To make segment and fullframe pointclouds more transparent, we added the topic in pointcloud messages in a new branch https://github.com/SICKAG/sick_scan_xd/tree/feature/picoscan_single_echo. This is the ros topic, which is used to publish pointcloud messages under ROS. Using the default launchfile settings, the topic is "/cloud_unstructured_fullframe" for fullframe pointclouds and "/cloud_unstructured_segments" for segment pointclouds. Thus the sequence of pointcloud messages looks like:

pySickScanCartesianPointCloudMsgCallback: 9912x1 pointcloud, segment -1, topic "/cloud_unstructured_fullframe"
pySickScanCartesianPointCloudMsgCallback:  888x1 pointcloud, segment  0, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  897x1 pointcloud, segment  1, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  889x1 pointcloud, segment  2, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  884x1 pointcloud, segment  3, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  753x1 pointcloud, segment  4, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  712x1 pointcloud, segment  5, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  768x1 pointcloud, segment  6, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  861x1 pointcloud, segment  7, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  849x1 pointcloud, segment  8, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  749x1 pointcloud, segment  9, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  810x1 pointcloud, segment 10, topic "/cloud_unstructured_segments"
pySickScanCartesianPointCloudMsgCallback:  868x1 pointcloud, segment 11, topic "/cloud_unstructured_segments"

Using the update in branch feature/picoscan_single_echo, you can either use segment_idx < 0 or the topic "/cloud_unstructured_fullframe" to identify the fullframe pointclouds with all scan points of a 360 degree fullframe combined in one message. Note that the new branch is a collective update incl. adaptations for the new picoScan 150 series w/o addons. It also corrects property timestamp_nsec.

poettinger commented 7 months ago

Dear rostest. Thank you for your reply and for providing a new version of the library. We just tested the new library but we still face some problems. Below you will find the output of our program. We just print the segment id (i.e. the information in the message content: msg.contents.segment_idx), the size of the x-coordinates, the sensor times (i.e. the information in the message header: msg.contents.header.timestamp_sec and msg.contents.header.timestamp_nsec). As you can see we do not get any full frame (i.e. with segment index -1) and we also do not get a sequence beginning from 0 to 11 as in your example. Our sequence of segment_ids have gaps, e.g. we get segment_id 0, then 2, then 7, and then it starts from 0 again. Can you please provide some hints what we can further test or if we obviously do some mistakes in using the library. Thanks a lot.

-------------------- output of our program ----------------------------- Sick Lidar: seg_id: 3 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 103352069 as str: 2024-02-2613_18_35103352 Sick Lidar: seg_id: 8 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 119189500 as str: 2024-02-2613_18_35119190 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 131927013 as str: 2024-02-2613_18_35131927 Sick Lidar: seg_id: 6 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 153031349 as str: 2024-02-2613_18_35153031 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 171691179 as str: 2024-02-2613_18_35171691 Sick Lidar: seg_id: 3 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 183132886 as str: 2024-02-2613_18_35183133 Sick Lidar: seg_id: 8 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 199134826 as str: 2024-02-2613_18_35199135 Sick Lidar: seg_id: 1 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 215722322 as str: 2024-02-2613_18_35215722 Sick Lidar: seg_id: 6 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 232462406 as str: 2024-02-2613_18_35232462 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 251914024 as str: 2024-02-2613_18_35251914 Sick Lidar: seg_id: 4 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 266855716 as str: 2024-02-2613_18_35266856 Sick Lidar: seg_id: 8 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 279267549 as str: 2024-02-2613_18_35279268 Sick Lidar: seg_id: 1 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 295663118 as str: 2024-02-2613_18_35295663 Sick Lidar: seg_id: 6 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 312409400 as str: 2024-02-2613_18_35312409 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 331755638 as str: 2024-02-2613_18_35331756 Sick Lidar: seg_id: 4 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 347208261 as str: 2024-02-2613_18_35347208 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 372378110 as str: 2024-02-2613_18_35372378 Sick Lidar: seg_id: 2 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 379674196 as str: 2024-02-2613_18_35379674 Sick Lidar: seg_id: 7 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 395957231 as str: 2024-02-2613_18_35395957 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 411661148 as str: 2024-02-2613_18_35411661 Sick Lidar: seg_id: 5 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 428687334 as str: 2024-02-2613_18_35428687 Sick Lidar: seg_id: 0 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 451800584 as str: 2024-02-2613_18_35451801 Sick Lidar: seg_id: 3 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 462730646 as str: 2024-02-2613_18_35462731 Sick Lidar: seg_id: 8 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 479123115 as str: 2024-02-2613_18_35479123 Sick Lidar: seg_id: 1 . size: 120 sensor_time_sec: 1708949915 sensor_time_nsec: 495575904 as str: 2024-02-2613_18_35495576

rostest commented 7 months ago

Thanks for following up. It looks like the application does not receive all point cloud messages. Did you or your customer use one of the WaitNext...PointCloudMsg-functions to receive the point cloud messages?

The WaitNext-functions of the API just return the next message received by sick_scan_xd. For multiScan and picoScan, this can be a scan segment (i.e. a part of the full point cloud) or a fullframe scan (i.e. all scan points of a 360 degree scan). Depending on the timing, the calling application may not receive all messages. We therefore recommend to register a message callback instead of polling with a WaitNext-function. By using a registered message callback, the application is notified with all fullframe and segment point cloud messages.

If WaitNext-functions are used to receive point cloud messages from a multiScan, please exchange them with SickScanApiRegisterCartesianPointCloudMsg resp. SickScanApiRegisterPolarPointCloudMsg. See the usage example, https://github.com/SICKAG/sick_scan_xd/issues/270#issuecomment-1929215742 and https://github.com/SICKAG/sick_scan_xd/issues/274#issuecomment-1936204259 for further details and examples.

If you are using SickScanApiRegisterCartesianPointCloudMsg or SickScanApiRegisterPolarPointCloudMsg, please run the python example sick_scan_xd_api_test.py as described in the complete usage example in python. Use branch https://github.com/SICKAG/sick_scan_xd/blob/feature/api_logging for the latest update based on the feature/picoscan_single_echo branch. Using this example, you should receive all point cloud messages incl. fullframe scans. Please post a complete logfile, if segment or fullframe point cloud messages are still missing.

poettinger commented 7 months ago

We have been utilizing the 'SickScanApiRegisterCartesianPointCloudMsg' in our implementation. However, upon comparing our code with the provided example (which functioned correctly), I realied we used performance consuming functions inbetween the messages, which caused the data loss. After optimizing the code, all messeges could be received.

Thanks for your help!