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

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

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

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

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

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

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

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

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

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

@ -18,8 +18,7 @@
#include <memory>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -37,18 +36,18 @@ class Publisher
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
Publisher(ros_middleware_interface::PublisherHandle publisher_handle)
Publisher(rmw_publisher_t * publisher_handle)
: publisher_handle_(publisher_handle)
{}
template<typename MessageT> void
publish(std::shared_ptr<MessageT> &msg)
{
::ros_middleware_interface::publish(publisher_handle_, msg.get());
rmw_publish(publisher_handle_, msg.get());
}
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 <string>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -41,7 +40,7 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
SubscriptionBase(
ros_middleware_interface::SubscriberHandle subscription_handle,
rmw_subscription_t * subscription_handle,
std::string &topic_name)
: subscription_handle_(subscription_handle), topic_name_(topic_name)
{}
@ -57,7 +56,7 @@ public:
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
ros_middleware_interface::SubscriberHandle subscription_handle_;
rmw_subscription_t * subscription_handle_;
std::string topic_name_;
};
@ -69,7 +68,7 @@ public:
typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType;
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(ros_middleware_interface::SubscriberHandle subscription_handle,
Subscription(rmw_subscription_t * subscription_handle,
std::string &topic_name,
CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)

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

@ -21,8 +21,7 @@
#include <memory>
#include <thread>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp>
@ -50,7 +49,7 @@ public:
callback_(callback),
canceled_(false)
{
guard_condition_ = ros_middleware_interface::create_guard_condition();
guard_condition_ = rmw_create_guard_condition();
}
void
@ -64,7 +63,7 @@ public:
protected:
std::chrono::nanoseconds period_;
CallbackType callback_;
ros_middleware_interface::GuardConditionHandle guard_condition_;
rmw_guard_condition_t * guard_condition_;
bool canceled_;
@ -98,7 +97,7 @@ public:
{
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_
#define RCLCPP_RCLCPP_UTILITIES_HPP_
// TODO: remove
// TODO(wjwwood): remove
#include <iostream>
#include <cerrno>
@ -27,8 +27,7 @@
#include <mutex>
#include <thread>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rmw/rmw.h>
// Determine if sigaction is available
#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
@ -38,8 +37,8 @@
namespace
{
volatile sig_atomic_t g_signal_status = 0;
ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle = \
ros_middleware_interface::create_guard_condition();
rmw_guard_condition_t * g_sigint_guard_cond_handle = \
rmw_create_guard_condition();
std::condition_variable g_interrupt_condition_variable;
std::mutex g_interrupt_mutex;
@ -56,7 +55,7 @@ namespace
signal_handler(int signal_value)
#endif
{
// TODO: remove
// TODO(wjwwood): remove
std::cout << "signal_handler(" << signal_value << ")" << std::endl;
#ifdef HAS_SIGACTION
if (old_action.sa_flags & SA_SIGINFO)
@ -82,8 +81,7 @@ namespace
}
#endif
g_signal_status = signal_value;
using ros_middleware_interface::trigger_guard_condition;
trigger_guard_condition(g_sigint_guard_cond_handle);
rmw_trigger_guard_condition(g_sigint_guard_cond_handle);
g_interrupt_condition_variable.notify_all();
}
}
@ -99,7 +97,8 @@ namespace utilities
void
init(int argc, char *argv[])
{
ros_middleware_interface::init();
// TODO(wjwwood): Handle rmw_ret_t's value.
rmw_init();
#ifdef HAS_SIGACTION
struct sigaction action;
memset(&action, 0, sizeof(action));
@ -126,7 +125,7 @@ ok()
return ::g_signal_status == 0;
}
ros_middleware_interface::GuardConditionHandle
rmw_guard_condition_t *
get_global_sigint_guard_condition()
{
return ::g_sigint_guard_cond_handle;