niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Added support for services

Browse source
This commit is contained in:
Esteve Fernandez 2014-12-11 17:07:36 -08:00
parent f9337a5f32
commit 81040198e1
6 changed files with 436 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

8
rclcpp/include/rclcpp/callback_group.hpp
View file

@ -22,6 +22,7 @@
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
#include <rclcpp/service.hpp>
namespace rclcpp
{
@ -62,9 +63,16 @@ private:
timer_ptrs_.push_back(timer_ptr);
}
void
add_service(const service::ServiceBase::SharedPtr &service_ptr)
{
service_ptrs_.push_back(service_ptr);
}
CallbackGroupType type_;
std::vector<subscription::SubscriptionBase::SharedPtr> subscription_ptrs_;
std::vector<timer::TimerBase::SharedPtr> timer_ptrs_;
std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
std::atomic_bool can_be_taken_from_;
};

65
rclcpp/include/rclcpp/client.hpp Normal file
View file

@ -0,0 +1,65 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#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/macros.hpp>
namespace rclcpp
{
// Forward declaration for friend statement
namespace node {class Node;}
namespace client
{
template<typename ServiceT>
class Client
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
Client(ros_middleware_interface::ClientHandle client_handle,
std::string& service_name)
: client_handle_(client_handle), service_name_(service_name)
{}
std::shared_ptr<typename ServiceT::Response>
send_request(std::shared_ptr<typename ServiceT::Request> &req)
{
::ros_middleware_interface::send_request(client_handle_, req.get());
std::shared_ptr<typename ServiceT::Response> res = std::make_shared<typename ServiceT::Response>();
::ros_middleware_interface::receive_response(client_handle_, res.get());
return res;
}
private:
ros_middleware_interface::ClientHandle client_handle_;
std::string service_name_;
};
} /* namespace client */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */

170
rclcpp/include/rclcpp/executor.hpp
View file

