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

updated to use new C rmw api

Browse source
This commit is contained in:
William Woodall 2015-03-03 22:11:59 -08:00
parent 9d63b20893
commit 35cf3952e3
9 changed files with 99 additions and 104 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -39,11 +39,16 @@ class Executor
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
Executor() Executor() : interrupt_guard_condition_(rmw_create_guard_condition()) {}
: interrupt_guard_condition_(
ros_middleware_interface::create_guard_condition()) virtual ~Executor()
{} {
virtual ~Executor() {} if (interrupt_guard_condition_ != nullptr)
{
// TODO(wjwwood): Check ret code.
rmw_destroy_guard_condition(interrupt_guard_condition_);
}
}
virtual void spin() = 0; virtual void spin() = 0;
@ -63,8 +68,7 @@ public:
} }
weak_nodes_.push_back(node_ptr); weak_nodes_.push_back(node_ptr);
// Interrupt waiting to handle new node // Interrupt waiting to handle new node
using ros_middleware_interface::trigger_guard_condition; rmw_trigger_guard_condition(interrupt_guard_condition_);
trigger_guard_condition(interrupt_guard_condition_);
} }
virtual void virtual void
@ -82,8 +86,7 @@ public:
// If the node was matched and removed, interrupt waiting // If the node was matched and removed, interrupt waiting
if (node_removed) if (node_removed)
{ {
using ros_middleware_interface::trigger_guard_condition; rmw_trigger_guard_condition(interrupt_guard_condition_);
trigger_guard_condition(interrupt_guard_condition_);
} }
} }
@ -140,8 +143,7 @@ protected:
any_exec->callback_group->can_be_taken_from_.store(true); any_exec->callback_group->can_be_taken_from_.store(true);
// Wake the wait, because it may need to be recalculated or work that // Wake the wait, because it may need to be recalculated or work that
// was previously blocked is now available. // was previously blocked is now available.
using ros_middleware_interface::trigger_guard_condition; rmw_trigger_guard_condition(interrupt_guard_condition_);
trigger_guard_condition(interrupt_guard_condition_);
} }
static void static void
@ -149,9 +151,8 @@ protected:
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
{ {
std::shared_ptr<void> message = subscription->create_message(); std::shared_ptr<void> message = subscription->create_message();
bool taken = ros_middleware_interface::take( auto taken = rmw_take(subscription->subscription_handle_, message.get());
subscription->subscription_handle_, // TODO(wjwwood): taken is no longer a boolean, check against return types.
message.get());
if (taken) if (taken)
{ {
subscription->handle_message(message); subscription->handle_message(message);
@ -268,22 +269,22 @@ protected:
} }
// Use the number of subscriptions to allocate memory in the handles // Use the number of subscriptions to allocate memory in the handles
size_t number_of_subscriptions = subs.size(); size_t number_of_subscriptions = subs.size();
ros_middleware_interface::SubscriberHandles subscriber_handles; rmw_subscriptions_t subscriber_handles;
subscriber_handles.subscriber_count_ = number_of_subscriptions; subscriber_handles.subscriber_count = number_of_subscriptions;
// TODO: Avoid redundant malloc's // TODO(wjwwood): Avoid redundant malloc's
subscriber_handles.subscribers_ = static_cast<void **>( subscriber_handles.subscribers = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_subscriptions)); std::malloc(sizeof(void *) * number_of_subscriptions));
if (subscriber_handles.subscribers_ == NULL) if (subscriber_handles.subscribers == NULL)
{ {
// TODO: Use a different error here? maybe std::bad_alloc? // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error("Could not malloc for subscriber pointers."); throw std::runtime_error("Could not malloc for subscriber pointers.");
} }
// Then fill the SubscriberHandles with ready subscriptions // Then fill the SubscriberHandles with ready subscriptions
size_t subscriber_handle_index = 0; size_t subscriber_handle_index = 0;
for (auto &subscription : subs) for (auto &subscription : subs)
{ {
subscriber_handles.subscribers_[subscriber_handle_index] = \ subscriber_handles.subscribers[subscriber_handle_index] = \
subscription->subscription_handle_.data_; subscription->subscription_handle_->data;
subscriber_handle_index += 1; subscriber_handle_index += 1;
} }
@ -333,54 +334,54 @@ protected:
// Add 2 to the number for the ctrl-c guard cond and the executor's // Add 2 to the number for the ctrl-c guard cond and the executor's
size_t start_of_timer_guard_conds = 2; size_t start_of_timer_guard_conds = 2;
size_t number_of_guard_conds = timers.size() + start_of_timer_guard_conds; size_t number_of_guard_conds = timers.size() + start_of_timer_guard_conds;
ros_middleware_interface::GuardConditionHandles guard_condition_handles; rmw_guard_conditions_t guard_condition_handles;
guard_condition_handles.guard_condition_count_ = number_of_guard_conds; guard_condition_handles.guard_condition_count = number_of_guard_conds;
// TODO: Avoid redundant malloc's // TODO(wjwwood): Avoid redundant malloc's
guard_condition_handles.guard_conditions_ = static_cast<void **>( guard_condition_handles.guard_conditions = static_cast<void **>(
std::malloc(sizeof(void *) * number_of_guard_conds)); std::malloc(sizeof(void *) * number_of_guard_conds));
if (guard_condition_handles.guard_conditions_ == NULL) if (guard_condition_handles.guard_conditions == NULL)
{ {
// TODO: Use a different error here? maybe std::bad_alloc? // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
throw std::runtime_error( throw std::runtime_error(
"Could not malloc for guard condition pointers."); "Could not malloc for guard condition pointers.");
} }
// Put the global ctrl-c guard condition in // Put the global ctrl-c guard condition in
assert(guard_condition_handles.guard_condition_count_ > 1); assert(guard_condition_handles.guard_condition_count > 1);
guard_condition_handles.guard_conditions_[0] = \ guard_condition_handles.guard_conditions[0] = \
rclcpp::utilities::get_global_sigint_guard_condition().data_; rclcpp::utilities::get_global_sigint_guard_condition()->data;
// Put the executor's guard condition in // Put the executor's guard condition in
guard_condition_handles.guard_conditions_[1] = \ guard_condition_handles.guard_conditions[1] = \
interrupt_guard_condition_.data_; interrupt_guard_condition_->data;
// Then fill the SubscriberHandles with ready subscriptions // Then fill the SubscriberHandles with ready subscriptions
size_t guard_cond_handle_index = start_of_timer_guard_conds; size_t guard_cond_handle_index = start_of_timer_guard_conds;
for (auto &timer : timers) for (auto &timer : timers)
{ {
guard_condition_handles.guard_conditions_[guard_cond_handle_index] = \ guard_condition_handles.guard_conditions[guard_cond_handle_index] = \
timer->guard_condition_.data_; timer->guard_condition_->data;
guard_cond_handle_index += 1; guard_cond_handle_index += 1;
} }
// Now wait on the waitable subscriptions and timers // Now wait on the waitable subscriptions and timers
ros_middleware_interface::wait(subscriber_handles, rmw_wait(&subscriber_handles,
guard_condition_handles, &guard_condition_handles,
service_handles, &client_handles,
client_handles, nonblocking);
nonblocking);
// If ctrl-c guard condition, return directly // If ctrl-c guard condition, return directly
if (guard_condition_handles.guard_conditions_[0] != 0) if (guard_condition_handles.guard_conditions[0] != 0)
{ {
// Make sure to free memory // Make sure to free memory
// TODO: Remove theses when "Avoid redundant malloc's" todo is addressed // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's"
std::free(subscriber_handles.subscribers_); // todo has been addressed.
std::free(service_handles.services_); std::free(subscriber_handles.subscribers);
std::free(guard_condition_handles.guard_conditions_); std::free(service_handles.services);
std::free(guard_condition_handles.guard_conditions);
return; return;
} }
// Add the new work to the class's list of things waiting to be executed // Add the new work to the class's list of things waiting to be executed
// Starting with the subscribers // Starting with the subscribers
for (size_t i = 0; i < number_of_subscriptions; ++i) for (size_t i = 0; i < number_of_subscriptions; ++i)
{ {
void *handle = subscriber_handles.subscribers_[i]; void *handle = subscriber_handles.subscribers[i];
if (handle) if (handle)
{ {
subscriber_handles_.push_back(handle); subscriber_handles_.push_back(handle);
@ -389,7 +390,7 @@ protected:
// Then the timers, start with start_of_timer_guard_conds // Then the timers, start with start_of_timer_guard_conds
for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i)
{ {
void *handle = guard_condition_handles.guard_conditions_[i]; void *handle = guard_condition_handles.guard_conditions[i];
if (handle) if (handle)
{ {
guard_condition_handles_.push_back(handle); guard_condition_handles_.push_back(handle);
@ -415,10 +416,11 @@ protected:
} }
// Make sure to free memory // Make sure to free memory
// TODO: Remove theses when "Avoid redundant malloc's" todo is addressed // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's"
std::free(subscriber_handles.subscribers_); // todo has been addressed.
std::free(service_handles.services_); std::free(subscriber_handles.subscribers);
std::free(guard_condition_handles.guard_conditions_); std::free(service_handles.services);
std::free(guard_condition_handles.guard_conditions);
} }
/******************************/ /******************************/
@ -442,7 +444,7 @@ protected:
} }
for (auto subscription : group->subscription_ptrs_) for (auto subscription : group->subscription_ptrs_)
{ {
if (subscription->subscription_handle_.data_ == subscriber_handle) if (subscription->subscription_handle_->data == subscriber_handle)
{ {
return subscription; return subscription;
} }
@ -471,7 +473,7 @@ protected:
} }
for (auto timer : group->timer_ptrs_) for (auto timer : group->timer_ptrs_)
{ {
if (timer->guard_condition_.data_ == guard_condition_handle) if (timer->guard_condition_->data == guard_condition_handle)
{ {
return timer; return timer;
} }
@ -874,9 +876,8 @@ protected:
std::shared_ptr<AnyExecutable> std::shared_ptr<AnyExecutable>
get_next_executable(bool nonblocking=false) get_next_executable(bool nonblocking=false)
{ {
namespace rmi = ros_middleware_interface;
// Check to see if there are any subscriptions or timers needing service // Check to see if there are any subscriptions or timers needing service
// TODO: improve run to run efficiency of this function // TODO(wjwwood): improve run to run efficiency of this function
auto any_exec = get_next_ready_executable(); auto any_exec = get_next_ready_executable();
// If there are none // If there are none
if (!any_exec) if (!any_exec)
@ -904,7 +905,7 @@ protected:
return any_exec; return any_exec;
} }
ros_middleware_interface::GuardConditionHandle interrupt_guard_condition_; rmw_guard_condition_t * interrupt_guard_condition_;
private: private:
RCLCPP_DISABLE_COPY(Executor); RCLCPP_DISABLE_COPY(Executor);

3
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
View file

@ -22,8 +22,7 @@
#include <mutex> #include <mutex>
#include <vector> #include <vector>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp> #include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp> #include <rclcpp/macros.hpp>

3
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
View file

@ -21,8 +21,7 @@
#include <memory> #include <memory>
#include <vector> #include <vector>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp> #include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp> #include <rclcpp/macros.hpp>

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

@ -29,6 +29,9 @@
#include <rclcpp/subscription.hpp> #include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp> #include <rclcpp/timer.hpp>
// Forward declaration of ROS middleware class
namespace rmw {struct rmw_node_t;}
namespace rclcpp namespace rclcpp
{ {
@ -115,7 +118,7 @@ private:
std::string name_; std::string name_;
ros_middleware_interface::NodeHandle node_handle_; rmw_node_t * node_handle_;
rclcpp::context::Context::SharedPtr context_; rclcpp::context::Context::SharedPtr context_;

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

@ -19,11 +19,8 @@
#include <memory> #include <memory>
#include <string> #include <string>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h> #include <rosidl_generator_cpp/MessageTypeSupport.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> #include <rclcpp/contexts/default_context.hpp>
@ -44,7 +41,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context), : name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
{ {
node_handle_ = ::ros_middleware_interface::create_node(); node_handle_ = rmw_create_node(name_.c_str());
using rclcpp::callback_group::CallbackGroupType; using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = \ default_callback_group_ = \
create_callback_group(CallbackGroupType::MutuallyExclusive); create_callback_group(CallbackGroupType::MutuallyExclusive);
@ -65,12 +62,12 @@ template<typename MessageT>
publisher::Publisher::SharedPtr publisher::Publisher::SharedPtr
Node::create_publisher(std::string topic_name, size_t queue_size) Node::create_publisher(std::string topic_name, size_t queue_size)
{ {
namespace rmi = ::ros_middleware_interface; using rosidl_generator_cpp::get_type_support_handle;
auto type_support_handle = get_type_support_handle<MessageT>();
auto type_support_handle = rmi::get_type_support_handle<MessageT>(); auto publisher_handle = rmw_create_publisher(node_handle_,
auto publisher_handle = rmi::create_publisher(this->node_handle_, type_support_handle,
type_support_handle, topic_name.c_str(),
topic_name.c_str()); queue_size);
return publisher::Publisher::make_shared(publisher_handle); return publisher::Publisher::make_shared(publisher_handle);
} }
@ -98,12 +95,12 @@ Node::create_subscription(
std::function<void(const std::shared_ptr<MessageT> &)> callback, std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group) rclcpp::callback_group::CallbackGroup::SharedPtr group)
{ {
namespace rmi = ::ros_middleware_interface; using rosidl_generator_cpp::get_type_support_handle;
auto type_support_handle = get_type_support_handle<MessageT>();
auto &type_support_handle = rmi::get_type_support_handle<MessageT>(); auto subscriber_handle = rmw_create_subscription(node_handle_,
auto subscriber_handle = rmi::create_subscriber(this->node_handle_, type_support_handle,
type_support_handle, topic_name.c_str(),
topic_name.c_str()); queue_size);
using namespace rclcpp::subscription; using namespace rclcpp::subscription;

9
rclcpp/include/rclcpp/publisher.hpp
View file

@ -18,8 +18,7 @@
#include <memory> #include <memory>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp> #include <rclcpp/macros.hpp>
@ -37,18 +36,18 @@ class Publisher
public: public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
Publisher(ros_middleware_interface::PublisherHandle publisher_handle) Publisher(rmw_publisher_t * publisher_handle)
: publisher_handle_(publisher_handle) : publisher_handle_(publisher_handle)
{} {}
template<typename MessageT> void template<typename MessageT> void
publish(std::shared_ptr<MessageT> &msg) publish(std::shared_ptr<MessageT> &msg)
{ {
::ros_middleware_interface::publish(publisher_handle_, msg.get()); rmw_publish(publisher_handle_, msg.get());
} }
private: private:
ros_middleware_interface::PublisherHandle publisher_handle_; rmw_publisher_t * publisher_handle_;
}; };

9
rclcpp/include/rclcpp/subscription.hpp
View file

@ -20,8 +20,7 @@
#include <memory> #include <memory>
#include <string> #include <string>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp> #include <rclcpp/macros.hpp>
@ -41,7 +40,7 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
SubscriptionBase( SubscriptionBase(
ros_middleware_interface::SubscriberHandle subscription_handle, rmw_subscription_t * subscription_handle,
std::string &topic_name) std::string &topic_name)
: subscription_handle_(subscription_handle), topic_name_(topic_name) : subscription_handle_(subscription_handle), topic_name_(topic_name)
{} {}
@ -57,7 +56,7 @@ public:
private: private:
RCLCPP_DISABLE_COPY(SubscriptionBase); RCLCPP_DISABLE_COPY(SubscriptionBase);
ros_middleware_interface::SubscriberHandle subscription_handle_; rmw_subscription_t * subscription_handle_;
std::string topic_name_; std::string topic_name_;
}; };
@ -69,7 +68,7 @@ public:
typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType; typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(ros_middleware_interface::SubscriberHandle subscription_handle, Subscription(rmw_subscription_t * subscription_handle,
std::string &topic_name, std::string &topic_name,
CallbackType callback) CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback) : SubscriptionBase(subscription_handle, topic_name), callback_(callback)

9
rclcpp/include/rclcpp/timer.hpp
View file

@ -21,8 +21,7 @@
#include <memory> #include <memory>
#include <thread> #include <thread>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp> #include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp> #include <rclcpp/rate.hpp>
@ -50,7 +49,7 @@ public:
callback_(callback), callback_(callback),
canceled_(false) canceled_(false)
{ {
guard_condition_ = ros_middleware_interface::create_guard_condition(); guard_condition_ = rmw_create_guard_condition();
} }
void void
@ -64,7 +63,7 @@ public:
protected: protected:
std::chrono::nanoseconds period_; std::chrono::nanoseconds period_;
CallbackType callback_; CallbackType callback_;
ros_middleware_interface::GuardConditionHandle guard_condition_; rmw_guard_condition_t * guard_condition_;
bool canceled_; bool canceled_;
@ -98,7 +97,7 @@ public:
{ {
return; return;
} }
ros_middleware_interface::trigger_guard_condition(guard_condition_); rmw_trigger_guard_condition(guard_condition_);
} }
} }

