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

WIP changes to support multithreaded executor and timers

Browse source
This commit is contained in:
William Woodall 2014-08-27 00:54:00 -07:00
parent b6e1a2687a
commit 222a0535b6
7 changed files with 287 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -20,6 +20,7 @@
#include <vector>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
namespace rclcpp
{
@ -31,7 +32,7 @@ namespace executor {class Executor;}
namespace callback_group
{
enum class CallbackGroupType {NonThreadSafe, ThreadSafe};
enum class CallbackGroupType {MutuallyExclusive, Reentrant};
class CallbackGroup
{
@ -40,21 +41,29 @@ class CallbackGroup
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
private:
CallbackGroup(std::string group_name, CallbackGroupType group_type)
: name_(group_name), type_(group_type)
CallbackGroup(CallbackGroupType group_type)
: type_(group_type)
{}
private:
RCLCPP_DISABLE_COPY(CallbackGroup);
void
add_subscription(subscription::SubscriptionBase::SharedPtr &subscription_ptr)
add_subscription(
const subscription::SubscriptionBase::SharedPtr &subscription_ptr)
{
subscription_ptrs_.push_back(subscription_ptr);
}
std::string name_;
void
add_timer(const timer::TimerBase::SharedPtr &timer_ptr)
{
timer_ptrs_.push_back(timer_ptr);
}
CallbackGroupType type_;
std::vector<subscription::SubscriptionBase::SharedPtr> subscription_ptrs_;
std::vector<timer::TimerBase::SharedPtr> timer_ptrs_;
};

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

@ -88,8 +88,13 @@ protected:
std::malloc(sizeof(void *) * node.number_of_subscriptions_));
// Add subscriptions from groups
size_t handles_index = 0;
for (auto group : node.callback_groups_)
for (auto weak_group : node.callback_groups_)
{
auto group = weak_group.lock();
if (!group)
{
continue;
}
for (auto subscription : group->subscription_ptrs_)
{
assert(handles_index < node.number_of_subscriptions_);
@ -140,8 +145,13 @@ protected:
size_t handles_index = 0;
for (auto &node : nodes)
{
for (auto group : node->callback_groups_)
for (auto weak_group : node->callback_groups_)
{
auto group = weak_group.lock();
if (!group)
{
continue;
}
for (auto subscription : group->subscription_ptrs_)
{
assert(handles_index < number_of_subscriptions);
@ -151,7 +161,6 @@ protected:
}
}
}
assert(handles_index == number_of_subscriptions);
}
struct AnyExecutable
@ -186,8 +195,13 @@ protected:
{
continue;
}
for (auto group : node->callback_groups_)
for (auto weak_group : node->callback_groups_)
{
auto group = weak_group.lock();
if (!group)
{
continue;
}
for (auto subscription : group->subscription_ptrs_)
{
if (subscription->subscription_handle_.data_ == subscriber_handle)

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

@ -19,6 +19,7 @@
#include <cassert>
#include <cstdlib>
#include <memory>
#include <mutex>
#include <vector>
#include <ros_middleware_interface/functions.h>
@ -41,6 +42,10 @@ class MultiThreadedExecutor : public executor::Executor
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(MultiThreadedExecutor);
MultiThreadedExecutor() {}
~MultiThreadedExecutor() {}
void add_node(rclcpp::node::Node::SharedPtr &node_ptr)
{
this->weak_nodes_.push_back(node_ptr);
@ -48,16 +53,48 @@ public:
void spin()
{
}
void spin_some()
std::vector<std::thread> threads;
size_t number_of_threads = std::thread::hardware_concurrency();
if (number_of_threads == 0)
{
number_of_threads = 1;
}
for (; number_of_threads > 0; --number_of_threads)
{
threads.emplace_back(std::thread(&MultiThreadedExecutor::run, this));
}
for (auto &thread : threads)
{
thread.join();
}
}
private:
void run()
{
while (rclcpp::utilities::ok())
{
std::shared_ptr<AnyExecutable> any_exec;
{
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
if (!rclcpp::utilities::ok())
{
return;
}
any_exec = get_next_executable();
}
if (any_exec && any_exec->subscription)
{
// Do callback
execute_subscription(any_exec->subscription);
}
}
}
RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
std::mutex wait_mutex_;
};

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

@ -16,6 +16,7 @@
#ifndef RCLCPP_RCLCPP_NODE_HPP_
#define RCLCPP_RCLCPP_NODE_HPP_
#include <list>
#include <memory>
#include <string>
@ -24,6 +25,7 @@
#include <rclcpp/macros.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
namespace rclcpp
{
@ -53,15 +55,9 @@ public:
std::string
get_name();
/* Creates a named callback group. */
void
create_callback_group(
std::string group_name,
rclcpp::callback_group::CallbackGroupType callback_group_type);
/* Return callback group if it exists, otherwise evaluates to false. */
/* Create and return a callback group. */
rclcpp::callback_group::CallbackGroup::SharedPtr
get_callback_group(std::string group_name);
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
/* Create and return a Publisher. */
template<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
@ -73,7 +69,21 @@ public:
create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback);
std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
/* Create a timer. */
rclcpp::timer::WallTimer::SharedPtr
create_wall_timer(
std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
rclcpp::timer::WallTimer::SharedPtr
create_wall_timer(
std::chrono::duration<long double, std::nano> period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
private:
RCLCPP_DISABLE_COPY(Node);
@ -85,9 +95,10 @@ private:
rclcpp::context::Context::SharedPtr context_;
rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
std::vector<rclcpp::callback_group::CallbackGroup::SharedPtr> callback_groups_;
std::list<std::weak_ptr<rclcpp::callback_group::CallbackGroup>> callback_groups_;
size_t number_of_subscriptions_;
size_t number_of_timers_;
};

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

@ -43,14 +43,22 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context), number_of_subscriptions_(0)
{
node_handle_ = ::ros_middleware_interface::create_node();
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_.reset(new CallbackGroup(
"default_group", CallbackGroupType::NonThreadSafe));
callback_groups_.push_back(default_callback_group_);
default_callback_group_ = \
create_callback_group(CallbackGroupType::MutuallyExclusive);
}
template<typename MessageT> publisher::Publisher::SharedPtr
rclcpp::callback_group::CallbackGroup::SharedPtr
Node::create_callback_group(
rclcpp::callback_group::CallbackGroupType group_type)
{
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
return CallbackGroup::SharedPtr(new CallbackGroup(group_type));
}
template<typename MessageT>
publisher::Publisher::SharedPtr
Node::create_publisher(std::string topic_name, size_t queue_size)
{
namespace rmi = ::ros_middleware_interface;
@ -68,7 +76,8 @@ typename subscription::Subscription<MessageT>::SharedPtr
Node::create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback)
std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
namespace rmi = ::ros_middleware_interface;
@ -83,9 +92,46 @@ Node::create_subscription(
topic_name,
callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
this->default_callback_group_->add_subscription(sub_base_ptr);
if (group)
{
group->add_subscription(sub_base_ptr);
}
else
{
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
return sub;
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer::make_shared(period);
if (group)
{
group->add_timer(timer);
}
else
{
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
return timer;
}
rclcpp::timer::WallTimer::SharedPtr
Node::create_wall_timer(
std::chrono::duration<long double, std::nano> period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
return create_wall_timer(
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
callback,
group);
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

39
rclcpp/include/rclcpp/rclcpp.hpp
View file

@ -18,23 +18,39 @@
#include <csignal>
#include <memory>
#include <mutex>
#include <rclcpp/node.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
namespace
{
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> g_executor(0);
std::mutex g_executor_mutex;
}
namespace rclcpp
{
constexpr std::chrono::seconds operator ""_s(unsigned long long s)
{
return std::chrono::seconds(s);
}
constexpr std::chrono::duration<long double> operator ""_s(long double s)
{
return std::chrono::duration<long double>(s);
}
constexpr std::chrono::nanoseconds
operator ""_ns(unsigned long long ns)
{
return std::chrono::nanoseconds(ns);
}
constexpr std::chrono::duration<long double, std::nano>
operator ""_ns(long double ns)
{
return std::chrono::duration<long double, std::nano>(ns);
}
using rclcpp::node::Node;
using rclcpp::publisher::Publisher;
using rclcpp::subscription::SubscriptionBase;
using rclcpp::subscription::Subscription;
using rclcpp::rate::GenericRate;
using rclcpp::rate::WallRate;
using rclcpp::utilities::ok;
@ -42,13 +58,8 @@ using rclcpp::utilities::init;
void spin_some(Node &node)
{
std::lock_guard<std::mutex> lock(g_executor_mutex);
if (!g_executor)
{
// Create an executor
g_executor.reset(new rclcpp::executors::SingleThreadedExecutor());
}
g_executor->spin_node_some(node);
rclcpp::executors::SingleThreadedExecutor executor;
executor.spin_node_some(node);
}
void spin_some(Node::SharedPtr &node_ptr)

114
rclcpp/include/rclcpp/timer.hpp Normal file
View file

@ -0,0 +1,114 @@
/* 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_TIMER_HPP_
#define RCLCPP_RCLCPP_TIMER_HPP_
#include <chrono>
#include <functional>
#include <memory>
#include <thread>
#include <rclcpp/macros.hpp>
namespace rclcpp
{
namespace timer
{
typedef std::function<void()> CallbackType;
class TimerBase
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
virtual bool sleep() = 0;
virtual bool is_steady() = 0;
virtual void reset() = 0;
};
template<class Clock = std::chrono::high_resolution_clock>
class GenericTimer : public TimerBase
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
GenericTimer(std::chrono::nanoseconds period)
: period_(period)
{
}
virtual bool
sleep()
{
// Time coming into sleep
auto now = Clock::now();
// Time of next interval
auto next_interval = last_interval_ + period_;
// Detect backwards time flow
if (now < last_interval_)
{
// Best thing to do is to set the next_interval to now + period
next_interval = now + period_;
}
// Calculate the time to sleep
auto time_to_sleep = next_interval - now;
// Update the interval
last_interval_ += period_;
// If the time_to_sleep is negative or zero, don't sleep
if (time_to_sleep <= std::chrono::seconds(0))
{
// If an entire cycle was missed then reset next interval.
// This might happen if the loop took more than a cycle.
// Or if time jumps forward.
if (now > next_interval + period_)
{
last_interval_ = now + period_;
}
// Either way do not sleep and return false
return false;
}
// Sleep
std::this_thread::sleep_for(time_to_sleep);
return true;
}
virtual bool
is_steady()
{
return Clock::is_steady;
}
virtual void
reset()
{
last_interval_ = Clock::now();
}
private:
RCLCPP_DISABLE_COPY(GenericTimer);
std::chrono::nanoseconds period_;
std::chrono::time_point<Clock> last_interval_;
};
typedef GenericTimer<std::chrono::steady_clock> WallTimer;
} /* namespace timer */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */