YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Map2D_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
8#endif
9
10#include "Map2D_nws_ros2.h"
11
12#include <chrono>
13#include <vector>
14#include <cmath>
15#include <yarp/dev/IMap2D.h>
18#include <yarp/math/Math.h>
19#include <yarp/os/Log.h>
21#include <yarp/os/LogStream.h>
22#include <mutex>
23#include <cstdlib>
24#include <fstream>
25#include <Ros2Utils.h>
26#include <rclcpp/qos.hpp>
27#include <rclcpp/version.h>
28
29using namespace yarp::sig;
30using namespace yarp::dev;
31using namespace yarp::dev::Nav2D;
32using namespace yarp::os;
33using namespace std;
34using namespace std::placeholders;
35
36namespace {
37YARP_LOG_COMPONENT(MAP2D_NWS_ROS2, "yarp.device.map2D_nws_ros2")
38}
39
40
43
45{
46 if (driver->isValid())
47 {
48 driver->view(m_iMap2D);
50 m_iMap2D->get_map_names(maps);
51 m_currentMapName = maps[0];
52 }
53
54 if (nullptr == m_iMap2D)
55 {
56 yCError(MAP2D_NWS_ROS2, "Subdevice passed to attach method is invalid");
57 return false;
58 }
59
60 //updateRvizMarkers on startup
61 if (1)
62 {
64 }
65
66 return true;
67}
68
70{
71 m_iMap2D = nullptr;
72 return true;
73}
74
76{
77 parseParams(config);
78
79 if(m_name[0] == '/'){
80 yCError(MAP2D_NWS_ROS2) << "Nws name parameter cannot begin with an initial /";
81 return false;
82 }
83
84 m_rpcPortName = "/"+m_name+"/rpc";
85
86 //open rpc port
87 if (!m_rpcPort.open(m_rpcPortName))
88 {
89 yCError(MAP2D_NWS_ROS2, "Failed to open port %s", m_rpcPortName.c_str());
90 return false;
91 }
92 m_rpcPort.setReader(*this);
93
94 //ROS2 configuration
95 if(m_node_name[0] == '/'){
96 yCError(MAP2D_NWS_ROS2) << "node_name cannot begin with an initial /";
97 return false;
98 }
99 if(m_namespace.empty()) {
101 } else {
103 }
104 if (m_node == nullptr) {
105 yCError(MAP2D_NWS_ROS2) << " opening " << m_node_name << " Node, check your yarp-ROS2 network configuration\n";
106 return false;
107 }
110 qos_rmw.depth=10;
111 rmw_time_t time;
112 time.sec=10000;
113 time.nsec = 0;
114 qos_rmw.deadline= time;
115 qos_rmw.lifespan=time;
116 qos_rmw.liveliness_lease_duration=time;
120 qos_rmw.avoid_ros_namespace_conventions = true;
121 m_ros2Service_getMap = m_node->create_service<nav_msgs::srv::GetMap>(m_getmap,
122 std::bind(&Map2D_nws_ros2::getMapCallback,this,_1,_2,_3),
123#if RCLCPP_VERSION_GTE(17,0,0)
124 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_rmw))
125#else
126 qos_rmw
127#endif
128 );
129 m_ros2Service_getMapByName = m_node->create_service<map2d_nws_ros2_msgs::srv::GetMapByName>(m_getmapbyname,
130 std::bind(&Map2D_nws_ros2::getMapByNameCallback,this,_1,_2,_3));
131 m_ros2Service_rosCmdParser = m_node->create_service<test_msgs::srv::BasicTypes>(m_roscmdparser,
132 std::bind(&Map2D_nws_ros2::rosCmdParserCallback,this,_1,_2,_3));
133
134 yCInfo(MAP2D_NWS_ROS2) << "Waiting for device to attach";
135 start();
136
137 return true;
138}
139
140
142{
143
144// if(!m_spinned) //This is just a temporary solution.
145// {
146// rclcpp::spin(m_node);
147// m_spinned = true;
148// }
149}
150
152{
153 yCTrace(MAP2D_NWS_ROS2, "Close");
154 m_rpcPort.close();
155 rclcpp::shutdown();
156 return true;
157}
158
160{
161 std::lock_guard<std::mutex> lock(m_mutex);
164 bool ok = in.read(connection);
165 if (!ok) return false;
166
167 //parse string command
168 if(in.get(0).isString())
169 {
170 std::string ss = in.get(0).asString();
171 if (ss == "help")
172 {
173 yInfo("updateMarkers");
174 yInfo("publishMap <name>");
175 out.addString("updateMarkers");
176 out.addString("publishMap");
177 }
178 else if (ss == "updateMarkers")
179 {
181 out.addString("ok");
182 }
183 else if (ss == "publishMap")
184 {
185 std::string mapname = in.get(1).asString();
187 out.addString("ok");
188 }
189 }
190
192 if (returnToSender != nullptr)
193 {
194 out.write(*returnToSender);
195 }
196 else
197 {
198 yCError(MAP2D_NWS_ROS2) << "Invalid return to sender";
199 }
200 return true;
201}
202
204{
205 yCInfo(MAP2D_NWS_ROS2) << "Updating rviz markers. Current map is: " << m_currentMapName;
206 if (!m_ros2Publisher_markers)
207 {
208 m_ros2Publisher_markers = m_node->create_publisher<visualization_msgs::msg::MarkerArray>(m_markers_pub, 10);
209 }
210 builtin_interfaces::msg::Duration dur;
211 dur.sec = 0xFFFFFFFF;
213 uint64_t time;
215 rclcpp::Time ret;
216 time = (uint64_t)(yarpTimeStamp * 1000000000UL);
217 sec_part = (time / 1000000000UL);
218 if (sec_part > std::numeric_limits<unsigned int>::max())
219 {
220 yCWarning(MAP2D_NWS_ROS2) << "Timestamp exceeded the 64 bit representation, resetting it to 0";
221 sec_part = 0;
222 }
223 visualization_msgs::msg::Marker marker;
224 builtin_interfaces::msg::Time tt;
225 yarp::sig::Vector rpy(3);
227 visualization_msgs::msg::MarkerArray markers;
228
229 std::vector<std::string> locations;
230 std::vector<std::string> objects;
231 int count = 1;
232 m_iMap2D->getLocationsList(locations);
233 m_iMap2D->getObjectsList(objects);
234 for (auto it : locations)
235 {
237 m_iMap2D->getLocation(it, loc);
238
239 if(loc.map_id != m_currentMapName && m_currentMapName != "none")
240 {
241 continue;
242 }
243
244 rpy[0] = 0; //x
245 rpy[1] = 0; //y
246 rpy[2] = loc.theta / 180 * M_PI; //z
249
250 marker.header.frame_id = "map";
252 marker.header.stamp = tt;
253 marker.ns = m_markers_pub+"_ns";
254 marker.id = count;
255 marker.type = visualization_msgs::msg::Marker::ARROW;
256 marker.action = visualization_msgs::msg::Marker::ADD;
257 marker.pose.position.x = loc.x;
258 marker.pose.position.y = loc.y;
259 marker.pose.position.z = 0;
260 marker.pose.orientation.x = q.x();
261 marker.pose.orientation.y = q.y();
262 marker.pose.orientation.z = q.z();
263 marker.pose.orientation.w = q.w();
264 marker.scale.x = 1;
265 marker.scale.y = 0.1;
266 marker.scale.z = 0.1;
267 marker.color.a = 1.0;
268 marker.color.r = 0.0;
269 marker.color.g = 1.0;
270 marker.color.b = 0.0;
271 marker.lifetime = dur;
272 marker.text = it;
273 markers.markers.push_back(marker);
274 count++;
275 }
276 for (auto it : objects)
277 {
279 m_iMap2D->getObject(it, obj);
280
281 if(obj.map_id != m_currentMapName && m_currentMapName != "none")
282 {
283 continue;
284 }
285
286 rpy[0] = obj.roll / 180.0 * M_PI; //x
287 rpy[1] = obj.pitch / 180.0 * M_PI; //y
288 rpy[2] = obj.yaw / 180.0 * M_PI; //z
291
292 marker.header.frame_id = "map";
294 marker.header.stamp = tt;
295 marker.ns = m_markers_pub+"_ns";
296 marker.id = count;
297 marker.type = visualization_msgs::msg::Marker::ARROW;
298 marker.action = visualization_msgs::msg::Marker::ADD;
299 marker.pose.position.x = obj.x;
300 marker.pose.position.y = obj.y;
301 marker.pose.position.z = obj.z;
302 marker.pose.orientation.x = q.x();
303 marker.pose.orientation.y = q.y();
304 marker.pose.orientation.z = q.z();
305 marker.pose.orientation.w = q.w();
306 marker.scale.x = 1;
307 marker.scale.y = 0.1;
308 marker.scale.z = 0.1;
309 marker.color.a = 1.0;
310 marker.color.r = 0.0;
311 marker.color.g = 0.0;
312 marker.color.b = 1.0;
313 marker.lifetime = dur;
314 marker.text = it;
315 markers.markers.push_back(marker);
316 count++;
317 }
318
319 m_ros2Publisher_markers->publish(markers);
320 return true;
321}
322
323
324void Map2D_nws_ros2::getMapCallback(const std::shared_ptr<rmw_request_id_t> request_header,
325 const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
326 std::shared_ptr<nav_msgs::srv::GetMap::Response> response)
327{
328 yCWarning(MAP2D_NWS_ROS2) << "Not yet implemented";
329 nav_msgs::msg::OccupancyGrid mapToGo;
330 mapToGo.header.frame_id = "map";
331
332 response->map = mapToGo;
333}
334
335void Map2D_nws_ros2::rosCmdParserCallback(const std::shared_ptr<rmw_request_id_t> request_header,
336 const std::shared_ptr<test_msgs::srv::BasicTypes::Request> request,
337 std::shared_ptr<test_msgs::srv::BasicTypes::Response> response)
338{
339 if(request->string_value == "updatemarkerviz")
340 {
341 if(updateVizMarkers()) response->set__string_value("done");
342 else response->set__string_value("error");
343 }
344 else
345 {
346 response->set__string_value("unknown_command");
347 }
348}
349
350nav_msgs::msg::OccupancyGrid Map2D_nws_ros2::publishMap(std::string mapname)
351{
352 if (!m_ros2Publisher_map)
353 {
354 m_ros2Publisher_map = m_node->create_publisher<nav_msgs::msg::OccupancyGrid>(m_getmapbyname+"/pub", 10);
355 }
356 nav_msgs::msg::OccupancyGrid mapToGo;
357 nav_msgs::msg::MapMetaData metaToGo;
358 mapToGo.header.frame_id = "map";
360 if(!m_iMap2D->get_map(mapname,theMap))
361 {
362 mapToGo.header.frame_id = "invalid_frame";
363 return mapToGo;
364 }
365 mapToGo.info.map_load_time = m_node->get_clock()->now();
366 mapToGo.header.stamp = m_node->get_clock()->now();
367 mapToGo.info.height = theMap.height();
368 mapToGo.info.width = theMap.width();
369
370 double DEG2RAD = M_PI/180.0;
371 double tmp=0;
372 theMap.getResolution(tmp);
373 mapToGo.info.resolution=tmp;
374 double x, y, t;
375 theMap.getOrigin(x,y,t);
376 mapToGo.info.origin.position.x=x;
377 mapToGo.info.origin.position.y=y;
380 v[0]=0; v[1]=0; v[2]=1; v[3]=t*DEG2RAD;
381 q.fromAxisAngle(v);
382 mapToGo.info.origin.orientation.x = q.x();
383 mapToGo.info.origin.orientation.y = q.y();
384 mapToGo.info.origin.orientation.z = q.z();
385 mapToGo.info.origin.orientation.w = q.w();
386 mapToGo.data.resize(theMap.width()*theMap.height());
387 int index=0;
389 for (cell.y=theMap.height(); cell.y-- > 0;)
390 {
391 for (cell.x=0; cell.x<theMap.width(); cell.x++)
392 {
393 theMap.getOccupancyData(cell,tmp);
394 mapToGo.data[index++]=(int)tmp;
395 }
396 }
397
398 metaToGo.map_load_time = mapToGo.info.map_load_time;
399 metaToGo.height = theMap.height();
400 metaToGo.width = theMap.width();
401 metaToGo.origin = mapToGo.info.origin;
402 metaToGo.resolution = mapToGo.info.resolution;
403
404 if (m_ros2Publisher_map)
405 {
406 m_ros2Publisher_map->publish(mapToGo);
407 }
408
409 return mapToGo;
410}
411
412//void Map2D_nws_ros2::prepareMapMsg(MapGrid2D inputMap, nav_msgs::msg::OccupancyGrid &outputMsg)
413void Map2D_nws_ros2::getMapByNameCallback(const std::shared_ptr<rmw_request_id_t> request_header,
414 const std::shared_ptr<map2d_nws_ros2_msgs::srv::GetMapByName::Request> request,
415 std::shared_ptr<map2d_nws_ros2_msgs::srv::GetMapByName::Response> response)
416{
417 m_currentMapName = request->name;
418 auto rmap = publishMap(request->name);
419 response->map = rmap;
420}
#define DEG2RAD
#define yInfo(...)
Definition Log.h:319
#define M_PI
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void getMapCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< nav_msgs::srv::GetMap::Request > request, std::shared_ptr< nav_msgs::srv::GetMap::Response > response)
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
nav_msgs::msg::OccupancyGrid publishMap(std::string mapname)
void getMapByNameCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< map2d_nws_ros2_msgs::srv::GetMapByName::Request > request, std::shared_ptr< map2d_nws_ros2_msgs::srv::GetMapByName::Response > response)
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
void rosCmdParserCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< test_msgs::srv::BasicTypes::Request > request, std::shared_ptr< test_msgs::srv::BasicTypes::Response > response)
void run() override
Main body of the new thread.
bool detach() override
Detach the object (you must have first called attach).
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool view(T *&x)
Get an interface to the device driver.
std::string map_id
name of the map
double theta
orientation [deg] in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
double x
x position of the location [m], expressed in the map reference frame
double yaw
orientation [deg] in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
double pitch
orientation [deg] in the map reference frame
double roll
orientation [deg] in the map reference frame
std::string map_id
name of the map
double z
z position of the location [m], expressed in the map reference frame
double x
x position of the location [m], expressed in the map reference frame
virtual yarp::dev::ReturnValue get_map_names(std::vector< std::string > &map_names)=0
Gets a list containing the names of all registered maps.
virtual yarp::dev::ReturnValue getLocationsList(std::vector< std::string > &locations)=0
Get a list of the names of all stored locations.
virtual yarp::dev::ReturnValue get_map(std::string map_name, yarp::dev::Nav2D::MapGrid2D &map)=0
Gets a map from the map server.
virtual yarp::dev::ReturnValue getObject(std::string object_name, yarp::dev::Nav2D::Map2DObject &obj)=0
Retrieves a object specified by the user in the world reference frame.
virtual yarp::dev::ReturnValue getLocation(std::string location_name, yarp::dev::Nav2D::Map2DLocation &loc)=0
Retrieves a location specified by the user in the world reference frame.
virtual yarp::dev::ReturnValue getObjectsList(std::vector< std::string > &objects)=0
Get a list of the names of all stored objects.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
void fromAxisAngle(const yarp::sig::Vector &v)
Computes the quaternion from an axis-angle representation.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void setReader(PortReader &reader) override
Set an external reader for port data.
void close() override
Stop port activity.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition Bottle.cpp:241
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition Bottle.cpp:231
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition Bottle.cpp:176
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
A base class for nested structures that can be searched.
Definition Searchable.h:31
bool start()
Start the new thread running.
Definition Thread.cpp:93
virtual bool isString() const
Checks if value is a string.
Definition Value.cpp:156
virtual std::string asString() const
Get string value.
Definition Value.cpp:240
A class for a Matrix.
Definition Matrix.h:40
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
yarp::math::Vec2D< size_t > XYCell
Definition NavTypes.h:16
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
An interface to the operating system, including Port based communication.
std::uint32_t NetUint32
Definition of the NetUint32 type.
Definition NetUint32.h:29