Skip to content

Commit

Permalink
Added ParamsParser to Rangefinder2D_controlBoard_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 28, 2024
1 parent a6b0de7 commit 4786d5d
Show file tree
Hide file tree
Showing 6 changed files with 270 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@ yarp_prepare_plugin(rangefinder2D_controlBoard_nws_ros2

if(NOT SKIP_rangefinder2D_controlBoard_nws_ros2)
yarp_add_plugin(yarp_rangefinder2D_controlBoard_nws_ros2)
generateDeviceParamsParser(Rangefinder2D_controlBoard_nws_ros2 rangefinder2D_controlBoard_nws_ros2)

target_sources(yarp_rangefinder2D_controlBoard_nws_ros2
PRIVATE
Rangefinder2D_controlBoard_nws_ros2.cpp
Rangefinder2D_controlBoard_nws_ros2.h
Rangefinder2D_controlBoard_nws_ros2_ParamsParser.h
Rangefinder2D_controlBoard_nws_ros2_ParamsParser.cpp
)
target_sources(yarp_rangefinder2D_controlBoard_nws_ros2 PRIVATE $<TARGET_OBJECTS:Ros2Utils>)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -185,35 +185,35 @@ bool Rangefinder2D_controlBoard_nws_ros2::setDevice(yarp::dev::DeviceDriver* dri

m_driver_cb->view(iPositionControl);
if (!iPositionControl) {
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IPositionControl interface was not found in attached device. Quitting", m_node_name.c_str(), m_topic_cb.c_str());
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IPositionControl interface was not found in attached device. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
return false;
}

m_driver_cb->view(iEncodersTimed);
if (!iEncodersTimed) {
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IEncodersTimed interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_cb.c_str());
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IEncodersTimed interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
return false;
}

m_driver_cb->view(iTorqueControl);
if (!iTorqueControl) {
yCWarning(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: ITorqueControl interface was not found in attached device..", m_node_name.c_str(), m_topic_cb.c_str());
yCWarning(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: ITorqueControl interface was not found in attached device..", m_node_name.c_str(), m_topic_joint.c_str());
}

m_driver_cb->view(iAxisInfo);
if (!iAxisInfo) {
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IAxisInfo interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_cb.c_str());
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IAxisInfo interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
return false;
}

// Get the number of controlled joints
int tmp_axes;
if (!iPositionControl->getAxes(&tmp_axes)) {
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: Failed to get axes number for attached device. ", m_node_name.c_str(), m_topic_cb.c_str());
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: Failed to get axes number for attached device. ", m_node_name.c_str(), m_topic_joint.c_str());
return false;
}
if (tmp_axes <= 0) {
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: attached device has an invalid number of joints (%d)", m_node_name.c_str(), m_topic_cb.c_str(), tmp_axes);
yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: attached device has an invalid number of joints (%d)", m_node_name.c_str(), m_topic_joint.c_str(), tmp_axes);
return false;
}

Expand All @@ -235,18 +235,12 @@ bool Rangefinder2D_controlBoard_nws_ros2::setDevice(yarp::dev::DeviceDriver* dri

bool Rangefinder2D_controlBoard_nws_ros2::open(yarp::os::Searchable &config)
{
//wrapper params
m_topic = config.check("topic_name_lidar", yarp::os::Value("topic_name_lidar"), "Name of the ROS2 topic").asString();
m_topic_cb = config.check("topic_name_joint", yarp::os::Value("topic_name_joint"), "Name of the ROS2 topic").asString();
m_frame_id = config.check("frame_id", yarp::os::Value("laser_frame"), "Name of the frameId").asString();
m_node_name = config.check("node_name", yarp::os::Value("laser_node"), "Name of the node").asString();
m_period = config.check("period", yarp::os::Value(0.010), "Period of the thread").asFloat64();

parseParams(config);
//create the topic
m_node = NodeCreator::createNode(m_node_name);
m_publisher_laser = m_node->create_publisher<sensor_msgs::msg::LaserScan>(m_topic, 10);
m_publisher_joint = m_node->create_publisher<sensor_msgs::msg::JointState>(m_topic_cb, 10);
yCInfo(RANGEFINDER2D_NWS_ROS2, "Opened topic: %s", m_topic.c_str());
m_publisher_laser = m_node->create_publisher<sensor_msgs::msg::LaserScan>(m_topic_lidar, 10);
m_publisher_joint = m_node->create_publisher<sensor_msgs::msg::JointState>(m_topic_joint, 10);
yCInfo(RANGEFINDER2D_NWS_ROS2, "Opened topic: %s", m_topic_lidar.c_str());

yCWarning(RANGEFINDER2D_NWS_ROS2) << "Waiting for device to attach";

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

#include "Rangefinder2D_controlBoard_nws_ros2_ParamsParser.h"

#include <mutex>

/**
Expand All @@ -36,7 +38,8 @@
class Rangefinder2D_controlBoard_nws_ros2 :
public yarp::dev::DeviceDriver,
public yarp::os::PeriodicThread,
public yarp::dev::WrapperSingle
public yarp::dev::WrapperSingle,
Rangefinder2D_controlBoard_nws_ros2_ParamsParser
{
public:
Rangefinder2D_controlBoard_nws_ros2();
Expand Down Expand Up @@ -81,13 +84,7 @@ class Rangefinder2D_controlBoard_nws_ros2 :
double m_minAngle, m_maxAngle;
double m_minDistance, m_maxDistance;
double m_resolution;
double m_period;
sensor_msgs::msg::JointState m_ros_struct;

std::string m_topic;
std::string m_topic_cb;
std::string m_node_name;
std::string m_frame_id;
};

#endif // YARP_ROS2_RANGEFINDER2D_CONTROLBOARD_NWS_ROS2_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,167 @@
/*
* 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: Wed Aug 28 15:59:35 2024


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

namespace {
YARP_LOG_COMPONENT(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT, "yarp.device.Rangefinder2D_controlBoard_nws_ros2")
}


Rangefinder2D_controlBoard_nws_ros2_ParamsParser::Rangefinder2D_controlBoard_nws_ros2_ParamsParser()
{
}


std::vector<std::string> Rangefinder2D_controlBoard_nws_ros2_ParamsParser::getListOfParams() const
{
std::vector<std::string> params;
params.push_back("period");
params.push_back("node_name");
params.push_back("topic_lidar");
params.push_back("topic_joint");
params.push_back("frame_id");
return params;
}


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

//Parser of parameter topic_lidar
{
if (config.check("topic_lidar"))
{
m_topic_lidar = config.find("topic_lidar").asString();
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_lidar' using value:" << m_topic_lidar;
}
else
{
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_lidar' using DEFAULT value:" << m_topic_lidar;
}
prop_check.unput("topic_lidar");
}

//Parser of parameter topic_joint
{
if (config.check("topic_joint"))
{
m_topic_joint = config.find("topic_joint").asString();
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_joint' using value:" << m_topic_joint;
}
else
{
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'topic_joint' using DEFAULT value:" << m_topic_joint;
}
prop_check.unput("topic_joint");
}

//Parser of parameter frame_id
{
if (config.check("frame_id"))
{
m_frame_id = config.find("frame_id").asString();
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'frame_id' using value:" << m_frame_id;
}
else
{
yCInfo(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "Parameter 'frame_id' using DEFAULT value:" << m_frame_id;
}
prop_check.unput("frame_id");
}

/*
//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(Rangefinder2D_controlBoard_nws_ros2ParamsCOMPONENT) << "User asking for parameter: "<<it->name <<" which is unknown to this parser!";
extra_params_found = true;
}
else
{
yCWarning(Rangefinder2D_controlBoard_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 Rangefinder2D_controlBoard_nws_ros2_ParamsParser::getDocumentationOfDeviceParams() const
{
std::string doc;
doc = doc + std::string("\n=============================================\n");
doc = doc + std::string("This is the help for device: Rangefinder2D_controlBoard_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_lidar': name of the topic for lidar data\n");
doc = doc + std::string("'topic_joint': name of the topic for joints data\n");
doc = doc + std::string("'frame_id': name of the base frameId\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 rangefinder2D_controlBoard_nws_ros2 --period 0.02 --node_name laser_node --topic_lidar topic_name_lidar --topic_joint topic_lidar_joint --frame_id laser_frame\n";
doc = doc + std::string("Using only mandatory params:\n");
doc = doc + " yarpdev --device rangefinder2D_controlBoard_nws_ros2\n";
doc = doc + std::string("=============================================\n\n"); return doc;
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
/*
* 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: Wed Aug 28 15:59:35 2024


#ifndef RANGEFINDER2D_CONTROLBOARD_NWS_ROS2_PARAMSPARSER_H
#define RANGEFINDER2D_CONTROLBOARD_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 Rangefinder2D_controlBoard_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 | - |
* | - | node_name | string | - | laser_node | 0 | name of the ros2 node | - |
* | - | topic_lidar | string | - | topic_name_lidar | 0 | name of the topic for lidar data | - |
* | - | topic_joint | string | - | topic_lidar_joint | 0 | name of the topic for joints data | - |
* | - | frame_id | string | - | laser_frame | 0 | name of the base frameId | - |
*
* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters):
* \code{.unparsed}
* yarpdev --device rangefinder2D_controlBoard_nws_ros2 --period 0.02 --node_name laser_node --topic_lidar topic_name_lidar --topic_joint topic_lidar_joint --frame_id laser_frame
* \endcode
*
* \code{.unparsed}
* yarpdev --device rangefinder2D_controlBoard_nws_ros2
* \endcode
*
*/

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

public:
const std::string m_device_classname = {"Rangefinder2D_controlBoard_nws_ros2"};
const std::string m_device_name = {"rangefinder2D_controlBoard_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 = {"laser_node"};
const std::string m_topic_lidar_defaultValue = {"topic_name_lidar"};
const std::string m_topic_joint_defaultValue = {"topic_lidar_joint"};
const std::string m_frame_id_defaultValue = {"laser_frame"};

double m_period = {0.02};
std::string m_node_name = {"laser_node"};
std::string m_topic_lidar = {"topic_name_lidar"};
std::string m_topic_joint = {"topic_lidar_joint"};
std::string m_frame_id = {"laser_frame"};

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,5 @@
| | period | double | s | 0.02 | No | refresh period of the broadcasted values in s | |
| | node_name | string | - | laser_node | No | name of the ros2 node | |
| | topic_lidar | string | - | topic_name_lidar | No | name of the topic for lidar data | |
| | topic_joint | string | - | topic_lidar_joint | No | name of the topic for joints data | |
| | frame_id | string | - | laser_frame | No | name of the base frameId | |

0 comments on commit 4786d5d

Please sign in to comment.