From 9478425e8362ff94ba71a52eb8bea3bdd1e31adc Mon Sep 17 00:00:00 2001 From: "EU\\jhander" Date: Mon, 11 Nov 2024 07:42:01 +0100 Subject: [PATCH] - readme extended for topics and separated/new file docs/topics.md added - pf_description and pf_driver -> 2.0.0 --- README.md | 3 + docs/services.md | 33 +++++++++ docs/topics.md | 117 +++++++++++++++++++++++++++++++ src/pf_description/CHANGELOG.rst | 3 + src/pf_description/package.xml | 2 +- src/pf_driver/CHANGELOG.rst | 5 ++ src/pf_driver/package.xml | 2 +- 7 files changed, 163 insertions(+), 2 deletions(-) create mode 100644 docs/topics.md diff --git a/README.md b/README.md index cd93b8f..5312a66 100644 --- a/README.md +++ b/README.md @@ -62,6 +62,9 @@ turn. This is not strictly from bottom to top: |2 |+4.5° | top | |3 |+1.5° | - | +**Topics:** +The R2000 and R2300 devices each publish two topics. See the topics documentation [topics.md](./docs/topics.md) for more details. + **Services:** The ROS driver offers several ROS services which can be used to communicate with the sensor. Especially services for sensor parametrization are available. For example to list, get and set parameters. See the diff --git a/docs/services.md b/docs/services.md index 5cb6198..7452064 100644 --- a/docs/services.md +++ b/docs/services.md @@ -71,3 +71,36 @@ and reboots the device. ``` ros2 service call /pf_r2000/pfsdp_factory_reset pf_interfaces/srv/PfsdpFactoryReset ``` + +## Service type and interface: +The service type determines which data is used for the request and response of a service. The following command is an +example for determining the type of the pfsdp_set_parameter service: +``` +ros2 service type /pf_r2000/pfsdp_set_parameter +``` +The return should be the following. Which is also the path to the file where the request and response data is described in detail: +``` +pf_interfaces/srv/PfsdpSetParameter +``` +To show the request and response interface/data for the pfsdp_set_parameter service the following command can be used: +``` +ros2 interface show pf_interfaces/srv/PfsdpSetParameter +``` +The return should be the following and shows the data name and type of the request above and of the response below the three dashes. +``` +string name +string value +- - - +int32 error_code +string error_text +``` + +## Service execution status +All previously described services return at least the error code (error_code) and error text (error_text) in the response. +Both contain the status of the service execution. Error codes (data type int32) not equal to 0 indicate an error during service +execution. Error text (data type string) shows the status of the service execution as a description readable by humans. +The following response of the service pfsdp_set_parameter shows the error code, error text and as first return value +the read parameter user_tag. +``` +pf_interfaces.srv.PfsdpGetParameter_Response(value='OMD10M-R2000', error_code=0, error_text='success') +``` diff --git a/docs/topics.md b/docs/topics.md new file mode 100644 index 0000000..5cd5607 --- /dev/null +++ b/docs/topics.md @@ -0,0 +1,117 @@ +## ROS Topics + +**R2000 & R2300 1-layer:** + +| Topic | Type | Description | +| ------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------- | +| /pf/scan | LaserScan | ROS standard message, see [sensor_msgs/LaserScan](https://docs.ros.org/en/jazzy/p/sensor_msgs/interfaces/msg/LaserScan.html) | +| /r2000_header | PFR2000Header | Sensor specific message, see [pf_interfaces/msg/PFR2000Header.msg] (../src/pf_interfaces/msg/PFR2000Header.msg) | + +Topic PFR2000Header details: + +| Type | Name | Description | +| ------ | ----------------- | ------------------------------------------------------------------------------------------------------------------------- | +| uint16 | magic | magic byte (0xa25c) marking the beginning of a packet | +| uint16 | packet_type | scan data packet type (0x0041 - 'A' 0x0042 - 'B' 0x0043 - 'C') | +| uint32 | packet_size | overall packet size in bytes (header + payload) | +| uint16 | header_size | header size in bytes (offset to payload data) | +| uint16 | scan_number | sequence number for scan (incremented for every scan, starting with 0, overflows) | +| uint16 | packet_number | sequence number for packet (counting packets of a particular scan, starting with 1) | +| | | | +| uint64 | timestamp_raw | raw timestamp of first scan point in this packet in NTP time format | +| uint64 | timestamp_sync | synchronized timestamp of first scan point in this packet in NTP time format (currenty not available and and set to zero) | +| uint32 | status_flags | scan status flags | +| uint32 | scan_frequency | frequency of head rotation (1/1000Hz) | +| uint16 | num_points_scan | number of scan points (samples) within complete scan | +| uint16 | num_points_packet | total number of scan points within this packet | +| uint16 | first_index | index of first scan point within this packet | +| int32 | first_angle | absolute angle of first scan point within this packet (1/10000°) | +| int32 | angular_increment | delta between two succeding scan points (1/10000°) CCW rotation: +ve, CW rotation: -ve | +| uint32 | iq_input | reserved - all bits zero for devices without switching I/Q | +| uint32 | iq_overload | reserved - all bits zero for devices without switching I/Q | +| uint64 | iq_timestamp_raw | raw timestamp for status of switching I/Q | +| uint64 | iq_timestamp_sync | synchronized timestamp for status of switching I/Q | + +status_flags details: + +| Bit | Flag name | Description | +| --------------- |--------------------------- | --------------------------------------------------------------------------------- | +| *Informational* | | | +| 0 | scan_data_info | Accumulative flag – set if any informational flag (bits 1..7) is set | +| 1 | new_settings | System settings for scan data acquisition changed during recording of this packet | +| 2 | invalid_data | Consistency of scan data is not guaranteed for this packet | +| 3 | unstable_rotation | Scan frequency did not match set value while recording this scan data packet | +| 4 | skipped_packets | Preceding scan data packets have been skipped due to connection issues | +| *Warnings* | | | +| 8 | device_warning | Accumulative flag – set if any warning flag (bits 9..15) is set | +| 9 | lens_contamination_warning | LCM warning threshold triggered for at least one sector | +| 10 | low_temperature_warning | Current internal temperature below warning threshold | +| 11 | high_temperature_warning | Current internal temperature above warning threshold | +| 12 | device_overload | Overload warning – sensor CPU overload is imminent | +| *Errors* | | | +| 16 | device_error | Accumulative flag – set if any error flag (bits 17..23) is set | +| 17 | lens_contamination_error | LCM error threshold triggered for at least one sector | +| 18 | low_temperature_error | Current internal temperature below error threshold | +| 19 | high_temperature_error | Current internal temperature above error threshold | +| 20 | device_overload | Overload error – sensor CPU is in overload state | +| *Defects* | | | +| 30 | device_defect | Accumulative flag – set if device detected an unrecoverable defect | + +**R2300 4-layer:** + +| Topic | Type | Description | +| ------------- |-------------- | -------------------------------------------------------------------------------------------------------------------------------- | +| /pf/cloud | PointCloud2 | ROS standard message, see [sensor_msgs/PointCloud2](https://docs.ros.org/en/jazzy/p/sensor_msgs/interfaces/msg/PointCloud2.html) | +| /r2300_header | PFR2300Header | Sensor specific message, see [pf_interfaces/msg/PFR2300Header.msg] (../src/pf_interfaces/msg/PFR2300Header.msg) | + +Topic PFR2300Header details: + +| Type | Name | Description | +| ------ | ----------------- | -------------------------------------------------------------------------------------- | +| uint16 | magic | magic byte (0xa25c) marking the beginning of a packet | +| uint16 | packet_type | scan data packet type (0x0041 - 'A' 0x0042 - 'B' 0x0043 - 'C') | +| uint32 | packet_size | overall packet size in bytes (header + payload) | +| uint16 | header_size | header size in bytes (offset to payload data) | +| uint16 | scan_number | sequence number for scan (incremented for every scan, starting with 0, overflows) | +| uint16 | packet_number | sequence number for packet (counting packets of a particular scan, starting with 1) | +| | | | +| uint16 | layer_index | vertical layer index (0..3) | +| int32 | layer_inclination | vertical layer inclination [1/10000 degree] | +| uint64 | timestamp_raw | raw timestamp of first scan point in this packet in NTP time format | +| uint64 | reserved1 | reserved - all bits zero for devices without switching I/Q | +| uint32 | status_flags | scan status flags | +| uint32 | scan_frequency | frequency of head rotation (1/1000Hz) | +| uint16 | num_points_scan | number of scan points (samples) within complete scan | +| uint16 | num_points_packet | total number of scan points within this packet | +| uint16 | first_index | index of first scan point within this packet | +| int32 | first_angle | absolute angle of first scan point within this packet (1/10000°) | +| int32 | angular_increment | delta between two succeding scan points (1/10000°) CCW rotation: +ve, CW rotation: -ve | + +status_flags details: + +| Bit | Flag name | Description | +| --------------- |--------------------------- | --------------------------------------------------------------------------------- | +| *Informational* | | | +| 0 | scan_data_info | Accumulative flag – set if any informational flag (bits 1..7) is set | +| 1 | new_settings | System settings for scan data acquisition changed during recording of this packet | +| 2 | invalid_data | Consistency of scan data is not guaranteed for this packet | +| 3 | unstable_rotation | Scan frequency did not match set value while recording this scan data packet | +| 4 | skipped_packets | Preceding scan data packets have been skipped due to connection issues | +| *Warnings* | | | +| 8 | device_warning | Accumulative flag – set if any warning flag (bits 9..15) is set | +| 10 | low_temperature_warning | Current internal temperature below warning threshold | +| 11 | high_temperature_warning | Current internal temperature above warning threshold | +| 12 | device_overload | Overload warning – sensor CPU overload is imminent | +| *Errors* | | | +| 16 | device_error | Accumulative flag – set if any error flag (bits 17..23) is set | +| 18 | low_temperature_error | Current internal temperature below error threshold | +| 19 | high_temperature_error | Current internal temperature above error threshold | +| 20 | device_overload | Overload error – sensor CPU is in overload state | +| *Defects* | | | +| 30 | device_defect | Accumulative flag – set if device detected an unrecoverable defect | + + + + + + diff --git a/src/pf_description/CHANGELOG.rst b/src/pf_description/CHANGELOG.rst index 32d2ef2..fa5d00b 100644 --- a/src/pf_description/CHANGELOG.rst +++ b/src/pf_description/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pf_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.0.0 (2024-11-xx) +------------------ + 1.3.0 (2024-09-17) ------------------- diff --git a/src/pf_description/package.xml b/src/pf_description/package.xml index d689a0f..3dbfeba 100644 --- a/src/pf_description/package.xml +++ b/src/pf_description/package.xml @@ -1,7 +1,7 @@ pf_description - 1.3.0 + 2.0.0 The pf_description package Harsh Deshpande diff --git a/src/pf_driver/CHANGELOG.rst b/src/pf_driver/CHANGELOG.rst index dfd7222..cb04faf 100644 --- a/src/pf_driver/CHANGELOG.rst +++ b/src/pf_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pf_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.0.0 (2024-11-xx) +------------------ +* ROS/sensor parameters replaced by ROS services (pfsdp_get_parameter & pfsdp_set_parameter) +* add more ROS services -> pfsdp_list_parameters, pfsdp_reset_parameter, pfsdp_get_protocol_info etc. + 1.3.0 (2024-09-17) ------------------- * fix driver build errors under ROS Jazzy `#12 `_ diff --git a/src/pf_driver/package.xml b/src/pf_driver/package.xml index 9810942..fce9dd5 100644 --- a/src/pf_driver/package.xml +++ b/src/pf_driver/package.xml @@ -2,7 +2,7 @@ pf_driver - 1.3.0 + 2.0.0 The Pepperl+Fuchs LiDAR package Harsh Deshpande