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
107 stars 87 forks source link

Failed to connect picoScan150 #410

Open xielingsen opened 1 week ago

xielingsen commented 1 week ago

Hello, I'm running Ubuntu 18.04, with ROS_VERSION=0. I'm trying to communicate to a Sick picoScan150. I was able to ping the device, but it can't communicate and the device reported the tcp error. Here is the full output:

Info: sick_scan_api V. 3.6.0 githash:"c70c20ba4f2e21bdc36ec3972b8bdaaa35e7e96f" gitinfo:"2024-10-09 01:51:39 +0000 xielingsen ??sick?????" Info: SickScanApiInitByCli: node scanner_type:=sick_picoscan hostname:=192.168.1.150 port:=2111 fields:=range,azimuth,elevation,t [Error]: ## ERROR SickCloudTransform(): Can't parse config string "", use 6D pose "x,y,z,roll,pitch,yaw" in [m] resp. [rad] [Error]: ## ERROR SickCloudTransform(): Initialization by "" failed, use 6D pose "x,y,z,roll,pitch,yaw" in [m] resp. rad: sick_scansegment_xd configuration: Info: scanner_type: sick_picoscan

Info: udp_port: 2115 Info: check_udp_receiver_ip: 0 Info: check_udp_receiver_port: 2116 Info: all_segments_min_deg: -180 Info: all_segments_max_deg: 180

Info: udp_input_fifolength: 20 Info: msgpack_output_fifolength: 20 Info: verbose_level: 1 Info: measure_timing: 1 Info: export_csv: 0 Info: export_udp_msg: 0

Info: hostname: 192.168.1.150

Info: udp_timeout_ms: 10000 Info: udp_timeout_ms_initial: 60000 Info: scandataformat: 2 Info: performanceprofilenumber: -1 Info: imu_enable: 0

Info: imu_udp_port: 7503 Info: imu_latency_microsec: 0

Info: start_sopas_service: 1 Info: send_sopas_start_stop_cmd: 1 Info: sopas_cola_binary: 0 Info: sopas_timeout_ms: 5000 Info: host_read_filtersettings: 1 Info: host_FREchoFilter: 1 Info: host_set_FREchoFilter: 0

Info: host_set_LFPangleRangeFilter: 0

Info: host_set_LFPlayerFilter: 0

Info: host_set_LFPintervalFilter: 0

Info: msgpack_validator_enabled: 0 Info: msgpack_validator_verbose: 0 Info: msgpack_validator_discard_msgpacks_out_of_bounds: 1 Info: msgpack_validator_check_missing_scandata_interval: 12

Info: msgpack_validator_azimuth_start: -3.14159 rad: msgpack_validator_azimuth_end: 3.14159 rad: msgpack_validator_elevation_start: -1.5708 rad: msgpack_validator_elevation_end: 1.5708 [rad]

Info: sick_scansegment_xd (sick_picoscan) started. Info: sick_scansegment_xd::runThreadCb() start (173,1,1)... Info: sick_scansegment_xd initializing... Info: UdpReceiverSocketImpl: udp socket created, binding to port 2115 ... Info: sick_scansegment_xd: udp socket to :2115 initialized Info: MsgPackThreads: Start msgpack converter, udp receiver and msgpack exporter, receiving from :2115 Info: MsgPackThreads: initializing sopas tcp (192.168.1.150:, timeout:5, binary:0) [Error]: ## ERROR SickCloudTransform(): Can't parse config string "", use 6D pose "x,y,z,roll,pitch,yaw" in [m] resp. [rad] [Error]: ## ERROR SickCloudTransform(): Initialization by "" failed, use 6D pose "x,y,z,roll,pitch,yaw" in [m] resp. rad: MsgPackThreads: initializing device Info: sick_scan_xd: Tcp::open: connecting to 192.168.1.150:127 ... [Error]: Tcp::open: Failed to open TCP connection to 192.168.1.150:127, aborting. Info: MsgPackThreads: initializing services Info: MsgPackThreads: ros services initialized [Warn]: ## ERROR sick_scansegment_xd: no sopas tcp connection, multiScan136 filter settings not queried or written [Error]: ## ERROR sick_scansegment_xd: no sopas tcp connection, startup sequence not sent, receiving scan data may fail. [Error]: ## ERROR sick_scansegment_xd: sopas tcp connection lost, stop and reconnect... Info: sick_scansegment_xd finishing (382) Info: sick_scansegment_xd finishing (392) sick_scansegment_xd exit: stopping services and communication (403) SickScanCommon closed.