19
rclcpp/include/rclcpp/utilities.hpp
View file

@ -16,7 +16,7 @@
#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ #ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
#define RCLCPP_RCLCPP_UTILITIES_HPP_ #define RCLCPP_RCLCPP_UTILITIES_HPP_
// TODO: remove // TODO(wjwwood): remove
#include <iostream> #include <iostream>
#include <cerrno> #include <cerrno>
@ -27,8 +27,7 @@
#include <mutex> #include <mutex>
#include <thread> #include <thread>
#include <ros_middleware_interface/functions.h> #include <rmw/rmw.h>
#include <ros_middleware_interface/handles.h>
// Determine if sigaction is available // Determine if sigaction is available
#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
@ -38,8 +37,8 @@
namespace namespace
{ {
volatile sig_atomic_t g_signal_status = 0; volatile sig_atomic_t g_signal_status = 0;
ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle = \ rmw_guard_condition_t * g_sigint_guard_cond_handle = \
ros_middleware_interface::create_guard_condition(); rmw_create_guard_condition();
std::condition_variable g_interrupt_condition_variable; std::condition_variable g_interrupt_condition_variable;
std::mutex g_interrupt_mutex; std::mutex g_interrupt_mutex;
@ -56,7 +55,7 @@ namespace
signal_handler(int signal_value) signal_handler(int signal_value)
#endif #endif
{ {
// TODO: remove // TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl; std::cout << "signal_handler(" << signal_value << ")" << std::endl;
#ifdef HAS_SIGACTION #ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO) if (old_action.sa_flags & SA_SIGINFO)
@ -82,8 +81,7 @@ namespace
} }
#endif #endif
g_signal_status = signal_value; g_signal_status = signal_value;
using ros_middleware_interface::trigger_guard_condition; rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all(); g_interrupt_condition_variable.notify_all();
} }
} }
@ -99,7 +97,8 @@ namespace utilities
void void
init(int argc, char *argv[]) init(int argc, char *argv[])
{ {
ros_middleware_interface::init(); // TODO(wjwwood): Handle rmw_ret_t's value.
rmw_init();
#ifdef HAS_SIGACTION #ifdef HAS_SIGACTION
struct sigaction action; struct sigaction action;
memset(&action, 0, sizeof(action)); memset(&action, 0, sizeof(action));
@ -126,7 +125,7 @@ ok()
return ::g_signal_status == 0; return ::g_signal_status == 0;
} }
ros_middleware_interface::GuardConditionHandle rmw_guard_condition_t *
get_global_sigint_guard_condition() get_global_sigint_guard_condition()
{ {
return ::g_sigint_guard_cond_handle; return ::g_sigint_guard_cond_handle;