swri-robotics / sicktoolbox

This package contains the SICK LIDAR Matlab/C++ Toolbox, available from http://sicktoolbox.sourceforge.net/
Other
1 stars 0 forks source link

Nav350 reflector and scan data intermittently not published #4

Open mitchallain opened 6 years ago

mitchallain commented 6 years ago

Please let me know if I should move this issue to a more actively supported fork of sicktoolbox.

Hello,

We have been using the swri sicktoolbox and sicktoolbox_wrapper drivers for the NAV350 sensor for about two years now to retrieve pose data from the sensor. Recently an application has called for using the reflector and raw scan information from the sensor. However, this data appears to be available only in particular regions of the map.

If I move the sensor outside of a few regions of the map, the reflector transforms are no longer published and the scan data, while still being published, goes stale (identical msgs). If I start the driver in one of these "dark" areas (the majority of the map), I receive the following empty LaserScan msgs.

mallain@HP-EliteBook:src$ rostopic echo -n 1 /nav350/scan
header: 
  seq: 31
  stamp: 
    secs: 1536707508
    nsecs: 121521096
  frame_id: nav350
angle_min: -3.14159274101
angle_max: -3.14159274101
angle_increment: 0.0
time_increment: inf
scan_time: 0.125
range_min: 0.10000000149
range_max: 250.0
ranges: []
intensities: []
---

I suspect that this may be an issue with the driver, because I did not notice any significant change in the packet sizes (via Wireshark) between the areas where the ROS driver publishes all the information (pose, reflector TFs, and scan data) and those in which it only publishes pose.

