SICKAG / sick_scan

sick_scan is an open-source project to support the laser scanner of the company SICK using the ROS-framework
Apache License 2.0
130 stars 104 forks source link

fix publish frequency not appearing in diagnostics #146

Closed LKSeng closed 3 years ago

LKSeng commented 3 years ago

Currently, the /diagnostics message for the, say, TiM_240 LIDAR does not show publish frequency (e.g. Actual Frequency: 0.000000). Also addresses #143, since now data is sent through the diagnostic publisher.

header: 
  seq: 18
  stamp: 
    secs: 1634897754
    nsecs: 821772575
  frame_id: ''
status: 
  - 
    level: 2
    name: "sick_tim_240: /scan topic status"
    message: "No events recorded.; No data since last update."
    hardware_id: "none"
    values: 
      - 
        key: "Events in window"
        value: "0"
      - 
        key: "Events since startup"
        value: "0"
      - 
        key: "Duration of window (s)"
        value: "10.455575"
      - 
        key: "Actual frequency (Hz)"
        value: "0.000000"
      - 
        key: "Target frequency (Hz)"
        value: "15.000000"
      - 
        key: "Minimum acceptable frequency (Hz)"
        value: "13.500000"
      - 
        key: "Maximum acceptable frequency (Hz)"
        value: "16.500000"
      - 
        key: "Earliest timestamp delay"
        value: "No data"
      - 
        key: "Latest timestamp delay"
        value: "No data"
      - 
        key: "Earliest acceptable timestamp delay"
        value: "-1.000000"
      - 
        key: "Latest acceptable timestamp delay"
        value: "0.086667"
      - 
        key: "Late diagnostic update count"
        value: "0"
      - 
        key: "Early diagnostic update count"
        value: "0"
      - 
        key: "Zero seen diagnostic update count"
        value: "0"

But we can fix this by publishing the /scan topic via the already declared diagnosticPub_ instead of pub_. Now, the diagnosticPub_ is able to accurately track messages that are published (e.g. Actual Frequency: 14.347611):

header: 
  seq: 14
  stamp: 
    secs: 1634897944
    nsecs: 890888408
  frame_id: ''
status: 
  - 
    level: 0
    name: "sick_tim_240: /scan topic status"
    message: ''
    hardware_id: "none"
    values: 
      - 
        key: "Events in window"
        value: "150"
      - 
        key: "Events since startup"
        value: "196"
      - 
        key: "Duration of window (s)"
        value: "10.454702"
      - 
        key: "Actual frequency (Hz)"
        value: "14.347611"
      - 
        key: "Target frequency (Hz)"
        value: "15.000000"
      - 
        key: "Minimum acceptable frequency (Hz)"
        value: "13.500000"
      - 
        key: "Maximum acceptable frequency (Hz)"
        value: "16.500000"
      - 
        key: "Earliest timestamp delay"
        value: "0.046788"
      - 
        key: "Latest timestamp delay"
        value: "0.046845"
      - 
        key: "Earliest acceptable timestamp delay"
        value: "-1.000000"
      - 
        key: "Latest acceptable timestamp delay"
        value: "0.086667"
      - 
        key: "Late diagnostic update count"
        value: "0"
      - 
        key: "Early diagnostic update count"
        value: "0"
      - 
        key: "Zero seen diagnostic update count"
        value: "0"

The above results were conducted on a SICK TiM 240 connect via ethernet cable to a network switch using static IP (both laptop and TiM 240). The build was done using catkin build on noetic and on ubuntu 20.04, and launched via roslaunch sick_scan sick_tim_240.launch

rostest commented 3 years ago

Many thanks for your input! We appreciate and committed your pull request.