diff --git a/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/CMakeLists.txt b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/CMakeLists.txt
index deb8d56..26e04d7 100644
--- a/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/CMakeLists.txt
+++ b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/CMakeLists.txt
@@ -44,8 +44,9 @@ find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/GetMapByName.srv"
+ "srv/GetLocationByName.srv"
LIBRARY_NAME ${PROJECT_NAME}
- DEPENDENCIES nav_msgs builtin_interfaces
+ DEPENDENCIES nav_msgs builtin_interfaces geometry_msgs
)
ament_export_dependencies(rosidl_default_runtime)
diff --git a/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/package.xml b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/package.xml
index 9371503..d7e90c0 100644
--- a/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/package.xml
+++ b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/package.xml
@@ -10,6 +10,7 @@
ament_cmake
rclcpp
nav_msgs
+ geometry_msgs
builtin_interfaces
rosidl_default_generators
builtin_interfaces
diff --git a/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/srv/GetLocationByName.srv b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/srv/GetLocationByName.srv
new file mode 100644
index 0000000..b037879
--- /dev/null
+++ b/ros2_interfaces_ws/src/map2d_nws_ros2_msgs/srv/GetLocationByName.srv
@@ -0,0 +1,4 @@
+string name
+---
+geometry_msgs/Pose pose
+bool ok
\ No newline at end of file
diff --git a/src/devices/map2D_nws_ros2/CMakeLists.txt b/src/devices/map2D_nws_ros2/CMakeLists.txt
index c203639..dd432c8 100644
--- a/src/devices/map2D_nws_ros2/CMakeLists.txt
+++ b/src/devices/map2D_nws_ros2/CMakeLists.txt
@@ -33,6 +33,7 @@ if(NOT SKIP_map2D_nws_ros2)
PRIVATE
YARP::YARP_os
YARP::YARP_sig
+ YARP::YARP_math
YARP::YARP_dev
rclcpp::rclcpp
std_msgs::std_msgs__rosidl_typesupport_cpp
diff --git a/src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp b/src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp
index 5e3be30..9da5628 100644
--- a/src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp
+++ b/src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp
@@ -15,7 +15,6 @@
#include
#include
#include
-#include
#include
#include
#include
@@ -32,6 +31,10 @@ using namespace yarp::os;
using namespace std;
using namespace std::placeholders;
+#ifndef DEG2RAD
+#define DEG2RAD M_PI / 180.0
+#endif
+
namespace {
YARP_LOG_COMPONENT(MAP2D_NWS_ROS2, "yarp.device.map2D_nws_ros2")
}
@@ -96,6 +99,7 @@ bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
//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("getlocbyname")) m_getLocByNameName = config.find("getlocbyname").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")) {
@@ -128,6 +132,8 @@ bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
std::bind(&Map2D_nws_ros2::getMapCallback,this,_1,_2,_3),qos );
m_ros2Service_getMapByName = m_node->create_service(m_getMapByNameName,
std::bind(&Map2D_nws_ros2::getMapByNameCallback,this,_1,_2,_3));
+ m_ros2Service_getLocByName = m_node->create_service(m_getLocByNameName,
+ std::bind(&Map2D_nws_ros2::getLocByNameCallback,this,_1,_2,_3));
m_ros2Service_rosCmdParser = m_node->create_service(m_rosCmdParserName,
std::bind(&Map2D_nws_ros2::rosCmdParserCallback,this,_1,_2,_3));
@@ -141,11 +147,11 @@ bool Map2D_nws_ros2::open(yarp::os::Searchable &config)
void Map2D_nws_ros2::run()
{
-// if(!m_spinned) //This is just a temporary solution.
-// {
-// rclcpp::spin(m_node);
-// m_spinned = true;
-// }
+ if(!m_spinned) //This is just a temporary solution.
+ {
+ rclcpp::spin(m_node);
+ m_spinned = true;
+ }
}
bool Map2D_nws_ros2::close()
@@ -315,7 +321,6 @@ void Map2D_nws_ros2::getMapByNameCallback(const std::shared_ptrpublish(mapToGo);
}
}
+
+void Map2D_nws_ros2::getLocByNameCallback(const std::shared_ptr request_header,
+ const std::shared_ptr request,
+ std::shared_ptr response)
+{
+ YARP_UNUSED(request_header);
+ YARP_UNUSED(response);
+
+ yarp::dev::Nav2D::Map2DLocation loc;
+ geometry_msgs::msg::Pose locPose;
+ if(!m_iMap2D->getLocation(request->name, loc))
+ {
+ yCError(MAP2D_NWS_ROS2) << "Unable to retrieve the requested location";
+ response->pose = locPose;
+ response->ok = false;
+
+ return;
+ }
+ geometry_msgs::msg::Pose goal_pose;
+ goal_pose.position.x = loc.x;
+ goal_pose.position.y = loc.y;
+ goal_pose.position.z = 0;
+ yarp::math::Quaternion q;
+ yarp::sig::Vector v(4);
+ v[0] = 0;
+ v[1] = 0;
+ v[2] = 1;
+ v[3] = loc.theta * DEG2RAD;
+ q.fromAxisAngle(v);
+ goal_pose.orientation.x = q.x();
+ goal_pose.orientation.y = q.y();
+ goal_pose.orientation.z = q.z();
+ goal_pose.orientation.w = q.w();
+
+ response->pose = goal_pose;
+ response->ok = true;
+}
diff --git a/src/devices/map2D_nws_ros2/Map2D_nws_ros2.h b/src/devices/map2D_nws_ros2/Map2D_nws_ros2.h
index 8da3fc7..44fc9a4 100644
--- a/src/devices/map2D_nws_ros2/Map2D_nws_ros2.h
+++ b/src/devices/map2D_nws_ros2/Map2D_nws_ros2.h
@@ -12,6 +12,8 @@
#include
#include
+#include
+
#include
#include
#include
@@ -22,6 +24,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -45,6 +48,7 @@
//Custom ros2 interfaces
#include
+#include
/**
@@ -60,6 +64,7 @@
* | 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 |
+ * | getlocbyname | - | string | - | getLocationByName | No | The "GetLocationByName" ROS2 custom service name | |
* | 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 | |
@@ -95,6 +100,9 @@ class Map2D_nws_ros2 :
void getMapByNameCallback(const std::shared_ptr request_header,
const std::shared_ptr request,
std::shared_ptr response);
+ void getLocByNameCallback(const std::shared_ptr request_header,
+ const std::shared_ptr request,
+ std::shared_ptr response);
void rosCmdParserCallback(const std::shared_ptr request_header,
const std::shared_ptr request,
std::shared_ptr response);
@@ -113,18 +121,20 @@ class Map2D_nws_ros2 :
std::string m_rosCmdParserName{"rosCmdParser"};
std::string m_getMapName{"getMap"};
std::string m_getMapByNameName{"getMapByName"};
+ std::string m_getLocByNameName{"getLocationByName"};
std::string m_markersName{"locationServerMarkers"};
std::string m_currentMapName{"none"};
std::string m_nodeName;
bool m_spinned{false};
- yarp::os::RpcServer m_rpcPort;
- rclcpp::Node::SharedPtr m_node;
- rclcpp::Publisher::SharedPtr m_ros2Publisher_markers{nullptr};
- rclcpp::Publisher::SharedPtr m_ros2Publisher_map{nullptr};
- rclcpp::Service::SharedPtr m_ros2Service_getMap{nullptr};
- rclcpp::Service::SharedPtr m_ros2Service_getMapByName{nullptr};
- rclcpp::Service::SharedPtr m_ros2Service_rosCmdParser{nullptr};
+ yarp::os::RpcServer m_rpcPort;
+ rclcpp::Node::SharedPtr m_node;
+ rclcpp::Publisher::SharedPtr m_ros2Publisher_markers{nullptr};
+ rclcpp::Publisher::SharedPtr m_ros2Publisher_map{nullptr};
+ rclcpp::Service::SharedPtr m_ros2Service_getMap{nullptr};
+ rclcpp::Service::SharedPtr m_ros2Service_getMapByName{nullptr};
+ rclcpp::Service::SharedPtr m_ros2Service_getLocByName{nullptr};
+ rclcpp::Service::SharedPtr m_ros2Service_rosCmdParser{nullptr};
};