It was suggested to me that the scanner may not publish the information when there is a shortage of visible reflectors, however in the small region where all information is present (6" radius), we are seeing all 9 reflector frames. Upon leaving the region, all 9 abruptly disappear, as seen in the node DEBUG output below.

[ INFO] [1536766269.918180376]: Getting sick range/scan measurements
[ WARN] [1536766269.918293923]: Sending transform from odom to base_link
[DEBUG] [1536766269.918353327]: NAV350 # reflectors seen:9
[DEBUG] [1536766269.918397913]: NAV350 # reflectors used:32766
[DEBUG] [1536766269.918544385]: Reflector 0 x pos: 16657 y pos: 2998
[DEBUG] [1536766269.918583344]: Reflector 1 x pos: 4436 y pos: 3377
[DEBUG] [1536766269.918608460]: Reflector 2 x pos: 4703 y pos: 9362
[DEBUG] [1536766269.918782922]: Reflector 3 x pos: -2528 y pos: 1619
[DEBUG] [1536766269.918814963]: Reflector 4 x pos: -7367 y pos: 1587
[DEBUG] [1536766269.918838821]: Reflector 5 x pos: -7475 y pos: -1821
[DEBUG] [1536766269.918861581]: Reflector 6 x pos: -2395 y pos: -3173
[DEBUG] [1536766269.918883269]: Reflector 7 x pos: 749 y pos: -3272
[DEBUG] [1536766269.918988182]: Reflector 8 x pos: 7923 y pos: -4791
[ INFO] [1536766270.054645314]: Getting sick range/scan measurements
[ WARN] [1536766270.054696515]: Sending transform from odom to base_link
[DEBUG] [1536766270.054720722]: NAV350 # reflectors seen:9
[DEBUG] [1536766270.054741052]: NAV350 # reflectors used:32766
[DEBUG] [1536766270.054768389]: Reflector 0 x pos: 16634 y pos: 2994
[DEBUG] [1536766270.054805764]: Reflector 1 x pos: 4431 y pos: 3372
[DEBUG] [1536766270.054828921]: Reflector 2 x pos: 4705 y pos: 9367
[DEBUG] [1536766270.054851134]: Reflector 3 x pos: -2524 y pos: 1616
[DEBUG] [1536766270.054911682]: Reflector 4 x pos: -7381 y pos: 1590
[DEBUG] [1536766270.054949992]: Reflector 5 x pos: -7478 y pos: -1821
[DEBUG] [1536766270.054993670]: Reflector 6 x pos: -2398 y pos: -3176
[DEBUG] [1536766270.055037868]: Reflector 7 x pos: 748 y pos: -3273
[DEBUG] [1536766270.055062522]: Reflector 8 x pos: 7898 y pos: -4776
[ INFO] [1536766270.190424808]: Getting sick range/scan measurements
[ WARN] [1536766270.190715967]: Sending transform from odom to base_link
[DEBUG] [1536766270.190890751]: NAV350 # reflectors seen:9
[DEBUG] [1536766270.191026297]: NAV350 # reflectors used:32766
[DEBUG] [1536766270.191195154]: Reflector 0 x pos: 16625 y pos: 2993
[DEBUG] [1536766270.191335761]: Reflector 1 x pos: 4433 y pos: 3376
[DEBUG] [1536766270.191471780]: Reflector 2 x pos: 4700 y pos: 9359
[DEBUG] [1536766270.191587828]: Reflector 3 x pos: -2533 y pos: 1621
[DEBUG] [1536766270.191694312]: Reflector 4 x pos: -7374 y pos: 1588
[DEBUG] [1536766270.191799876]: Reflector 5 x pos: -7481 y pos: -1822
[DEBUG] [1536766270.191899414]: Reflector 6 x pos: -2406 y pos: -3185
[DEBUG] [1536766270.192002324]: Reflector 7 x pos: 745 y pos: -3274
[DEBUG] [1536766270.192102506]: Reflector 8 x pos: 7907 y pos: -4784
[ INFO] [1536766270.294674436]: Getting sick range/scan measurements
[ WARN] [1536766270.294731567]: Sending transform from odom to base_link
[DEBUG] [1536766270.294758445]: NAV350 # reflectors seen:0
[DEBUG] [1536766270.294781800]: NAV350 # reflectors used:32766
[DEBUG] [1536766270.294810273]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.294842620]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.294866684]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.294923442]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.294971499]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.294997359]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.295017263]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.295037955]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.295073980]: Publisher on '/tf' deregistering callbacks.
[ INFO] [1536766270.416868122]: Getting sick range/scan measurements
[ WARN] [1536766270.416932437]: Sending transform from odom to base_link
[DEBUG] [1536766270.416965030]: NAV350 # reflectors seen:0
[DEBUG] [1536766270.417060000]: NAV350 # reflectors used:32766
[ INFO] [1536766270.540683335]: Getting sick range/scan measurements
[ WARN] [1536766270.540798482]: Sending transform from odom to base_link
[DEBUG] [1536766270.540863482]: NAV350 # reflectors seen:0
[DEBUG] [1536766270.540921341]: NAV350 # reflectors used:32766
[ INFO] [1536766270.662572291]: Getting sick range/scan measurements
[ WARN] [1536766270.662620722]: Sending transform from odom to base_link
[DEBUG] [1536766270.662645628]: NAV350 # reflectors seen:0
[DEBUG] [1536766270.662662397]: NAV350 # reflectors used:32766
[ INFO] [1536766270.818958024]: Getting sick range/scan measurements
[ WARN] [1536766270.819025341]: Sending transform from odom to base_link
[DEBUG] [1536766270.819051037]: NAV350 # reflectors seen:9
[DEBUG] [1536766270.819072146]: NAV350 # reflectors used:32766
[DEBUG] [1536766270.819100839]: Reflector 0 x pos: 16607 y pos: 2994
[DEBUG] [1536766270.819122359]: Reflector 1 x pos: 4406 y pos: 3374
[DEBUG] [1536766270.819151767]: Reflector 2 x pos: 4671 y pos: 9360
[DEBUG] [1536766270.819171241]: Reflector 3 x pos: -2557 y pos: 1620
[DEBUG] [1536766270.819193244]: Reflector 4 x pos: -7403 y pos: 1591
[DEBUG] [1536766270.819213534]: Reflector 5 x pos: -7509 y pos: -1816
[DEBUG] [1536766270.819235872]: Reflector 6 x pos: -2430 y pos: -3178
[DEBUG] [1536766270.819255538]: Reflector 7 x pos: 718 y pos: -3279
[DEBUG] [1536766270.819278494]: Reflector 8 x pos: 7877 y pos: -4787
[ INFO] [1536766270.913199404]: Getting sick range/scan measurements
[ WARN] [1536766270.913257246]: Sending transform from odom to base_link
[DEBUG] [1536766270.913286336]: NAV350 # reflectors seen:0
[DEBUG] [1536766270.913306802]: NAV350 # reflectors used:32766
[DEBUG] [1536766270.913331279]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913363808]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913386631]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913411888]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913436493]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913470769]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913523278]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913543440]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1536766270.913562182]: Publisher on '/tf' deregistering callbacks.
[ INFO] [1536766271.033683268]: Getting sick range/scan measurements
[ WARN] [1536766271.033733926]: Sending transform from odom to base_link
[DEBUG] [1536766271.033768441]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.033870163]: NAV350 # reflectors used:32766
[ INFO] [1536766271.161088403]: Getting sick range/scan measurements
[ WARN] [1536766271.161133558]: Sending transform from odom to base_link
[DEBUG] [1536766271.161146195]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.161161150]: NAV350 # reflectors used:32766
[ INFO] [1536766271.306872729]: Getting sick range/scan measurements
[ WARN] [1536766271.306950314]: Sending transform from odom to base_link
[DEBUG] [1536766271.306976387]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.306997006]: NAV350 # reflectors used:32766
[ INFO] [1536766271.410798262]: Getting sick range/scan measurements
[ WARN] [1536766271.410945424]: Sending transform from odom to base_link
[DEBUG] [1536766271.411021209]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.411186044]: NAV350 # reflectors used:32766
[ INFO] [1536766271.547282289]: Getting sick range/scan measurements
[ WARN] [1536766271.547374595]: Sending transform from odom to base_link
[DEBUG] [1536766271.547405457]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.547425418]: NAV350 # reflectors used:32766
[ INFO] [1536766271.677672983]: Getting sick range/scan measurements
[ WARN] [1536766271.677724821]: Sending transform from odom to base_link
[DEBUG] [1536766271.677743662]: NAV350 # reflectors seen:0
[DEBUG] [1536766271.677870782]: NAV350 # reflectors used:32766

sicktoolbox: euler_integration sicktoolbox_wrapper: indigo-devel

Nodes running:

mallain@HP-EliteBook:sicktoolbox_euler$ rosnode list
/robot_state_publisher
/rosout
/rviz_1536765964599739396
/sicknav350

Running ROS Indigo

mallain@HP-EliteBook:sicktoolbox_euler$ uname -a
Linux HP-EliteBook 4.4.0-130-generic #156~14.04.1-Ubuntu SMP Thu Jun 14 13:51:47 UTC 2018 x86_64 x86_64 x86_64 GNU/Linux
mitchallain commented 6 years ago

This appears to have been due to a reflector that was too far away (est. 10 m). Masking that reflector resolved the issues. I will continue to seek whether this is a sensor or driver issue. SICK technical support seemed unaware of any similar issues.

jrgnicho commented 6 years ago

@MitchAllain thanks for reporting this. I haven't seen this issue before but It seems to me like a hardware problem based on what you described.

mitchallain commented 6 years ago

I can provide a link to a rosbag if there is interest.

mitchallain commented 5 years ago

@jrgnicho What leads you to believe that this may be a hardware issue? After comparing the packets over wireshark while experiencing the issue to the packets when the sensor and driver work normally, there doesn't seem to be any difference in packet length or structure.

I am leaning toward this being a driver issue but I am unable to find any leads in the driver source code. Even after masking the farthest reflector (12 days ago), I occasionally will experience the issue again with a different reflector (always the farthest visible reflector). Masking that reflector returns the NAV350 + driver to its expected output.