Skip to content

Commit

Permalink
Added ParamsParser to RgbdToPointCloudSensor_nws_ros2
Browse files Browse the repository at this point in the history
Signed-off-by: Ettore Landini <[email protected]>
  • Loading branch information
elandini84 committed Sep 2, 2024
1 parent 1626bb1 commit 502cc7c
Show file tree
Hide file tree
Showing 6 changed files with 251 additions and 42 deletions.
3 changes: 3 additions & 0 deletions src/devices/rgbdToPointCloudSensor_nws_ros2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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 $<TARGET_OBJECTS:Ros2Utils>)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<param<std::string>> 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;
}
Expand All @@ -84,8 +61,8 @@ bool RgbdToPointCloudSensor_nws_ros2::fromConfig(yarp::os::Searchable &config)
bool RgbdToPointCloudSensor_nws_ros2::initialize_ROS2(yarp::os::Searchable &params)
{

m_node = NodeCreator::createNode(m_nodeName);
m_rosPublisher_pointCloud2 = m_node->create_publisher<sensor_msgs::msg::PointCloud2>(m_pointCloudTopicName, 10);
m_node = NodeCreator::createNode(m_node_name);
m_rosPublisher_pointCloud2 = m_node->create_publisher<sensor_msgs::msg::PointCloud2>(m_topic_name, 10);
return true;
}

Expand Down Expand Up @@ -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());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <mutex>
#include "RgbdToPointCloudSensor_nws_ros2_ParamsParser.h"

namespace RGBDToPointCloudRos2Impl {

Expand All @@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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};
Expand Down
Original file line number Diff line number Diff line change
@@ -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 <yarp/os/LogStream.h>
#include <yarp/os/Value.h>

namespace {
YARP_LOG_COMPONENT(RgbdToPointCloudSensor_nws_ros2ParamsCOMPONENT, "yarp.device.RgbdToPointCloudSensor_nws_ros2")
}


RgbdToPointCloudSensor_nws_ros2_ParamsParser::RgbdToPointCloudSensor_nws_ros2_ParamsParser()
{
}


std::vector<std::string> RgbdToPointCloudSensor_nws_ros2_ParamsParser::getListOfParams() const
{
std::vector<std::string> 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: "<<it->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 <mandatory_value> --frame_id <mandatory_value> --node_name <mandatory_value>\n";
doc = doc + std::string("Using only mandatory params:\n");
doc = doc + " yarpdev --device rgbdToPointCloudSensor_nws_ros2 --topic_name <mandatory_value> --frame_id <mandatory_value> --node_name <mandatory_value>\n";
doc = doc + std::string("=============================================\n\n"); return doc;
}
Original file line number Diff line number Diff line change
@@ -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 <yarp/os/Searchable.h>
#include <yarp/dev/IDeviceDriverParams.h>
#include <string>
#include <cmath>

/**
* 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 <mandatory_value> --frame_id <mandatory_value> --node_name <mandatory_value>
* \endcode
*
* \code{.unparsed}
* yarpdev --device rgbdToPointCloudSensor_nws_ros2 --topic_name <mandatory_value> --frame_id <mandatory_value> --node_name <mandatory_value>
* \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<std::string> getListOfParams() const override;
};

#endif
Original file line number Diff line number Diff line change
@@ -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 '/' |

0 comments on commit 502cc7c

Please sign in to comment.