From 1626bb182297697c3b988dc36220befa8d5c7aa4 Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Fri, 30 Aug 2024 13:04:52 +0200 Subject: [PATCH] Added ParamsParser to RgbdSensor_nwc_ros2 Signed-off-by: Ettore Landini --- .../rgbdSensor_nwc_ros2/CMakeLists.txt | 3 + .../RgbdSensor_nwc_ros2.cpp | 41 +---- .../rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h | 16 +- .../RgbdSensor_nwc_ros2_ParamsParser.cpp | 157 ++++++++++++++++++ .../RgbdSensor_nwc_ros2_ParamsParser.h | 78 +++++++++ .../RgbdSensor_nwc_ros2_params.md | 4 + 6 files changed, 256 insertions(+), 43 deletions(-) create mode 100644 src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.cpp create mode 100644 src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.h create mode 100644 src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_params.md diff --git a/src/devices/rgbdSensor_nwc_ros2/CMakeLists.txt b/src/devices/rgbdSensor_nwc_ros2/CMakeLists.txt index e75a2b6..652083f 100644 --- a/src/devices/rgbdSensor_nwc_ros2/CMakeLists.txt +++ b/src/devices/rgbdSensor_nwc_ros2/CMakeLists.txt @@ -11,11 +11,14 @@ yarp_prepare_plugin(rgbdSensor_nwc_ros2 if(NOT SKIP_rgbdSensor_nwc_ros2) yarp_add_plugin(yarp_rgbdSensor_nwc_ros2) + generateDeviceParamsParser(RgbdSensor_nwc_ros2 rgbdSensor_nwc_ros2) target_sources(yarp_rgbdSensor_nwc_ros2 PRIVATE RgbdSensor_nwc_ros2.cpp RgbdSensor_nwc_ros2.h + RgbdSensor_nwc_ros2_ParamsParser.h + RgbdSensor_nwc_ros2_ParamsParser.cpp ) target_sources(yarp_rgbdSensor_nwc_ros2 PRIVATE $ $) diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp index 5e918c9..bf6fe1b 100644 --- a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp @@ -34,42 +34,19 @@ RgbdSensor_nwc_ros2::RgbdSensor_nwc_ros2() bool RgbdSensor_nwc_ros2::open(yarp::os::Searchable& config) { + parseParams(config); + m_topic_depth_camera_info = m_depth_topic_name.substr(0, m_depth_topic_name.rfind('/')) + "/camera_info"; + m_topic_rgb_camera_info = m_color_topic_name.substr(0, m_color_topic_name.rfind('/')) + "/camera_info"; - // node_name check - if (!config.check("node_name")) { - yCError(RGBDSENSOR_NWC_ROS2) << "missing node_name parameter"; - return false; - } - m_ros2_node_name = config.find("node_name").asString(); - - - // depth topic base name check - if (!config.check("depth_topic_name")) { - yCError(RGBDSENSOR_NWC_ROS2) << "missing depth_topic_name parameter"; - return false; - } - m_topic_depth_image_raw = config.find("depth_topic_name").asString(); - - m_topic_depth_camera_info = m_topic_depth_image_raw.substr(0, m_topic_depth_image_raw.rfind('/')) + "/camera_info"; - - // color topic base name check - if (!config.check("color_topic_name")) { - yCError(RGBDSENSOR_NWC_ROS2) << "missing color_topic_name parameter"; - return false; - } - m_topic_rgb_image_raw = config.find("color_topic_name").asString(); - - m_topic_rgb_camera_info = m_topic_rgb_image_raw.substr(0, m_topic_rgb_image_raw.rfind('/')) + "/camera_info"; - - m_verbose = config.check("verbose"); + m_verbose = m_verbose_on == 1; - m_node = NodeCreator::createNode(m_ros2_node_name); + m_node = NodeCreator::createNode(m_node_name); m_sub1= new Ros2Subscriber(m_node, this); m_sub1->subscribe_to_topic(m_topic_rgb_camera_info); m_sub1->subscribe_to_topic(m_topic_depth_camera_info); m_sub2= new Ros2Subscriber(m_node, this); - m_sub2->subscribe_to_topic(m_topic_rgb_image_raw); - m_sub2->subscribe_to_topic(m_topic_depth_image_raw); + m_sub2->subscribe_to_topic(m_color_topic_name); + m_sub2->subscribe_to_topic(m_depth_topic_name); m_spinner = new Ros2Spinner(m_node); m_spinner->start(); @@ -91,9 +68,9 @@ bool RgbdSensor_nwc_ros2::close() void RgbdSensor_nwc_ros2::callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic) { - if (topic == m_topic_rgb_image_raw) { + if (topic == m_color_topic_name) { color_raw_callback(msg); - } else if (topic == m_topic_depth_image_raw) { + } else if (topic == m_depth_topic_name) { depth_raw_callback(msg); } } diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h index db080d6..52d7276 100644 --- a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h @@ -30,6 +30,8 @@ #include #include +#include "RgbdSensor_nwc_ros2_ParamsParser.h" + /** * @ingroup dev_impl_nwc_ros2 * @@ -37,13 +39,7 @@ * This device is an nwc take a stream of data from ros2 and exposes to the user IRGBDSensor interface. * See its documentation for more details about the interface. * - * Parameters required by this device are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:----------------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:------------------------------: |:---------------------------------------------------------------------------------------------------:|:-----:| - * | rgb_data_topic | - | string | - | - | no | ros rgb topic | must start with a leading '/' | - * | rgb_info_topic | - | string | - | | no | ros rgb camera info topic | must start with a leading '/' | - * | depth_data_topic | - | string | - | - | no | ros depth topic | must start with a leading '/' | - * | depth_info_topic | - | string | - | - | no | ros depth camera info topic | must start with a leading '/' | + * Parameters required by this device are:shown in class: RgbdSensor_nwc_ros2_ParamsParser * * example of configuration file: * @@ -73,7 +69,8 @@ typedef yarp::sig::FlexImage flexImage; */ class RgbdSensor_nwc_ros2: public yarp::dev::DeviceDriver, - public yarp::dev::IRGBDSensor + public yarp::dev::IRGBDSensor, + RgbdSensor_nwc_ros2_ParamsParser { private: // mutex for writing or retrieving images @@ -105,10 +102,7 @@ class RgbdSensor_nwc_ros2: // ros2 variables for topics and subscriptions std::string m_topic_rgb_camera_info; - std::string m_topic_rgb_image_raw; std::string m_topic_depth_camera_info; - std::string m_topic_depth_image_raw; - std::string m_ros2_node_name; // yarp variables int m_verbose{2}; diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.cpp b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.cpp new file mode 100644 index 0000000..1693275 --- /dev/null +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.cpp @@ -0,0 +1,157 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Fri Aug 30 12:59:16 2024 + + +#include "RgbdSensor_nwc_ros2_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(RgbdSensor_nwc_ros2ParamsCOMPONENT, "yarp.device.RgbdSensor_nwc_ros2") +} + + +RgbdSensor_nwc_ros2_ParamsParser::RgbdSensor_nwc_ros2_ParamsParser() +{ +} + + +std::vector RgbdSensor_nwc_ros2_ParamsParser::getListOfParams() const +{ + std::vector params; + params.push_back("node_name"); + params.push_back("color_topic_name"); + params.push_back("depth_topic_name"); + params.push_back("verbose_on"); + return params; +} + + +bool RgbdSensor_nwc_ros2_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << getDocumentationOfDeviceParams(); + } + + std::string config_string = config.toString(); + yarp::os::Property prop_check(config_string.c_str()); + //Parser of parameter node_name + { + if (config.check("node_name")) + { + m_node_name = config.find("node_name").asString(); + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Parameter 'node_name' using value:" << m_node_name; + } + else + { + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Mandatory parameter 'node_name' not found!"; + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Description of the parameter: name of the ros2 node"; + return false; + } + prop_check.unput("node_name"); + } + + //Parser of parameter color_topic_name + { + if (config.check("color_topic_name")) + { + m_color_topic_name = config.find("color_topic_name").asString(); + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Parameter 'color_topic_name' using value:" << m_color_topic_name; + } + else + { + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Mandatory parameter 'color_topic_name' not found!"; + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Description of the parameter: ros rgb topic (it's also the base name for the rgb camera_info topic)"; + return false; + } + prop_check.unput("color_topic_name"); + } + + //Parser of parameter depth_topic_name + { + if (config.check("depth_topic_name")) + { + m_depth_topic_name = config.find("depth_topic_name").asString(); + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Parameter 'depth_topic_name' using value:" << m_depth_topic_name; + } + else + { + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Mandatory parameter 'depth_topic_name' not found!"; + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Description of the parameter: ros depth topic (it's also the base name for the depth camera_info topic)"; + return false; + } + prop_check.unput("depth_topic_name"); + } + + //Parser of parameter verbose_on + { + if (config.check("verbose_on")) + { + m_verbose_on = config.find("verbose_on").asInt64(); + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Parameter 'verbose_on' using value:" << m_verbose_on; + } + else + { + yCInfo(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "Parameter 'verbose_on' using DEFAULT value:" << m_verbose_on; + } + prop_check.unput("verbose_on"); + } + + /* + //This code check if the user set some parameter which are not check by the parser + //If the parser is set in strict mode, this will generate an error + if (prop_check.size() > 0) + { + bool extra_params_found = false; + for (auto it=prop_check.begin(); it!=prop_check.end(); it++) + { + if (m_parser_is_strict) + { + yCError(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(RgbdSensor_nwc_ros2ParamsCOMPONENT) << "User asking for parameter: "<< it->name <<" which is unknown to this parser!"; + } + } + + if (m_parser_is_strict && extra_params_found) + { + return false; + } + } + */ + return true; +} + + +std::string RgbdSensor_nwc_ros2_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: RgbdSensor_nwc_ros2\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("This is the list of the parameters accepted by the device:\n"); + doc = doc + std::string("'node_name': name of the ros2 node\n"); + doc = doc + std::string("'color_topic_name': ros rgb topic (it's also the base name for the rgb camera_info topic)\n"); + doc = doc + std::string("'depth_topic_name': ros depth topic (it's also the base name for the depth camera_info topic)\n"); + doc = doc + std::string("'verbose_on': if 1, it enables the verbose mode of the device\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("Here are some examples of invocation command with yarpdev, with all params:\n"); + doc = doc + " yarpdev --device rgbdSensor_nwc_ros2 --node_name --color_topic_name --depth_topic_name --verbose_on 0\n"; + doc = doc + std::string("Using only mandatory params:\n"); + doc = doc + " yarpdev --device rgbdSensor_nwc_ros2 --node_name --color_topic_name --depth_topic_name \n"; + doc = doc + std::string("=============================================\n\n"); return doc; +} diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.h b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.h new file mode 100644 index 0000000..5600511 --- /dev/null +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_ParamsParser.h @@ -0,0 +1,78 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Fri Aug 30 12:59:16 2024 + + +#ifndef RGBDSENSOR_NWC_ROS2_PARAMSPARSER_H +#define RGBDSENSOR_NWC_ROS2_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class RgbdSensor_nwc_ros2. +* +* These are the used parameters: +* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:----------:|:----------------:|:------:|:-----:|:-------------:|:--------:|:-------------------------------------------------------------------------:|:-----------------------------:| +* | - | node_name | string | - | - | 1 | name of the ros2 node | - | +* | - | color_topic_name | string | - | - | 1 | ros rgb topic (it's also the base name for the rgb camera_info topic) | must start with a leading '/' | +* | - | depth_topic_name | string | - | - | 1 | ros depth topic (it's also the base name for the depth camera_info topic) | must start with a leading '/' | +* | - | verbose_on | int | - | 0 | 0 | if 1, it enables the verbose mode of the device | - | +* +* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): +* \code{.unparsed} +* yarpdev --device rgbdSensor_nwc_ros2 --node_name --color_topic_name --depth_topic_name --verbose_on 0 +* \endcode +* +* \code{.unparsed} +* yarpdev --device rgbdSensor_nwc_ros2 --node_name --color_topic_name --depth_topic_name +* \endcode +* +*/ + +class RgbdSensor_nwc_ros2_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + RgbdSensor_nwc_ros2_ParamsParser(); + ~RgbdSensor_nwc_ros2_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"RgbdSensor_nwc_ros2"}; + const std::string m_device_name = {"rgbdSensor_nwc_ros2"}; + bool m_parser_is_strict = false; + struct parser_version_type + { + int major = 1; + int minor = 0; + }; + const parser_version_type m_parser_version = {}; + + const std::string m_node_name_defaultValue = {""}; + const std::string m_color_topic_name_defaultValue = {""}; + const std::string m_depth_topic_name_defaultValue = {""}; + const std::string m_verbose_on_defaultValue = {"0"}; + + std::string m_node_name = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::string m_color_topic_name = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::string m_depth_topic_name = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + int m_verbose_on = {0}; + + bool parseParams(const yarp::os::Searchable & config) override; + std::string getDeviceClassName() const override { return m_device_classname; } + std::string getDeviceName() const override { return m_device_name; } + std::string getDocumentationOfDeviceParams() const override; + std::vector getListOfParams() const override; +}; + +#endif diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_params.md b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_params.md new file mode 100644 index 0000000..bcea523 --- /dev/null +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2_params.md @@ -0,0 +1,4 @@ +| | node_name | string | - | - | Yes | name of the ros2 node | | +| | color_topic_name | string | - | - | Yes | ros rgb topic (it's also the base name for the rgb camera_info topic) | must start with a leading '/' | +| | depth_topic_name | string | - | - | Yes | ros depth topic (it's also the base name for the depth camera_info topic) | must start with a leading '/' | +| | verbose_on | int | - | 0 | No | if 1, it enables the verbose mode of the device | | \ No newline at end of file