6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
26#include <rclcpp/qos.hpp>
27#include <rclcpp/version.h>
34using namespace std::placeholders;
48 driver->
view(m_iMap2D);
51 m_currentMapName =
maps[0];
54 if (
nullptr == m_iMap2D)
84 m_rpcPortName =
"/"+
m_name+
"/rpc";
87 if (!m_rpcPort.
open(m_rpcPortName))
104 if (m_node ==
nullptr) {
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,
123#if RCLCPP_VERSION_GTE(17,0,0)
124 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(
qos_rmw))
129 m_ros2Service_getMapByName = m_node->create_service<map2d_nws_ros2_msgs::srv::GetMapByName>(
m_getmapbyname,
131 m_ros2Service_rosCmdParser = m_node->create_service<test_msgs::srv::BasicTypes>(
m_roscmdparser,
161 std::lock_guard<std::mutex> lock(m_mutex);
164 bool ok = in.
read(connection);
165 if (!ok)
return false;
173 yInfo(
"updateMarkers");
174 yInfo(
"publishMap <name>");
178 else if (
ss ==
"updateMarkers")
183 else if (
ss ==
"publishMap")
206 if (!m_ros2Publisher_markers)
208 m_ros2Publisher_markers = m_node->create_publisher<visualization_msgs::msg::MarkerArray>(
m_markers_pub, 10);
210 builtin_interfaces::msg::Duration
dur;
211 dur.sec = 0xFFFFFFFF;
218 if (
sec_part > std::numeric_limits<unsigned int>::max())
223 visualization_msgs::msg::Marker
marker;
224 builtin_interfaces::msg::Time
tt;
227 visualization_msgs::msg::MarkerArray
markers;
229 std::vector<std::string> locations;
230 std::vector<std::string> objects;
234 for (
auto it : locations)
239 if(loc.
map_id != m_currentMapName && m_currentMapName !=
"none")
250 marker.header.frame_id =
"map";
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();
276 for (
auto it : objects)
281 if(obj.
map_id != m_currentMapName && m_currentMapName !=
"none")
288 rpy[2] = obj.
yaw / 180.0 *
M_PI;
292 marker.header.frame_id =
"map";
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();
319 m_ros2Publisher_markers->publish(
markers);
325 const std::shared_ptr<nav_msgs::srv::GetMap::Request>
request,
326 std::shared_ptr<nav_msgs::srv::GetMap::Response> response)
329 nav_msgs::msg::OccupancyGrid
mapToGo;
330 mapToGo.header.frame_id =
"map";
336 const std::shared_ptr<test_msgs::srv::BasicTypes::Request>
request,
337 std::shared_ptr<test_msgs::srv::BasicTypes::Response> response)
339 if(
request->string_value ==
"updatemarkerviz")
342 else response->set__string_value(
"error");
346 response->set__string_value(
"unknown_command");
352 if (!m_ros2Publisher_map)
354 m_ros2Publisher_map = m_node->create_publisher<nav_msgs::msg::OccupancyGrid>(
m_getmapbyname+
"/pub", 10);
356 nav_msgs::msg::OccupancyGrid
mapToGo;
357 nav_msgs::msg::MapMetaData
metaToGo;
358 mapToGo.header.frame_id =
"map";
362 mapToGo.header.frame_id =
"invalid_frame";
365 mapToGo.info.map_load_time = m_node->get_clock()->now();
366 mapToGo.header.stamp = m_node->get_clock()->now();
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;
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();
404 if (m_ros2Publisher_map)
406 m_ros2Publisher_map->publish(
mapToGo);
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)
417 m_currentMapName =
request->name;
419 response->map =
rmap;
std::string m_roscmdparser
std::string m_markers_pub
std::string m_getmapbyname
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)
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.
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.
A simple collection of objects that can be described and transmitted in a portable way.
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
void addString(const char *str)
Places a string in the bottle, at the end of the list.
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.
bool start()
Start the new thread running.
virtual bool isString() const
Checks if value is a string.
virtual std::string asString() const
Get string value.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
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...
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.
std::uint32_t NetUint32
Definition of the NetUint32 type.