replacing ros_middleware with rmw
This commit is contained in:
parent
35cf3952e3
commit
209a31c815
12 changed files with 117 additions and 100 deletions
|
@ -4,7 +4,7 @@ project(rclcpp)
|
|||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
ament_export_dependencies(ros_middleware_interface)
|
||||
ament_export_dependencies(rmw)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
|
||||
|
|
|
@ -16,22 +16,21 @@
|
|||
#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
|
||||
#define RCLCPP_RCLCPP_CLIENT_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <ros_middleware_interface/functions.h>
|
||||
#include <ros_middleware_interface/handles.h>
|
||||
|
||||
#include <rclcpp/utilities.hpp>
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <future>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <rclcpp/utilities.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
// Forward declaration for friend statement
|
||||
namespace node {class Node;}
|
||||
namespace executor {class Executor;}
|
||||
|
||||
namespace client
|
||||
{
|
||||
|
@ -42,18 +41,26 @@ class ClientBase
|
|||
public:
|
||||
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
|
||||
|
||||
ClientBase(
|
||||
ros_middleware_interface::ClientHandle client_handle,
|
||||
std::string &service_name)
|
||||
ClientBase(rmw_client_t * client_handle,
|
||||
const std::string &service_name)
|
||||
: client_handle_(client_handle), service_name_(service_name)
|
||||
{}
|
||||
|
||||
~ClientBase()
|
||||
{
|
||||
if (client_handle_ != nullptr)
|
||||
{
|
||||
rmw_destroy_client(client_handle_);
|
||||
client_handle_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
std::string get_service_name()
|
||||
{
|
||||
return this->service_name_;
|
||||
}
|
||||
|
||||
ros_middleware_interface::ClientHandle get_client_handle()
|
||||
const rmw_client_t * get_client_handle()
|
||||
{
|
||||
return this->client_handle_;
|
||||
}
|
||||
|
@ -65,7 +72,7 @@ public:
|
|||
private:
|
||||
RCLCPP_DISABLE_COPY(ClientBase);
|
||||
|
||||
ros_middleware_interface::ClientHandle client_handle_;
|
||||
rmw_client_t * client_handle_;
|
||||
std::string service_name_;
|
||||
|
||||
};
|
||||
|
@ -82,8 +89,8 @@ public:
|
|||
|
||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
|
||||
|
||||
Client(ros_middleware_interface::ClientHandle client_handle,
|
||||
std::string& service_name)
|
||||
Client(rmw_client_t * client_handle,
|
||||
const std::string& service_name)
|
||||
: ClientBase(client_handle, service_name)
|
||||
{}
|
||||
|
||||
|
@ -94,12 +101,14 @@ public:
|
|||
|
||||
std::shared_ptr<void> create_request_header()
|
||||
{
|
||||
return std::shared_ptr<void>(new ros_middleware_interface::RequestId());
|
||||
// TODO(wjwwood): This should probably use rmw_request_id's allocator.
|
||||
// (since it is a C type)
|
||||
return std::shared_ptr<void>(new rmw_request_id_t);
|
||||
}
|
||||
|
||||
void handle_response(std::shared_ptr<void> &response, std::shared_ptr<void> &req_id)
|
||||
{
|
||||
auto typed_req_id = std::static_pointer_cast<ros_middleware_interface::RequestId>(req_id);
|
||||
auto typed_req_id = std::static_pointer_cast<rmw_request_id_t>(req_id);
|
||||
auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
|
||||
int64_t sequence_number = typed_req_id->sequence_number;
|
||||
auto tuple = this->pending_requests_[sequence_number];
|
||||
|
@ -121,7 +130,8 @@ public:
|
|||
typename ServiceT::Request::Ptr &request,
|
||||
CallbackType cb)
|
||||
{
|
||||
int64_t sequence_number = ::ros_middleware_interface::send_request(get_client_handle(), request.get());
|
||||
int64_t sequence_number =
|
||||
rmw_send_request(get_client_handle(), request.get());
|
||||
|
||||
SharedPromise call_promise = std::make_shared<Promise>();
|
||||
SharedFuture f(call_promise->get_future());
|
||||
|
|
|
@ -178,8 +178,7 @@ protected:
|
|||
{
|
||||
std::shared_ptr<void> request = service->create_request();
|
||||
std::shared_ptr<void> request_header = service->create_request_header();
|
||||
bool taken = ros_middleware_interface::take_request(
|
||||
service->service_handle_,
|
||||
bool taken = rmw_take_request(service->service_handle_,
|
||||
request.get(),
|
||||
request_header.get());
|
||||
if (taken)
|
||||
|
@ -200,8 +199,7 @@ protected:
|
|||
{
|
||||
std::shared_ptr<void> response = client->create_response();
|
||||
std::shared_ptr<void> request_header = client->create_request_header();
|
||||
bool taken = ros_middleware_interface::take_response(
|
||||
client->client_handle_,
|
||||
bool taken = rmw_take_response(client->client_handle_,
|
||||
response.get(),
|
||||
request_header.get());
|
||||
if (taken)
|
||||
|
@ -290,33 +288,33 @@ protected:
|
|||
|
||||
// Use the number of services to allocate memory in the handles
|
||||
size_t number_of_services = services.size();
|
||||
ros_middleware_interface::ServiceHandles service_handles;
|
||||
service_handles.service_count_ = number_of_services;
|
||||
// TODO: Avoid redundant malloc's
|
||||
service_handles.services_ = static_cast<void **>(
|
||||
rmw_services_t service_handles;
|
||||
service_handles.service_count = number_of_services;
|
||||
// TODO(esteve): Avoid redundant malloc's
|
||||
service_handles.services = static_cast<void **>(
|
||||
std::malloc(sizeof(void *) * number_of_services));
|
||||
if (service_handles.services_ == NULL)
|
||||
if (service_handles.services == NULL)
|
||||
{
|
||||
// TODO: Use a different error here? maybe std::bad_alloc?
|
||||
// TODO(esteve): Use a different error here? maybe std::bad_alloc?
|
||||
throw std::runtime_error("Could not malloc for service pointers.");
|
||||
}
|
||||
// Then fill the ServiceHandles with ready services
|
||||
size_t service_handle_index = 0;
|
||||
for (auto &service : services)
|
||||
{
|
||||
service_handles.services_[service_handle_index] = \
|
||||
service->service_handle_.data_;
|
||||
service_handles.services[service_handle_index] = \
|
||||
service->service_handle_->data;
|
||||
service_handle_index += 1;
|
||||
}
|
||||
|
||||
// Use the number of clients to allocate memory in the handles
|
||||
size_t number_of_clients = clients.size();
|
||||
ros_middleware_interface::ClientHandles client_handles;
|
||||
client_handles.client_count_ = number_of_clients;
|
||||
rmw_clients_t client_handles;
|
||||
client_handles.client_count = number_of_clients;
|
||||
// TODO: Avoid redundant malloc's
|
||||
client_handles.clients_ = static_cast<void **>(
|
||||
client_handles.clients = static_cast<void **>(
|
||||
std::malloc(sizeof(void *) * number_of_clients));
|
||||
if (client_handles.clients_ == NULL)
|
||||
if (client_handles.clients == NULL)
|
||||
{
|
||||
// TODO: Use a different error here? maybe std::bad_alloc?
|
||||
throw std::runtime_error("Could not malloc for client pointers.");
|
||||
|
@ -325,8 +323,8 @@ protected:
|
|||
size_t client_handle_index = 0;
|
||||
for (auto &client : clients)
|
||||
{
|
||||
client_handles.clients_[client_handle_index] = \
|
||||
client->client_handle_.data_;
|
||||
client_handles.clients[client_handle_index] = \
|
||||
client->client_handle_->data;
|
||||
client_handle_index += 1;
|
||||
}
|
||||
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
#include <rmw/rmw.h>
|
||||
#include <rosidl_generator_cpp/MessageTypeSupport.h>
|
||||
#include <rosidl_generator_cpp/ServiceTypeSupport.h>
|
||||
|
||||
#include <rclcpp/contexts/default_context.hpp>
|
||||
|
||||
|
@ -166,11 +167,11 @@ Node::create_client(
|
|||
std::string service_name,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
namespace rmi = ::ros_middleware_interface;
|
||||
using rosidl_generator_cpp::get_service_type_support_handle;
|
||||
auto service_type_support_handle =
|
||||
get_service_type_support_handle<ServiceT>();
|
||||
|
||||
auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
|
||||
|
||||
auto client_handle = rmi::create_client(this->node_handle_,
|
||||
auto client_handle = rmw_create_client(this->node_handle_,
|
||||
service_type_support_handle,
|
||||
service_name.c_str());
|
||||
|
||||
|
@ -184,7 +185,7 @@ Node::create_client(
|
|||
{
|
||||
if (!group_in_node(group))
|
||||
{
|
||||
// TODO: use custom exception
|
||||
// TODO(esteve): use custom exception
|
||||
throw std::runtime_error("Cannot create client, group not in node.");
|
||||
}
|
||||
group->add_client(cli_base_ptr);
|
||||
|
@ -206,11 +207,11 @@ Node::create_service(
|
|||
std::shared_ptr<typename ServiceT::Response>&)> callback,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
namespace rmi = ::ros_middleware_interface;
|
||||
using rosidl_generator_cpp::get_service_type_support_handle;
|
||||
auto service_type_support_handle =
|
||||
get_service_type_support_handle<ServiceT>();
|
||||
|
||||
auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
|
||||
|
||||
auto service_handle = rmi::create_service(this->node_handle_,
|
||||
auto service_handle = rmw_create_service(this->node_handle_,
|
||||
service_type_support_handle,
|
||||
service_name.c_str());
|
||||
|
||||
|
|
|
@ -16,11 +16,11 @@
|
|||
#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
|
||||
#define RCLCPP_RCLCPP_SERVICE_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <ros_middleware_interface/functions.h>
|
||||
#include <ros_middleware_interface/handles.h>
|
||||
#include <ros_middleware_interface/rpc.h>
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
#include <rclcpp/macros.hpp>
|
||||
|
||||
|
@ -29,7 +29,7 @@ namespace rclcpp
|
|||
{
|
||||
|
||||
// Forward declaration for friend statement
|
||||
namespace node {class Node;}
|
||||
namespace executor {class Executor;}
|
||||
|
||||
namespace service
|
||||
{
|
||||
|
@ -40,18 +40,26 @@ class ServiceBase
|
|||
public:
|
||||
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
|
||||
|
||||
ServiceBase(
|
||||
ros_middleware_interface::ServiceHandle service_handle,
|
||||
std::string &service_name)
|
||||
ServiceBase(rmw_service_t * service_handle,
|
||||
const std::string service_name)
|
||||
: service_handle_(service_handle), service_name_(service_name)
|
||||
{}
|
||||
|
||||
~ServiceBase()
|
||||
{
|
||||
if (service_handle_ != nullptr)
|
||||
{
|
||||
rmw_destroy_service(service_handle_);
|
||||
service_handle_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
std::string get_service_name()
|
||||
{
|
||||
return this->service_name_;
|
||||
}
|
||||
|
||||
ros_middleware_interface::ServiceHandle get_service_handle()
|
||||
const rmw_service_t * get_service_handle()
|
||||
{
|
||||
return this->service_handle_;
|
||||
}
|
||||
|
@ -63,7 +71,7 @@ public:
|
|||
private:
|
||||
RCLCPP_DISABLE_COPY(ServiceBase);
|
||||
|
||||
ros_middleware_interface::ServiceHandle service_handle_;
|
||||
rmw_service_t * service_handle_;
|
||||
std::string service_name_;
|
||||
|
||||
};
|
||||
|
@ -77,9 +85,8 @@ public:
|
|||
std::shared_ptr<typename ServiceT::Response>&)> CallbackType;
|
||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
|
||||
|
||||
Service(
|
||||
ros_middleware_interface::ServiceHandle service_handle,
|
||||
std::string &service_name,
|
||||
Service(rmw_service_t * service_handle,
|
||||
const std::string &service_name,
|
||||
CallbackType callback)
|
||||
: ServiceBase(service_handle, service_name), callback_(callback)
|
||||
{}
|
||||
|
@ -91,23 +98,24 @@ public:
|
|||
|
||||
std::shared_ptr<void> create_request_header()
|
||||
{
|
||||
return std::shared_ptr<void>(new ros_middleware_interface::RequestId());
|
||||
// TODO(wjwwood): This should probably use rmw_request_id's allocator.
|
||||
// (since it is a C type)
|
||||
return std::shared_ptr<void>(new rmw_request_id_t);
|
||||
}
|
||||
|
||||
void handle_request(std::shared_ptr<void> &request, std::shared_ptr<void> &req_id)
|
||||
{
|
||||
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
|
||||
auto typed_req_id = std::static_pointer_cast<ros_middleware_interface::RequestId>(req_id);
|
||||
auto typed_req_id = std::static_pointer_cast<rmw_request_id_t>(req_id);
|
||||
auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
|
||||
callback_(typed_request, response);
|
||||
send_response(typed_req_id, response);
|
||||
}
|
||||
|
||||
void send_response(
|
||||
std::shared_ptr<ros_middleware_interface::RequestId> &req_id,
|
||||
void send_response(std::shared_ptr<rmw_request_id_t> &req_id,
|
||||
std::shared_ptr<typename ServiceT::Response> &response)
|
||||
{
|
||||
::ros_middleware_interface::send_response(get_service_handle(), req_id.get(), response.get());
|
||||
rmw_send_response(get_service_handle(), req_id.get(), response.get());
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
|
@ -8,5 +8,5 @@
|
|||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<build_export_depend>ros_middleware_interface</build_export_depend>
|
||||
<build_export_depend>rmw</build_export_depend>
|
||||
</package>
|
||||
|
|
|
@ -1,11 +1,11 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
|
||||
project(ros_middleware_implementation NONE)
|
||||
project(rmw_implementation NONE)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
ament_package(
|
||||
CONFIG_EXTRAS "ros_middleware_implementation-extras.cmake"
|
||||
CONFIG_EXTRAS "rmw_implementation-extras.cmake"
|
||||
)
|
||||
|
||||
install(
|
|
@ -4,7 +4,7 @@
|
|||
# :param var: the output variable name containing the package names
|
||||
# :type var: list of strings
|
||||
#
|
||||
function(get_available_ros_middleware_implementations var)
|
||||
ament_index_get_resources(middleware_implementations "ros_middleware_implementation")
|
||||
function(get_available_rmw_implementations var)
|
||||
ament_index_get_resources(middleware_implementations "rmw_implementation")
|
||||
set(${var} ${middleware_implementations} PARENT_SCOPE)
|
||||
endfunction()
|
|
@ -7,8 +7,8 @@
|
|||
# :param var: the output variable name containing the package name
|
||||
# :type var: string
|
||||
#
|
||||
function(get_default_ros_middleware_implementation var)
|
||||
get_available_ros_middleware_implementations(middleware_implementations)
|
||||
function(get_default_rmw_implementation var)
|
||||
get_available_rmw_implementations(middleware_implementations)
|
||||
|
||||
if("${middleware_implementations} " STREQUAL " ")
|
||||
message(FATAL_ERROR "Could not find any ROS middleware implementation.")
|
|
@ -1,6 +1,6 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ros_middleware_implementation</name>
|
||||
<name>rmw_implementation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The decision which ROS middleware implementation should be used for C++.</description>
|
||||
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
|
||||
|
@ -9,7 +9,7 @@
|
|||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_export_depend>ament_cmake</buildtool_export_depend>
|
||||
|
||||
<exec_depend>ros_middleware_connext_cpp</exec_depend>
|
||||
<exec_depend>ros_middleware_connext_xtypes_dynamic_cpp</exec_depend>
|
||||
<exec_depend>ros_middleware_opensplice_cpp</exec_depend>
|
||||
<exec_depend>rmw_connext_cpp</exec_depend>
|
||||
<exec_depend>rmw_connext_xtypes_dynamic_cpp</exec_depend>
|
||||
<exec_depend>rmw_opensplice_cpp</exec_depend>
|
||||
</package>
|
12
rmw_implementation/rmw_implementation-extras.cmake
Normal file
12
rmw_implementation/rmw_implementation-extras.cmake
Normal file
|
@ -0,0 +1,12 @@
|
|||
# copied from rmw_implementation/rmw_implementation-extras.cmake
|
||||
|
||||
include("${rmw_implementation_DIR}/get_available_rmw_implementations.cmake")
|
||||
include("${rmw_implementation_DIR}/get_default_rmw_implementation.cmake")
|
||||
|
||||
get_default_rmw_implementation(_middleware_implementation)
|
||||
find_package("${_middleware_implementation}" REQUIRED)
|
||||
|
||||
# TODO should never need definitions and include dirs?
|
||||
list(APPEND rmw_implementation_DEFINITIONS ${${_middleware_implementation}_DEFINITIONS})
|
||||
list(APPEND rmw_implementation_INCLUDE_DIRS ${${_middleware_implementation}_INCLUDE_DIRS})
|
||||
list(APPEND rmw_implementation_LIBRARIES ${${_middleware_implementation}_LIBRARIES})
|
|
@ -1,12 +0,0 @@
|
|||
# copied from ros_middleware_implementation/ros_middleware_implementation-extras.cmake
|
||||
|
||||
include("${ros_middleware_implementation_DIR}/get_available_ros_middleware_implementations.cmake")
|
||||
include("${ros_middleware_implementation_DIR}/get_default_ros_middleware_implementation.cmake")
|
||||
|
||||
get_default_ros_middleware_implementation(_middleware_implementation)
|
||||
find_package("${_middleware_implementation}" REQUIRED)
|
||||
|
||||
# TODO should never need definitions and include dirs?
|
||||
list(APPEND ros_middleware_implementation_DEFINITIONS ${${_middleware_implementation}_DEFINITIONS})
|
||||
list(APPEND ros_middleware_implementation_INCLUDE_DIRS ${${_middleware_implementation}_INCLUDE_DIRS})
|
||||
list(APPEND ros_middleware_implementation_LIBRARIES ${${_middleware_implementation}_LIBRARIES})
|
Loading…
Add table
Add a link
Reference in a new issue