rostest commented 1 week ago

Thanks for your feedback. It looks like the launch file is missing from the argument list of the SickScanApiInitByCli function. SickScanApiInitByCli expects (argc, argv) arguments as provided from the command line:

sick_scan_xd_api_test <launchfile> hostname:=<lidar-ip-address> udp_receiver_ip:=<pc-ip-address>

Example:

sick_scan_xd_api_test launch/sick_picoscan.launch hostname:=192.168.1.150 udp_receiver_ip:=192.168.1.100

To call SickScanApiInitByCli directly with these arguments, you can provide (argc, argv) like this:

char* argv[] = { "sick_scan_xd_api_test", "launch/sick_picoscan.launch", "hostname:=192.168.1.150", "udp_receiver_ip:=192.168.1.100" }; 
int argc = 4;
SickScanApiHandle apiHandle = SickScanApiCreate(argc, &argv[0]);
SickScanApiInitByCli(apiHandle, argc, &argv[0]);

Replace 192.168.1.100 with the IP address of your PC and adapt the path to the launch file. See https://github.com/SICKAG/sick_scan_xd?tab=readme-ov-file#generic-driver-api for details and examples.

xielingsen commented 6 days ago

Thanks for your reply, @rostest . My app does not report the errors now. But I can not get the fields I'm interested in. I want the fields: i,range,azimuth,elevation,t. But I got only the i(intensity) field.

The launch file I am using:

