Skip to content

Commit

Permalink
Added ParamsParser to FrameTransformSet_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 Aug 26, 2024
1 parent fdb6f33 commit 76bf4d5
Show file tree
Hide file tree
Showing 6 changed files with 301 additions and 58 deletions.
3 changes: 3 additions & 0 deletions src/devices/frameTransformSet_nwc_ros2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,14 @@ yarp_prepare_plugin(frameTransformSet_nwc_ros2

if(NOT SKIP_frameTransformGet_nwc_ros2)
yarp_add_plugin(yarp_frameTransformSet_nwc_ros2)
generateDeviceParamsParser(FrameTransformSet_nwc_ros2 frameTransformSet_nwc_ros2)

target_sources(yarp_frameTransformSet_nwc_ros2
PRIVATE
frameTransformSet_nwc_ros2.cpp
frameTransformSet_nwc_ros2.h
FrameTransformSet_nwc_ros2_ParamsParser.h
FrameTransformSet_nwc_ros2_ParamsParser.cpp
)

target_include_directories(yarp_frameTransformSet_nwc_ros2
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,195 @@
/*
* 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 Aug 26 14:52:35 2024


#include "FrameTransformSet_nwc_ros2_ParamsParser.h"
#include <yarp/os/LogStream.h>
#include <yarp/os/Value.h>

namespace {
YARP_LOG_COMPONENT(FrameTransformSet_nwc_ros2ParamsCOMPONENT, "yarp.device.FrameTransformSet_nwc_ros2")
}


FrameTransformSet_nwc_ros2_ParamsParser::FrameTransformSet_nwc_ros2_ParamsParser()
{
}


std::vector<std::string> FrameTransformSet_nwc_ros2_ParamsParser::getListOfParams() const
{
std::vector<std::string> params;
params.push_back("GENERAL::refresh_interval");
params.push_back("GENERAL::period");
params.push_back("GENERAL::asynch_pub");
params.push_back("ROS2::ft_node");
params.push_back("ROS2::ft_topic");
params.push_back("ROS2::ft_topic_static");
return params;
}


bool FrameTransformSet_nwc_ros2_ParamsParser::parseParams(const yarp::os::Searchable & config)
{
//Check for --help option
if (config.check("help"))
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << getDocumentationOfDeviceParams();
}

std::string config_string = config.toString();
yarp::os::Property prop_check(config_string.c_str());
//Parser of parameter GENERAL::refresh_interval
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("GENERAL");
if (sectionp.check("refresh_interval"))
{
m_GENERAL_refresh_interval = sectionp.find("refresh_interval").asFloat64();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::refresh_interval' using value:" << m_GENERAL_refresh_interval;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::refresh_interval' using DEFAULT value:" << m_GENERAL_refresh_interval;
}
prop_check.unput("GENERAL::refresh_interval");
}

//Parser of parameter GENERAL::period
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("GENERAL");
if (sectionp.check("period"))
{
m_GENERAL_period = sectionp.find("period").asFloat64();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::period' using value:" << m_GENERAL_period;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::period' using DEFAULT value:" << m_GENERAL_period;
}
prop_check.unput("GENERAL::period");
}

//Parser of parameter GENERAL::asynch_pub
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("GENERAL");
if (sectionp.check("asynch_pub"))
{
m_GENERAL_asynch_pub = sectionp.find("asynch_pub").asInt64();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::asynch_pub' using value:" << m_GENERAL_asynch_pub;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'GENERAL::asynch_pub' using DEFAULT value:" << m_GENERAL_asynch_pub;
}
prop_check.unput("GENERAL::asynch_pub");
}

//Parser of parameter ROS2::ft_node
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("ROS2");
if (sectionp.check("ft_node"))
{
m_ROS2_ft_node = sectionp.find("ft_node").asString();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_node' using value:" << m_ROS2_ft_node;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_node' using DEFAULT value:" << m_ROS2_ft_node;
}
prop_check.unput("ROS2::ft_node");
}

//Parser of parameter ROS2::ft_topic
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("ROS2");
if (sectionp.check("ft_topic"))
{
m_ROS2_ft_topic = sectionp.find("ft_topic").asString();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_topic' using value:" << m_ROS2_ft_topic;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_topic' using DEFAULT value:" << m_ROS2_ft_topic;
}
prop_check.unput("ROS2::ft_topic");
}

//Parser of parameter ROS2::ft_topic_static
{
yarp::os::Bottle sectionp;
sectionp = config.findGroup("ROS2");
if (sectionp.check("ft_topic_static"))
{
m_ROS2_ft_topic_static = sectionp.find("ft_topic_static").asString();
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_topic_static' using value:" << m_ROS2_ft_topic_static;
}
else
{
yCInfo(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "Parameter 'ROS2::ft_topic_static' using DEFAULT value:" << m_ROS2_ft_topic_static;
}
prop_check.unput("ROS2::ft_topic_static");
}

/*
//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(FrameTransformSet_nwc_ros2ParamsCOMPONENT) << "User asking for parameter: "<<it->name <<" which is unknown to this parser!";
extra_params_found = true;
}
else
{
yCWarning(FrameTransformSet_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 FrameTransformSet_nwc_ros2_ParamsParser::getDocumentationOfDeviceParams() const
{
std::string doc;
doc = doc + std::string("\n=============================================\n");
doc = doc + std::string("This is the help for device: FrameTransformSet_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("'GENERAL::refresh_interval': The time interval outside which timed ft will be deleted\n");
doc = doc + std::string("'GENERAL::period': The PeriodicThread period in seconds\n");
doc = doc + std::string("'GENERAL::asynch_pub': If 1, the fts will be published not only every \"period\" seconds but also when set functions are called\n");
doc = doc + std::string("'ROS2::ft_node': The name of the ROS2 node\n");
doc = doc + std::string("'ROS2::ft_topic': The name of the ROS2 topic from which fts will be received\n");
doc = doc + std::string("'ROS2::ft_topic_static': The name of the ROS2 topic from which static fts will be received\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 frameTransformSet_nwc_ros2 --GENERAL::refresh_interval 0.1 --GENERAL::period 0.01 --GENERAL::asynch_pub 1 --ROS2::ft_node tfNodeSet --ROS2::ft_topic /tf --ROS2::ft_topic_static /tf_static\n";
doc = doc + std::string("Using only mandatory params:\n");
doc = doc + " yarpdev --device frameTransformSet_nwc_ros2\n";
doc = doc + std::string("=============================================\n\n"); return doc;
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
/*
* 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 Aug 26 14:52:35 2024


#ifndef FRAMETRANSFORMSET_NWC_ROS2_PARAMSPARSER_H
#define FRAMETRANSFORMSET_NWC_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 FrameTransformSet_nwc_ros2.
*
* These are the used parameters:
* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes |
* |:----------:|:----------------:|:------:|:-------:|:-------------:|:--------:|:--------------------------------------------------------------------------------------------------------:|:-----:|
* | GENERAL | refresh_interval | double | seconds | 0.1 | 0 | The time interval outside which timed ft will be deleted | - |
* | GENERAL | period | double | seconds | 0.01 | 0 | The PeriodicThread period in seconds | - |
* | GENERAL | asynch_pub | int | - | 1 | 0 | If 1, the fts will be published not only every \"period\" seconds but also when set functions are called | - |
* | ROS2 | ft_node | string | - | tfNodeSet | 0 | The name of the ROS2 node | - |
* | ROS2 | ft_topic | string | - | /tf | 0 | The name of the ROS2 topic from which fts will be received | - |
* | ROS2 | ft_topic_static | string | - | /tf_static | 0 | The name of the ROS2 topic from which static fts will be received | - |
*
* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters):
* \code{.unparsed}
* yarpdev --device frameTransformSet_nwc_ros2 --GENERAL::refresh_interval 0.1 --GENERAL::period 0.01 --GENERAL::asynch_pub 1 --ROS2::ft_node tfNodeSet --ROS2::ft_topic /tf --ROS2::ft_topic_static /tf_static
* \endcode
*
* \code{.unparsed}
* yarpdev --device frameTransformSet_nwc_ros2
* \endcode
*
*/

class FrameTransformSet_nwc_ros2_ParamsParser : public yarp::dev::IDeviceDriverParams
{
public:
FrameTransformSet_nwc_ros2_ParamsParser();
~FrameTransformSet_nwc_ros2_ParamsParser() override = default;

public:
const std::string m_device_classname = {"FrameTransformSet_nwc_ros2"};
const std::string m_device_name = {"frameTransformSet_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_GENERAL_refresh_interval_defaultValue = {"0.1"};
const std::string m_GENERAL_period_defaultValue = {"0.01"};
const std::string m_GENERAL_asynch_pub_defaultValue = {"1"};
const std::string m_ROS2_ft_node_defaultValue = {"tfNodeSet"};
const std::string m_ROS2_ft_topic_defaultValue = {"/tf"};
const std::string m_ROS2_ft_topic_static_defaultValue = {"/tf_static"};

double m_GENERAL_refresh_interval = {0.1};
double m_GENERAL_period = {0.01};
int m_GENERAL_asynch_pub = {1};
std::string m_ROS2_ft_node = {"tfNodeSet"};
std::string m_ROS2_ft_topic = {"/tf"};
std::string m_ROS2_ft_topic_static = {"/tf_static"};

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,6 @@
| GENERAL | refresh_interval | double | seconds | 0.1 | No | The time interval outside which timed ft will be deleted |
| GENERAL | period | double | seconds | 0.01 | No | The PeriodicThread period in seconds |
| GENERAL | asynch_pub | int | - | 1 | No | If 1, the fts will be published not only every "period" seconds but also when set functions are called|
| ROS2 | ft_node | string | - | tfNodeSet | No | The name of the ROS2 node |
| ROS2 | ft_topic | string | - | /tf | No | The name of the ROS2 topic from which fts will be received |
| ROS2 | ft_topic_static | string | - | /tf_static | No | The name of the ROS2 topic from which static fts will be received |
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,7 @@ YARP_LOG_COMPONENT(FRAMETRANSFORMSETNWCROS2, "yarp.device.frameTransformSet_nwc_
//------------------------------------------------------------------------------------------------------------------------------

FrameTransformSet_nwc_ros2::FrameTransformSet_nwc_ros2(double tperiod) :
PeriodicThread(tperiod),
m_period(tperiod)
PeriodicThread(tperiod)
{
}

Expand All @@ -33,39 +32,13 @@ bool FrameTransformSet_nwc_ros2::open(yarp::os::Searchable& config)
return false;
}

bool okGeneral = config.check("GENERAL");
if(okGeneral)
{
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
if (general_config.check("period"))
{
m_period = general_config.find("period").asFloat64();
setPeriod(m_period);
}
if (general_config.check("refresh_interval")) {m_refreshInterval = general_config.find("refresh_interval").asFloat64();}
if (general_config.check("asynch_pub")) {m_asynchPub = general_config.find("asynch_pub").asInt16();}
}

m_ftContainer.m_timeout = m_refreshInterval;

//ROS2 configuration
if (config.check("ROS2"))
{
yCInfo(FRAMETRANSFORMSETNWCROS2, "Configuring ROS2 params");
Bottle ROS2_config = config.findGroup("ROS2");
if(ROS2_config.check("ft_node")) m_ftNodeName = ROS2_config.find("ft_node").asString();
if(ROS2_config.check("ft_topic")) m_ftTopic = ROS2_config.find("ft_topic").asString();
if(ROS2_config.check("ft_topic_static")) m_ftTopicStatic = ROS2_config.find("ft_topic_static").asString();
}
else
{
//no ROS2 options
yCWarning(FRAMETRANSFORMSETNWCROS2) << "ROS2 Group not configured";
}
parseParams(config);

m_node = NodeCreator::createNode(m_ftNodeName);
m_publisherFtTimed = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_ftTopic, 10);
m_publisherFtStatic = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_ftTopicStatic, rclcpp::QoS(10).reliable().durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL));
m_ftContainer.m_timeout = m_GENERAL_refresh_interval;
setPeriod(m_GENERAL_period);
m_node = NodeCreator::createNode(m_ROS2_ft_node);
m_publisherFtTimed = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_ROS2_ft_topic, 10);
m_publisherFtStatic = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_ROS2_ft_topic_static, rclcpp::QoS(10).reliable().durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL));

start();

Expand Down Expand Up @@ -100,7 +73,7 @@ bool FrameTransformSet_nwc_ros2::setTransforms(const std::vector<yarp::math::Fra
yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms");
return false;
}
if(m_asynchPub)
if(m_GENERAL_asynch_pub)
{
if (!publishFrameTransforms())
{
Expand All @@ -119,7 +92,7 @@ bool FrameTransformSet_nwc_ros2::setTransform(const yarp::math::FrameTransform&
yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform");
return false;
}
if(m_asynchPub)
if(m_GENERAL_asynch_pub)
{
if (!publishFrameTransforms())
{
Expand Down
Loading

0 comments on commit 76bf4d5

Please sign in to comment.