gramaziokohler / roslibpy

Python ROS Bridge library
MIT License
273 stars 56 forks source link

subscriber sensor_msgs/PointCloud2 topic #30

Closed learn275 closed 5 years ago

learn275 commented 5 years ago

Summary. How to parse message data in order to obtain lidar xyz coordinates?

Expected Result

Actual Result

std_msgs/Header header(sensor_msgs/PointCloud2) uint32 height uint32 width sensor_msgs/PointField[] fields bool is_bigendian uint32 point_step uint32 row_step uint8[] data bool is_dense

message: <class 'dict'>: {'fields': [{'name': 'x', 'count': 1, 'datatype': 7, 'offset': 0}, {'name': 'y', 'count': 1, 'datatype': 7, 'offset': 4}, {'name': 'z', 'count': 1, 'datatype': 7, 'offset': 8}, {'name': 'intensity', 'count': 1, 'datatype': 7, 'offset': 16}, {'name': 'ring', 'count': 1, 'datatype': 4, 'offset': 20}], 'row_step': 933888, 'data': 'gzMzP805pT6YflO+M38AAAAAwEIAAAAAWAC74jN/AAASn6ZAg44ZQJryzD0zfwAAAAAAQAgAAABYALviM38AAAJcLz8CiaE+c0oyvjN/AAAAAMhCAQAAAFgAu+IzfwAAlQanQK3JGUArL5o+M38AAAAAgD8JAAAAWAC74jN/AAA5Ejc/w4+oPpezHL4zfwAAAADIQgIAAABYALviM38AAHa7p0AbXhpA+DoBPzN/AAAAAIA/CgAAAFgAu+IzfwAAWTajQHgjFkDioGO/M38AAAAAAEADAAAAWAC74jN/AACRIKtA3VgdQOcBOT8zfwAAAABAQAsAAABYALviM38AAO8so0B99xVAiWUwvzN/AAAAAIBABAAAAFgAu+IzfwAAdk6oQHmcGkA/rWo/M38AAAAAgD8MAAAAWAC74jN/AAAqt6RA+08XQAi7/b4zfwAAAADIQgUAAABYALviM38AABpYp0AOqBlAviuPPzN/AAAAAIA/DQAAAFgAu+IzfwAApNqkQOZMF0AbGJi+M38AAAAAgEAGAAAAWAC74jN/AADqxplArxENQBw8nD8zfwAAAACQQQ4AAABYALviM38AACJ9pUA7vhdA/1....','width': 29184, 'point_step': 32, 'header': {'frame_id': 'lidar', 'seq': 12, 'stamp': {'secs': 1561513778, 'nsecs': 620011000}}, 'is_bigendian': False, 'height': 1, 'is_dense': True}

(message['data'])Why isn't a byte stream?How to parse message data in order to obtain lidar xyz coordinates?

Reproduction Steps

!/usr/bin/env python

-- coding: utf-8 --

from roslibpy import Ros, Topic .... client= Ros(host='192.168.121.12', port=9090)
listener_lidar = Topic(client, '/lidar_points_origin', 'sensor_msgs/PointCloud2') def start_listening():
listener_lidar.subscribe(receive_message_lidar) def receive_message_lidar(message): LOGGER.debug(str(message)) client.on_ready(start_listening()) client.run() ...

System Information

win7/10、python3.6、roslibpy0.4.1/0.6.0 indigo

gonzalocasas commented 5 years ago

@learn275 the data is encoded in base64, I assume a basic decoding is rather simple, just convert back to a byte array and that's it. converting back to the lidar data is perhaps more work, I would need to test that a bit...

learn275 commented 5 years ago

