Skip to content

Commit

Permalink
Added ParamsParser to MultipleAnalogSensors_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 5c0aaf5 commit 07ca1a7
Show file tree
Hide file tree
Showing 7 changed files with 239 additions and 60 deletions.
5 changes: 5 additions & 0 deletions src/devices/multipleAnalogSensors_nws_ros2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,15 @@ yarp_prepare_plugin(wrenchStamped_nws_ros2

if(ENABLE_wrenchStamped_nws_ros2)
yarp_add_plugin(yarp_wrenchStamped_nws_ros2)
generateDeviceParamsParser(GenericSensor_nws_ros2 genericSensor_nws_ros2)

target_sources(yarp_wrenchStamped_nws_ros2
PRIVATE
WrenchStamped_nws_ros2.cpp
WrenchStamped_nws_ros2.h
GenericSensor_nws_ros2.h
GenericSensor_nws_ros2_ParamsParser.h
GenericSensor_nws_ros2_ParamsParser.cpp
)

target_link_libraries(yarp_wrenchStamped_nws_ros2
Expand Down Expand Up @@ -69,6 +72,8 @@ if(ENABLE_imu_nws_ros2)
Imu_nws_ros2.cpp
Imu_nws_ros2.h
GenericSensor_nws_ros2.h
GenericSensor_nws_ros2_ParamsParser.h
GenericSensor_nws_ros2_ParamsParser.cpp
)

target_link_libraries(yarp_imu_nws_ros2
Expand Down
61 changes: 13 additions & 48 deletions src/devices/multipleAnalogSensors_nws_ros2/GenericSensor_nws_ros2.h
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include <yarp/os/Log.h>
#include <yarp/os/LogComponent.h>
#include <yarp/os/LogStream.h>
#include "GenericSensor_nws_ros2_ParamsParser.h"

#include <rclcpp/rclcpp.hpp>
#include <Ros2Utils.h>
Expand All @@ -31,24 +32,18 @@ YARP_DECLARE_LOG_COMPONENT(GENERICSENSOR_NWS_ROS2)
* |:-----------------:|
* | `GenericSensor_nws_ros2` |
*
* The parameters accepted by this device are:
* | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
* |:--------------:|:--------------:|:-------:|:--------------:|:----------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:|
* | topic_name | - | string | - | - | Yes | The name of the ROS topic opened by this device. | MUST start with a '/' character |
* | node_name | - | string | - | - | Yes | The name of the ROS node opened by this device | Autogenerated by default |
* | period | - | double | s | - | Yes | Refresh period of the broadcasted values in seconds | |
* The parameters accepted by this device are shown in class: GenericSensor_nws_ros2_ParamsParser
*
*/

template <class ROS_MSG>
class GenericSensor_nws_ros2 :
public yarp::os::PeriodicThread,
public yarp::dev::DeviceDriver,
public yarp::dev::IMultipleWrapper
public yarp::dev::IMultipleWrapper,
protected GenericSensor_nws_ros2_ParamsParser
{
protected:
double m_periodInS{0.01};
std::string m_publisherName;
std::string m_rosNodeName;
rclcpp::Node::SharedPtr m_node;
typename rclcpp::Publisher<ROS_MSG>::SharedPtr m_publisher;
yarp::dev::PolyDriver* m_poly;
Expand Down Expand Up @@ -93,57 +88,27 @@ GenericSensor_nws_ros2<ROS_MSG>::~GenericSensor_nws_ros2() = default;
template <class ROS_MSG>
bool GenericSensor_nws_ros2<ROS_MSG>::open(yarp::os::Searchable & config)
{
if (!config.check("topic_name")) {
yCError(GENERICSENSOR_NWS_ROS2, "Missing `topic_name` parameter, exiting.");
return false;
}

if (!config.check("period")) {
yCError(GENERICSENSOR_NWS_ROS2, "Missing `period` parameter, exiting.");
return false;
}

if (config.find("period").isFloat32()==false && config.find("period").isFloat64()==false) {
yCError(GENERICSENSOR_NWS_ROS2, "`period` parameter is present but it is not a float, exiting.");
return false;
}

m_periodInS = config.find("period").asFloat64();

if (m_periodInS <= 0) {
yCError(GENERICSENSOR_NWS_ROS2, "`period` parameter is present (%f) but it is not a positive value, exiting.", m_periodInS);
return false;
}

if (!config.check("node_name"))
{
yCError(GENERICSENSOR_NWS_ROS2) << "Missing node_name parameter";
return false;
}

m_rosNodeName = config.find("node_name").asString();
if (m_rosNodeName.c_str()[0] == '/') {
parseParams(config);
if (m_node_name.c_str()[0] == '/') {
yCError(GENERICSENSOR_NWS_ROS2) << "node name cannot begin with /";
return false;
}

m_publisherName = config.find("topic_name").asString();

if (m_publisherName.c_str()[0] != '/') {
if (m_topic_name.c_str()[0] != '/') {
yCError(GENERICSENSOR_NWS_ROS2) << "Missing '/' in topic_name parameter";
return false;
}

m_node = NodeCreator::createNode(m_rosNodeName); // add a ROS node
m_publisher = m_node->create_publisher<ROS_MSG>(m_publisherName,rclcpp::QoS(10));
m_node = NodeCreator::createNode(m_node_name); // add a ROS node
m_publisher = m_node->create_publisher<ROS_MSG>(m_topic_name,rclcpp::QoS(10));

if (m_node == nullptr) {
yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_rosNodeName << " Node, check your yarp-ROS network configuration\n";
yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_node_name << " Node, check your yarp-ROS network configuration\n";
return false;
}

if (m_publisher == nullptr) {
yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_publisherName << " Topic, check your yarp-ROS network configuration\n";
yCError(GENERICSENSOR_NWS_ROS2) << "Opening " << m_topic_name << " Topic, check your yarp-ROS network configuration\n";
return false;
}

Expand Down Expand Up @@ -190,7 +155,7 @@ bool GenericSensor_nws_ros2<ROS_MSG>::attachAll(const yarp::dev::PolyDriverList
bool ok = viewInterfaces();

// Set rate period
ok &= this->setPeriod(m_periodInS);
ok &= this->setPeriod(m_period);
ok &= this->start();

return ok;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,139 @@
/*
* 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 5 16:25:52 2024


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

namespace {
YARP_LOG_COMPONENT(GenericSensor_nws_ros2ParamsCOMPONENT, "yarp.device.GenericSensor_nws_ros2")
}


GenericSensor_nws_ros2_ParamsParser::GenericSensor_nws_ros2_ParamsParser()
{
}


std::vector<std::string> GenericSensor_nws_ros2_ParamsParser::getListOfParams() const
{
std::vector<std::string> params;
params.push_back("period");
params.push_back("node_name");
params.push_back("topic_name");
return params;
}


bool GenericSensor_nws_ros2_ParamsParser::parseParams(const yarp::os::Searchable & config)
{
//Check for --help option
if (config.check("help"))
{
yCInfo(GenericSensor_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(GenericSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'period' using value:" << m_period;
}
else
{
yCInfo(GenericSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'period' using DEFAULT value:" << m_period;
}
prop_check.unput("period");
}

//Parser of parameter node_name
{
if (config.check("node_name"))
{
m_node_name = config.find("node_name").asString();
yCInfo(GenericSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'node_name' using value:" << m_node_name;
}
else
{
yCError(GenericSensor_nws_ros2ParamsCOMPONENT) << "Mandatory parameter 'node_name' not found!";
yCError(GenericSensor_nws_ros2ParamsCOMPONENT) << "Description of the parameter: name of the ros2 node";
return false;
}
prop_check.unput("node_name");
}

//Parser of parameter topic_name
{
if (config.check("topic_name"))
{
m_topic_name = config.find("topic_name").asString();
yCInfo(GenericSensor_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_name' using value:" << m_topic_name;
}
else
{
yCError(GenericSensor_nws_ros2ParamsCOMPONENT) << "Mandatory parameter 'topic_name' not found!";
yCError(GenericSensor_nws_ros2ParamsCOMPONENT) << "Description of the parameter: name of the topic where the device must publish the data";
return false;
}
prop_check.unput("topic_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(GenericSensor_nws_ros2ParamsCOMPONENT) << "User asking for parameter: "<<it->name <<" which is unknown to this parser!";
extra_params_found = true;
}
else
{
yCWarning(GenericSensor_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 GenericSensor_nws_ros2_ParamsParser::getDocumentationOfDeviceParams() const
{
std::string doc;
doc = doc + std::string("\n=============================================\n");
doc = doc + std::string("This is the help for device: GenericSensor_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 s\n");
doc = doc + std::string("'node_name': name of the ros2 node\n");
doc = doc + std::string("'topic_name': name of the topic where the device must publish the data\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 genericSensor_nws_ros2 --period 0.02 --node_name <mandatory_value> --topic_name <mandatory_value>\n";
doc = doc + std::string("Using only mandatory params:\n");
doc = doc + " yarpdev --device genericSensor_nws_ros2 --node_name <mandatory_value> --topic_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,75 @@
/*
* 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 5 16:25:52 2024


#ifndef GENERICSENSOR_NWS_ROS2_PARAMSPARSER_H
#define GENERICSENSOR_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 GenericSensor_nws_ros2.
*
* These are the used parameters:
* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes |
* |:----------:|:--------------:|:------:|:-----:|:-------------:|:--------:|:--------------------------------------------------------:|:------------------------------:|
* | - | period | double | s | 0.02 | 0 | refresh period of the broadcasted values in s | default 0.02s |
* | - | node_name | string | - | - | 1 | name of the ros2 node | - |
* | - | topic_name | string | - | - | 1 | name of the topic where the device must publish the data | must begin with an initial '/' |
*
* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters):
* \code{.unparsed}
* yarpdev --device genericSensor_nws_ros2 --period 0.02 --node_name <mandatory_value> --topic_name <mandatory_value>
* \endcode
*
* \code{.unparsed}
* yarpdev --device genericSensor_nws_ros2 --node_name <mandatory_value> --topic_name <mandatory_value>
* \endcode
*
*/

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

public:
const std::string m_device_classname = {"GenericSensor_nws_ros2"};
const std::string m_device_name = {"genericSensor_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.02"};
const std::string m_node_name_defaultValue = {""};
const std::string m_topic_name_defaultValue = {""};

double m_period = {0.02};
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_topic_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,3 @@
| | period | double | s | 0.02 | No | refresh period of the broadcasted values in s | default 0.02s |
| | node_name | string | - | - | Yes | name of the ros2 node | |
| | topic_name | string | - | - | Yes | name of the topic where the device must publish the data| must begin with an initial '/' |
8 changes: 2 additions & 6 deletions src/devices/multipleAnalogSensors_nws_ros2/Imu_nws_ros2.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,8 @@
* |:-----------------:|
* | `Imu_nws_ros2` |
*
* The parameters accepted by this device are:
* | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
* |:--------------:|:--------------:|:-------:|:--------------:|:----------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:|
* | topic_name | - | string | - | - | Yes | The name of the ROS topic opened by this device. | MUST start with a '/' character |
* | node_name | - | string | - | - | Yes | The name of the ROS node opened by this device | Autogenerated by default |
* | period | - | double | s | - | Yes | Refresh period of the broadcasted values in seconds | |
* The parameters accepted by this device are shown in class: GenericSensor_nws_ros2_ParamsParser
*
*/
class Imu_nws_ros2 : public GenericSensor_nws_ros2<sensor_msgs::msg::Imu>
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,8 @@
* |:-----------------:|
* | `WrenchStamped_nws_ros2` |
*
* The parameters accepted by this device are:
* | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
* |:--------------:|:--------------:|:-------:|:--------------:|:----------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:|
* | topic_name | - | string | - | - | Yes | The name of the ROS topic opened by this device. | MUST start with a '/' character |
* | node_name | - | string | - | - | Yes | The name of the ROS node opened by this device | Autogenerated by default |
* | period | - | double | s | - | Yes | Refresh period of the broadcasted values in seconds | |
* The parameters accepted by this device are shown in class: GenericSensor_nws_ros2_ParamsParser
*
*/
class WrenchStamped_nws_ros2 : public GenericSensor_nws_ros2<geometry_msgs::msg::WrenchStamped>
{
Expand Down

0 comments on commit 07ca1a7

Please sign in to comment.