@ -106,6 +106,7 @@ protected:
// Either the subscription or the timer will be set, but not both
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
rclcpp::timer::TimerBase::SharedPtr timer;
rclcpp::service::ServiceBase::SharedPtr service;
// These are used to keep the scope on the containing items
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
rclcpp::node::Node::SharedPtr node;
@ -126,6 +127,10 @@ protected:
{
execute_subscription(any_exec->subscription);
}
if (any_exec->service)
{
execute_service(any_exec->service);
}
// Reset the callback_group, regardless of type
any_exec->callback_group->can_be_taken_from_.store(true);
// Wake the wait, because it may need to be recalculated or work that
@ -161,6 +166,26 @@ protected:
timer->callback_();
}
static void
execute_service(
rclcpp::service::ServiceBase::SharedPtr &service)
{
std::shared_ptr<void> request = service->create_request();
bool taken = ros_middleware_interface::take_request(
service->service_handle_,
request.get());
if (taken)
{
service->handle_request(request);
}
else
{
std::cout << "[rclcpp::error] take failed for service on service: "
<< service->get_service_name()
<< std::endl;
}
}
/*** Populate class storage from stored weak node pointers and wait. ***/
void
@ -170,6 +195,7 @@ protected:
bool has_invalid_weak_nodes = false;
std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
std::vector<rclcpp::timer::TimerBase::SharedPtr> timers;
std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
for (auto &weak_node : weak_nodes_)
{
auto node = weak_node.lock();
@ -193,6 +219,10 @@ protected:
{
timers.push_back(timer);
}
for (auto &service : group->service_ptrs_)
{
services.push_back(service);
}
}
}
// Clean up any invalid nodes, if they were detected
@ -224,6 +254,28 @@ protected:
subscription->subscription_handle_.data_;
subscriber_handle_index += 1;
}
// 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 **>(
std::malloc(sizeof(void *) * number_of_services));
if (service_handles.services_ == NULL)
{
// TODO: 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_handle_index += 1;
}
// Use the number of guard conditions to allocate memory in the handles
// Add 2 to the number for the ctrl-c guard cond and the executor's
size_t start_of_timer_guard_conds = 2;
@ -254,9 +306,11 @@ protected:
timer->guard_condition_.data_;
guard_cond_handle_index += 1;
}
// Now wait on the waitable subscriptions and timers
ros_middleware_interface::wait(subscriber_handles,
guard_condition_handles,
service_handles,
nonblocking);
// If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions_[0] != 0)
@ -264,6 +318,7 @@ protected:
// Make sure to free memory
// TODO: Remove theses when "Avoid redundant malloc's" todo is addressed
std::free(subscriber_handles.subscribers_);
std::free(service_handles.services_);
std::free(guard_condition_handles.guard_conditions_);
return;
}
@ -286,9 +341,20 @@ protected:
guard_condition_handles_.push_back(handle);
}
}
// Then the services
for (size_t i = 0; i < number_of_services; ++i)
{
void *handle = service_handles.services_[i];
if (handle)
{
service_handles_.push_back(handle);
}
}
// Make sure to free memory
// TODO: Remove theses when "Avoid redundant malloc's" todo is addressed
std::free(subscriber_handles.subscribers_);
std::free(service_handles.services_);
std::free(guard_condition_handles.guard_conditions_);
}
@ -352,6 +418,35 @@ protected:
return rclcpp::timer::TimerBase::SharedPtr();
}
rclcpp::service::ServiceBase::SharedPtr
get_service_by_handle(void * service_handle)
{
for (auto weak_node : weak_nodes_)
{
auto node = weak_node.lock();
if (!node)
{
continue;
}
for (auto weak_group : node->callback_groups_)
{
auto group = weak_group.lock();
if (!group)
{
continue;
}
for (auto service : group->service_ptrs_)
{
if (service->service_handle_.data_ == service_handle)
{
return service;
}
}
}
}
return rclcpp::service::ServiceBase::SharedPtr();
}
void
remove_subscriber_handle_from_subscriber_handles(void *handle)
{
@ -364,6 +459,12 @@ protected:
guard_condition_handles_.remove(handle);
}
void
remove_service_handle_from_service_handles(void *handle)
{
service_handles_.remove(handle);
}
rclcpp::node::Node::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr &group)
{
@ -516,6 +617,67 @@ protected:
}
}
rclcpp::callback_group::CallbackGroup::SharedPtr
get_group_by_service(
rclcpp::service::ServiceBase::SharedPtr &service)
{
for (auto &weak_node : weak_nodes_)
{
auto node = weak_node.lock();
if (!node)
{
continue;
}
for (auto &weak_group : node->callback_groups_)
{
auto group = weak_group.lock();
for (auto &serv : group->service_ptrs_)
{
if (serv == service)
{
return group;
}
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
void
get_next_service(std::shared_ptr<AnyExecutable> &any_exec)
{
for (auto handle : service_handles_)
{
auto service = get_service_by_handle(handle);
if (service)
{
// Find the group for this handle and see if it can be serviced
auto group = get_group_by_service(service);
if (!group)
{
// Group was not found, meaning the service is not valid...
// Remove it from the ready list and continue looking
remove_service_handle_from_service_handles(handle);
continue;
}
if (!group->can_be_taken_from_.load())
{
// Group is mutually exclusive and is being used, so skip it for now
// Leave it to be checked next time, but continue searching
continue;
}
// Otherwise it is safe to set and return the any_exec
any_exec->service = service;
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group);
remove_service_handle_from_service_handles(handle);
return;
}
// Else, the service is no longer valid, remove it and continue
remove_service_handle_from_service_handles(handle);
}
}
std::shared_ptr<AnyExecutable>
get_next_ready_executable()
{
@ -532,6 +694,12 @@ protected:
{
return any_exec;
}
// Check the services to see if there are any that are ready
get_next_service(any_exec);
if (any_exec->service)
{
return any_exec;
}
// If there is neither a ready timer nor subscription, return a null ptr
any_exec.reset();
return any_exec;
@ -580,6 +748,8 @@ private:
SubscriberHandles subscriber_handles_;
typedef std::list<void*> GuardConditionHandles;
GuardConditionHandles guard_condition_handles_;
typedef std::list<void*> ServiceHandles;
ServiceHandles service_handles_;
};

17
rclcpp/include/rclcpp/node.hpp
View file

@ -21,9 +21,11 @@
#include <string>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/client.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
@ -89,6 +91,20 @@ public:
typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList;
/* Create and return a Client. */
template <typename ServiceT>
typename rclcpp::client::Client<ServiceT>::SharedPtr
create_client(std::string service_name);
/* Create and return a Service. */
template <typename ServiceT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service(
std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
private:
RCLCPP_DISABLE_COPY(Node);
@ -106,6 +122,7 @@ private:
size_t number_of_subscriptions_;
size_t number_of_timers_;
size_t number_of_services_;
};

64
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -23,6 +23,7 @@
#include <ros_middleware_interface/handles.h>
#include <ros_middleware_interface/get_type_support_handle.h>
#include <ros_middleware_interface/get_service_type_support_handle.h>
#include <rclcpp/contexts/default_context.hpp>
@ -41,7 +42,7 @@ Node::Node(std::string node_name)
Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0)
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
{
node_handle_ = ::ros_middleware_interface::create_node();
using rclcpp::callback_group::CallbackGroupType;
@ -162,4 +163,65 @@ Node::create_wall_timer(
group);
}
template<typename ServiceT>
typename client::Client<ServiceT>::SharedPtr
Node::create_client(std::string service_name)
{
namespace rmi = ::ros_middleware_interface;
auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
auto client_handle = rmi::create_client(this->node_handle_,
service_type_support_handle,
service_name.c_str());
using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(client_handle,
service_name);
return cli;
}
template <typename ServiceT>
typename service::Service<ServiceT>::SharedPtr
Node::create_service(
std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
namespace rmi = ::ros_middleware_interface;
auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
auto service_handle = rmi::create_service(this->node_handle_,
service_type_support_handle,
service_name.c_str());
using namespace rclcpp::service;
auto serv = Service<ServiceT>::make_shared(service_handle,
service_name,
callback);
auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
if (group)
{
if (!group_in_node(group))
{
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_service(serv_base_ptr);
}
else
{
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
return serv;
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

113
rclcpp/include/rclcpp/service.hpp Normal file
View file

@ -0,0 +1,113 @@
/* Copyright 2014 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef RCLCPP_RCLCPP_SERVICE_HPP_
#define RCLCPP_RCLCPP_SERVICE_HPP_
#include <memory>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
// Forward declaration for friend statement
namespace node {class Node;}
namespace service
{
class ServiceBase
{
friend class rclcpp::executor::Executor;
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
ServiceBase(
ros_middleware_interface::ServiceHandle service_handle,
std::string &service_name)
: service_handle_(service_handle), service_name_(service_name)
{}
std::string get_service_name()
{
return this->service_name_;
}
ros_middleware_interface::ServiceHandle get_service_handle()
{
return this->service_handle_;
}
virtual std::shared_ptr<void> create_request() = 0;
virtual void handle_request(std::shared_ptr<void> &request) = 0;
private:
RCLCPP_DISABLE_COPY(ServiceBase);
ros_middleware_interface::ServiceHandle service_handle_;
std::string service_name_;
};
template<typename ServiceT>
class Service : public ServiceBase
{
public:
typedef std::function<
void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response>&)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
Service(
ros_middleware_interface::ServiceHandle service_handle,
std::string &service_name,
CallbackType callback)
: ServiceBase(service_handle, service_name), callback_(callback)
{}
std::shared_ptr<void> create_request()
{
return std::shared_ptr<void>(new typename ServiceT::Request());
}
void handle_request(std::shared_ptr<void> &request)
{
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
callback_(typed_request, response);
send_response(typed_request, response);
}
void send_response(
std::shared_ptr<typename ServiceT::Request> &request,
std::shared_ptr<typename ServiceT::Response> &response)
{
::ros_middleware_interface::send_response(get_service_handle(), request.get(), response.get());
}
private:
RCLCPP_DISABLE_COPY(Service);
CallbackType callback_;
};
} /* namespace service */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */