Skip to content

Commit

Permalink
Added ParamsParser to Map2D_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 d258a0f commit 584bc88
Show file tree
Hide file tree
Showing 6 changed files with 293 additions and 50 deletions.
3 changes: 3 additions & 0 deletions src/devices/map2D_nws_ros2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,14 @@ yarp_prepare_plugin(map2D_nws_ros2

if(NOT SKIP_map2D_nws_ros2)
yarp_add_plugin(yarp_map2D_nws_ros2)
generateDeviceParamsParser(Map2D_nws_ros2 map2D_nws_ros2)

target_sources(yarp_map2D_nws_ros2
PRIVATE
Map2D_nws_ros2.cpp
Map2D_nws_ros2.h
Map2D_nws_ros2_ParamsParser.h
Map2D_nws_ros2_ParamsParser.cpp
)

target_sources(yarp_localization2D_nws_ros2 PRIVATE $<TARGET_OBJECTS:Ros2Utils>)
Expand Down
45 changes: 12 additions & 33 deletions src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,20 +67,11 @@ bool Map2D_nws_ros2::detach()

bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
{
Property params;
params.fromString(config.toString());
parseParams(config);

if (!config.check("name"))
{
yCWarning(MAP2D_NWS_ROS2) << "Missing name parameter. Using:" << m_name;
}
else
{
m_name = config.find("name").asString();
if(m_name[0] == '/'){
yCError(MAP2D_NWS_ROS2) << "Nws name parameter cannot begin with an initial /";
return false;
}
if(m_name[0] == '/'){
yCError(MAP2D_NWS_ROS2) << "Nws name parameter cannot begin with an initial /";
return false;
}

m_rpcPortName = "/"+m_name+"/rpc";
Expand All @@ -94,23 +85,11 @@ bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
m_rpcPort.setReader(*this);

//ROS2 configuration
if(config.check("getmap")) m_getMapName = config.find("getmap").asString();
if(config.check("getmapbyname")) m_getMapByNameName = config.find("getmapbyname").asString();
if(config.check("roscmdparser")) m_rosCmdParserName = config.find("roscmdparser").asString();
if(config.check("markers_pub")) m_markersName = config.find("markers_pub").asString();
if (!config.check("node_name")) {
yCWarning(MAP2D_NWS_ROS2) << "Missing node_name parameter. Using:" << m_name;
m_nodeName = m_name;
}
else
{
m_nodeName = config.find("node_name").asString();
}
if(m_nodeName[0] == '/'){
if(m_node_name[0] == '/'){
yCError(MAP2D_NWS_ROS2) << "node_name cannot begin with an initial /";
return false;
}
m_node = NodeCreator::createNode(m_nodeName);
m_node = NodeCreator::createNode(m_node_name);
rmw_qos_profile_t qos;
qos.history = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT;
qos.depth=10;
Expand All @@ -124,11 +103,11 @@ bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
qos.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
qos.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
qos.avoid_ros_namespace_conventions = true;
m_ros2Service_getMap = m_node->create_service<nav_msgs::srv::GetMap>(m_getMapName,
m_ros2Service_getMap = m_node->create_service<nav_msgs::srv::GetMap>(m_getmap,
std::bind(&Map2D_nws_ros2::getMapCallback,this,_1,_2,_3),qos );
m_ros2Service_getMapByName = m_node->create_service<map2d_nws_ros2_msgs::srv::GetMapByName>(m_getMapByNameName,
m_ros2Service_getMapByName = m_node->create_service<map2d_nws_ros2_msgs::srv::GetMapByName>(m_getmapbyname,
std::bind(&Map2D_nws_ros2::getMapByNameCallback,this,_1,_2,_3));
m_ros2Service_rosCmdParser = m_node->create_service<test_msgs::srv::BasicTypes>(m_rosCmdParserName,
m_ros2Service_rosCmdParser = m_node->create_service<test_msgs::srv::BasicTypes>(m_roscmdparser,
std::bind(&Map2D_nws_ros2::rosCmdParserCallback,this,_1,_2,_3));

yCInfo(MAP2D_NWS_ROS2) << "Waiting for device to attach";
Expand Down Expand Up @@ -193,7 +172,7 @@ bool Map2D_nws_ros2::updateVizMarkers()
{
if (!m_ros2Publisher_markers)
{
m_ros2Publisher_markers = m_node->create_publisher<visualization_msgs::msg::MarkerArray>(m_markersName, 10);
m_ros2Publisher_markers = m_node->create_publisher<visualization_msgs::msg::MarkerArray>(m_markers_pub, 10);
}
builtin_interfaces::msg::Duration dur;
dur.sec = 0xFFFFFFFF;
Expand Down Expand Up @@ -236,7 +215,7 @@ bool Map2D_nws_ros2::updateVizMarkers()
marker.header.frame_id = "map";
tt.sec = (yarp::os::NetUint32) sec_part;;
marker.header.stamp = tt;
marker.ns = m_markersName+"_ns";
marker.ns = m_markers_pub+"_ns";
marker.id = count;
marker.type = visualization_msgs::msg::Marker::ARROW;//yarp::rosmsg::visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::msg::Marker::ADD;//yarp::rosmsg::visualization_msgs::Marker::ADD;
Expand Down Expand Up @@ -298,7 +277,7 @@ void Map2D_nws_ros2::getMapByNameCallback(const std::shared_ptr<rmw_request_id_t
{
if (!m_ros2Publisher_map)
{
m_ros2Publisher_map = m_node->create_publisher<nav_msgs::msg::OccupancyGrid>(m_getMapByNameName+"/pub", 10);
m_ros2Publisher_map = m_node->create_publisher<nav_msgs::msg::OccupancyGrid>(m_getmapbyname+"/pub", 10);
}
nav_msgs::msg::OccupancyGrid mapToGo;
nav_msgs::msg::MapMetaData metaToGo;
Expand Down
22 changes: 5 additions & 17 deletions src/devices/map2D_nws_ros2/Map2D_nws_ros2.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/api.h>
#include "Map2D_nws_ros2_ParamsParser.h"

#include <rclcpp/rclcpp.hpp>

Expand All @@ -54,16 +55,8 @@
*
* \brief `Map2D_nws_ros2`: A device capable of read/save collections of maps from disk, and make them accessible to any Map2DClient device.
*
* Parameters required by this device are:
* | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
* |:--------------:|:-------------:|:-------:|:-------:|:---------------------:|:----------: |:-------------------------------------------------------------:|:-----------------------------------------------------------------------------------------------:|
* | name | - | string | - | map2D_nws_ros | No | Device name prefix | |
* | getmap | - | string | - | getMap | No | The "GetMap" ROS service name | For the moment being the service always responds with an empty map |
* | getmapbyname | - | string | - | getMapByName | No | The "GetMapByName" ROS2 custom service name | The map returned by this service is also available via publisher named "getmapbyname value"/pub |
* | roscmdparser | - | string | - | rosCmdParser | No | The "BasicTypes" ROS service name | This is used to send commands to the nws via ros2 BasicTypes service |
* | markers_pub | - | string | - | locationServerMarkers | No | The visual markers array publisher name | |
* | node_name | - | string | - | - | No | The ROS2 node name. If absent, the device name will be used | |
* Parameters required by this device are shown in class: Map2D_nws_ros2_ParamsParser
*
* \section Notes:
* Integration with ROS2 map server is currently under development.
*/
Expand All @@ -72,7 +65,8 @@ class Map2D_nws_ros2 :
public yarp::os::Thread,
public yarp::dev::DeviceDriver,
public yarp::os::PortReader,
public yarp::dev::WrapperSingle
public yarp::dev::WrapperSingle,
Map2D_nws_ros2_ParamsParser
{
public:
Map2D_nws_ros2();
Expand Down Expand Up @@ -108,14 +102,8 @@ class Map2D_nws_ros2 :
yarp::dev::PolyDriver m_drv;

std::mutex m_mutex;
std::string m_name{"map2D_nws_ros"};
std::string m_rpcPortName;
std::string m_rosCmdParserName{"rosCmdParser"};
std::string m_getMapName{"getMap"};
std::string m_getMapByNameName{"getMapByName"};
std::string m_markersName{"locationServerMarkers"};
std::string m_currentMapName{"none"};
std::string m_nodeName;
bool m_spinned{false};

yarp::os::RpcServer m_rpcPort;
Expand Down
183 changes: 183 additions & 0 deletions src/devices/map2D_nws_ros2/Map2D_nws_ros2_ParamsParser.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,183 @@
/*
* 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 "Map2D_nws_ros2_ParamsParser.h"
#include <yarp/os/LogStream.h>
#include <yarp/os/Value.h>

namespace {
YARP_LOG_COMPONENT(Map2D_nws_ros2ParamsCOMPONENT, "yarp.device.Map2D_nws_ros2")
}


Map2D_nws_ros2_ParamsParser::Map2D_nws_ros2_ParamsParser()
{
}


std::vector<std::string> Map2D_nws_ros2_ParamsParser::getListOfParams() const
{
std::vector<std::string> params;
params.push_back("name");
params.push_back("getmap");
params.push_back("getmapbyname");
params.push_back("roscmdparser");
params.push_back("markers_pub");
params.push_back("node_name");
return params;
}


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

std::string config_string = config.toString();
yarp::os::Property prop_check(config_string.c_str());
//Parser of parameter name
{
if (config.check("name"))
{
m_name = config.find("name").asString();
yCInfo(Map2D_nws_ros2ParamsCOMPONENT) << "Parameter 'name' using value:" << m_name;
}
else
{
yCInfo(Map2D_nws_ros2ParamsCOMPONENT) << "Parameter 'name' using DEFAULT value:" << m_name;
}
prop_check.unput("name");
}

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

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

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

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

//Parser of parameter node_name
{
if (config.check("node_name"))
{
m_node_name = config.find("node_name").asString();
yCInfo(Map2D_nws_ros2ParamsCOMPONENT) << "Parameter 'node_name' using value:" << m_node_name;
}
else
{
yCInfo(Map2D_nws_ros2ParamsCOMPONENT) << "Parameter 'node_name' using DEFAULT value:" << m_node_name;
}
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(Map2D_nws_ros2ParamsCOMPONENT) << "User asking for parameter: "<<it->name <<" which is unknown to this parser!";
extra_params_found = true;
}
else
{
yCWarning(Map2D_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 Map2D_nws_ros2_ParamsParser::getDocumentationOfDeviceParams() const
{
std::string doc;
doc = doc + std::string("\n=============================================\n");
doc = doc + std::string("This is the help for device: Map2D_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("'name': Device name prefix\n");
doc = doc + std::string("'getmap': The \"GetMap\" ROS service name\n");
doc = doc + std::string("'getmapbyname': The \"GetMapByName\" ROS2 custom service name\n");
doc = doc + std::string("'roscmdparser': The \"BasicTypes\" ROS service name\n");
doc = doc + std::string("'markers_pub': The visual markers array publisher name\n");
doc = doc + std::string("'node_name': The ROS2 node name. If absent, the device name will be used\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 map2D_nws_ros2 --name map2D_nws_ros2 --getmap getMap --getmapbyname getMapByName --roscmdparser rosCmdParser --markers_pub locationServerMarkers --node_name map2D_nws_ros2\n";
doc = doc + std::string("Using only mandatory params:\n");
doc = doc + " yarpdev --device map2D_nws_ros2\n";
doc = doc + std::string("=============================================\n\n"); return doc;
}
Loading

0 comments on commit 584bc88

Please sign in to comment.