......
    <arg name="custom_pointclouds" default="cloud_unstructured_segments cloud_unstructured_fullframe cloud_polar_unstructured_segments cloud_polar_unstructured_fullframe cloud_all_fields_fullframe"/>`
......

I am using the c++ callback to fetch scan data. The codes to fetch the fields:

void customizedPointCloudMsgCb(SickScanApiHandle apiHandle, const SickScanPointCloudMsg* msg) {
    LOG(INFO) << "PointCloudMsgCb: " << msg->width << " x " << msg->height << " pointcloud message received";
    // Create field descriptions
    int num_fields = msg.fields.size;
    SickScanPointFieldMsg* msg_fields_buffer = (SickScanPointFieldMsg*)msg.fields.buffer;
    int field_offset_range = -1, field_offset_azimuth = -1, field_offset_elevation = -1, field_offset_intensity = -1,
        field_offset_t = -1;
    for (int n = 0; n < num_fields; n++) {
        if (strcmp(msg_fields_buffer[n].name, "range") == 0 &&
            msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32) {
            field_offset_range = msg_fields_buffer[n].offset;
        } else if (strcmp(msg_fields_buffer[n].name, "azimuth") == 0 &&
                   msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32) {
            field_offset_azimuth = msg_fields_buffer[n].offset;
        } else if (strcmp(msg_fields_buffer[n].name, "elevation") == 0 &&
                   msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32) {
            field_offset_elevation = msg_fields_buffer[n].offset;
        } else if (strcmp(msg_fields_buffer[n].name, "t") == 0 &&
                   msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_UINT32) {
            field_offset_t = msg_fields_buffer[n].offset;
        } else if (strcmp(msg_fields_buffer[n].name, "i") == 0 &&
                   msg_fields_buffer[n].datatype == SICK_SCAN_POINTFIELD_DATATYPE_FLOAT32) {
            field_offset_intensity = msg_fields_buffer[n].offset;
        }
    }

    if (field_offset_range == -1 || field_offset_azimuth == -1 || field_offset_elevation == -1 ||
        field_offset_intensity == -1 || field_offset_t == -1) {
        LOG(INFO) << "Not all fields found. field_offset_range:" << field_offset_range
                  << "; field_offset_azimuth:" << field_offset_azimuth
                  << "; field_offset_elevation:" << field_offset_elevation
                  << "; field_offset_intensity:" << field_offset_intensity << "; field_offset_t:" << field_offset_t;
        return nullptr;
    } else {
        LOG(INFO) << "All fields found. field_offset_range:" << field_offset_range
                  << "; field_offset_azimuth:" << field_offset_azimuth
                  << "; field_offset_elevation:" << field_offset_elevation
                  << "; field_offset_intensity:" << field_offset_intensity << "; field_offset_t:" << field_offset_t;
    }
......

After the code snippet, only field_offset_intensity was set. So what do I need to modify the launch file or c++ code to get the fields I want(i,range,azimuth,elevation,t)?

rostest commented 6 days ago

Thank you for your reply. You can get the range,azimuth,elevation,i fields by registering a polar point cloud callback.

Function SickScanApiRegisterCartesianPointCloudMsg registers for cartesian point clouds with the fields x,y,z,i; function SickScanApiRegisterPolarPointCloudMsg registers for polar point clouds with the fields azimuth,elevation,range,i. Simply replace SickScanApiRegisterCartesianPointCloudMsg(apiHandle, customizedPointCloudMsgCb) with SickScanApiRegisterPolarPointCloudMsg(apiHandle, customizedPointCloudMsgCb) in your code and you should get azimuth,elevation,range,i in your callback.

xielingsen commented 6 days ago

Thanks you @rostest . Now I can get i,range,azimuth,elevation. But I can't get t. The launch file:

......
    <arg name="custom_pointclouds" default="cloud_proton"/>
......
         <param name="cloud_proton" type="string" value="coordinateNotation=3 updateMethod=0 fields=i,range,azimuth,elevation,t echos=2 layers=6 reflectors=0,1 infringed=0,1  topic=/cloud_proton frameid=base_link publish=1"/>
......

If I set coordinateNotation=1, I can get i,range,azimuth,elevation. But If I set coordinateNotation=3, I can not get any scan and the app outputs:

[Info]: MsgPack/Compact-Exporter:   0 udp packages still in input fifo, 0 messages still in output fifo, current segment index: 7
[Info]: MsgPack/Compact-Exporter: 261 udp scandata messages received, 393 messages exported (scan+imu), 0% package lost.
[Info]: MsgPack/Compact-Exporter: max. 2 udp messages buffered, max 2 export messages buffered.
[Info]: MsgPack/Compact-Exporter: 393 messages exported at 303.700 Hz, mean time: 0.333 milliseconds/messages, stddev time: 0.205, max time: 1.497 milliseconds between udp receive and messages export, histogram=[387,6,0,0,0,0,0,0,0,0,0]
xielingsen commented 6 days ago

And the scans fetched are with width=0 and height=1 like: I1029 21:33:46.997144 127 node_main.cc:632] PointCloudMsgCb: 0 x 1 pointcloud message received Any polar point cloud example?

rostest commented 5 days ago

Thanks for your reply. sick_scan_xd_api_test.cpp shows an example for the polar point cloud, see the usage example for further information. The example simply prints the width and height of the point cloud like this:

[Info]: apiTestCartesianPointCloudMsgCallback(apiHandle:00000182506DC0B0): 1124x1 pointcloud callback...
[Info]: apiTestPolarPointCloudMsgCallback(apiHandle:00000182506DC0B0): 1124x1 pointcloud callback...

If you insert your callback in the apiTestPolarPointCloudMsgCallback function, you should receive the fields range,azimuth,elevation,i. Use the unmodified launch file with coordinateNotation=1, which indicates polar point clouds. Feel free to start with the example and the provided launch file; if it works as expected, customising sick_scan_xd and integrating it into your application will be easier.

Customized point clouds are currently only exported in ROS. The sick_scan_xd API exports cartesian point clouds with fields (x,y,z,i) and polar point clouds with fields (range,azimuth,elevation,i); field t is not exported by the API. Note that field "t" is the time offset in nanoseconds relative to the header timestamp for each scan point in the point cloud. This field can be used for deskewing, e.g. for 3D-SLAM with rtabmap in ROS.

The header of each point cloud contains the timestamp of the point cloud in seconds (msg->header.timestamp_sec) and nanoseconds (msg->header.timestamp_nsec). Use this timestamp as the generation time of the point cloud.