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}; };