Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

GetLocationByName srv #65

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

GetLocationByName srv #65

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b037c27
Added new service for getLocationByName
elandini84 Feb 8, 2024
4f704be
Added support for GetLocationByName service
elandini84 Feb 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion ros2_interfaces_ws/src/map2d_nws_ros2_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
1 change: 1 addition & 0 deletions ros2_interfaces_ws/src/map2d_nws_ros2_msgs/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<build_depend>builtin_interfaces</build_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
Expand Down
4 changes: 4 additions & 0 deletions ros2_interfaces_ws/src/map2d_nws_ros2_msgs/srv/GetLocationByName.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
string name
---
geometry_msgs/Pose pose
bool ok
1 change: 1 addition & 0 deletions src/devices/map2D_nws_ros2/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
56 changes: 49 additions & 7 deletions src/devices/map2D_nws_ros2/Map2D_nws_ros2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#include <yarp/dev/IMap2D.h>
#include <yarp/dev/INavigation2D.h>
#include <yarp/dev/GenericVocabs.h>
#include <yarp/math/Math.h>
#include <yarp/os/Log.h>
#include <yarp/os/LogComponent.h>
#include <yarp/os/LogStream.h>
Expand All @@ -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")
}
Expand Down Expand Up @@ -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")) {
Expand Down Expand Up @@ -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<map2d_nws_ros2_msgs::srv::GetMapByName>(m_getMapByNameName,
std::bind(&Map2D_nws_ros2::getMapByNameCallback,this,_1,_2,_3));
m_ros2Service_getLocByName = m_node->create_service<map2d_nws_ros2_msgs::srv::GetLocationByName>(m_getLocByNameName,
std::bind(&Map2D_nws_ros2::getLocByNameCallback,this,_1,_2,_3));
m_ros2Service_rosCmdParser = m_node->create_service<test_msgs::srv::BasicTypes>(m_rosCmdParserName,
std::bind(&Map2D_nws_ros2::rosCmdParserCallback,this,_1,_2,_3));

Expand All @@ -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()
Expand Down Expand Up @@ -315,7 +321,6 @@ void Map2D_nws_ros2::getMapByNameCallback(const std::shared_ptr<rmw_request_id_t
mapToGo.info.height = theMap.height();
mapToGo.info.width = theMap.width();

double DEG2RAD = M_PI/180.0;
double tmp=0;
theMap.getResolution(tmp);
mapToGo.info.resolution=tmp;
Expand Down Expand Up @@ -357,3 +362,40 @@ void Map2D_nws_ros2::getMapByNameCallback(const std::shared_ptr<rmw_request_id_t
m_ros2Publisher_map->publish(mapToGo);
}
}

void Map2D_nws_ros2::getLocByNameCallback(const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<map2d_nws_ros2_msgs::srv::GetLocationByName::Request> request,
std::shared_ptr<map2d_nws_ros2_msgs::srv::GetLocationByName::Response> 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;
}
24 changes: 17 additions & 7 deletions src/devices/map2D_nws_ros2/Map2D_nws_ros2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
#include <sstream>
#include <mutex>

#include <Ros2Utils.h>

#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/Bottle.h>
Expand All @@ -22,6 +24,7 @@
#include <yarp/os/Thread.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/RpcServer.h>
#include <yarp/math/Math.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/IMap2D.h>
#include <yarp/dev/MapGrid2D.h>
Expand All @@ -45,6 +48,7 @@

//Custom ros2 interfaces
#include <map2d_nws_ros2_msgs/srv/get_map_by_name.hpp>
#include <map2d_nws_ros2_msgs/srv/get_location_by_name.hpp>


/**
Expand All @@ -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 | |
Expand Down Expand Up @@ -95,6 +100,9 @@ class Map2D_nws_ros2 :
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);
void getLocByNameCallback(const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<map2d_nws_ros2_msgs::srv::GetLocationByName::Request> request,
std::shared_ptr<map2d_nws_ros2_msgs::srv::GetLocationByName::Response> response);
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);
Expand All @@ -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<visualization_msgs::msg::MarkerArray>::SharedPtr m_ros2Publisher_markers{nullptr};
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr m_ros2Publisher_map{nullptr};
rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr m_ros2Service_getMap{nullptr};
rclcpp::Service<map2d_nws_ros2_msgs::srv::GetMapByName>::SharedPtr m_ros2Service_getMapByName{nullptr};
rclcpp::Service<test_msgs::srv::BasicTypes>::SharedPtr m_ros2Service_rosCmdParser{nullptr};
yarp::os::RpcServer m_rpcPort;
rclcpp::Node::SharedPtr m_node;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_ros2Publisher_markers{nullptr};
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr m_ros2Publisher_map{nullptr};
rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr m_ros2Service_getMap{nullptr};
rclcpp::Service<map2d_nws_ros2_msgs::srv::GetMapByName>::SharedPtr m_ros2Service_getMapByName{nullptr};
rclcpp::Service<map2d_nws_ros2_msgs::srv::GetLocationByName>::SharedPtr m_ros2Service_getLocByName{nullptr};
rclcpp::Service<test_msgs::srv::BasicTypes>::SharedPtr m_ros2Service_rosCmdParser{nullptr};
};


Expand Down