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

modify factories in node to use groups

Browse source 
also check to make sure given groups are in node
This commit is contained in:
William Woodall 2014-08-29 17:50:17 -07:00
parent f4c97ecc62
commit b78164e4fe
2 changed files with 40 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -85,17 +85,24 @@ public:
rclcpp::timer::CallbackType callback, rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
typedef rclcpp::callback_group::CallbackGroup CallbackGroup;
typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList;
private: private:
RCLCPP_DISABLE_COPY(Node); RCLCPP_DISABLE_COPY(Node);
bool
group_in_node(callback_group::CallbackGroup::SharedPtr &group);
std::string name_; std::string name_;
ros_middleware_interface::NodeHandle node_handle_; ros_middleware_interface::NodeHandle node_handle_;
rclcpp::context::Context::SharedPtr context_; rclcpp::context::Context::SharedPtr context_;
rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; CallbackGroup::SharedPtr default_callback_group_;
std::list<std::weak_ptr<rclcpp::callback_group::CallbackGroup>> callback_groups_; CallbackGroupWeakPtrList callback_groups_;
size_t number_of_subscriptions_; size_t number_of_subscriptions_;
size_t number_of_timers_; size_t number_of_timers_;

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

@ -40,7 +40,8 @@ Node::Node(std::string node_name)
{} {}
Node::Node(std::string node_name, context::Context::SharedPtr context) Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context), number_of_subscriptions_(0) : name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0)
{ {
node_handle_ = ::ros_middleware_interface::create_node(); node_handle_ = ::ros_middleware_interface::create_node();
using rclcpp::callback_group::CallbackGroupType; using rclcpp::callback_group::CallbackGroupType;
@ -54,7 +55,9 @@ Node::create_callback_group(
{ {
using rclcpp::callback_group::CallbackGroup; using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType; using rclcpp::callback_group::CallbackGroupType;
return CallbackGroup::SharedPtr(new CallbackGroup(group_type)); auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
callback_groups_.push_back(group);
return group;
} }
template<typename MessageT> template<typename MessageT>
@ -71,6 +74,21 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
return publisher::Publisher::make_shared(publisher_handle); return publisher::Publisher::make_shared(publisher_handle);
} }
bool
Node::group_in_node(callback_group::CallbackGroup::SharedPtr &group)
{
bool group_belongs_to_this_node = false;
for (auto &weak_group : this->callback_groups_)
{
auto cur_group = weak_group.lock();
if (cur_group && cur_group == group)
{
group_belongs_to_this_node = true;
}
}
return group_belongs_to_this_node;
}
template<typename MessageT> template<typename MessageT>
typename subscription::Subscription<MessageT>::SharedPtr typename subscription::Subscription<MessageT>::SharedPtr
Node::create_subscription( Node::create_subscription(
@ -94,6 +112,11 @@ Node::create_subscription(
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub); auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
if (group) if (group)
{ {
if (!group_in_node(group))
{
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_subscription(sub_base_ptr); group->add_subscription(sub_base_ptr);
} }
else else
@ -109,9 +132,14 @@ Node::create_wall_timer(std::chrono::nanoseconds period,
rclcpp::timer::CallbackType callback, rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group) rclcpp::callback_group::CallbackGroup::SharedPtr group)
{ {
auto timer = rclcpp::timer::WallTimer::make_shared(period); auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
if (group) if (group)
{ {
if (!group_in_node(group))
{
// TODO: use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer); group->add_timer(timer);
} }
else else