@gonzalocasas Thanks! len(base64.b64decode(message['data']))=933900 >rowstep(933888) base64.b64decode(message['data']): b'\x8333?\xcd9\xa5>\x98~S\xbe3\x7f\x00\x00\x00\x00\xc0B\x00\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x12\x9f\xa6@\x83\x8e\x19@\x9a\xf2\xcc=3\x7f\x00\x00\x00\x00\x00@\x08\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x02\/?\x02\x89\xa1>sJ2\xbe3\x7f\x00\x00\x00\x00\xc8B\x01\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x95\x06\xa7@\xad\xc9\x19@+/\x9a>3\x7f\x00\x00\x00\x00\x80?\t\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x009\x127?\xc3\x8f\xa8>\x97\xb3\x1c\xbe3\x7f\x00\x00\x00\x00\xc8B\x02\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00v\xbb\xa7@\x1b^\x1a@\xf8:\x01?3\x7f\x00\x00\x00\x00\x80?\n\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00Y6\xa3@x#\x16@\xe2\xa0c\xbf3\x7f\x00\x00\x00\x00\x00@\x03\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x91 \xab@\xddX\x1d@\xe7\x019?3\x7f\x00\x00\x00\x00@@\x0b\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xef,\xa3@}\xf7\x15@\x89e0\xbf3\x7f\x00\x00\x00\x00\x80@\x04\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00vN\xa8@y\x9c\x1a@?\xadj?3\x7f\x00\x00\x00\x00\x80?\x0c\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00*\xb7\xa4@\xfbO\x17@\x08\xbb\xfd\xbe3\x7f\x00\x00\x00\x00\xc8B\x05\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x1aX\xa7@\x0e\xa8\x19@\xbe+\x8f?3\x7f\x00\x00\x00\x00\x80?\r\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xa4\xda\xa4@\xe6L\x17@\x1b\x18\x98\xbe3\x7f\x00\x00\x00\x00\x80@\x06\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xea\xc6\x99@\xaf\x11\r@\x1c<\x9c?3\x7f\x00\x00\x00\x00\x90A\x0e\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00"}\xa5@;\xbe\x17@\xff\xcb\xbd3\x7f\x00\x00\x00\x00\x00@\x07\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xdcA\x9e@\x9e\x0b\x11@\xe6\x94\xba?3\x7f\x00\x00\x00\x00\x80@\x0f\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xbb\x962?\xe2&\xa3>4oR\xbe3\x7f\x00\x00\x00\x00\xc8B\x00\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xa8\xef\xa7@fY\x19@\x01\xce=3\x7f\x00\x00\x00\x00\x00@\x08\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00J\x8c0?j#\xa1S63\xbe3\x7f\x00\x00\x00\x00\xc8B\x01\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00Y\xa4\xa7@m\xf0\x18@}\x81\x9a>3\x7f\x00\x00\x00\x00\x80?\t\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xe2\x9e4?\x90\xc7\xa4>[[\x1a\xbe3\x7f\x00\x00\x00\x00\xc8B\x02\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x99d\xa9@\x15w\x1a@#M\x02?3\x7f\x00\x00\x00\x00\x80?\n\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xe7\x87\xa3@\xf0\x0c\x15@c\xb5c\xbf3\x7f\x00\x00\x00\x00@@\x03\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x00\x00\xc0\x7f\x00\x00\xc0\xff\x00\x00\xc0\x7f3\x7f\x00\x00\x00\x000A\x0b\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00HC\xa3@6\xab\x14@\x9e50\xbf3\x7f\x00\x00\x00\x00\xa0@\x04\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x005\xce\xa8@\n\xa5\x19@E\xffj?3\x7f\x00\x00\x00\x00\x80?\x0c\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00AH\xa4@\x16\x87\x15@\xdc\xa8\xfc\xbe3\x7f\x00\x00\x00\x00\xc8B\x05\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x00\x00\xc0\x7f\x00\x00\xc0\xff\x00\x00\xc0\x7f3\x7f\x00\x00\x00\x00\xa8A\r\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\x9e\xc4\xa4@\xca\xd4\x15@\xca\xc5\x97\xbe3\x7f\x00\x00\x00\x00\x80@\x06\x00\x00\x00X\x00\xbb\xe23\x7f\x00\x00\xc8\xfc\x9a@......

learn275 commented 5 years ago

@gonzalocasas I use another frame of data len(base64.b64decode(message['data']))=933888 == row_step(933888) .Thanks!