From 502cc7c752185654b3348a50a0399b76ed58f869 Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Mon, 2 Sep 2024 10:56:27 +0200 Subject: [PATCH] Added ParamsParser to RgbdToPointCloudSensor_nws_ros2 Signed-off-by: Ettore Landini --- .../CMakeLists.txt | 3 + .../RgbdToPointCloudSensor_nws_ros2.cpp | 33 +--- .../RgbdToPointCloudSensor_nws_ros2.h | 18 +- ...PointCloudSensor_nws_ros2_ParamsParser.cpp | 157 ++++++++++++++++++ ...ToPointCloudSensor_nws_ros2_ParamsParser.h | 78 +++++++++ .../RgbdToPointCloudSensor_nws_ros2_params.md | 4 + 6 files changed, 251 insertions(+), 42 deletions(-) create mode 100644 src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.cpp create mode 100644 src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.h create mode 100644 src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_params.md diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/CMakeLists.txt b/src/devices/rgbdToPointCloudSensor_nws_ros2/CMakeLists.txt index 014f5f7..9a50735 100644 --- a/src/devices/rgbdToPointCloudSensor_nws_ros2/CMakeLists.txt +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/CMakeLists.txt @@ -11,11 +11,14 @@ yarp_prepare_plugin(rgbdToPointCloudSensor_nws_ros2 if(NOT SKIP_rgbdToPointCloudSensor_nws_ros2) yarp_add_plugin(yarp_rgbdToPointCloudSensor_nws_ros2) + generateDeviceParamsParser(RgbdToPointCloudSensor_nws_ros2 rgbdToPointCloudSensor_nws_ros2) target_sources(yarp_rgbdToPointCloudSensor_nws_ros2 PRIVATE RgbdToPointCloudSensor_nws_ros2.cpp RgbdToPointCloudSensor_nws_ros2.h + RgbdToPointCloudSensor_nws_ros2_ParamsParser.cpp + RgbdToPointCloudSensor_nws_ros2_ParamsParser.h ) target_sources(yarp_rgbdToPointCloudSensor_nws_ros2 PRIVATE $) diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp index 9f13ec7..e875673 100644 --- a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp @@ -51,31 +51,8 @@ bool RgbdToPointCloudSensor_nws_ros2::open(yarp::os::Searchable &config) bool RgbdToPointCloudSensor_nws_ros2::fromConfig(yarp::os::Searchable &config) { - if (!config.check("period", "refresh period of the broadcasted values in s")) { - yCDebug(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Using default 'period' parameter of " << DEFAULT_THREAD_PERIOD << "s"; - } else { - setPeriod(config.find("period").asFloat64()); - } - - //check if param exist and assign it to corresponding variable.. if it doesn't, initialize the variable with default value. - std::vector> rosStringParam; - - rosStringParam.emplace_back(m_nodeName, nodeName_param ); - rosStringParam.emplace_back(m_rosFrameId, frameId_param ); - rosStringParam.emplace_back(m_pointCloudTopicName, pointCloudTopicName_param ); - - for (auto &prm : rosStringParam) { - if (!config.check(prm.parname)) { - yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Missing " << prm.parname << "check your configuration file"; - return false; - } - *(prm.var) = config.find(prm.parname).asString(); - } - - if (config.check("m_forceInfoSync")) - { - m_forceInfoSync = config.find("m_forceInfoSync").asBool(); - } + parseParams(config); + setPeriod(m_period); return true; } @@ -84,8 +61,8 @@ bool RgbdToPointCloudSensor_nws_ros2::fromConfig(yarp::os::Searchable &config) bool RgbdToPointCloudSensor_nws_ros2::initialize_ROS2(yarp::os::Searchable ¶ms) { - m_node = NodeCreator::createNode(m_nodeName); - m_rosPublisher_pointCloud2 = m_node->create_publisher(m_pointCloudTopicName, 10); + m_node = NodeCreator::createNode(m_node_name); + m_rosPublisher_pointCloud2 = m_node->create_publisher(m_topic_name, 10); return true; } @@ -214,7 +191,7 @@ bool RgbdToPointCloudSensor_nws_ros2::writeData() sensor_msgs::msg::PointCloud2 pc2Ros; // filling ros header - pc2Ros.header.frame_id = m_rosFrameId; + pc2Ros.header.frame_id = m_frame_id; //pc2Ros.header.stamp.sec = depthStamp.; pc2Ros.header.stamp.sec = int(depthStamp.getTime()); diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.h b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.h index 50b7485..8f040f6 100644 --- a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.h +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.h @@ -18,6 +18,7 @@ #include #include +#include "RgbdToPointCloudSensor_nws_ros2_ParamsParser.h" namespace RGBDToPointCloudRos2Impl { @@ -37,13 +38,7 @@ const std::string pointCloudTopicName_param = "topic_name"; * derived fron the combination of the data derived from Framegrabber and IDepthSensor interfaces. * See they documentation for more details about each interface. * - * Parameters required by this device are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:----------------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:------------------------------: |:---------------------------------------------------------------------------------------------------:|:-----:| - * | period | - | double | s | 0.03 | No | refresh period of the broadcasted values in ms | default 20ms | - * | topic_name | - | string | - | - | Yes | set the name for ROS point cloud topic | must start with a leading '/' | - * | frame_id | - | string | - | | Yes | set the name of the reference frame | | - * | node_name | - | string | - | - | Yes | set the name for ROS node | must start with a leading '/' | + * Parameters required by this device are described in class: RgbdToPointClpidSensor_nws_ros2_ParamsParser * * ROS2 message type used is sensor_msgs/PointCloud2.msg ( https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) * Some example of configuration files: @@ -63,7 +58,8 @@ const std::string pointCloudTopicName_param = "topic_name"; class RgbdToPointCloudSensor_nws_ros2 : public yarp::dev::DeviceDriver, public yarp::dev::WrapperSingle, - public yarp::os::PeriodicThread + public yarp::os::PeriodicThread, + RgbdToPointCloudSensor_nws_ros2_ParamsParser { private: @@ -91,12 +87,6 @@ class RgbdToPointCloudSensor_nws_ros2 : std::string parname; }; - - - std::string m_nodeName; - std::string m_pointCloudTopicName; - std::string m_rosFrameId; - UInt nodeSeq {0}; yarp::dev::IRGBDSensor* m_sensor_p {nullptr}; diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.cpp b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.cpp new file mode 100644 index 0000000..e155491 --- /dev/null +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_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: Mon Sep 2 10:47:32 2024 + + +#include "RgbdToPointCloudSensor_nws_ros2_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT, "yarp.device.RgbdToPointCloudSensor_nws_ros2") +} + + +RgbdToPointCloudSensor_nws_ros2_ParamsParser::RgbdToPointCloudSensor_nws_ros2_ParamsParser() +{ +} + + +std::vector RgbdToPointCloudSensor_nws_ros2_ParamsParser::getListOfParams() const +{ + std::vector params; + params.push_back("period"); + params.push_back("topic_name"); + params.push_back("frame_id"); + params.push_back("node_name"); + return params; +} + + +bool RgbdToPointCloudSensor_nws_ros2_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << getDocumentationOfDeviceParams(); + } + + std::string config_string = config.toString(); + yarp::os::Property prop_check(config_string.c_str()); + //Parser of parameter period + { + if (config.check("period")) + { + m_period = config.find("period").asFloat64(); + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'period' using value:" << m_period; + } + else + { + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'period' using DEFAULT value:" << m_period; + } + prop_check.unput("period"); + } + + //Parser of parameter topic_name + { + if (config.check("topic_name")) + { + m_topic_name = config.find("topic_name").asString(); + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_name' using value:" << m_topic_name; + } + else + { + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Mandatory parameter 'topic_name' not found!"; + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Description of the parameter: set the name for ROS point cloud topic"; + return false; + } + prop_check.unput("topic_name"); + } + + //Parser of parameter frame_id + { + if (config.check("frame_id")) + { + m_frame_id = config.find("frame_id").asString(); + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'frame_id' using value:" << m_frame_id; + } + else + { + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Mandatory parameter 'frame_id' not found!"; + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Description of the parameter: set the name of the reference frame"; + return false; + } + prop_check.unput("frame_id"); + } + + //Parser of parameter node_name + { + if (config.check("node_name")) + { + m_node_name = config.find("node_name").asString(); + yCInfo(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'node_name' using value:" << m_node_name; + } + else + { + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Mandatory parameter 'node_name' not found!"; + yCError(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "Description of the parameter: set the name for ROS node"; + return false; + } + prop_check.unput("node_name"); + } + + /* + //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(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(RgbdToPointCloudSensor_nws_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 RgbdToPointCloudSensor_nws_ros2_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: RgbdToPointCloudSensor_nws_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("'period': refresh period of the broadcasted values in ms\n"); + doc = doc + std::string("'topic_name': set the name for ROS point cloud topic\n"); + doc = doc + std::string("'frame_id': set the name of the reference frame\n"); + doc = doc + std::string("'node_name': set the name for ROS node\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 rgbdToPointCloudSensor_nws_ros2 --period 0.03 --topic_name --frame_id --node_name \n"; + doc = doc + std::string("Using only mandatory params:\n"); + doc = doc + " yarpdev --device rgbdToPointCloudSensor_nws_ros2 --topic_name --frame_id --node_name \n"; + doc = doc + std::string("=============================================\n\n"); return doc; +} diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.h b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_ParamsParser.h new file mode 100644 index 0000000..3e0216e --- /dev/null +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_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: Mon Sep 2 10:47:32 2024 + + +#ifndef RGBDTOPOINTCLOUDSENSOR_NWS_ROS2_PARAMSPARSER_H +#define RGBDTOPOINTCLOUDSENSOR_NWS_ROS2_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class RgbdToPointCloudSensor_nws_ros2. +* +* These are the used parameters: +* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:----------:|:--------------:|:------:|:-----:|:-------------:|:--------:|:----------------------------------------------:|:-----------------------------:| +* | - | period | double | s | 0.03 | 0 | refresh period of the broadcasted values in ms | default 20ms | +* | - | topic_name | string | - | - | 1 | set the name for ROS point cloud topic | must start with a leading '/' | +* | - | frame_id | string | - | - | 1 | set the name of the reference frame | - | +* | - | node_name | string | - | - | 1 | set the name for ROS node | must start with a leading '/' | +* +* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): +* \code{.unparsed} +* yarpdev --device rgbdToPointCloudSensor_nws_ros2 --period 0.03 --topic_name --frame_id --node_name +* \endcode +* +* \code{.unparsed} +* yarpdev --device rgbdToPointCloudSensor_nws_ros2 --topic_name --frame_id --node_name +* \endcode +* +*/ + +class RgbdToPointCloudSensor_nws_ros2_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + RgbdToPointCloudSensor_nws_ros2_ParamsParser(); + ~RgbdToPointCloudSensor_nws_ros2_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"RgbdToPointCloudSensor_nws_ros2"}; + const std::string m_device_name = {"rgbdToPointCloudSensor_nws_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_period_defaultValue = {"0.03"}; + const std::string m_topic_name_defaultValue = {""}; + const std::string m_frame_id_defaultValue = {""}; + const std::string m_node_name_defaultValue = {""}; + + double m_period = {0.03}; + std::string m_topic_name = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::string m_frame_id = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::string m_node_name = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + + 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/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_params.md b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_params.md new file mode 100644 index 0000000..271c273 --- /dev/null +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2_params.md @@ -0,0 +1,4 @@ +| | period | double | s | 0.03 | No | refresh period of the broadcasted values in ms | default 20ms | +| | topic_name | string | - | - | Yes | set the name for ROS point cloud topic | must start with a leading '/' | +| | frame_id | string | - | | Yes | set the name of the reference frame | | +| | node_name | string | - | - | Yes | set the name for ROS node | must start with a leading '/' | \ No newline at end of file