From a8be0daf4b2be1f7453d83c77595e393d2f829ed Mon Sep 17 00:00:00 2001 From: JnsHndr Date: Tue, 15 Oct 2024 10:16:49 +0200 Subject: [PATCH] added ROS services which replaces ROS (device) parameters -> have generic access to device parameters --- src/pf_driver/CMakeLists.txt | 1 + src/pf_driver/cfg/PFDriverR2000.cfg | 121 --------- src/pf_driver/cfg/PFDriverR2300.cfg | 71 ------ .../pf/http_helpers/http_interface.h | 9 +- .../include/pf_driver/pf/pf_interface.h | 41 +++ .../include/pf_driver/pf/pfsdp_base.h | 27 +- .../include/pf_driver/ros/pf_services.h | 53 ++++ .../src/pf/http_helpers/http_interface.cpp | 42 +--- src/pf_driver/src/pf/pf_interface.cpp | 112 +++++++++ src/pf_driver/src/pf/pfsdp_base.cpp | 235 ++++++++++++------ src/pf_driver/src/pf/r2000/pfsdp_2000.cpp | 104 +------- src/pf_driver/src/pf/r2300/pfsdp_2300.cpp | 47 +--- src/pf_driver/src/ros/pf_services.cpp | 74 ++++++ src/pf_driver/src/ros/ros_main.cpp | 38 +-- src/pf_interfaces/CMakeLists.txt | 11 + src/pf_interfaces/srv/PfsdpFactoryReset.srv | 5 + src/pf_interfaces/srv/PfsdpGetIqParameter.srv | 7 + src/pf_interfaces/srv/PfsdpGetParameter.srv | 7 + .../srv/PfsdpGetProtocolInfo.srv | 9 + .../srv/PfsdpListIqParameters.srv | 4 + src/pf_interfaces/srv/PfsdpListParameters.srv | 4 + src/pf_interfaces/srv/PfsdpRebootDevice.srv | 5 + .../srv/PfsdpResetIqParameter.srv | 6 + src/pf_interfaces/srv/PfsdpResetParameter.srv | 6 + src/pf_interfaces/srv/PfsdpSetIqParameter.srv | 7 + src/pf_interfaces/srv/PfsdpSetParameter.srv | 7 + 26 files changed, 565 insertions(+), 488 deletions(-) delete mode 100755 src/pf_driver/cfg/PFDriverR2000.cfg delete mode 100755 src/pf_driver/cfg/PFDriverR2300.cfg create mode 100644 src/pf_driver/include/pf_driver/ros/pf_services.h create mode 100644 src/pf_driver/src/ros/pf_services.cpp create mode 100644 src/pf_interfaces/srv/PfsdpFactoryReset.srv create mode 100644 src/pf_interfaces/srv/PfsdpGetIqParameter.srv create mode 100644 src/pf_interfaces/srv/PfsdpGetParameter.srv create mode 100644 src/pf_interfaces/srv/PfsdpGetProtocolInfo.srv create mode 100644 src/pf_interfaces/srv/PfsdpListIqParameters.srv create mode 100644 src/pf_interfaces/srv/PfsdpListParameters.srv create mode 100644 src/pf_interfaces/srv/PfsdpRebootDevice.srv create mode 100644 src/pf_interfaces/srv/PfsdpResetIqParameter.srv create mode 100644 src/pf_interfaces/srv/PfsdpResetParameter.srv create mode 100644 src/pf_interfaces/srv/PfsdpSetIqParameter.srv create mode 100644 src/pf_interfaces/srv/PfsdpSetParameter.srv diff --git a/src/pf_driver/CMakeLists.txt b/src/pf_driver/CMakeLists.txt index 18588ce..83317bd 100755 --- a/src/pf_driver/CMakeLists.txt +++ b/src/pf_driver/CMakeLists.txt @@ -53,6 +53,7 @@ set(${PROJECT_NAME}_SOURCES src/ros/laser_scan_publisher.cpp src/ros/point_cloud_publisher.cpp src/ros/pf_data_publisher.cpp + src/ros/pf_services.cpp ) add_executable(ros_main diff --git a/src/pf_driver/cfg/PFDriverR2000.cfg b/src/pf_driver/cfg/PFDriverR2000.cfg deleted file mode 100755 index 6835203..0000000 --- a/src/pf_driver/cfg/PFDriverR2000.cfg +++ /dev/null @@ -1,121 +0,0 @@ -#!/usr/bin/env python -# coding=utf-8 - -PACKAGE = "pf_driver" - -from dynamic_reconfigure.parameter_generator_catkin import * - -#----------R2000 Parameters------------------------------------------------------ -gen_r2000 = ParameterGenerator() - -ip_mode_enum = gen_r2000.enum([ gen_r2000.const("static", int_t, 0, "Static IP mode"), - gen_r2000.const("dhcp", int_t, 1, "DHCP IP mode"), - gen_r2000.const("autoip", int_t, 2, "AutoIP mode")], - "An enum to set IP mode") -gen_r2000.add("ip_mode", int_t, 1, "ip_mode value determines the method used by the device to determine its own IP and network configuration.", 2, edit_method=ip_mode_enum) -gen_r2000.add("ip_address", str_t, 2, "IP address", "10.0.10.9") -gen_r2000.add("subnet_mask", str_t, 3, "IP netmask", "255.0.0.0") -gen_r2000.add("gateway", str_t, 4, "IP gateway", "0.0.0.0") -gen_r2000.add("scan_frequency", int_t, 5, "The parameter scan_frequency defines the set point for the rotational speed of the sensor head and therefore the number of scans recorded per second. For the R2000 valid values range from 10 Hz to 50 Hz with steps of 1 Hz.", 35, 10, 50) - -scan_direction_enum = gen_r2000.enum([ gen_r2000.const("cw", str_t, "cw", "Clock-wise"), - gen_r2000.const("ccw", str_t, "ccw", "Counter clock-wise")], - "An enum to set scan direction") -gen_r2000.add("scan_direction", str_t, 6, "The parameter scan_direction defines the direction of rotation of the sensors head. User applications can choose between clockwise rotation (cw) or counter-clockwise rotation (ccw).", "ccw", edit_method=scan_direction_enum) - -samples_per_scan_enum = gen_r2000.enum([ gen_r2000.const("samples_per_scan_72", int_t, 72, ""), - gen_r2000.const("samples_per_scan_90", int_t, 90, ""), - gen_r2000.const("samples_per_scan_120", int_t, 120, ""), - gen_r2000.const("samples_per_scan_144", int_t, 144, ""), - gen_r2000.const("samples_per_scan_180", int_t, 180, ""), - gen_r2000.const("samples_per_scan_240", int_t, 240, ""), - gen_r2000.const("samples_per_scan_360", int_t, 360, ""), - gen_r2000.const("samples_per_scan_400", int_t, 400, ""), - gen_r2000.const("samples_per_scan_480", int_t, 480, ""), - gen_r2000.const("samples_per_scan_600", int_t, 600, ""), - gen_r2000.const("samples_per_scan_720", int_t, 720, ""), - gen_r2000.const("samples_per_scan_800", int_t, 800, ""), - gen_r2000.const("samples_per_scan_900", int_t, 900, ""), - gen_r2000.const("samples_per_scan_1200", int_t, 1200, ""), - gen_r2000.const("samples_per_scan_1440", int_t, 1440, ""), - gen_r2000.const("samples_per_scan_1680", int_t, 1680, ""), - gen_r2000.const("samples_per_scan_1800", int_t, 1800, ""), - gen_r2000.const("samples_per_scan_2100", int_t, 2100, ""), - gen_r2000.const("samples_per_scan_2400", int_t, 2400, ""), - gen_r2000.const("samples_per_scan_2520", int_t, 2520, ""), - gen_r2000.const("samples_per_scan_2800", int_t, 2800, ""), - gen_r2000.const("samples_per_scan_3150", int_t, 3150, ""), - gen_r2000.const("samples_per_scan_3600", int_t, 3600, ""), - gen_r2000.const("samples_per_scan_4200", int_t, 4200, ""), - gen_r2000.const("samples_per_scan_5040", int_t, 5040, ""), - gen_r2000.const("samples_per_scan_5600", int_t, 5600, ""), - gen_r2000.const("samples_per_scan_6300", int_t, 6300, ""), - gen_r2000.const("samples_per_scan_7200", int_t, 7200, ""), - gen_r2000.const("samples_per_scan_8400", int_t, 8400, ""), - gen_r2000.const("samples_per_scan_10800", int_t, 10080, ""), - gen_r2000.const("samples_per_scan_12600", int_t, 12600, ""), - gen_r2000.const("samples_per_scan_16800", int_t, 16800, ""), - gen_r2000.const("samples_per_scan_25200", int_t, 25200, "")], - "An enum to set samples per scan") - -gen_r2000.add("samples_per_scan", int_t, 7, "The parameter samples_per_scan defines the number of samples recorded during one revolution of the sensor head (for details please refer to section 3.1 in the R2000 Ethernet communication protocol).", 3600, 72, 25200) - -hmi_display_mode_enum = gen_r2000.enum([ gen_r2000.const("hmi_display_off", str_t, "off", ""), - gen_r2000.const("static_logo", str_t, "static_logo", ""), - gen_r2000.const("static_text", str_t, "static_text", ""), - gen_r2000.const("bargraph_distance", str_t, "bargraph_distance", ""), - gen_r2000.const("bargraph_echo", str_t, "bargraph_echo", ""), - gen_r2000.const("bargraph_reflector", str_t, "bargraph_reflector", ""), - gen_r2000.const("application_bitmap", str_t, "application_bitmap", ""), - gen_r2000.const("application_text", str_t, "application_text", "")], - "An enum to set hmi display mode") -gen_r2000.add("hmi_display_mode", str_t, 8, "The parameter hmi_display_mode controls the content of the HMI LED display during normal operation, i.e. while neither warnings nor errors are displayed and the user did not activate the HMI menu.", "static_logo", edit_method=hmi_display_mode_enum) - -hmi_language_enum = gen_r2000.enum([ gen_r2000.const("english", str_t, "english", ""), - gen_r2000.const("german", str_t, "german", "")], - "An enum to set hmi language") -gen_r2000.add("hmi_language", str_t, 9, "change display language", "english", edit_method=hmi_language_enum) - -hmi_button_lock_enum = gen_r2000.enum([ gen_r2000.const("hmi_button_lock_on", str_t, "on", "hmi button lock on"), - gen_r2000.const("hmi_button_lock_off", str_t, "off", "hmi button lock off")], - "An enum to set hmi button lock") -gen_r2000.add("hmi_button_lock", str_t, 10, "lock HMI buttons", "off", edit_method=hmi_button_lock_enum) - -hmi_parameter_lock_enum = gen_r2000.enum([ gen_r2000.const("hmi_parameter_lock_on", str_t, "on", "hmi parameter lock on"), - gen_r2000.const("hmi_parameter_lock_off", str_t, "off", "hmi parameter lock off")], - "An enum to set hmi parameter lock") -gen_r2000.add("hmi_parameter_lock", str_t, 11, "set HMI to read-only", "off", edit_method=hmi_parameter_lock_enum) -gen_r2000.add("hmi_static_text_1", str_t, 12, "text line 1 for mode static_text (max. 30 chars)", "Pepperl+Fuchs") -gen_r2000.add("hmi_static_text_2", str_t, 13, "text line 2 for mode static_text (max. 30 chars)", "R2000") -gen_r2000.add("hmi_application_bitmap", str_t, 27, "Bitmap graphic encoded as base64url string that is shown in the application_bitmap display mode. See the operation manual for more information.") - -locator_indicator_enum = gen_r2000.enum([ gen_r2000.const("locator_indicator_on", str_t, "on", "locator indicator on"), - gen_r2000.const("locator_indicator_off", str_t, "off", "locator indicator off")], - "An enum to set locator indicator") -gen_r2000.add("locator_indication", str_t, 14, "LED locator indication", "off", edit_method=locator_indicator_enum) - -operating_mode_enum = gen_r2000.enum([ gen_r2000.const("measure", str_t, "measure", "measure mode"), - gen_r2000.const("emitter_off", str_t, "emitter_off", "emitter_off mode")], - "An enum to set operating mode") -gen_r2000.add("operating_mode", str_t, 15, "The operating_mode, always measure after power on, can be temporarily set to emitter_off to disable the laser pulse, e.g. in order to avoid affecting other scanners.", "measure", edit_method=operating_mode_enum) -gen_r2000.add("address", str_t, 16, "When initiating scan data output, request_handle_udp must be given an IPv4 address and port in order to know where to send scandata to.") -gen_r2000.add("port", str_t, 17, "See address") - -packet_type_enum = gen_r2000.enum([ gen_r2000.const("packet_type_A", str_t, "A", "packet type A"), - gen_r2000.const("packet_type_B", str_t, "B", "packet type B"), - gen_r2000.const("packet_type_C", str_t, "C", "packettype C")], - "An enum to set packet type") -gen_r2000.add("packet_type", str_t, 18, "Packet type for scan data output", "A", edit_method=packet_type_enum) -gen_r2000.add("packet_crc", str_t, 19, "Append extra CRC32 for scan data integrity check") -watchdog_enum = gen_r2000.enum([ gen_r2000.const("watchdog_on", str_t, "on", "watchdog on"), - gen_r2000.const("watchdog_off", str_t, "off", "watchdog off")], - "An enum to set operating mode") -gen_r2000.add("watchdog", str_t, 20, "Cease scan data output if watchdog isn't fed in time", "off", edit_method=watchdog_enum) -gen_r2000.add("watchdogtimeout", int_t, 21, "Maximum time for client to feed watchdog", 60000) -gen_r2000.add("start_angle", double_t, 22, "angle of first scan point for scan data output", -1800000) -gen_r2000.add("max_num_points_scan", int_t, 23, "limit number of points in scan data output", 0) -gen_r2000.add("skip_scans", int_t, 24, "reduce scan output rate to every (n+1)th scan", 0) -gen_r2000.add("user_tag", str_t, 25, "User defined name (max. 32 chars)") -gen_r2000.add("user_notes", str_t, 26, "User notes (max. 1000 Byte)") - -exit(gen_r2000.generate(PACKAGE, "r2000_node", "PFDriverR2000")) diff --git a/src/pf_driver/cfg/PFDriverR2300.cfg b/src/pf_driver/cfg/PFDriverR2300.cfg deleted file mode 100755 index 51c9994..0000000 --- a/src/pf_driver/cfg/PFDriverR2300.cfg +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python -# coding=utf-8 - -PACKAGE = "pf_driver" - -from dynamic_reconfigure.parameter_generator_catkin import * - -#----------R2300 Parameters------------------------------------------------------ -gen_r2300 = ParameterGenerator() - -ip_mode_enum = gen_r2300.enum([ gen_r2300.const("static", str_t, "static", "Static IP mode"), - gen_r2300.const("dhcp", str_t, "dhcp", "DHCP IP mode"), - gen_r2300.const("autoip", str_t, "autoip", "AutoIP mode")], - "An enum to set IP mode") -gen_r2300.add("ip_mode", str_t, 1, "ip_mode value determines the method used by the device to determine its own IP and network configuration.", "autoip", edit_method=ip_mode_enum) -gen_r2300.add("ip_address", str_t, 2, "IP address", "10.0.10.76") -gen_r2300.add("subnet_mask", str_t, 3, "IP netmask", "255.0.0.0") -gen_r2300.add("gateway", str_t, 4, "IP gateway", "0.0.0.0") -gen_r2300.add("user_tag", str_t, 5, "Short UTF8 string for user purposes, up to 32 bytes not chars! (32 chars on R2000)", "OMD10M-R2300") - -layer_enum = gen_r2300.enum([ gen_r2300.const("on_on_on_on", str_t, "on,on,on,on", ""), - gen_r2300.const("on_on_on_off", str_t, "on,on,on,off", ""), - gen_r2300.const("on_on_off_on", str_t, "on,on,off,on", ""), - gen_r2300.const("on_on_off_off", str_t, "on,on,off,off", ""), - gen_r2300.const("on_off_on_on", str_t, "on,off,on,on", ""), - gen_r2300.const("on_off_on_off", str_t, "on,off,on,off", ""), - gen_r2300.const("on_off_off_on", str_t, "on,off,off,on", ""), - gen_r2300.const("on_off_off_off", str_t, "on,off,off,off", ""), - gen_r2300.const("off_on_on_on", str_t, "off,on,on,on", ""), - gen_r2300.const("off_on_on_off", str_t, "off,on,on,off", ""), - gen_r2300.const("off_on_off_on", str_t, "off,on,off,on", ""), - gen_r2300.const("off_on_off_off", str_t, "off,on,off,off", ""), - gen_r2300.const("off_off_on_on", str_t, "off,off,on,on", ""), - gen_r2300.const("off_off_on_off", str_t, "off,off,on,off", ""), - gen_r2300.const("off_off_off_on", str_t, "off,off,off,on", ""), - gen_r2300.const("off_off_off_off", str_t, "off,off,off,off", "")], - "An enum to set layers") -gen_r2300.add("layer_enable", str_t, 6, "An on or off value for each of the layers indexed 0..3 determines whether measurement takes place in that layer. When setting, specify the values as comma-separated list.", "on,on,on,on", edit_method=layer_enum) -gen_r2300.add("scan_frequency", int_t, 7, "The scan_frequency determines the number of scans taken per second. A scan means a contiguous set of measurements taken within range measure_start_angle to measure_stop_angle. Changing the scan_frequency to either 50 or 100 Hz as a side effect also determines the angular resolution: 0.1° at 50 Hz or 0.2° at 100 Hz.", 100) -gen_r2300.add("scan_direction", str_t, 8, "The scan_direction may allow to change the direction of scanning and therefore also the order of the measurements (direction of first measurement) in time. Currently, the setting is fixed to 'ccw' (counter-clockwise, mathematically positive), and 'cw' (clockwise) is not really supported.", "ccw") -gen_r2300.add("measure_start_angle", double_t, 9, "measure_start_angle (unit: 1/10000 degree) limits the scanning range at one end, while measure_stop_angle determines the other end limit. Scans consist of all samples exactly at and between those two limits, taken at angles rounded to multiples of the current angular resolution as determined by scan_frequency. This is a persistent setting, kept in persistent memory. For dynamic selection of measurement range without wearing out parameter memory, it is recommended to utilize the Scan output options start_angle and max_num_points_scan instead.", -500000, -500000, 500000) -gen_r2300.add("measure_stop_angle", double_t, 10, "See measure_start_angle for description", 500000, -500000, 500000) - -locator_indicator_enum = gen_r2300.enum([ gen_r2300.const("locator_on", str_t, "on", ""), - gen_r2300.const("locator_off", str_t, "off", "")], - "An enum to set locator indicator") -gen_r2300.add("locator_indication", str_t, 11, "Enable locator indication (on/off)", "off", edit_method=locator_indicator_enum) - -pilot_laser_enum = gen_r2300.enum([ gen_r2300.const("pilot_on", str_t, "on", ""), - gen_r2300.const("pilot_off", str_t, "off", "")], - "An enum to set pilot laser") -gen_r2300.add("pilot_laser", str_t, 12, "General on or off switch for the red pilot laser. Its actual visibility (in current scanners) is further limited to the range in which measurement takes place, i.e. from measure_start_angle to measure_stop_angle, and also depends on the value of layer_enable.", "off", edit_method=pilot_laser_enum) -gen_r2300.add("pilot_start_angle", double_t, 13, "Angle where to enable red pilot laser", -500000, -500000, 500000) -gen_r2300.add("pilot_stop_angle", double_t, 14, "Angle where to disable red pilot laser", 500000, -500000, 500000) - -operating_mode_enum = gen_r2300.enum([ gen_r2300.const("measure", str_t, "measure", "measure mode"), - gen_r2300.const("emitter_off", str_t, "emitter_off", "emitter_off mode")], - "An enum to set operating mode") -gen_r2300.add("operating_mode", str_t, 15, "The operating_mode, always measure after power on, can be temporarily set to emitter_off to disable the laser pulse, e.g. in order to avoid affecting other scanners.", "measure", edit_method=operating_mode_enum) -gen_r2300.add("address", str_t, 16, "When initiating scan data output, request_handle_udp must be given an IPv4 address and port in order to know where to send scandata to. The IPv4 typically should be the IP address of the host who also initiates the connection, e.g. 10.0.10.10, the port something like 6464.") -gen_r2300.add("port", str_t, 17, "See address") -gen_r2300.add("packet_type", str_t, 18, "Packet type for scan data output (always C1 on R2300)", "C1") -gen_r2300.add("packet_crc", str_t, 19, "Append extra CRC32 for scan data integrity check (currenty always none on R2300)") -gen_r2300.add("watchdog", str_t, 20, "Cease scan data output if watchdog isn't fed in time (always off on R2300)", "off") -gen_r2300.add("watchdogtimeout", int_t, 21, "Maximum time for client to feed watchdog", 0) -gen_r2300.add("start_angle", double_t, 22, "Specifying a start_angle (in 1/10000 degree) allows to reduce the output of data. ∗ While device global settings measure_start_angle and measure_stop_angle determine the range where actual measurements (and laser pulses) take place, start_angle may be used to tell the scanner to include only a subset of the data (starting at or after start_angle) in the scan output. This is provided for compatibility with R2000, where more than one consumer might want different portions of the data, and so you don't have to dynamically update measure_start_angle continuously just to get another range output. The default is -1800000, which means all data.", -500000, -500000) -gen_r2300.add("max_num_points_scan", int_t, 23, "Specifying a max_num_points_scan (in 1/10000 degree) allows to reduce the output of data to the given number of measurements (or less if there are less in actual scan range). The default is 0, meaning all data. Note that the same number of measurements may cover double (or half) the angular range dependent on device global setting scan_frequency and thus angular resolution.", 0, 0) -gen_r2300.add("skip_scans", int_t, 24, "Omit this number of scans (x layers) between output (always 0)", 0, 0) - -gen_r2300.generate(PACKAGE, "r2300_node", "PFDriverR2300") - diff --git a/src/pf_driver/include/pf_driver/pf/http_helpers/http_interface.h b/src/pf_driver/include/pf_driver/pf/http_helpers/http_interface.h index eb8c056..8ee8405 100644 --- a/src/pf_driver/include/pf_driver/pf/http_helpers/http_interface.h +++ b/src/pf_driver/include/pf_driver/pf/http_helpers/http_interface.h @@ -26,15 +26,14 @@ class HTTPInterface public: HTTPInterface(std::string host, std::string path = ""); - const std::map - get(const std::vector& json_keys, const std::string& command, + const Json::Value get(const std::string& command, const std::initializer_list& list = std::initializer_list()); - const std::map get(const std::vector& json_keys, const std::string& command, - const param_map_type& params = param_map_type()); + const Json::Value get(const std::string& command, + const param_map_type& params = param_map_type()); private: - const std::map get_(const std::vector& json_keys, CurlResource& res); + const Json::Value get_(CurlResource& res); const std::string host; const std::string base_path; diff --git a/src/pf_driver/include/pf_driver/pf/pf_interface.h b/src/pf_driver/include/pf_driver/pf/pf_interface.h index cbdf5bd..3ab1d65 100644 --- a/src/pf_driver/include/pf_driver/pf/pf_interface.h +++ b/src/pf_driver/include/pf_driver/pf/pf_interface.h @@ -14,6 +14,18 @@ #include "pf_driver/pf/r2000/pfsdp_2000.h" #include "pf_driver/pf/r2300/pfsdp_2300.h" +#include "pf_interfaces/srv/pfsdp_get_protocol_info.hpp" +#include "pf_interfaces/srv/pfsdp_reboot_device.hpp" +#include "pf_interfaces/srv/pfsdp_factory_reset.hpp" +#include "pf_interfaces/srv/pfsdp_list_parameters.hpp" +#include "pf_interfaces/srv/pfsdp_get_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_set_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_reset_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_list_iq_parameters.hpp" +#include "pf_interfaces/srv/pfsdp_get_iq_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_set_iq_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_reset_iq_parameter.hpp" + class PFInterface { public: @@ -28,6 +40,19 @@ class PFInterface void stop_transmission(); void terminate(); + void pfsdp_reboot(int32_t& error_code, std::string& error_text); + void pfsdp_factory(int32_t& error_code, std::string& error_text); + void pfsdp_info(std::string& protocol_name, int32_t& version_major, int32_t& version_minor, + std::vector& commands, int32_t& error_code, std::string& error_text); + void pfsdp_list(const char* cmd, const char* out, std::vector& params, int32_t& error_code, std::string& error_text); + void pfsdp_get(const char* cmd, const std::string& name, std::string& value, int32_t& error_code, std::string& error_text); + void pfsdp_set(const char* cmd, const std::string& name, const std::string& value, int32_t& error_code, std::string& error_text); + void pfsdp_reset(const char* cmd, const std::string& name, int32_t& error_code, std::string& error_text); + + std::vector pfsdp_list_iq(void); + std::string pfsdp_get_iq(const std::string& name); + void pfsdp_set_iq(const std::string& name, const std::string& value); + private: using PipelinePtr = std::unique_ptr; @@ -37,6 +62,19 @@ class PFInterface std::shared_ptr protocol_interface_; PipelinePtr pipeline_; std::shared_ptr> reader_; + + rclcpp::Service::SharedPtr info_service_; + rclcpp::Service::SharedPtr reboot_service_; + rclcpp::Service::SharedPtr factory_service_; + rclcpp::Service::SharedPtr listparams_service_; + rclcpp::Service::SharedPtr getparam_service_; + rclcpp::Service::SharedPtr setparam_service_; + rclcpp::Service::SharedPtr resetparam_service_; + rclcpp::Service::SharedPtr listiqparams_service_; + rclcpp::Service::SharedPtr getiqparam_service_; + rclcpp::Service::SharedPtr setiqparam_service_; + rclcpp::Service::SharedPtr resetiqparam_service_; + std::string topic_; std::string frame_id_; uint16_t num_layers_; @@ -57,6 +95,8 @@ class PFInterface }; PFState state_; + bool has_iq_parameters_; + bool init(); void change_state(PFState state); @@ -69,6 +109,7 @@ class PFInterface // factory functions bool handle_version(int major_version, int minor_version, int device_family, const std::string& topic, const std::string& frame_id, const uint16_t num_layers); + void add_pf_services(void); PipelinePtr get_pipeline(const std::string& packet_type, std::shared_ptr net_mtx, std::shared_ptr net_cv, bool& net_fail); void connection_failure_cb(); diff --git a/src/pf_driver/include/pf_driver/pf/pfsdp_base.h b/src/pf_driver/include/pf_driver/pf/pfsdp_base.h index ad8ae84..c81cc4a 100644 --- a/src/pf_driver/include/pf_driver/pf/pfsdp_base.h +++ b/src/pf_driver/include/pf_driver/pf/pfsdp_base.h @@ -53,8 +53,7 @@ class PFSDPBase bool is_connection_failure(const std::string& http_error); - bool check_error(std::map& mp, const std::string& err_code, const std::string& err_text, - const std::string& err_http); + void copy_status_from_json(int32_t&, std::string&, Json::Value); protected: std::shared_ptr node_; @@ -69,13 +68,13 @@ class PFSDPBase void set_connection_failure_cb(std::function callback); - const std::vector list_parameters(); - - bool reboot_device(); - - void factory_reset(); - - bool release_handle(const std::string& handle); + void list_parameters(const char* cmd, const char* out, std::vector& params, int32_t& error_code, std::string& error_text); + void get_parameter(const char* cmd, const std::string& name, std::string& value, int32_t& error_code, std::string& error_text); + void set_parameter(const char* cmd, const std::string& name, const std::string& value, int32_t& error_code, std::string& error_text); + void reset_parameter(const char* cmd, const std::string& name, int32_t& error_code, std::string& error_text); + void reboot(int32_t& error_code, std::string& error_text); + void factory(int32_t& error_code, std::string& error_text); + void info(std::string& n, int32_t& major, int32_t&minor, std::vector& cmds, int32_t& error_code, std::string& error_text); ProtocolInfo get_protocol_info(); @@ -97,16 +96,12 @@ class PFSDPBase std::string get_parameter_str(const std::string& param); - template - bool reset_parameter(const Ts&... ts) - { - return get_request_bool("reset_parameter", { "" }, { KV("list", ts...) }); - } - void request_handle_tcp(const std::string& port = "", const std::string& packet_type = ""); virtual void request_handle_udp(const std::string& packet_type = ""); + bool release_handle(const std::string& handle); + virtual void get_scanoutput_config(const std::string& handle); bool set_scanoutput_config(const std::string& handle, const ScanConfig& config); @@ -133,6 +128,8 @@ class PFSDPBase void declare_common_parameters(); + void pfsdp_init(const rclcpp::Parameter& parameter); + virtual void declare_specific_parameters() { } diff --git a/src/pf_driver/include/pf_driver/ros/pf_services.h b/src/pf_driver/include/pf_driver/ros/pf_services.h new file mode 100644 index 0000000..4ded7ca --- /dev/null +++ b/src/pf_driver/include/pf_driver/ros/pf_services.h @@ -0,0 +1,53 @@ +#pragma once + +//#include +//#include +//#include + +#include "pf_driver/pf/pf_interface.h" +#include "pf_interfaces/srv/pfsdp_reboot_device.hpp" +#include "pf_interfaces/srv/pfsdp_factory_reset.hpp" +#include "pf_interfaces/srv/pfsdp_get_protocol_info.hpp" +#include "pf_interfaces/srv/pfsdp_get_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_set_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_reset_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_list_parameters.hpp" +#include "pf_interfaces/srv/pfsdp_get_iq_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_set_iq_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_reset_iq_parameter.hpp" +#include "pf_interfaces/srv/pfsdp_list_iq_parameters.hpp" + +void pfsdp_reboot_device(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_factory_reset(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_get_protocol_info(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_list_parameters(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_get_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_set_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_reset_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_list_iq_parameters(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_get_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_set_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + +void pfsdp_reset_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response); + + diff --git a/src/pf_driver/src/pf/http_helpers/http_interface.cpp b/src/pf_driver/src/pf/http_helpers/http_interface.cpp index f76fd4c..9ad3153 100644 --- a/src/pf_driver/src/pf/http_helpers/http_interface.cpp +++ b/src/pf_driver/src/pf/http_helpers/http_interface.cpp @@ -6,62 +6,44 @@ HTTPInterface::HTTPInterface(std::string host, std::string path) : host(std::mov { } -const std::map HTTPInterface::get(const std::vector& json_keys, - const std::string& command, - const std::initializer_list& list) +const Json::Value HTTPInterface::get(const std::string& command, + const std::initializer_list& list) { CurlResource res(host); res.append_path(base_path); res.append_path(command); res.append_query(list); - return get_(json_keys, res); + return get_(res); } -const std::map HTTPInterface::get(const std::vector& json_keys, - const std::string& command, const param_map_type& params) +const Json::Value HTTPInterface::get(const std::string& command, + const param_map_type& params) { CurlResource res(host); res.append_path(base_path); res.append_path(command); res.append_query(params); - return get_(json_keys, res); + return get_(res); } -const std::map HTTPInterface::get_(const std::vector& json_keys, - CurlResource& res) +const Json::Value HTTPInterface::get_(CurlResource& res) { Json::Value json_resp; - std::map json_kv; try { res.get(json_resp); - json_kv[std::string("error_http")] = std::string("OK"); } catch (curlpp::RuntimeError& e) { - json_kv[std::string("error_http")] = std::string(e.what()); - return json_kv; + json_resp["error_code"] = -1; + json_resp["error_text"] = std::string(e.what()); } catch (curlpp::LogicError& e) { - json_kv[std::string("error_http")] = std::string(e.what()); - return json_kv; + json_resp["error_code"] = -1; + json_resp["error_text"] = std::string(e.what()); } - for (std::string key : json_keys) - { - try - { - if (json_resp[key].isArray()) - json_kv[key] = http_helpers::from_array(json_resp[key]); - else - json_kv[key] = json_resp[key].asString(); - } - catch (std::exception& e) - { - json_kv[key] = "--COULD NOT RETRIEVE VALUE--"; - } - } - return json_kv; + return json_resp; } diff --git a/src/pf_driver/src/pf/pf_interface.cpp b/src/pf_driver/src/pf/pf_interface.cpp index 483c2fa..ca26b38 100644 --- a/src/pf_driver/src/pf/pf_interface.cpp +++ b/src/pf_driver/src/pf/pf_interface.cpp @@ -5,6 +5,7 @@ #include #include "pf_driver/pf/pf_interface.h" +#include "pf_driver/ros/pf_services.h" #include "pf_driver/ros/laser_scan_publisher.h" #include "pf_driver/ros/point_cloud_publisher.h" #include "pf_driver/communication/udp_transport.h" @@ -26,6 +27,8 @@ bool PFInterface::init(std::shared_ptr info, std::shared_ptr(node_, info, config, params); // This is the first time ROS communicates with the device auto opi = protocol_interface_->get_protocol_info(); @@ -46,6 +49,12 @@ bool PFInterface::init(std::shared_ptr info, std::shared_ptrget_logger(), "Device unsupported"); return false; } + + if (std::find(opi.commands.begin(), opi.commands.end(), "list_iq_parameters") != opi.commands.end()) + { + has_iq_parameters_ = true; + } + RCLCPP_INFO(node_->get_logger(), "Device found: %s", product_.c_str()); // release previous handles @@ -54,6 +63,16 @@ bool PFInterface::init(std::shared_ptr info, std::shared_ptrrelease_handle(prev_handle_); } + // apply pfsdp_init name=value setup + rclcpp::Parameter pfsdp_init_param; + if (node_->get_parameter("pfsdp_init", pfsdp_init_param)) + { + protocol_interface_->pfsdp_init(pfsdp_init_param); + } + + // add services + add_pf_services(); + if (info->handle_type == HandleInfo::HANDLE_TYPE_UDP) { transport_ = std::make_unique(info->hostname, info->port); @@ -259,6 +278,61 @@ bool PFInterface::handle_version(int major_version, int minor_version, int devic return false; } +void PFInterface::add_pf_services(void) +{ + // Add PFSDP services as appropriate for the given product features + std::string basename = std::string(node_->get_namespace()) + std::string(node_->get_name()) + std::string("/pfsdp_"); + std::string svcname; + + svcname = basename + "get_protocol_info"; + info_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_get_protocol_info, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "reboot_device"; + reboot_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_reboot_device, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "factory_reset"; + factory_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_factory_reset, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "list_parameters"; + listparams_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_list_parameters, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "get_parameter"; + getparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_get_parameter, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "set_parameter"; + setparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_set_parameter, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "reset_parameter"; + resetparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_reset_parameter, this, std::placeholders::_1, std::placeholders::_2)); + + + if (has_iq_parameters_) + { + svcname = basename + "list_iq_parameters"; + listiqparams_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_list_iq_parameters, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "get_iq_parameter"; + getiqparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_get_iq_parameter, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "set_iq_parameter"; + setiqparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_set_iq_parameter, this, std::placeholders::_1, std::placeholders::_2)); + + svcname = basename + "reset_iq_parameter"; + resetiqparam_service_ = node_->create_service(svcname.c_str(), + std::bind(&pfsdp_reset_iq_parameter, this, std::placeholders::_1, std::placeholders::_2)); + } +} + std::unique_ptr PFInterface::get_pipeline(const std::string& packet_type, std::shared_ptr net_mtx, std::shared_ptr net_cv, bool& net_fail) { @@ -296,3 +370,41 @@ std::unique_ptr PFInterface::get_pipeline(const std::string& packet_ty return std::make_unique(writer, reader_, std::bind(&PFInterface::connection_failure_cb, this), net_mtx, net_cv, net_fail); } + +void PFInterface::pfsdp_reboot(int32_t& error_code, std::string& error_text) +{ + protocol_interface_->reboot(error_code, error_text); +} + +void PFInterface::pfsdp_factory(int32_t& error_code, std::string& error_text) +{ + protocol_interface_->factory(error_code, error_text); +} + +void PFInterface::pfsdp_info(std::string& protocol_name, int32_t& version_major, int32_t& version_minor, + std::vector& commands, int32_t& error_code, std::string& error_text) +{ + protocol_interface_->info(protocol_name, version_major, version_minor, commands, error_code, error_text); +} + +void PFInterface::pfsdp_list(const char* cmd, const char* out, std::vector& params, int32_t& error_code, std::string& error_text) +{ + protocol_interface_->list_parameters(cmd, out, params, error_code, error_text); +} + +void PFInterface::pfsdp_get(const char* cmd, const std::string& name, std::string& value, int32_t& error_code, std::string& error_text) +{ + protocol_interface_->get_parameter(cmd, name, value, error_code, error_text); +} + +void PFInterface::pfsdp_set(const char* cmd, const std::string& name, const std::string& value, int32_t& error_code, std::string& error_text) +{ + protocol_interface_->set_parameter(cmd, name, value, error_code, error_text); +} + +void PFInterface::pfsdp_reset(const char* cmd, const std::string& name, int32_t& error_code, std::string& error_text) +{ + protocol_interface_->reset_parameter(cmd, name, error_code, error_text); +} + + diff --git a/src/pf_driver/src/pf/pfsdp_base.cpp b/src/pf_driver/src/pf/pfsdp_base.cpp index 8188186..53bd115 100644 --- a/src/pf_driver/src/pf/pfsdp_base.cpp +++ b/src/pf_driver/src/pf/pfsdp_base.cpp @@ -5,6 +5,7 @@ #include "pf_driver/pf/pfsdp_base.h" #include "pf_driver/pf/parser_utils.h" +#include "pf_driver/pf/http_helpers/http_helpers.h" PFSDPBase::PFSDPBase(std::shared_ptr node, std::shared_ptr info, std::shared_ptr config, std::shared_ptr params) @@ -12,6 +13,51 @@ PFSDPBase::PFSDPBase(std::shared_ptr node, std::shared_ptrget(cmd, { KV("list", name) }); + + copy_status_from_json(error_code, error_text, json_resp); + + if (error_code == 0) + { + if (json_resp[name].isArray()) + { + value = http_helpers::from_array(json_resp[name]); + } + else + { + value = json_resp[name].asString(); + } + } + else + { + value = std::string(""); + } +} + +void PFSDPBase::set_parameter(const char* cmd, const std::string& name, const std::string& value, int32_t& error_code, std::string& error_text) +{ + Json::Value json_resp = http_interface->get(cmd, { KV(name, value) }); + + copy_status_from_json(error_code, error_text, json_resp); +} + +void PFSDPBase::reset_parameter(const char* cmd, const std::string& name, int32_t& error_code, std::string& error_text) +{ + Json::Value json_resp = http_interface->get(cmd, { KV("list", name) }); + + copy_status_from_json(error_code, error_text, json_resp); +} + + const std::map PFSDPBase::get_request(const std::string& command, const std::vector& json_keys, const std::initializer_list& query) @@ -23,19 +69,53 @@ const std::map PFSDPBase::get_request(const std::strin const std::vector& json_keys, const param_map_type& query) { - const std::string err_code = "error_code"; - const std::string err_text = "error_text"; - const std::string err_http = "error_http"; - std::vector keys = { err_code, err_text }; - keys.insert(keys.end(), json_keys.begin(), json_keys.end()); - std::map json_resp = http_interface->get(keys, command, query); + Json::Value json_resp = http_interface->get(command, query); + + const int error_code = json_resp["error_code"].asInt(); + const std::string error_text(json_resp["error_text"].asString()); - if (!check_error(json_resp, err_code, err_text, err_http)) + // check if HTTP has an error + if (error_code < 0) { + std::cerr << "HTTP ERROR: " << error_text << std::endl; + + if (is_connection_failure(error_text)) + { + if (handle_connection_failure) + { + handle_connection_failure(); + } + } return std::map(); } - return json_resp; + // check if PFSDP has an error + if (error_code != 0 || error_text.compare("success") != 0) + + { + std::cout << "protocol error: " << error_code << " " << error_text << std::endl; + return std::map(); + } + + // Flatten values + std::map json_kv; + for (std::string key : json_keys) + { + try + { + if (json_resp[key].isArray()) + json_kv[key] = http_helpers::from_array(json_resp[key]); + else + json_kv[key] = json_resp[key].asString(); + } + catch (std::exception& e) + { + std::cout << "Invalid command or parameter (" << key << ") requested." << std::endl; + return std::map(); + } + } + + return json_kv; } bool PFSDPBase::get_request_bool(const std::string& command, const std::vector& json_keys, @@ -61,67 +141,44 @@ bool PFSDPBase::is_connection_failure(const std::string& http_error) return false; } -bool PFSDPBase::check_error(std::map& mp, const std::string& err_code, - const std::string& err_text, const std::string& err_http) +void PFSDPBase::set_connection_failure_cb(std::function callback) { - const std::string http_error = mp[err_http]; - const std::string code = mp[err_code]; - const std::string text = mp[err_text]; + handle_connection_failure = callback; +} - // remove error related key-value pairs - mp.erase(err_http); - mp.erase(err_code); - mp.erase(err_text); +void PFSDPBase::list_parameters(const char* cmd, const char* out, std::vector& params, int32_t& error_code, std::string& error_text) +{ + Json::Value json_resp = http_interface->get(cmd, { }); - // check if HTTP has an error - if (http_error.compare(std::string("OK"))) + copy_status_from_json(error_code, error_text, json_resp); + + if (error_code == 0) { - std::cerr << "HTTP ERROR: " << http_error << std::endl; - if (is_connection_failure(http_error)) + const int sz = json_resp[out].size(); + params.clear(); + for (int i=0; i(); } - // check for error messages in protocol response - if (code.compare("0") || text.compare("success")) - { - std::cout << "protocol error: " << code << " " << text << std::endl; - return false; - } - return true; } -void PFSDPBase::set_connection_failure_cb(std::function callback) +void PFSDPBase::reboot(int32_t& error_code, std::string& error_text) { - handle_connection_failure = callback; -} + Json::Value json_resp = http_interface->get("reboot_device", { }); -const std::vector PFSDPBase::list_parameters() -{ - auto resp = get_request("list_parameters", { "parameters" }); - return parser_utils::split(resp["parameters"]); + copy_status_from_json(error_code, error_text, json_resp); } -bool PFSDPBase::reboot_device() +void PFSDPBase::factory(int32_t& error_code, std::string& error_text) { - return get_request_bool("reboot_device"); -} + Json::Value json_resp = http_interface->get("factory_reset", { }); -void PFSDPBase::factory_reset() -{ - get_request("factory_reset"); + copy_status_from_json(error_code, error_text, json_resp); } bool PFSDPBase::release_handle(const std::string& handle) @@ -130,19 +187,38 @@ bool PFSDPBase::release_handle(const std::string& handle) return true; } -ProtocolInfo PFSDPBase::get_protocol_info() +void PFSDPBase::info(std::string& protocol_name, int32_t& version_major, int32_t& version_minor, + std::vector& commands, int32_t& error_code, std::string& error_text) { - ProtocolInfo opi; - auto resp = get_request("get_protocol_info", { "protocol_name", "version_major", "version_minor", "commands" }); - if (resp.empty()) + Json::Value json_resp = http_interface->get("get_protocol_info", { }); + + copy_status_from_json(error_code, error_text, json_resp); + + if (error_code == 0) { - opi.isError = true; - return opi; + version_major = json_resp["version_major"].asInt(); + version_minor = json_resp["version_minor"].asInt(); + protocol_name = json_resp["protocol_name"].asString(); + + const int sz = json_resp["commands"].size(); + commands.clear(); + for (int i=0; i pfsdp_init = parameter.as_string_array(); + for (int i=0; i& parameters) { bool successful = true; for (const auto& parameter : parameters) { - if (parameter.get_name() == "ip_mode" || parameter.get_name() == "scan_frequency" || - parameter.get_name() == "subnet_mask" || parameter.get_name() == "gateway" || - parameter.get_name() == "scan_direction" || parameter.get_name() == "user_tag" || - parameter.get_name() == "ip_address" || parameter.get_name() == "subnet_mask" || - parameter.get_name() == "gateway") + if (parameter.get_name() == "pfsdp_init") { - return set_parameter({ KV(parameter.get_name(), parameter.value_to_string()) }); - } - else if (parameter.get_name() == "ip_address") - { - info_->hostname = parameter.as_string(); - return set_parameter({ KV(parameter.get_name(), parameter.value_to_string()) }); - } - else if (parameter.get_name() == "packet_crc") - { - return set_parameter({ KV(parameter.get_name(), parameter.as_int()) }); + pfsdp_init(parameter); } else if (parameter.get_name() == "port") { @@ -371,10 +450,6 @@ bool PFSDPBase::reconfig_callback_impl(const std::vector& par { config_->skip_scans = parameter.as_int(); } - else if (parameter.get_name() == "locator_indication") - { - return set_parameter({ KV(parameter.get_name(), parameter.as_bool() ? "on" : "off") }); - } } return successful; diff --git a/src/pf_driver/src/pf/r2000/pfsdp_2000.cpp b/src/pf_driver/src/pf/r2000/pfsdp_2000.cpp index 7aa78a7..d6da6ac 100644 --- a/src/pf_driver/src/pf/r2000/pfsdp_2000.cpp +++ b/src/pf_driver/src/pf/r2000/pfsdp_2000.cpp @@ -36,86 +36,6 @@ void PFSDP_2000::get_scan_parameters() void PFSDP_2000::declare_specific_parameters() { - std::string hmi_display_mode, hmi_application_bitmap, operating_mode, hmi_language, hmi_static_text_1, - hmi_static_text_2, user_notes, filter_type, filter_error_handling, filter_remission_threshold, - lcm_detection_sensitivity, lcm_sector_enable; - - bool hmi_button_lock, hmi_parameter_lock; - - int samples_per_scan, filter_width, filter_maximum_margin, lcm_detection_period; - - if (!node_->has_parameter("samples_per_scan")) - { - node_->declare_parameter("samples_per_scan", samples_per_scan); - } - if (!node_->has_parameter("hmi_application_bitmap")) - { - node_->declare_parameter("hmi_application_bitmap", hmi_application_bitmap); - } - if (!node_->has_parameter("operating_mode")) - { - node_->declare_parameter("operating_mode", operating_mode); - } - if (!node_->has_parameter("hmi_display_mode")) - { - node_->declare_parameter("hmi_display_mode", hmi_display_mode); - } - if (!node_->has_parameter("hmi_language")) - { - node_->declare_parameter("hmi_language", hmi_language); - } - if (!node_->has_parameter("hmi_button_lock")) - { - node_->declare_parameter("hmi_button_lock", hmi_button_lock); - } - if (!node_->has_parameter("hmi_parameter_lock")) - { - node_->declare_parameter("hmi_parameter_lock", hmi_parameter_lock); - } - if (!node_->has_parameter("hmi_static_text_1")) - { - node_->declare_parameter("hmi_static_text_1", hmi_static_text_1); - } - if (!node_->has_parameter("hmi_static_text_2")) - { - node_->declare_parameter("hmi_static_text_2", hmi_static_text_2); - } - if (!node_->has_parameter("user_notes")) - { - node_->declare_parameter("user_notes", user_notes); - } - if (!node_->has_parameter("filter_type")) - { - node_->declare_parameter("filter_type", filter_type); - } - if (!node_->has_parameter("filter_width")) - { - node_->declare_parameter("filter_width", filter_width); - } - if (!node_->has_parameter("filter_error_handling")) - { - node_->declare_parameter("filter_error_handling", filter_error_handling); - } - if (!node_->has_parameter("filter_maximum_margin")) - { - node_->declare_parameter("filter_maximum_margin", filter_maximum_margin); - } - if (!node_->has_parameter("filter_remission_threshold")) - { - node_->declare_parameter("filter_remission_threshold", filter_remission_threshold); - } - if (!node_->has_parameter("lcm_detection_sensitivity")) - { - node_->declare_parameter("lcm_detection_sensitivity", lcm_detection_sensitivity); - } - if (!node_->has_parameter("lcm_detection_period")) - { - node_->declare_parameter("lcm_detection_period", lcm_detection_period); - } - if (!node_->has_parameter("lcm_sector_enable")) - { - node_->declare_parameter("lcm_sector_enable", lcm_sector_enable); - } } bool PFSDP_2000::reconfig_callback_impl(const std::vector& parameters) @@ -124,15 +44,7 @@ bool PFSDP_2000::reconfig_callback_impl(const std::vector& pa for (const auto& parameter : parameters) { - if (parameter.get_name() == "hmi_language" || parameter.get_name() == "hmi_static_text_1" || - parameter.get_name() == "hmi_static_text_2" || parameter.get_name() == "user_notes" || - parameter.get_name() == "operating_mode" || parameter.get_name() == "filter_type" || - parameter.get_name() == "filter_error_handling" || parameter.get_name() == "filter_remission_threshold" || - parameter.get_name() == "lcm_detection_sensitivity" || parameter.get_name() == "lcm_sector_enable") - { - return set_parameter({ KV(parameter.get_name(), parameter.value_to_string()) }); - } - else if (parameter.get_name() == "packet_type") + if (parameter.get_name() == "packet_type") { std::string packet_type = parameter.as_string(); if (packet_type == "A" || packet_type == "B" || packet_type == "C") @@ -144,20 +56,6 @@ bool PFSDP_2000::reconfig_callback_impl(const std::vector& pa successful = false; } } - else if (parameter.get_name() == "samples_per_scan" || parameter.get_name() == "filter_width" || - parameter.get_name() == "filter_maximum_margin" || parameter.get_name() == "lcm_detection_period") - { - return set_parameter({ KV(parameter.get_name(), parameter.as_int()) }); - } - else if (parameter.get_name() == "hmi_button_lock" || parameter.get_name() == "hmi_parameter_lock") - { - return set_parameter({ KV(parameter.get_name(), parameter.as_bool() ? "on" : "off") }); - } - else if (parameter.get_name() == "hmi_display_mode") - { - return set_parameter({ KV(parameter.get_name(), - parameter.value_to_string() == "mode_off" ? "off" : parameter.value_to_string()) }); - } } return successful; diff --git a/src/pf_driver/src/pf/r2300/pfsdp_2300.cpp b/src/pf_driver/src/pf/r2300/pfsdp_2300.cpp index 38eb117..0acf1a3 100644 --- a/src/pf_driver/src/pf/r2300/pfsdp_2300.cpp +++ b/src/pf_driver/src/pf/r2300/pfsdp_2300.cpp @@ -77,55 +77,10 @@ std::string PFSDP_2300::get_start_angle_str() void PFSDP_2300::declare_specific_parameters() { - float measure_start_angle, measure_stop_angle, pilot_start_angle, pilot_stop_angle; - std::string layer_enable; - bool pilot_laser; - - if (!node_->has_parameter("measure_start_angle")) - { - node_->declare_parameter("measure_start_angle", measure_start_angle); - } - if (!node_->has_parameter("measure_stop_angle")) - { - node_->declare_parameter("measure_stop_angle", measure_stop_angle); - } - if (!node_->has_parameter("pilot_start_angle")) - { - node_->declare_parameter("pilot_start_angle", pilot_start_angle); - } - if (!node_->has_parameter("pilot_stop_angle")) - { - node_->declare_parameter("pilot_stop_angle", pilot_stop_angle); - } - if (!node_->has_parameter("layer_enable")) - { - node_->declare_parameter("layer_enable", layer_enable); - } - if (!node_->has_parameter("pilot_laser")) - { - node_->declare_parameter("pilot_laser", pilot_laser); - } } bool PFSDP_2300::reconfig_callback_impl(const std::vector& parameters) { bool successful = PFSDPBase::reconfig_callback_impl(parameters); - - for (const auto& parameter : parameters) - { - std::cout << parameter.get_name() << " " << parameter.value_to_string() << std::endl; - if (parameter.get_name() == "measure_start_angle" || parameter.get_name() == "measure_stop_angle" || - parameter.get_name() == "pilot_start_angle" || parameter.get_name() == "pilot_stop_angle" || - parameter.get_name() == "layer_enable") - { - std::cout << parameter.get_name() << " " << parameter.value_to_string() << std::endl; - return set_parameter({ KV(parameter.get_name(), parameter.value_to_string()) }); - } - else if (parameter.get_name() == "pilot_laser") - { - return set_parameter({ KV(parameter.get_name(), parameter.as_bool() ? "on" : "off") }); - } - } - return successful; -} \ No newline at end of file +} diff --git a/src/pf_driver/src/ros/pf_services.cpp b/src/pf_driver/src/ros/pf_services.cpp new file mode 100644 index 0000000..aea5d88 --- /dev/null +++ b/src/pf_driver/src/ros/pf_services.cpp @@ -0,0 +1,74 @@ +#include +#include +#include + +#include "pf_driver/ros/pf_services.h" +#include "pf_driver/pf/pf_interface.h" + +void pfsdp_reboot_device(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_reboot(response->error_code, response->error_text); +} + +void pfsdp_factory_reset(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_factory(response->error_code, response->error_text); +} + +void pfsdp_get_protocol_info(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_info(response->protocol_name, response->version_major, response->version_minor, response->commands, response->error_code, response->error_text); +} + +void pfsdp_list_parameters(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_list("list_parameters", "parameters", response->parameters, response->error_code, response->error_text); +} + +void pfsdp_get_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_get("get_parameter", request->name, response->value, response->error_code, response->error_text); +} + +void pfsdp_set_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_set("set_parameter", request->name, request->value, response->error_code, response->error_text); +} + +void pfsdp_reset_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_reset("reset_parameter", request->name, response->error_code, response->error_text); +} + +void pfsdp_list_iq_parameters(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_list("list_iq_parameters", "iq_parameters", response->iq_parameters, response->error_code, response->error_text); +} + +void pfsdp_get_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_get("get_iq_parameter", request->name, response->value, response->error_code, response->error_text); +} + +void pfsdp_set_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_set("set_iq_parameter", request->name, request->value, response->error_code, response->error_text); +} + +void pfsdp_reset_iq_parameter(PFInterface* pf, const std::shared_ptr request, + std::shared_ptr response) +{ + pf->pfsdp_reset("reset_iq_parameter", request->name, response->error_code, response->error_text); +} + + diff --git a/src/pf_driver/src/ros/ros_main.cpp b/src/pf_driver/src/ros/ros_main.cpp index b57a681..6ee530b 100644 --- a/src/pf_driver/src/ros/ros_main.cpp +++ b/src/pf_driver/src/ros/ros_main.cpp @@ -12,10 +12,12 @@ int main(int argc, char* argv[]) rclcpp::init(argc, argv); auto node = std::make_shared("pf_driver"); + std::vector pfsdp_init; std::string device, transport_str, scanner_ip, port, topic, frame_id, packet_type; - int samples_per_scan, start_angle, max_num_points_scan, watchdogtimeout, num_layers; + int samples_per_scan, start_angle, max_num_points_scan, watchdogtimeout, skip_scans, num_layers; port = "0"; num_layers = 0; + skip_scans = 0; bool watchdog, apply_correction = 0; if (!node->has_parameter("device")) @@ -109,20 +111,23 @@ int main(int argc, char* argv[]) node->get_parameter("apply_correction", apply_correction); RCLCPP_INFO(node->get_logger(), "apply_correction: %d", apply_correction); - std::string ip_mode, ip_address, subnet_mask, gateway, scan_direction, user_tag; - int packet_crc, skip_scans, scan_frequency; - bool locator_indication; - - node->declare_parameter("scan_frequency", scan_frequency); - node->declare_parameter("locator_indication", locator_indication); - node->declare_parameter("packet_crc", packet_crc); - node->declare_parameter("ip_mode", ip_mode); - node->declare_parameter("ip_address", ip_address); - node->declare_parameter("subnet_mask", subnet_mask); - node->declare_parameter("gateway", gateway); - node->declare_parameter("scan_direction", scan_direction); - node->declare_parameter("skip_scans", skip_scans); - node->declare_parameter("user_tag", user_tag); + if (!node->has_parameter("skip_scans")) + { + node->declare_parameter("skip_scans", skip_scans); + } + node->get_parameter("skip_scans", skip_scans); + RCLCPP_INFO(node->get_logger(), "skip_scans: %d", skip_scans); + + if (!node->has_parameter("pfsdp_init")) + { + node->declare_parameter("pfsdp_init", pfsdp_init); + } + node->get_parameter("pfsdp_init", pfsdp_init); + RCLCPP_INFO(node->get_logger(), "pfsdp_init.size: %d", (int)pfsdp_init.size()); + for (int i=0; iget_logger(), "pfsdp_init[%d]: %s", i, pfsdp_init[i].c_str()); + } std::shared_ptr info = std::make_shared(); @@ -134,11 +139,10 @@ int main(int argc, char* argv[]) std::shared_ptr config = std::make_shared(); config->start_angle = node->get_parameter("start_angle").get_parameter_value().get(); config->max_num_points_scan = node->get_parameter("max_num_points_scan").get_parameter_value().get(); + config->skip_scans = node->get_parameter("skip_scans").get_parameter_value().get(); config->packet_type = node->get_parameter("packet_type").get_parameter_value().get(); config->watchdogtimeout = node->get_parameter("watchdogtimeout").get_parameter_value().get(); config->watchdog = node->get_parameter("watchdog").get_parameter_value().get(); - // config->scan_frequency = scan_frequency; - // config->samples_per_scan = samples_per_scan; RCLCPP_INFO(node->get_logger(), "start_angle: %d", config->start_angle); std::shared_ptr params = std::make_shared(); diff --git a/src/pf_interfaces/CMakeLists.txt b/src/pf_interfaces/CMakeLists.txt index 8751373..6b6814a 100644 --- a/src/pf_interfaces/CMakeLists.txt +++ b/src/pf_interfaces/CMakeLists.txt @@ -8,6 +8,17 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/PFHeader.msg" "msg/PFR2000Header.msg" "msg/PFR2300Header.msg" + "srv/PfsdpRebootDevice.srv" + "srv/PfsdpFactoryReset.srv" + "srv/PfsdpGetProtocolInfo.srv" + "srv/PfsdpGetParameter.srv" + "srv/PfsdpSetParameter.srv" + "srv/PfsdpResetParameter.srv" + "srv/PfsdpListParameters.srv" + "srv/PfsdpGetIqParameter.srv" + "srv/PfsdpSetIqParameter.srv" + "srv/PfsdpResetIqParameter.srv" + "srv/PfsdpListIqParameters.srv" ) ament_export_dependencies(rosidl_default_runtime) diff --git a/src/pf_interfaces/srv/PfsdpFactoryReset.srv b/src/pf_interfaces/srv/PfsdpFactoryReset.srv new file mode 100644 index 0000000..155fc09 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpFactoryReset.srv @@ -0,0 +1,5 @@ +--- +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpGetIqParameter.srv b/src/pf_interfaces/srv/PfsdpGetIqParameter.srv new file mode 100644 index 0000000..54af9f8 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpGetIqParameter.srv @@ -0,0 +1,7 @@ +string name # name of the parameter, e.g. scan_frequency +--- +string value # value of the parameter as string, e.g. "10.5" +int32 error_code # 0 if everything went well in the sensor +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpGetParameter.srv b/src/pf_interfaces/srv/PfsdpGetParameter.srv new file mode 100644 index 0000000..348faeb --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpGetParameter.srv @@ -0,0 +1,7 @@ +string name # name of the parameter, e.g. scan_frequency +--- +string value # value of the parameter as string, e.g. "10.5". array values are returned as one string of semicolon-separated values +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpGetProtocolInfo.srv b/src/pf_interfaces/srv/PfsdpGetProtocolInfo.srv new file mode 100644 index 0000000..bfd4d90 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpGetProtocolInfo.srv @@ -0,0 +1,9 @@ +--- +string protocol_name # pfsdp +int32 version_major # e.g. 1 +int32 version_minor # e.g. 5 +string[] commands # list of all PFSDP commands supported by the sensor +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpListIqParameters.srv b/src/pf_interfaces/srv/PfsdpListIqParameters.srv new file mode 100644 index 0000000..76241b2 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpListIqParameters.srv @@ -0,0 +1,4 @@ +--- +string[] iq_parameters +int32 error_code +string error_text diff --git a/src/pf_interfaces/srv/PfsdpListParameters.srv b/src/pf_interfaces/srv/PfsdpListParameters.srv new file mode 100644 index 0000000..65a3b0a --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpListParameters.srv @@ -0,0 +1,4 @@ +--- +string[] parameters +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text diff --git a/src/pf_interfaces/srv/PfsdpRebootDevice.srv b/src/pf_interfaces/srv/PfsdpRebootDevice.srv new file mode 100644 index 0000000..155fc09 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpRebootDevice.srv @@ -0,0 +1,5 @@ +--- +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpResetIqParameter.srv b/src/pf_interfaces/srv/PfsdpResetIqParameter.srv new file mode 100644 index 0000000..b807cc2 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpResetIqParameter.srv @@ -0,0 +1,6 @@ +string name # name of the parameter, e.g. iq_output +--- +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpResetParameter.srv b/src/pf_interfaces/srv/PfsdpResetParameter.srv new file mode 100644 index 0000000..5cc5cbd --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpResetParameter.srv @@ -0,0 +1,6 @@ +string name # name of the parameter, e.g. scan_frequency +--- +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpSetIqParameter.srv b/src/pf_interfaces/srv/PfsdpSetIqParameter.srv new file mode 100644 index 0000000..93e1591 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpSetIqParameter.srv @@ -0,0 +1,7 @@ +string name # name of the parameter, e.g. scan_frequency +string value # value for the parameter as string, e.g. "10.5" +--- +int32 error_code # 0 if everything went well in the sensor +string error_text # verbose explanation of error_code value + + diff --git a/src/pf_interfaces/srv/PfsdpSetParameter.srv b/src/pf_interfaces/srv/PfsdpSetParameter.srv new file mode 100644 index 0000000..e95f128 --- /dev/null +++ b/src/pf_interfaces/srv/PfsdpSetParameter.srv @@ -0,0 +1,7 @@ +string name # name of the parameter, e.g. scan_frequency +string value # value of the parameter as string, e.g. "10.5". array values are expected as one string of semicolon-separated values +--- +int32 error_code # 0 if everything went well in the sensor, <0 network/http error, >0 pfsdp error +string error_text # verbose explanation of error_code value + +