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

break Node into several separate interfaces (#277)

Browse source 
* add the NodeBaseInterface and impl NodeBase

* refactor rclcpp to use NodeBaseInterface

* triggering a guard condition is not const

* remove unnecessary pure virtual destructor

* remove unused private member, style

* create NodeTopics interface, refactor pub/sub

* add convenience functions to fix API breaks

* fix compilation errors from NodeTopics refactor

* move "Event" based exceptions to exceptions.hpp

* add the NodeGraphInterface and related API's

* update node and graph_listener to use NodeGraph API

* initialize node_topics_ and node_graph_ in Node

* remove methods from Node and reorganize the order

the removed methods are really low level and still
available via their respective Node*Interface class

* add the NodeServices API and implementation

* add the NodeParameters API and refactor Node

* mixups

* fixup NodeParameters constructor

* added NodeTimers API and refactor Node

* make new create_publisher and create_subscription free template functions

* fixup

* fixup

* fixup

* fixup share pointer to node in any_executable

* free env value before throwing on Windows

* uncrustify and cpplint

* address constness issues

* do not store the topic name as a std::string in subscription

* fixes to support const char * topic name

* fix incomplete type specification, which fails on Windows

* refactor after rebase from type support changes

* fixup Windows build

* fix template issues on Windows

* uncrustify

* remove the unnecessary callback group argument from the add_publisher func

* remove unnecessary using = directive

* do not store node name in C++

* fix client and service creation in Node constructor

* fix include orders
This commit is contained in:
William Woodall 2016-12-09 17:09:29 -08:00 • committed by GitHub
parent 2309e5e250
commit 734ac278db
53 changed files with 3106 additions and 1033 deletions
Download patch file Download diff file Expand all files Collapse all files

8
rclcpp/CMakeLists.txt
View file

@ -35,11 +35,17 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/intra_process_manager_impl.cpp
src/rclcpp/memory_strategies.cpp
src/rclcpp/memory_strategy.cpp
src/rclcpp/node.cpp
src/rclcpp/node_interfaces/node_base.cpp
src/rclcpp/node_interfaces/node_graph.cpp
src/rclcpp/node_interfaces/node_parameters.cpp
src/rclcpp/node_interfaces/node_services.cpp
src/rclcpp/node_interfaces/node_timers.cpp
src/rclcpp/node_interfaces/node_topics.cpp
src/rclcpp/parameter.cpp
src/rclcpp/parameter_client.cpp
src/rclcpp/parameter_service.cpp
src/rclcpp/publisher.cpp
src/rclcpp/node.cpp
src/rclcpp/service.cpp
src/rclcpp/subscription.cpp
src/rclcpp/timer.cpp

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

@ -17,8 +17,13 @@
#include <memory>
#include "rclcpp/callback_group.hpp"
#include "rclcpp/client.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
@ -44,7 +49,7 @@ struct AnyExecutable
rclcpp::client::ClientBase::SharedPtr client;
// These are used to keep the scope on the containing items
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
rclcpp::node::Node::SharedPtr node;
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
};
} // namespace executor

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

@ -30,10 +30,12 @@ namespace rclcpp
{
// Forward declarations for friend statement in class CallbackGroup
namespace node
namespace node_interfaces
{
class Node;
} // namespace node
class NodeServices;
class NodeTimers;
class NodeTopics;
} // namespace node_interfaces
namespace callback_group
{
@ -46,7 +48,9 @@ enum class CallbackGroupType
class CallbackGroup
{
friend class rclcpp::node::Node;
friend class rclcpp::node_interfaces::NodeServices;
friend class rclcpp::node_interfaces::NodeTimers;
friend class rclcpp::node_interfaces::NodeTopics;
public:
RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
@ -78,7 +82,7 @@ public:
const CallbackGroupType &
type() const;
private:
protected:
RCLCPP_DISABLE_COPY(CallbackGroup)
RCLCPP_PUBLIC

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

@ -29,6 +29,7 @@
#include "rclcpp/function_traits.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
@ -39,10 +40,10 @@
namespace rclcpp
{
namespace node
namespace node_interfaces
{
class Node;
} // namespace node
class NodeBaseInterface;
} // namespace node_interfaces
namespace client
{
@ -54,7 +55,8 @@ public:
RCLCPP_PUBLIC
ClientBase(
std::shared_ptr<rclcpp::node::Node> parent_node,
rclcpp::node_interfaces::NodeBaseInterface * node_base,
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
const std::string & service_name);
RCLCPP_PUBLIC
@ -98,7 +100,7 @@ protected:
rcl_node_t *
get_rcl_node_handle() const;
std::weak_ptr<rclcpp::node::Node> node_;
rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
std::shared_ptr<rcl_node_t> node_handle_;
rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
@ -127,10 +129,11 @@ public:
RCLCPP_SMART_PTR_DEFINITIONS(Client)
Client(
std::shared_ptr<rclcpp::node::Node> parent_node,
rclcpp::node_interfaces::NodeBaseInterface * node_base,
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
const std::string & service_name,
rcl_client_options_t & client_options)
: ClientBase(parent_node, service_name)
: ClientBase(node_base, node_graph, service_name)
{
using rosidl_typesupport_cpp::get_service_type_support_handle;
auto service_type_support_handle =

51
rclcpp/include/rclcpp/create_publisher.hpp Normal file
View file

@ -0,0 +1,51 @@
// Copyright 2016 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__CREATE_PUBLISHER_HPP_
#define RCLCPP__CREATE_PUBLISHER_HPP_
#include <memory>
#include <string>
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/publisher_factory.hpp"
#include "rmw/qos_profiles.h"
namespace rclcpp
{
template<typename MessageT, typename AllocatorT, typename PublisherT>
std::shared_ptr<PublisherT>
create_publisher(
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
const std::string & topic_name,
const rmw_qos_profile_t & qos_profile,
bool use_intra_process_comms,
std::shared_ptr<AllocatorT> allocator)
{
auto publisher_options = rcl_publisher_get_default_options();
publisher_options.qos = qos_profile;
auto pub = node_topics->create_publisher(
topic_name,
rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(allocator),
publisher_options,
use_intra_process_comms);
node_topics->add_publisher(pub);
return std::dynamic_pointer_cast<PublisherT>(pub);
}
} // namespace rclcpp
#endif // RCLCPP__CREATE_PUBLISHER_HPP_

62
rclcpp/include/rclcpp/create_subscription.hpp Normal file
View file

@ -0,0 +1,62 @@
// Copyright 2016 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__CREATE_SUBSCRIPTION_HPP_
#define RCLCPP__CREATE_SUBSCRIPTION_HPP_
#include <memory>
#include <string>
#include <utility>
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/subscription_factory.hpp"
#include "rmw/qos_profiles.h"
namespace rclcpp
{
template<typename MessageT, typename CallbackT, typename AllocatorT, typename SubscriptionT>
typename rclcpp::subscription::Subscription<MessageT, AllocatorT>::SharedPtr
create_subscription(
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
const std::string & topic_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
bool use_intra_process_comms,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, AllocatorT>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<AllocatorT> allocator)
{
auto subscription_options = rcl_subscription_get_default_options();
subscription_options.qos = qos_profile;
subscription_options.ignore_local_publications = ignore_local_publications;
auto factory =
rclcpp::create_subscription_factory<MessageT, CallbackT, AllocatorT, SubscriptionT>(
std::forward<CallbackT>(callback), msg_mem_strat, allocator);
auto sub = node_topics->create_subscription(
topic_name,
factory,
subscription_options,
use_intra_process_comms);
node_topics->add_subscription(sub, group);
return std::dynamic_pointer_cast<SubscriptionT>(sub);
}
} // namespace rclcpp
#endif // RCLCPP__CREATE_SUBSCRIPTION_HPP_

16
rclcpp/include/rclcpp/exceptions.hpp
View file

@ -95,6 +95,22 @@ public:
RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
};
/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
class InvalidEventError : public std::runtime_error
{
public:
InvalidEventError()
: std::runtime_error("event is invalid") {}
};
/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
class EventNotRegisteredError : public std::runtime_error
{
public:
EventNotRegisteredError()
: std::runtime_error("event already registered") {}
};
} // namespace exceptions
} // namespace rclcpp

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

@ -28,16 +28,21 @@
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/memory_strategies.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
// Forward declaration is used in convenience method signature.
namespace node
{
class Node;
} // namespace node
namespace executor
{
@ -114,7 +119,12 @@ public:
*/
RCLCPP_PUBLIC
virtual void
add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
/// Convenience function which takes Node and forwards NodeBaseInterface.
RCLCPP_PUBLIC
virtual void
add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
/// Remove a node from the executor.
/**
@ -125,7 +135,12 @@ public:
*/
RCLCPP_PUBLIC
virtual void
remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true);
remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
/// Convenience function which takes Node and forwards NodeBaseInterface.
RCLCPP_PUBLIC
virtual void
remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify = true);
/// Add a node to executor, execute the next available unit of work, and remove the node.
/**
@ -136,7 +151,7 @@ public:
*/
template<typename T = std::milli>
void
spin_node_once(rclcpp::node::Node::SharedPtr node,
spin_node_once(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
return spin_node_once_nanoseconds(
@ -145,13 +160,30 @@ public:
);
}
/// Convenience function which takes Node and forwards NodeBaseInterface.
template<typename NodeT = rclcpp::node::Node, typename T = std::milli>
void
spin_node_once(std::shared_ptr<NodeT> node,
std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{
return spin_node_once_nanoseconds(
node->get_node_base_interface(),
std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
);
}
/// Add a node, complete all immediately available work, and remove the node.
/**
* \param[in] node Shared pointer to the node to add.
*/
RCLCPP_PUBLIC
void
spin_node_some(rclcpp::node::Node::SharedPtr node);
spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
/// Convenience function which takes Node and forwards NodeBaseInterface.
RCLCPP_PUBLIC
void
spin_node_some(std::shared_ptr<rclcpp::node::Node> node);
/// Complete all available queued work without blocking.
/**
@ -247,7 +279,9 @@ public:
protected:
RCLCPP_PUBLIC
void
spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout);
spin_node_once_nanoseconds(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::nanoseconds timeout);
/// Find the next available executable and do the work associated with it.
/** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
@ -284,7 +318,7 @@ protected:
wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
RCLCPP_PUBLIC
rclcpp::node::Node::SharedPtr
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group);
RCLCPP_PUBLIC
@ -318,7 +352,7 @@ protected:
private:
RCLCPP_DISABLE_COPY(Executor)
std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
};
} // namespace executor

44
rclcpp/include/rclcpp/executors.hpp
View file

@ -16,6 +16,7 @@
#define RCLCPP__EXECUTORS_HPP_
#include <future>
#include <memory>
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
@ -30,13 +31,21 @@ namespace rclcpp
// \param[in] node_ptr Shared pointer to the node to spin.
RCLCPP_PUBLIC
void
spin_some(node::Node::SharedPtr node_ptr);
spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
RCLCPP_PUBLIC
void
spin_some(rclcpp::node::Node::SharedPtr node_ptr);
/// Create a default single-threaded executor and spin the specified node.
// \param[in] node_ptr Shared pointer to the node to spin.
RCLCPP_PUBLIC
void
spin(node::Node::SharedPtr node_ptr);
spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
RCLCPP_PUBLIC
void
spin(rclcpp::node::Node::SharedPtr node_ptr);
namespace executors
{
@ -57,7 +66,8 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
template<typename ResponseT, typename TimeT = std::milli>
rclcpp::executor::FutureReturnCode
spin_node_until_future_complete(
rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
rclcpp::executor::Executor & executor,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
@ -69,18 +79,44 @@ spin_node_until_future_complete(
return retcode;
}
template<typename NodeT = rclcpp::node::Node, typename ResponseT, typename TimeT = std::milli>
rclcpp::executor::FutureReturnCode
spin_node_until_future_complete(
rclcpp::executor::Executor & executor,
std::shared_ptr<NodeT> node_ptr,
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
return rclcpp::executors::spin_node_until_future_complete(
executor,
node_ptr->get_node_base_interface(),
future,
timeout);
}
} // namespace executors
template<typename FutureT, typename TimeT = std::milli>
rclcpp::executor::FutureReturnCode
spin_until_future_complete(
node::Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
std::shared_future<FutureT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
}
template<typename NodeT = rclcpp::node::Node, typename FutureT, typename TimeT = std::milli>
rclcpp::executor::FutureReturnCode
spin_until_future_complete(
std::shared_ptr<NodeT> node_ptr,
std::shared_future<FutureT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
}
} // namespace rclcpp
#endif // RCLCPP__EXECUTORS_HPP_

18
rclcpp/include/rclcpp/graph_listener.hpp
View file

@ -24,16 +24,12 @@
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node
{
class Node;
} // namespace node
namespace graph_listener
{
@ -91,7 +87,7 @@ public:
RCLCPP_PUBLIC
virtual
void
add_node(rclcpp::node::Node * node);
add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Return true if the given node is in the graph listener's list of nodes.
/* Also return false if given nullptr.
@ -101,7 +97,7 @@ public:
RCLCPP_PUBLIC
virtual
bool
has_node(rclcpp::node::Node * node);
has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Remove a node from the graph listener's list of nodes.
/*
@ -112,7 +108,7 @@ public:
RCLCPP_PUBLIC
virtual
void
remove_node(rclcpp::node::Node * node);
remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Stop the listening thread.
/* The thread cannot be restarted, and the class is defunct after calling.
@ -159,9 +155,9 @@ private:
std::atomic_bool is_shutdown_;
mutable std::mutex shutdown_mutex_;
mutable std::mutex nodes_barrier_mutex_;
mutable std::mutex nodes_mutex_;
std::vector<rclcpp::node::Node *> nodes_;
mutable std::mutex node_graph_interfaces_barrier_mutex_;
mutable std::mutex node_graph_interfaces_mutex_;
std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
rcl_guard_condition_t * shutdown_guard_condition_;

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

@ -17,6 +17,7 @@
#include <algorithm>
#include <atomic>
#include <cstring>
#include <functional>
#include <limits>
#include <map>
@ -92,6 +93,8 @@ public:
add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
{
subscriptions_[id] = subscription;
// subscription->get_topic_name() -> const char * can be used as the key,
// since subscriptions_ shares the ownership of subscription
subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
}
@ -253,8 +256,20 @@ private:
using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
std::hash<uint64_t>, std::equal_to<uint64_t>,
RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
using IDTopicMap = std::map<std::string, AllocSet,
std::less<std::string>, RebindAlloc<std::pair<const std::string, AllocSet>>>;
struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
{
bool
operator()(const char * lhs, const char * rhs) const
{
return std::strcmp(lhs, rhs) < 0;
}
};
using IDTopicMap = std::map<
const char *,
AllocSet,
strcmp_wrapper,
RebindAlloc<std::pair<const std::string, AllocSet>>>;
SubscriptionMap subscriptions_;

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

@ -50,7 +50,23 @@
RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
__RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
#define __RCLCPP_SHARED_PTR_ALIAS(...) using SharedPtr = std::shared_ptr<__VA_ARGS__>;
/* Defines aliases only for using the Class with smart pointers.
*
* Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
* method definitions which do not work on pure virtual classes and classes
* which are not CopyConstructable.
*
* Use in the public section of the class.
* Make sure to include <memory> in the header when using this.
*/
#define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
__RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
__RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \
__RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
#define __RCLCPP_SHARED_PTR_ALIAS(...) \
using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
#define __RCLCPP_MAKE_SHARED_DEFINITION(...) \
template<typename ... Args> \
@ -65,7 +81,9 @@
__RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
__RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
#define __RCLCPP_WEAK_PTR_ALIAS(...) using WeakPtr = std::weak_ptr<__VA_ARGS__>;
#define __RCLCPP_WEAK_PTR_ALIAS(...) \
using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
/// Defines aliases and static functions for using the Class with weak_ptrs.
#define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__)

6
rclcpp/include/rclcpp/memory_strategy.hpp
View file

@ -23,7 +23,7 @@
#include "rclcpp/any_executable.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
@ -41,7 +41,7 @@ class RCLCPP_PUBLIC MemoryStrategy
{
public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>;
using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
@ -88,7 +88,7 @@ public:
static rclcpp::client::ClientBase::SharedPtr
get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
static rclcpp::node::Node::SharedPtr
static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
const WeakNodeVector & weak_nodes);

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

@ -39,6 +39,12 @@
#include "rclcpp/event.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/service.hpp"
@ -46,36 +52,12 @@
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rcl
{
struct rcl_node_t;
} // namespace rcl
namespace rclcpp
{
namespace graph_listener
{
class GraphListener;
} // namespace graph_listener
namespace node
{
class InvalidEventError : public std::runtime_error
{
public:
InvalidEventError()
: std::runtime_error("event is invalid") {}
};
class EventNotRegisteredError : public std::runtime_error
{
public:
EventNotRegisteredError()
: std::runtime_error("event already registered") {}
};
/// Node is the single point of entry for creating publishers and subscribers.
class Node : public std::enable_shared_from_this<Node>
{
@ -109,7 +91,7 @@ public:
/// Get the name of the node.
// \return The name of the node.
RCLCPP_PUBLIC
const std::string &
const char *
get_name() const;
/// Create and return a callback group.
@ -117,6 +99,11 @@ public:
rclcpp::callback_group::CallbackGroup::SharedPtr
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
/// Return the list of callback groups in the node.
RCLCPP_PUBLIC
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
get_callback_groups() const;
/// Create and return a Publisher.
/**
* \param[in] topic_name The topic for this publisher to publish on.
@ -160,7 +147,11 @@ public:
Windows build breaks when static member function passed as default
argument to msg_mem_strat, nullptr is a workaround.
*/
template<typename MessageT, typename CallbackT, typename Alloc = std::allocator<void>>
template<
typename MessageT,
typename CallbackT,
typename Alloc = std::allocator<void>,
typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
create_subscription(
const std::string & topic_name,
@ -186,7 +177,11 @@ public:
Windows build breaks when static member function passed as default
argument to msg_mem_strat, nullptr is a workaround.
*/
template<typename MessageT, typename CallbackT, typename Alloc = std::allocator<void>>
template<
typename MessageT,
typename CallbackT,
typename Alloc = std::allocator<void>,
typename SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
create_subscription(
const std::string & topic_name,
@ -204,30 +199,13 @@ public:
* \param[in] callback User-defined callback function.
* \param[in] group Callback group to execute this timer's callback in.
*/
template<typename CallbackType>
typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
template<typename DurationT = std::milli, typename CallbackT>
typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
create_wall_timer(
std::chrono::nanoseconds period,
CallbackType callback,
std::chrono::duration<int64_t, DurationT> period,
CallbackT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/// Create a timer.
/**
* \param[in] period Time interval between triggers of the callback.
* \param[in] callback User-defined callback function.
* \param[in] group Callback group to execute this timer's callback in.
*/
// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
// 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);
using CallbackGroup = rclcpp::callback_group::CallbackGroup;
using CallbackGroupWeakPtr = std::weak_ptr<CallbackGroup>;
using CallbackGroupWeakPtrList = std::list<CallbackGroupWeakPtr>;
/* Create and return a Client. */
template<typename ServiceT>
typename rclcpp::client::Client<ServiceT>::SharedPtr
@ -258,16 +236,18 @@ public:
get_parameters(const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
const rclcpp::parameter::ParameterVariant
rclcpp::parameter::ParameterVariant
get_parameter(const std::string & name) const;
RCLCPP_PUBLIC
bool get_parameter(
bool
get_parameter(
const std::string & name,
rclcpp::parameter::ParameterVariant & parameter) const;
template<typename ParameterT>
bool get_parameter(const std::string & name, ParameterT & parameter) const;
bool
get_parameter(const std::string & name, ParameterT & parameter) const;
RCLCPP_PUBLIC
std::vector<rcl_interfaces::msg::ParameterDescriptor>
@ -281,6 +261,15 @@ public:
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
/// Register the callback for parameter changes
/**
* \param[in] User defined callback function, It is expected to atomically set parameters.
* \note Repeated invocations of this function will overwrite previous callbacks
*/
template<typename CallbackT>
void
register_param_change_callback(CallbackT && callback);
RCLCPP_PUBLIC
std::map<std::string, std::string>
get_topic_names_and_types() const;
@ -293,53 +282,6 @@ public:
size_t
count_subscribers(const std::string & topic_name) const;
RCLCPP_PUBLIC
const CallbackGroupWeakPtrList &
get_callback_groups() const;
RCLCPP_PUBLIC
const rcl_guard_condition_t *
get_notify_guard_condition() const;
RCLCPP_PUBLIC
const rcl_guard_condition_t *
get_graph_guard_condition() const;
RCLCPP_PUBLIC
const rcl_node_t *
get_rcl_node_handle() const;
/// Return the rcl_node_t node handle (non-const version).
RCLCPP_PUBLIC
rcl_node_t *
get_rcl_node_handle();
/// Return the rcl_node_t node handle in a std::shared_ptr.
/* This handle remains valid after the Node is destroyed.
* The actual rcl node is not finalized until it is out of scope everywhere.
*/
RCLCPP_PUBLIC
std::shared_ptr<rcl_node_t>
get_shared_node_handle();
/// Notify threads waiting on graph changes.
/* Affects threads waiting on the notify guard condition, see:
* get_notify_guard_condition(), as well as the threads waiting on graph
* changes using a graph Event, see: wait_for_graph_change().
*
* This is typically only used by the rclcpp::graph_listener::GraphListener.
*
* \throws RCLBaseError (a child of that exception) when an rcl error occurs
*/
RCLCPP_PUBLIC
void
notify_graph_change();
/// Notify any and all blocking node actions that shutdown has occurred.
RCLCPP_PUBLIC
void
notify_shutdown();
/// Return a graph event, which will be set anytime a graph change occurs.
/* The graph Event object is a loan which must be returned.
* The Event object is scoped and therefore to return the load just let it go
@ -362,27 +304,35 @@ public:
rclcpp::event::Event::SharedPtr event,
std::chrono::nanoseconds timeout);
/// Return the number of on loan graph events, see get_graph_event().
/* This is typically only used by the rclcpp::graph_listener::GraphListener.
*/
/// Return the Node's internal NodeBaseInterface implementation.
RCLCPP_PUBLIC
size_t
count_graph_users();
/// Register the callback for parameter changes
/**
* \param[in] User defined callback function, It is expected to atomically set parameters.
* \note Repeated invocations of this function will overwrite previous callbacks
*/
template<typename CallbackT>
void register_param_change_callback(CallbackT && callback);
std::atomic_bool has_executor;
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
get_node_base_interface();
/// Return the Node's internal NodeGraphInterface implementation.
RCLCPP_PUBLIC
void
add_service(rclcpp::service::ServiceBase::SharedPtr service,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
get_node_graph_interface();
/// Return the Node's internal NodeTimersInterface implementation.
RCLCPP_PUBLIC
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
get_node_timers_interface();
/// Return the Node's internal NodeTopicsInterface implementation.
RCLCPP_PUBLIC
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr
get_node_topics_interface();
/// Return the Node's internal NodeServicesInterface implementation.
RCLCPP_PUBLIC
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
get_node_services_interface();
/// Return the Node's internal NodeParametersInterface implementation.
RCLCPP_PUBLIC
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
get_node_parameters_interface();
private:
RCLCPP_DISABLE_COPY(Node)
@ -391,51 +341,14 @@ private:
bool
group_in_node(callback_group::CallbackGroup::SharedPtr group);
std::string name_;
std::shared_ptr<rcl_node_t> node_handle_;
rclcpp::context::Context::SharedPtr context_;
CallbackGroup::SharedPtr default_callback_group_;
CallbackGroupWeakPtrList callback_groups_;
size_t number_of_subscriptions_;
size_t number_of_timers_;
size_t number_of_services_;
size_t number_of_clients_;
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
bool use_intra_process_comms_;
mutable std::mutex mutex_;
/// Guard condition for notifying the Executor of changes to this node.
mutable std::mutex notify_guard_condition_mutex_;
rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
bool notify_guard_condition_is_valid_;
/// Graph Listener which waits on graph changes for the node and is shared across nodes.
std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
/// Whether or not this node needs to be added to the graph listener.
std::atomic_bool should_add_to_graph_listener_;
/// Mutex to guard the graph event related data structures.
std::mutex graph_mutex_;
/// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
std::condition_variable graph_cv_;
/// Weak references to graph events out on loan.
std::vector<rclcpp::event::Event::WeakPtr> graph_events_;
/// Number of graph events out on loan, used to determine if the graph should be monitored.
/* graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
std::atomic_size_t graph_users_count_;
std::function<typename rcl_interfaces::msg::SetParametersResult(
const typename std::vector<rclcpp::parameter::ParameterVariant> &
)> parameters_callback_ = nullptr;
std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
};
} // namespace node

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

@ -38,6 +38,8 @@
#include "rclcpp/contexts/default_context.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/create_publisher.hpp"
#include "rclcpp/create_subscription.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
@ -73,66 +75,15 @@ Node::create_publisher(
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
auto publisher_options = rcl_publisher_get_default_options();
publisher_options.qos = qos_profile;
auto message_alloc =
std::make_shared<typename PublisherT::MessageAlloc>(
*allocator.get());
publisher_options.allocator = allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
auto publisher = std::make_shared<PublisherT>(
node_handle_, topic_name, publisher_options, message_alloc);
if (use_intra_process_comms_) {
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
uint64_t intra_process_publisher_id =
intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
// *INDENT-OFF*
auto shared_publish_callback =
[weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process publish called after destruction of intra process manager");
}
if (!msg) {
throw std::runtime_error("cannot publisher msg which is a null pointer");
}
auto & message_type_info = typeid(MessageT);
if (message_type_info != type_info) {
throw std::runtime_error(
std::string("published type '") + type_info.name() +
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
}
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
uint64_t message_seq =
ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
return message_seq;
};
// *INDENT-ON*
publisher->setup_intra_process(
intra_process_publisher_id,
shared_publish_callback,
publisher_options);
}
if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
}
return publisher;
return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
this->node_topics_.get(),
topic_name,
qos_profile,
use_intra_process_comms_,
allocator);
}
template<typename MessageT, typename CallbackT, typename Alloc>
template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
@ -142,102 +93,30 @@ Node::create_subscription(
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
typename std::shared_ptr<Alloc> allocator)
std::shared_ptr<Alloc> allocator)
{
if (!allocator) {
allocator = std::make_shared<Alloc>();
}
rclcpp::subscription::AnySubscriptionCallback<MessageT,
Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(std::forward<CallbackT>(callback));
if (!msg_mem_strat) {
msg_mem_strat =
rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
using rclcpp::message_memory_strategy::MessageMemoryStrategy;
msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
}
auto message_alloc =
std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();
auto subscription_options = rcl_subscription_get_default_options();
subscription_options.qos = qos_profile;
subscription_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
subscription_options.ignore_local_publications = ignore_local_publications;
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_handle_,
return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
this->node_topics_.get(),
topic_name,
subscription_options,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
// Setup intra process.
if (use_intra_process_comms_) {
auto intra_process_options = rcl_subscription_get_default_options();
intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
intra_process_options.qos = qos_profile;
intra_process_options.ignore_local_publications = false;
auto intra_process_manager =
context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
uint64_t intra_process_subscription_id =
intra_process_manager->add_subscription(sub_base_ptr);
// *INDENT-OFF*
sub->setup_intra_process(
intra_process_subscription_id,
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
},
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
},
intra_process_options
);
// *INDENT-ON*
}
// Assign to a group.
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, group not in node.");
}
group->add_subscription(sub_base_ptr);
} else {
default_callback_group_->add_subscription(sub_base_ptr);
}
number_of_subscriptions_++;
if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on subscription creation: ") + rmw_get_error_string());
}
return sub;
std::forward<CallbackT>(callback),
qos_profile,
group,
ignore_local_publications,
use_intra_process_comms_,
msg_mem_strat,
allocator);
}
template<typename MessageT, typename CallbackT, typename Alloc>
template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
Node::create_subscription(
const std::string & topic_name,
@ -251,7 +130,7 @@ Node::create_subscription(
{
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = qos_history_depth;
return this->create_subscription<MessageT, CallbackT, Alloc>(
return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
topic_name,
std::forward<CallbackT>(callback),
qos,
@ -261,30 +140,17 @@ Node::create_subscription(
allocator);
}
template<typename CallbackType>
typename rclcpp::timer::WallTimer<CallbackType>::SharedPtr
template<typename DurationT, typename CallbackT>
typename rclcpp::timer::WallTimer<CallbackT>::SharedPtr
Node::create_wall_timer(
std::chrono::nanoseconds period,
CallbackType callback,
std::chrono::duration<int64_t, DurationT> period,
CallbackT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::timer::WallTimer<CallbackType>::make_shared(
period, std::move(callback));
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
group->add_timer(timer);
} else {
default_callback_group_->add_timer(timer);
}
number_of_timers_++;
if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on timer creation: ") + rmw_get_error_string());
}
auto timer = rclcpp::timer::WallTimer<CallbackT>::make_shared(
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
std::move(callback));
node_timers_->add_timer(timer, group);
return timer;
}
@ -302,27 +168,13 @@ Node::create_client(
using rclcpp::client::ClientBase;
auto cli = Client<ServiceT>::make_shared(
shared_from_this(),
node_base_.get(),
node_graph_,
service_name,
options);
auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
if (group) {
if (!group_in_node(group)) {
// TODO(esteve): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(cli_base_ptr);
} else {
default_callback_group_->add_client(cli_base_ptr);
}
number_of_clients_++;
if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on client creation: ") + rmw_get_error_string());
}
node_services_->add_client(cli_base_ptr, group);
return cli;
}
@ -341,21 +193,23 @@ Node::create_service(
service_options.qos = qos_profile;
auto serv = service::Service<ServiceT>::make_shared(
node_handle_,
node_base_->get_shared_rcl_node_handle(),
service_name, any_service_callback, service_options);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
add_service(serv_base_ptr, group);
node_services_->add_service(serv_base_ptr, group);
return serv;
}
template<typename CallbackT>
void Node::register_param_change_callback(CallbackT && callback)
void
Node::register_param_change_callback(CallbackT && callback)
{
this->parameters_callback_ = callback;
this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
}
template<typename ParameterT>
bool Node::get_parameter(const std::string & name, ParameterT & parameter) const
bool
Node::get_parameter(const std::string & name, ParameterT & parameter) const
{
rclcpp::parameter::ParameterVariant parameter_variant(name, parameter);
bool result = get_parameter(name, parameter_variant);

131
rclcpp/include/rclcpp/node_interfaces/node_base.hpp Normal file
View file

@ -0,0 +1,131 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_BASE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/context.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeBase part of the Node API.
class NodeBase : public NodeBaseInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
RCLCPP_PUBLIC
NodeBase(const std::string & node_name, rclcpp::context::Context::SharedPtr context);
RCLCPP_PUBLIC
virtual
~NodeBase();
RCLCPP_PUBLIC
virtual
const char *
get_name() const;
RCLCPP_PUBLIC
virtual
rclcpp::context::Context::SharedPtr
get_context();
RCLCPP_PUBLIC
virtual
rcl_node_t *
get_rcl_node_handle();
RCLCPP_PUBLIC
virtual
const rcl_node_t *
get_rcl_node_handle() const;
RCLCPP_PUBLIC
virtual
std::shared_ptr<rcl_node_t>
get_shared_rcl_node_handle();
RCLCPP_PUBLIC
virtual
std::shared_ptr<const rcl_node_t>
get_shared_rcl_node_handle() const;
RCLCPP_PUBLIC
virtual
rclcpp::callback_group::CallbackGroup::SharedPtr
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
RCLCPP_PUBLIC
virtual
rclcpp::callback_group::CallbackGroup::SharedPtr
get_default_callback_group();
RCLCPP_PUBLIC
virtual
bool
callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group);
RCLCPP_PUBLIC
virtual
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
get_callback_groups() const;
RCLCPP_PUBLIC
virtual
std::atomic_bool &
get_associated_with_executor_atomic();
RCLCPP_PUBLIC
virtual
rcl_guard_condition_t *
get_notify_guard_condition();
RCLCPP_PUBLIC
virtual
std::unique_lock<std::recursive_mutex>
acquire_notify_guard_condition_lock() const;
private:
RCLCPP_DISABLE_COPY(NodeBase)
rclcpp::context::Context::SharedPtr context_;
std::shared_ptr<rcl_node_t> node_handle_;
rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> callback_groups_;
std::atomic_bool associated_with_executor_;
/// Guard condition for notifying the Executor of changes to this node.
mutable std::recursive_mutex notify_guard_condition_mutex_;
rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
bool notify_guard_condition_is_valid_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_

136
rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp Normal file
View file

@ -0,0 +1,136 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include "rcl/node.h"
#include "rclcpp/callback_group.hpp"
#include "rclcpp/context.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeBase part of the Node API.
class NodeBaseInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
/// Return the name of the node.
// \return The name of the node.
RCLCPP_PUBLIC
virtual
const char *
get_name() const = 0;
/// Return the context of the node.
// \return SharedPtr to the node's context.
RCLCPP_PUBLIC
virtual
rclcpp::context::Context::SharedPtr
get_context() = 0;
/// Return the rcl_node_t node handle (non-const version).
RCLCPP_PUBLIC
virtual
rcl_node_t *
get_rcl_node_handle() = 0;
/// Return the rcl_node_t node handle (const version).
RCLCPP_PUBLIC
virtual
const rcl_node_t *
get_rcl_node_handle() const = 0;
/// Return the rcl_node_t node handle in a std::shared_ptr.
/* This handle remains valid after the Node is destroyed.
* The actual rcl node is not finalized until it is out of scope everywhere.
*/
RCLCPP_PUBLIC
virtual
std::shared_ptr<rcl_node_t>
get_shared_rcl_node_handle() = 0;
/// Return the rcl_node_t node handle in a std::shared_ptr.
/* This handle remains valid after the Node is destroyed.
* The actual rcl node is not finalized until it is out of scope everywhere.
*/
RCLCPP_PUBLIC
virtual
std::shared_ptr<const rcl_node_t>
get_shared_rcl_node_handle() const = 0;
/// Create and return a callback group.
RCLCPP_PUBLIC
virtual
rclcpp::callback_group::CallbackGroup::SharedPtr
create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0;
/// Return the default callback group.
RCLCPP_PUBLIC
virtual
rclcpp::callback_group::CallbackGroup::SharedPtr
get_default_callback_group() = 0;
/// Return true if the given callback group is associated with this node.
RCLCPP_PUBLIC
virtual
bool
callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
/// Return list of callback groups associated with this node.
RCLCPP_PUBLIC
virtual
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
get_callback_groups() const = 0;
/// Return the atomic bool which is used to ensure only one executor is used.
RCLCPP_PUBLIC
virtual
std::atomic_bool &
get_associated_with_executor_atomic() = 0;
/// Return guard condition that should be notified when the internal node state changes.
/* For example, this should be notified when a publisher is added or removed.
*
* \return the rcl_guard_condition_t if it is valid, else nullptr
*/
RCLCPP_PUBLIC
virtual
rcl_guard_condition_t *
get_notify_guard_condition() = 0;
/// Acquire and return a scoped lock that protects the notify guard condition.
/* This should be used when triggering the notify guard condition. */
RCLCPP_PUBLIC
virtual
std::unique_lock<std::recursive_mutex>
acquire_notify_guard_condition_lock() const = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

129
rclcpp/include/rclcpp/node_interfaces/node_graph.hpp Normal file
View file

@ -0,0 +1,129 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_GRAPH_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
#include <chrono>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include "rcl/guard_condition.h"
#include "rclcpp/event.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace graph_listener
{
class GraphListener;
} // namespace graph_listener
namespace node_interfaces
{
/// Implementation the NodeGraph part of the Node API.
class NodeGraph : public NodeGraphInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraph)
RCLCPP_PUBLIC
explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base);
RCLCPP_PUBLIC
virtual
~NodeGraph();
RCLCPP_PUBLIC
virtual
std::map<std::string, std::string>
get_topic_names_and_types() const;
RCLCPP_PUBLIC
virtual
size_t
count_publishers(const std::string & topic_name) const;
RCLCPP_PUBLIC
virtual
size_t
count_subscribers(const std::string & topic_name) const;
RCLCPP_PUBLIC
virtual
const rcl_guard_condition_t *
get_graph_guard_condition() const;
RCLCPP_PUBLIC
virtual
void
notify_graph_change();
RCLCPP_PUBLIC
virtual
void
notify_shutdown();
RCLCPP_PUBLIC
virtual
rclcpp::event::Event::SharedPtr
get_graph_event();
RCLCPP_PUBLIC
virtual
void
wait_for_graph_change(
rclcpp::event::Event::SharedPtr event,
std::chrono::nanoseconds timeout);
RCLCPP_PUBLIC
virtual
size_t
count_graph_users();
private:
RCLCPP_DISABLE_COPY(NodeGraph)
/// Handle to the NodeBaseInterface given in the constructor.
rclcpp::node_interfaces::NodeBaseInterface * node_base_;
/// Graph Listener which waits on graph changes for the node and is shared across nodes.
std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
/// Whether or not this node needs to be added to the graph listener.
std::atomic_bool should_add_to_graph_listener_;
/// Mutex to guard the graph event related data structures.
mutable std::mutex graph_mutex_;
/// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
std::condition_variable graph_cv_;
/// Weak references to graph events out on loan.
std::vector<rclcpp::event::Event::WeakPtr> graph_events_;
/// Number of graph events out on loan, used to determine if the graph should be monitored.
/* graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
std::atomic_size_t graph_users_count_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_

122
rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp Normal file
View file

@ -0,0 +1,122 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
#include <chrono>
#include <map>
#include <string>
#include "rcl/guard_condition.h"
#include "rclcpp/event.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeGraph part of the Node API.
class NodeGraphInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
/// Return a map of existing topic names (string) to topic types (string).
/* A topic is considered to exist when at least one publisher or subscriber
* exists for it, whether they be local or remote to this process.
*/
RCLCPP_PUBLIC
virtual
std::map<std::string, std::string>
get_topic_names_and_types() const = 0;
/// Return the number of publishers that are advertised on a given topic.
RCLCPP_PUBLIC
virtual
size_t
count_publishers(const std::string & topic_name) const = 0;
/// Return the number of subscribers who have created a subscription for a given topic.
RCLCPP_PUBLIC
virtual
size_t
count_subscribers(const std::string & topic_name) const = 0;
/// Return the rcl guard condition which is triggered when the ROS graph changes.
RCLCPP_PUBLIC
virtual
const rcl_guard_condition_t *
get_graph_guard_condition() const = 0;
/// Notify threads waiting on graph changes.
/* Affects threads waiting on the notify guard condition, see:
* get_notify_guard_condition(), as well as the threads waiting on graph
* changes using a graph Event, see: wait_for_graph_change().
*
* This is typically only used by the rclcpp::graph_listener::GraphListener.
*
* \throws RCLBaseError (a child of that exception) when an rcl error occurs
*/
RCLCPP_PUBLIC
virtual
void
notify_graph_change() = 0;
/// Notify any and all blocking node actions that shutdown has occurred.
RCLCPP_PUBLIC
virtual
void
notify_shutdown() = 0;
/// Return a graph event, which will be set anytime a graph change occurs.
/* The graph Event object is a loan which must be returned.
* The Event object is scoped and therefore to return the load just let it go
* out of scope.
*/
RCLCPP_PUBLIC
virtual
rclcpp::event::Event::SharedPtr
get_graph_event() = 0;
/// Wait for a graph event to occur by waiting on an Event to become set.
/* The given Event must be acquire through the get_graph_event() method.
*
* \throws InvalidEventError if the given event is nullptr
* \throws EventNotRegisteredError if the given event was not acquired with
* get_graph_event().
*/
RCLCPP_PUBLIC
virtual
void
wait_for_graph_change(
rclcpp::event::Event::SharedPtr event,
std::chrono::nanoseconds timeout) = 0;
/// Return the number of on loan graph events, see get_graph_event().
/* This is typically only used by the rclcpp::graph_listener::GraphListener.
*/
RCLCPP_PUBLIC
virtual
size_t
count_graph_users() = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

122
rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp Normal file
View file

@ -0,0 +1,122 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_PARAMETERS_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
#include <map>
#include <string>
#include <vector>
#include "rcl_interfaces/msg/list_parameters_result.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/parameter_event.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeParameters part of the Node API.
class NodeParameters : public NodeParametersInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
RCLCPP_PUBLIC
NodeParameters(
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
bool use_intra_process);
RCLCPP_PUBLIC
virtual
~NodeParameters();
RCLCPP_PUBLIC
virtual
std::vector<rcl_interfaces::msg::SetParametersResult>
set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
virtual
rcl_interfaces::msg::SetParametersResult
set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
RCLCPP_PUBLIC
virtual
std::vector<rclcpp::parameter::ParameterVariant>
get_parameters(const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
virtual
rclcpp::parameter::ParameterVariant
get_parameter(const std::string & name) const;
RCLCPP_PUBLIC
virtual
bool
get_parameter(
const std::string & name,
rclcpp::parameter::ParameterVariant & parameter) const;
RCLCPP_PUBLIC
virtual
std::vector<rcl_interfaces::msg::ParameterDescriptor>
describe_parameters(const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
virtual
std::vector<uint8_t>
get_parameter_types(const std::vector<std::string> & names) const;
RCLCPP_PUBLIC
virtual
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
RCLCPP_PUBLIC
virtual
void
register_param_change_callback(ParametersCallbackFunction callback);
private:
RCLCPP_DISABLE_COPY(NodeParameters)
rclcpp::node_interfaces::NodeTopicsInterface * node_topics_;
mutable std::mutex mutex_;
ParametersCallbackFunction parameters_callback_ = nullptr;
std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_

97
rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp Normal file
View file

@ -0,0 +1,97 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
#include <string>
#include <vector>
#include "rcl_interfaces/msg/list_parameters_result.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeParameters part of the Node API.
class NodeParametersInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
RCLCPP_PUBLIC
virtual
std::vector<rcl_interfaces::msg::SetParametersResult>
set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
RCLCPP_PUBLIC
virtual
rcl_interfaces::msg::SetParametersResult
set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
RCLCPP_PUBLIC
virtual
std::vector<rclcpp::parameter::ParameterVariant>
get_parameters(const std::vector<std::string> & names) const = 0;
RCLCPP_PUBLIC
virtual
rclcpp::parameter::ParameterVariant
get_parameter(const std::string & name) const = 0;
RCLCPP_PUBLIC
virtual
bool
get_parameter(
const std::string & name,
rclcpp::parameter::ParameterVariant & parameter) const = 0;
RCLCPP_PUBLIC
virtual
std::vector<rcl_interfaces::msg::ParameterDescriptor>
describe_parameters(const std::vector<std::string> & names) const = 0;
RCLCPP_PUBLIC
virtual
std::vector<uint8_t>
get_parameter_types(const std::vector<std::string> & names) const = 0;
RCLCPP_PUBLIC
virtual
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
using ParametersCallbackFunction =
std::function<rcl_interfaces::msg::SetParametersResult(
const std::vector<rclcpp::parameter::ParameterVariant> &)>;
RCLCPP_PUBLIC
virtual
void
register_param_change_callback(ParametersCallbackFunction callback) = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

67
rclcpp/include/rclcpp/node_interfaces/node_services.hpp Normal file
View file

@ -0,0 +1,67 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_SERVICES_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
#include "rclcpp/callback_group.hpp"
#include "rclcpp/client.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeServices part of the Node API.
class NodeServices : public NodeServicesInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServices)
RCLCPP_PUBLIC
explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base);
RCLCPP_PUBLIC
virtual
~NodeServices();
RCLCPP_PUBLIC
virtual
void
add_client(
rclcpp::client::ClientBase::SharedPtr client_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group);
RCLCPP_PUBLIC
virtual
void
add_service(
rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group);
private:
RCLCPP_DISABLE_COPY(NodeServices)
rclcpp::node_interfaces::NodeBaseInterface * node_base_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_

53
rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp Normal file
View file

@ -0,0 +1,53 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
#include "rclcpp/callback_group.hpp"
#include "rclcpp/client.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeServices part of the Node API.
class NodeServicesInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
RCLCPP_PUBLIC
virtual
void
add_client(
rclcpp::client::ClientBase::SharedPtr client_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
RCLCPP_PUBLIC
virtual
void
add_service(
rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

60
rclcpp/include/rclcpp/node_interfaces/node_timers.hpp Normal file
View file

@ -0,0 +1,60 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_TIMERS_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_
#include "rclcpp/callback_group.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeTimers part of the Node API.
class NodeTimers : public NodeTimersInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimers)
RCLCPP_PUBLIC
explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base);
RCLCPP_PUBLIC
virtual
~NodeTimers();
/// Add a timer to the node.
RCLCPP_PUBLIC
virtual
void
add_timer(
rclcpp::timer::TimerBase::SharedPtr timer,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
private:
RCLCPP_DISABLE_COPY(NodeTimers)
rclcpp::node_interfaces::NodeBaseInterface * node_base_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_

46
rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp Normal file
View file

@ -0,0 +1,46 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
#include "rclcpp/callback_group.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeTimers part of the Node API.
class NodeTimersInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
/// Add a timer to the node.
RCLCPP_PUBLIC
virtual
void
add_timer(
rclcpp::timer::TimerBase::SharedPtr timer,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

88
rclcpp/include/rclcpp/node_interfaces/node_topics.hpp Normal file
View file

@ -0,0 +1,88 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_TOPICS_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_
#include <string>
#include "rcl/publisher.h"
#include "rcl/subscription.h"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/publisher_factory.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Implementation of the NodeTopics part of the Node API.
class NodeTopics : public NodeTopicsInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
RCLCPP_PUBLIC
explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
RCLCPP_PUBLIC
virtual
~NodeTopics();
RCLCPP_PUBLIC
virtual
rclcpp::publisher::PublisherBase::SharedPtr
create_publisher(
const std::string & topic_name,
const rclcpp::PublisherFactory & publisher_factory,
rcl_publisher_options_t & publisher_options,
bool use_intra_process);
RCLCPP_PUBLIC
virtual
void
add_publisher(
rclcpp::publisher::PublisherBase::SharedPtr publisher);
RCLCPP_PUBLIC
virtual
rclcpp::subscription::SubscriptionBase::SharedPtr
create_subscription(
const std::string & topic_name,
const rclcpp::SubscriptionFactory & subscription_factory,
rcl_subscription_options_t & subscription_options,
bool use_intra_process);
RCLCPP_PUBLIC
virtual
void
add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group);
private:
RCLCPP_DISABLE_COPY(NodeTopics)
NodeBaseInterface * node_base_;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_

78
rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp Normal file
View file

@ -0,0 +1,78 @@
// Copyright 2016 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__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
#include <functional>
#include <memory>
#include <string>
#include "rcl/publisher.h"
#include "rcl/subscription.h"
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/publisher_factory.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/subscription_factory.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace node_interfaces
{
/// Pure virtual interface class for the NodeTopics part of the Node API.
class NodeTopicsInterface
{
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
RCLCPP_PUBLIC
virtual
rclcpp::publisher::PublisherBase::SharedPtr
create_publisher(
const std::string & topic_name,
const rclcpp::PublisherFactory & publisher_factory,
rcl_publisher_options_t & publisher_options,
bool use_intra_process) = 0;
RCLCPP_PUBLIC
virtual
void
add_publisher(
rclcpp::publisher::PublisherBase::SharedPtr publisher) = 0;
RCLCPP_PUBLIC
virtual
rclcpp::subscription::SubscriptionBase::SharedPtr
create_subscription(
const std::string & topic_name,
const rclcpp::SubscriptionFactory & subscription_factory,
rcl_subscription_options_t & subscription_options,
bool use_intra_process) = 0;
RCLCPP_PUBLIC
virtual
void
add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
};
} // namespace node_interfaces
} // namespace rclcpp
#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

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

@ -28,43 +28,48 @@
#include "rcl/publisher.h"
#include "rcl_interfaces/msg/intra_process_message.hpp"
#include "rmw/impl/cpp/demangle.hpp"
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
// Forward declaration for friend statement
namespace node
// Forward declaration is used for friend statement.
namespace node_interfaces
{
class Node;
} // namespace node
class NodeTopicsInterface;
}
namespace publisher
{
class PublisherBase
{
friend rclcpp::node_interfaces::NodeTopicsInterface;
public:
RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
/// Default constructor.
/**
* Typically, a publisher is not created through this method, but instead is created through a
* call to `Node::create_publisher`.
* \param[in] node_handle The corresponding rcl representation of the owner node.
* \param[in] node_base A pointer to the NodeBaseInterface for the parent node.
* \param[in] topic The topic that this publisher publishes on.
* \param[in] queue_size The maximum number of unpublished messages to queue.
* \param[in] type_support The type support structure for the type to be published.
* \param[in] publisher_options QoS settings for this publisher.
*/
RCLCPP_PUBLIC
PublisherBase(
std::shared_ptr<rcl_node_t> node_handle,
std::string topic,
size_t queue_size);
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic,
const rosidl_message_type_support_t & type_support,
const rcl_publisher_options_t & publisher_options);
RCLCPP_PUBLIC
virtual ~PublisherBase();
@ -72,7 +77,7 @@ public:
/// Get the topic that this publisher publishes on.
// \return The topic name.
RCLCPP_PUBLIC
const std::string &
const char *
get_topic_name() const;
/// Get the queue size for this publisher.
@ -113,9 +118,9 @@ public:
bool
operator==(const rmw_gid_t * gid) const;
typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
using StoreMessageCallbackT = std::function<uint64_t(uint64_t, void *, const std::type_info &)>;
protected:
/// Implementation utility function used to setup intra process publishing after creation.
RCLCPP_PUBLIC
void
setup_intra_process(
@ -123,14 +128,11 @@ protected:
StoreMessageCallbackT callback,
const rcl_publisher_options_t & intra_process_options);
std::shared_ptr<rcl_node_t> node_handle_;
protected:
std::shared_ptr<rcl_node_t> rcl_node_handle_;
rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
rcl_allocator_t rcl_allocator_ = rcl_get_default_allocator();
std::string topic_;
size_t queue_size_;
uint64_t intra_process_publisher_id_;
StoreMessageCallbackT store_intra_process_message_;
@ -143,8 +145,6 @@ protected:
template<typename MessageT, typename Alloc = std::allocator<void>>
class Publisher : public PublisherBase
{
friend rclcpp::node::Node;
public:
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
using MessageAlloc = typename MessageAllocTraits::allocator_type;
@ -154,55 +154,22 @@ public:
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>)
Publisher(
std::shared_ptr<rcl_node_t> node_handle,
std::string topic,
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic,
const rcl_publisher_options_t & publisher_options,
std::shared_ptr<MessageAlloc> allocator)
: PublisherBase(node_handle, topic, publisher_options.qos.depth), message_allocator_(allocator)
const std::shared_ptr<MessageAlloc> & allocator)
: PublisherBase(
node_base,
topic,
*rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
publisher_options),
message_allocator_(allocator)
{
using rosidl_typesupport_cpp::get_message_type_support_handle;
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
rcl_allocator_ = publisher_options.allocator;
auto type_support_handle = get_message_type_support_handle<MessageT>();
if (rcl_publisher_init(
&publisher_handle_, node_handle_.get(), type_support_handle,
topic.c_str(), &publisher_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create publisher: ") +
rcl_get_error_string_safe());
}
// Life time of this object is tied to the publisher handle.
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
if (!publisher_rmw_handle) {
throw std::runtime_error(
std::string("failed to get rmw handle: ") + rcl_get_error_string_safe());
}
if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
virtual ~Publisher()
{
if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) {
fprintf(
stderr,
"Error in destruction of intra process rcl publisher handle: %s\n",
rcl_get_error_string_safe());
}
if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) {
fprintf(
stderr,
"Error in destruction of rcl publisher handle: %s\n",
rcl_get_error_string_safe());
}
}
{}
/// Send a message to the topic for this publisher.
/**
@ -318,6 +285,7 @@ protected:
};
} // namespace publisher
} // namespace rclcpp
#endif // RCLCPP__PUBLISHER_HPP_

147
rclcpp/include/rclcpp/publisher_factory.hpp Normal file
View file

@ -0,0 +1,147 @@
// Copyright 2016 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__PUBLISHER_FACTORY_HPP_
#define RCLCPP__PUBLISHER_FACTORY_HPP_
#include <functional>
#include <memory>
#include <string>
#include "rcl/publisher.h"
#include "rosidl_typesupport_cpp/message_type_support.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
/// Factory with functions used to create a MessageT specific PublisherT.
/* This factory class is used to encapsulate the template generated functions
* which are used during the creation of a Message type specific publisher
* within a non-templated class.
*
* It is created using the create_publisher_factory function, which is usually
* called from a templated "create_publisher" method on the Node class, and
* is passed to the non-templated "create_publisher" method on the NodeTopics
* class where it is used to create and setup the Publisher.
*/
struct PublisherFactory
{
// Creates a PublisherT<MessageT, ...> publisher object and returns it as a PublisherBase.
using PublisherFactoryFunction = std::function<
rclcpp::publisher::PublisherBase::SharedPtr(
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic_name,
rcl_publisher_options_t & publisher_options)>;
PublisherFactoryFunction create_typed_publisher;
// Adds the PublisherBase to the intraprocess manager with the correctly
// templated call to IntraProcessManager::store_intra_process_message.
using AddPublisherToIntraProcessManagerFunction = std::function<
uint64_t(
rclcpp::intra_process_manager::IntraProcessManager * ipm,
rclcpp::publisher::PublisherBase::SharedPtr publisher)>;
AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
// Creates the callback function which is called on each
// PublisherT::publish() and which handles the intra process transmission of
// the message being published.
using SharedPublishCallbackFactoryFunction = std::function<
rclcpp::publisher::PublisherBase::StoreMessageCallbackT(
rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
SharedPublishCallbackFactoryFunction create_shared_publish_callback;
};
/// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, Alloc>.
template<typename MessageT, typename Alloc, typename PublisherT>
PublisherFactory
create_publisher_factory(std::shared_ptr<Alloc> allocator)
{
PublisherFactory factory;
// factory function that creates a MessageT specific PublisherT
factory.create_typed_publisher =
[allocator](
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic_name,
rcl_publisher_options_t & publisher_options) -> std::shared_ptr<PublisherT>
{
auto message_alloc = std::make_shared<typename PublisherT::MessageAlloc>(*allocator.get());
publisher_options.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
return std::make_shared<PublisherT>(node_base, topic_name, publisher_options, message_alloc);
};
// function to add a publisher to the intra process manager
factory.add_publisher_to_intra_process_manager =
[](
rclcpp::intra_process_manager::IntraProcessManager * ipm,
rclcpp::publisher::PublisherBase::SharedPtr publisher) -> uint64_t
{
return ipm->add_publisher<MessageT, Alloc>(std::dynamic_pointer_cast<PublisherT>(publisher));
};
// function to create a shared publish callback std::function
using StoreMessageCallbackT = rclcpp::publisher::PublisherBase::StoreMessageCallbackT;
factory.create_shared_publish_callback =
[](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT
{
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
// this function is called on each call to publish() and handles storing
// of the published message in the intra process manager
auto shared_publish_callback =
[weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or maybe return with a warning?
throw std::runtime_error(
"intra process publish called after destruction of intra process manager");
}
if (!msg) {
throw std::runtime_error("cannot publisher msg which is a null pointer");
}
auto & message_type_info = typeid(MessageT);
if (message_type_info != type_info) {
throw std::runtime_error(
std::string("published type '") + type_info.name() +
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
}
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
uint64_t message_seq =
ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
return message_seq;
};
return shared_publish_callback;
};
// return the factory now that it is populated
return factory;
}
} // namespace rclcpp
#endif // RCLCPP__PUBLISHER_FACTORY_HPP_

8
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -52,8 +52,6 @@ public:
using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
using VoidAlloc = typename VoidAllocTraits::allocator_type;
using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
{
executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
@ -266,7 +264,7 @@ public:
any_exec->subscription = subscription;
}
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group, weak_nodes);
any_exec->node_base = get_node_by_group(group, weak_nodes);
subscription_handles_.erase(it);
return;
}
@ -300,7 +298,7 @@ public:
// 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, weak_nodes);
any_exec->node_base = get_node_by_group(group, weak_nodes);
service_handles_.erase(it);
return;
}
@ -333,7 +331,7 @@ public:
// Otherwise it is safe to set and return the any_exec
any_exec->client = client;
any_exec->callback_group = group;
any_exec->node = get_node_by_group(group, weak_nodes);
any_exec->node_base = get_node_by_group(group, weak_nodes);
client_handles_.erase(it);
return;
}

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

@ -38,10 +38,10 @@
namespace rclcpp
{
namespace node
namespace node_interfaces
{
class Node;
} // namespace node
class NodeTopicsInterface;
} // namespace node_interfaces
namespace subscription
{
@ -56,14 +56,16 @@ public:
/// Default constructor.
/**
* \param[in] node_handle The rcl representation of the node that owns this subscription.
* \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] ignore_local_publications True to ignore local publications (unused).
* \param[in] subscription_options options for the subscription.
*/
RCLCPP_PUBLIC
SubscriptionBase(
std::shared_ptr<rcl_node_t> node_handle,
const rosidl_message_type_support_t & type_support_handle,
const std::string & topic_name,
bool ignore_local_publications);
const rcl_subscription_options_t & subscription_options);
/// Default destructor.
RCLCPP_PUBLIC
@ -71,7 +73,7 @@ public:
/// Get the topic that this subscription is subscribed on.
RCLCPP_PUBLIC
const std::string &
const char *
get_topic_name() const;
RCLCPP_PUBLIC
@ -110,8 +112,6 @@ protected:
private:
RCLCPP_DISABLE_COPY(SubscriptionBase)
std::string topic_name_;
bool ignore_local_publications_;
};
using any_subscription_callback::AnySubscriptionCallback;
@ -120,7 +120,7 @@ using any_subscription_callback::AnySubscriptionCallback;
template<typename MessageT, typename Alloc = std::allocator<void>>
class Subscription : public SubscriptionBase
{
friend class rclcpp::node::Node;
friend class rclcpp::node_interfaces::NodeTopicsInterface;
public:
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
@ -136,8 +136,8 @@ public:
* should be instantiated through Node::create_subscription.
* \param[in] node_handle rcl representation of the node that owns this subscription.
* \param[in] topic_name Name of the topic to subscribe to.
* \param[in] ignore_local_publications True to ignore local publications (unused).
* \param[in] callback User-defined callback to call when a message is received.
* \param[in] subscription_options options for the subscription.
* \param[in] callback User defined callback to call when a message is received.
* \param[in] memory_strategy The memory strategy to be used for managing message memory.
*/
Subscription(
@ -149,23 +149,15 @@ public:
memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
Alloc>::create_default())
: SubscriptionBase(
node_handle, topic_name, subscription_options.ignore_local_publications),
node_handle,
*rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
topic_name,
subscription_options),
any_callback_(callback),
message_memory_strategy_(memory_strategy),
get_intra_process_message_callback_(nullptr),
matches_any_intra_process_publishers_(nullptr)
{
using rosidl_typesupport_cpp::get_message_type_support_handle;
auto type_support_handle = get_message_type_support_handle<MessageT>();
if (rcl_subscription_init(
&subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(),
&subscription_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create subscription: ") + rcl_get_error_string_safe());
}
}
{}
/// Support dynamically setting the message memory strategy.
/**
@ -232,13 +224,11 @@ public:
any_callback_.dispatch_intra_process(msg, message_info);
}
private:
typedef
std::function<
void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)
> GetMessageCallbackType;
typedef std::function<bool (const rmw_gid_t *)> MatchesAnyPublishersCallbackType;
using GetMessageCallbackType =
std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
/// Implemenation detail.
void setup_intra_process(
uint64_t intra_process_subscription_id,
GetMessageCallbackType get_message_callback,
@ -248,7 +238,7 @@ private:
if (rcl_subscription_init(
&intra_process_subscription_handle_, node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
(get_topic_name() + "__intra").c_str(),
(std::string(get_topic_name()) + "__intra").c_str(),
&intra_process_options) != RCL_RET_OK)
{
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
@ -262,6 +252,7 @@ private:
matches_any_intra_process_publishers_ = matches_any_publisher_callback;
}
/// Implemenation detail.
const rcl_subscription_t *
get_intra_process_subscription_handle() const
{
@ -271,6 +262,7 @@ private:
return &intra_process_subscription_handle_;
}
private:
RCLCPP_DISABLE_COPY(Subscription)
AnySubscriptionCallback<MessageT, Alloc> any_callback_;

170
rclcpp/include/rclcpp/subscription_factory.hpp Normal file
View file

@ -0,0 +1,170 @@
// Copyright 2016 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__SUBSCRIPTION_FACTORY_HPP_
#define RCLCPP__SUBSCRIPTION_FACTORY_HPP_
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include "rcl/subscription.h"
#include "rosidl_typesupport_cpp/message_type_support.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
/// Factory with functions used to create a Subscription<MessageT>.
/* This factory class is used to encapsulate the template generated functions
* which are used during the creation of a Message type specific subscription
* within a non-templated class.
*
* It is created using the create_subscription_factory function, which is
* usually called from a templated "create_subscription" method of the Node
* class, and is passed to the non-templated "create_subscription" method of
* the NodeTopics class where it is used to create and setup the Subscription.
*/
struct SubscriptionFactory
{
// Creates a Subscription<MessageT> object and returns it as a SubscriptionBase.
using SubscriptionFactoryFunction = std::function<
rclcpp::subscription::SubscriptionBase::SharedPtr(
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic_name,
rcl_subscription_options_t & subscription_options)>;
SubscriptionFactoryFunction create_typed_subscription;
// Function that takes a MessageT from the intra process manager
using SetupIntraProcessFunction = std::function<void(
rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
const rcl_subscription_options_t & subscription_options)>;
SetupIntraProcessFunction setup_intra_process;
};
/// Return a SubscriptionFactory with functions for creating a SubscriptionT<MessageT, Alloc>.
template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
SubscriptionFactory
create_subscription_factory(
CallbackT && callback,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
msg_mem_strat,
std::shared_ptr<Alloc> allocator)
{
SubscriptionFactory factory;
using rclcpp::subscription::AnySubscriptionCallback;
AnySubscriptionCallback<MessageT, Alloc> any_subscription_callback(allocator);
any_subscription_callback.set(std::forward<CallbackT>(callback));
auto message_alloc =
std::make_shared<typename subscription::Subscription<MessageT, Alloc>::MessageAlloc>();
// factory function that creates a MessageT specific SubscriptionT
factory.create_typed_subscription =
[allocator, msg_mem_strat, any_subscription_callback, message_alloc](
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic_name,
rcl_subscription_options_t & subscription_options
) -> rclcpp::subscription::SubscriptionBase::SharedPtr
{
subscription_options.allocator =
rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
using rclcpp::subscription::Subscription;
using rclcpp::subscription::SubscriptionBase;
auto sub = Subscription<MessageT, Alloc>::make_shared(
node_base->get_shared_rcl_node_handle(),
topic_name,
subscription_options,
any_subscription_callback,
msg_mem_strat);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
return sub_base_ptr;
};
// function that will setup intra process communications for the subscription
factory.setup_intra_process =
[message_alloc](
rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
const rcl_subscription_options_t & subscription_options)
{
rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm;
uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
auto intra_process_options = rcl_subscription_get_default_options();
intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
*message_alloc.get());
intra_process_options.qos = subscription_options.qos;
intra_process_options.ignore_local_publications = false;
// function that will be called to take a MessageT from the intra process manager
auto take_intra_process_message_func =
[weak_ipm](
uint64_t publisher_id,
uint64_t message_sequence,
uint64_t subscription_id,
typename rclcpp::subscription::Subscription<MessageT, Alloc>::MessageUniquePtr & message)
{
auto ipm = weak_ipm.lock();
if (!ipm) {
// TODO(wjwwood): should this just return silently? Or return with a logged warning?
throw std::runtime_error(
"intra process take called after destruction of intra process manager");
}
ipm->take_intra_process_message<MessageT, Alloc>(
publisher_id, message_sequence, subscription_id, message);
};
// function that is called to see if the publisher id matches any local publishers
auto matches_any_publisher_func =
[weak_ipm](const rmw_gid_t * sender_gid) -> bool
{
auto ipm = weak_ipm.lock();
if (!ipm) {
throw std::runtime_error(
"intra process publisher check called "
"after destruction of intra process manager");
}
return ipm->matches_any_publishers(sender_gid);
};
auto typed_sub_ptr = std::dynamic_pointer_cast<SubscriptionT>(subscription);
typed_sub_ptr->setup_intra_process(
intra_process_subscription_id,
take_intra_process_message_func,
matches_any_publisher_func,
intra_process_options
);
};
// end definition of factory function to setup intra process
// return the factory now that it is populated
return factory;
}
} // namespace rclcpp
#endif // RCLCPP__SUBSCRIPTION_FACTORY_HPP_

2
rclcpp/src/rclcpp/any_executable.cpp
View file

@ -23,7 +23,7 @@ AnyExecutable::AnyExecutable()
service(nullptr),
client(nullptr),
callback_group(nullptr),
node(nullptr)
node_base(nullptr)
{}
AnyExecutable::~AnyExecutable()

11
rclcpp/src/rclcpp/client.cpp
View file

@ -23,7 +23,8 @@
#include "rcl/node.h"
#include "rcl/wait.h"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/utilities.hpp"
using rclcpp::client::ClientBase;
@ -31,9 +32,11 @@ using rclcpp::exceptions::InvalidNodeError;
using rclcpp::exceptions::throw_from_rcl_error;
ClientBase::ClientBase(
std::shared_ptr<rclcpp::node::Node> parent_node,
rclcpp::node_interfaces::NodeBaseInterface * node_base,
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
const std::string & service_name)
: node_(parent_node), node_handle_(parent_node->get_shared_node_handle()),
: node_graph_(node_graph),
node_handle_(node_base->get_shared_rcl_node_handle()),
service_name_(service_name)
{}
@ -76,7 +79,7 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
return false;
}
// make an event to reuse, rather than create a new one each time
auto node_ptr = node_.lock();
auto node_ptr = node_graph_.lock();
if (!node_ptr) {
throw InvalidNodeError();
}

47
rclcpp/src/rclcpp/executor.cpp
View file

@ -21,6 +21,7 @@
#include "rcl/error_handling.h"
#include "rclcpp/executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/scope_exit.hpp"
#include "rclcpp/utilities.hpp"
@ -86,10 +87,11 @@ Executor::~Executor()
}
void
Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
{
// If the node already has an executor
if (node_ptr->has_executor.exchange(true)) {
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
if (has_executor.exchange(true)) {
throw std::runtime_error("Node has already been added to an executor.");
}
// Check to ensure node not already added
@ -112,14 +114,20 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
}
void
Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
Executor::add_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
{
this->add_node(node_ptr->get_node_base_interface(), notify);
}
void
Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
{
bool node_removed = false;
weak_nodes_.erase(
std::remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
[&](std::weak_ptr<rclcpp::node::Node> & i)
[&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i)
{
bool matched = (i.lock() == node_ptr);
node_removed |= matched;
@ -128,7 +136,8 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
// *INDENT-ON*
)
);
node_ptr->has_executor.store(false);
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
has_executor.store(false);
if (notify) {
// If the node was matched and removed, interrupt waiting
if (node_removed) {
@ -140,9 +149,15 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify)
memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
}
void
Executor::remove_node(std::shared_ptr<rclcpp::node::Node> node_ptr, bool notify)
{
this->remove_node(node_ptr->get_node_base_interface(), notify);
}
void
Executor::spin_node_once_nanoseconds(
rclcpp::node::Node::SharedPtr node,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::nanoseconds timeout)
{
this->add_node(node, false);
@ -152,13 +167,19 @@ Executor::spin_node_once_nanoseconds(
}
void
Executor::spin_node_some(rclcpp::node::Node::SharedPtr node)
Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
{
this->add_node(node, false);
spin_some();
this->remove_node(node, false);
}
void
Executor::spin_node_some(std::shared_ptr<rclcpp::node::Node> node)
{
this->spin_node_some(node->get_node_base_interface());
}
void
Executor::spin_some()
{
@ -248,7 +269,7 @@ Executor::execute_subscription(
} else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
fprintf(stderr,
"[rclcpp::error] take failed for subscription on topic '%s': %s\n",
subscription->get_topic_name().c_str(), rcl_get_error_string_safe());
subscription->get_topic_name(), rcl_get_error_string_safe());
}
subscription->return_message(message);
}
@ -270,7 +291,7 @@ Executor::execute_intra_process_subscription(
} else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
fprintf(stderr,
"[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
subscription->get_topic_name().c_str(), rcl_get_error_string_safe());
subscription->get_topic_name(), rcl_get_error_string_safe());
}
}
@ -332,7 +353,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
remove_if(
weak_nodes_.begin(), weak_nodes_.end(),
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
[](std::weak_ptr<rclcpp::node::Node> i)
[](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i)
{
return i.expired();
}
@ -412,11 +433,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
}
}
rclcpp::node::Node::SharedPtr
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (!group) {
return rclcpp::node::Node::SharedPtr();
return nullptr;
}
for (auto & weak_node : weak_nodes_) {
auto node = weak_node.lock();
@ -430,7 +451,7 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro
}
}
}
return rclcpp::node::Node::SharedPtr();
return nullptr;
}
rclcpp::callback_group::CallbackGroup::SharedPtr

16
rclcpp/src/rclcpp/executors.cpp
View file

@ -15,17 +15,29 @@
#include "rclcpp/executors.hpp"
void
rclcpp::spin_some(node::Node::SharedPtr node_ptr)
rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor exec;
exec.spin_node_some(node_ptr);
}
void
rclcpp::spin(node::Node::SharedPtr node_ptr)
rclcpp::spin_some(rclcpp::node::Node::SharedPtr node_ptr)
{
rclcpp::spin_some(node_ptr->get_node_base_interface());
}
void
rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
{
rclcpp::executors::SingleThreadedExecutor exec;
exec.add_node(node_ptr);
exec.spin();
exec.remove_node(node_ptr);
}
void
rclcpp::spin(rclcpp::node::Node::SharedPtr node_ptr)
{
rclcpp::spin(node_ptr->get_node_base_interface());
}

97
rclcpp/src/rclcpp/graph_listener.cpp
View file

@ -115,17 +115,17 @@ GraphListener::run_loop()
rcl_ret_t ret;
{
// This "barrier" lock ensures that other functions can acquire the
// nodes_mutex_ after waking up rcl_wait.
std::lock_guard<std::mutex> nodes_barrier_lock(nodes_barrier_mutex_);
// node_graph_interfaces_mutex_ after waking up rcl_wait.
std::lock_guard<std::mutex> nodes_barrier_lock(node_graph_interfaces_barrier_mutex_);
// This is ownership is passed to nodes_lock in the next line.
nodes_mutex_.lock();
node_graph_interfaces_mutex_.lock();
}
// This lock is released when the loop continues or exits.
std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
// Resize the wait set if necessary.
if (wait_set_.size_of_guard_conditions < (nodes_.size() + 2)) {
ret = rcl_wait_set_resize_guard_conditions(&wait_set_, nodes_.size() + 2);
if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to resize wait set");
}
@ -146,13 +146,13 @@ GraphListener::run_loop()
throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
}
// Put graph guard conditions for each node into the wait set.
for (const auto node_ptr : nodes_) {
for (const auto node_ptr : node_graph_interfaces_) {
// Only wait on graph changes if some user of the node is watching.
if (node_ptr->count_graph_users() == 0) {
continue;
}
// Add the graph guard condition for the node to the wait set.
auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle());
auto graph_gc = node_ptr->get_graph_guard_condition();
if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
}
@ -179,8 +179,8 @@ GraphListener::run_loop()
}
}
// Notify nodes who's guard conditions are set (triggered).
for (const auto node_ptr : nodes_) {
auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle());
for (const auto node_ptr : node_graph_interfaces_) {
auto graph_gc = node_ptr->get_graph_guard_condition();
if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
}
@ -208,25 +208,27 @@ interrupt_(rcl_guard_condition_t * interrupt_guard_condition)
static void
acquire_nodes_lock_(
std::mutex * nodes_barrier_mutex,
std::mutex * nodes_mutex,
std::mutex * node_graph_interfaces_barrier_mutex,
std::mutex * node_graph_interfaces_mutex,
rcl_guard_condition_t * interrupt_guard_condition)
{
{
// Acquire this lock to prevent the run loop from re-locking the
// nodes_mutext_ after being woken up.
std::lock_guard<std::mutex> nodes_barrier_lock(*nodes_barrier_mutex);
std::lock_guard<std::mutex> nodes_barrier_lock(*node_graph_interfaces_barrier_mutex);
// Trigger the interrupt guard condition to wake up rcl_wait.
interrupt_(interrupt_guard_condition);
nodes_mutex->lock();
node_graph_interfaces_mutex->lock();
}
}
static bool
has_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * node)
has_node_(
std::vector<rclcpp::node_interfaces::NodeGraphInterface *> * node_graph_interfaces,
rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
for (const auto node_ptr : (*nodes)) {
if (node == node_ptr) {
for (const auto node_ptr : (*node_graph_interfaces)) {
if (node_graph == node_ptr) {
return true;
}
}
@ -234,23 +236,26 @@ has_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * node)
}
bool
GraphListener::has_node(rclcpp::node::Node * node)
GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
if (!node) {
if (!node_graph) {
return false;
}
// Acquire the nodes mutex using the barrier to prevent the run loop from
// re-locking the nodes mutex after being interrupted.
acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
// Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
return has_node_(&nodes_, node);
acquire_nodes_lock_(
&node_graph_interfaces_barrier_mutex_,
&node_graph_interfaces_mutex_,
&interrupt_guard_condition_);
// Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
return has_node_(&node_graph_interfaces_, node_graph);
}
void
GraphListener::add_node(rclcpp::node::Node * node)
GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
if (!node) {
if (!node_graph) {
throw std::invalid_argument("node is nullptr");
}
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
@ -260,25 +265,30 @@ GraphListener::add_node(rclcpp::node::Node * node)
// Acquire the nodes mutex using the barrier to prevent the run loop from
// re-locking the nodes mutex after being interrupted.
acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
// Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
if (has_node_(&nodes_, node)) {
acquire_nodes_lock_(
&node_graph_interfaces_barrier_mutex_,
&node_graph_interfaces_mutex_,
&interrupt_guard_condition_);
// Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
if (has_node_(&node_graph_interfaces_, node_graph)) {
throw NodeAlreadyAddedError();
}
nodes_.push_back(node);
node_graph_interfaces_.push_back(node_graph);
// The run loop has already been interrupted by acquire_nodes_lock_() and
// will evaluate the new node when nodes_lock releases the nodes_mutex_.
// will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_.
}
static void
remove_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * node)
remove_node_(
std::vector<rclcpp::node_interfaces::NodeGraphInterface *> * node_graph_interfaces,
rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
// Remove the node if it is found.
for (auto it = nodes->begin(); it != nodes->end(); ++it) {
if (node == *it) {
for (auto it = node_graph_interfaces->begin(); it != node_graph_interfaces->end(); ++it) {
if (node_graph == *it) {
// Found the node, remove it.
nodes->erase(it);
node_graph_interfaces->erase(it);
// Now trigger the interrupt guard condition to make sure
return;
}
@ -288,23 +298,26 @@ remove_node_(std::vector<rclcpp::node::Node *> * nodes, rclcpp::node::Node * nod
}
void
GraphListener::remove_node(rclcpp::node::Node * node)
GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
if (!node) {
if (!node_graph) {
throw std::invalid_argument("node is nullptr");
}
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (is_shutdown()) {
// If shutdown, then the run loop has been joined, so we can remove them directly.
return remove_node_(&nodes_, node);
return remove_node_(&node_graph_interfaces_, node_graph);
}
// Otherwise, first interrupt and lock against the run loop to safely remove the node.
// Acquire the nodes mutex using the barrier to prevent the run loop from
// re-locking the nodes mutex after being interrupted.
acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_);
// Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(nodes_mutex_, std::adopt_lock);
remove_node_(&nodes_, node);
acquire_nodes_lock_(
&node_graph_interfaces_barrier_mutex_,
&node_graph_interfaces_mutex_,
&interrupt_guard_condition_);
// Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
remove_node_(&node_graph_interfaces_, node_graph);
}
void

2
rclcpp/src/rclcpp/memory_strategy.cpp
View file

@ -96,7 +96,7 @@ MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle,
return nullptr;
}
rclcpp::node::Node::SharedPtr
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
const WeakNodeVector & weak_nodes)
{

497
rclcpp/src/rclcpp/node.cpp
View file

@ -20,11 +20,15 @@
#include <utility>
#include <vector>
#include "rcl/graph.h"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/graph_listener.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_base.hpp"
#include "rclcpp/node_interfaces/node_graph.hpp"
#include "rclcpp/node_interfaces/node_parameters.hpp"
#include "rclcpp/node_interfaces/node_services.hpp"
#include "rclcpp/node_interfaces/node_timers.hpp"
#include "rclcpp/node_interfaces/node_topics.hpp"
using rclcpp::node::Node;
using rclcpp::exceptions::throw_from_rcl_error;
@ -40,474 +44,123 @@ Node::Node(
const std::string & node_name,
rclcpp::context::Context::SharedPtr context,
bool use_intra_process_comms)
: name_(node_name), context_(context),
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), number_of_clients_(0),
use_intra_process_comms_(use_intra_process_comms), notify_guard_condition_is_valid_(false),
graph_listener_(context->get_sub_context<rclcpp::graph_listener::GraphListener>()),
should_add_to_graph_listener_(true), graph_users_count_(0)
: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)),
node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
node_parameters_(new rclcpp::node_interfaces::NodeParameters(
node_topics_.get(),
use_intra_process_comms
)),
use_intra_process_comms_(use_intra_process_comms)
{
rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
if (ret != RCL_RET_OK) {
throw_from_rcl_error(ret, "failed to create interrupt guard condition");
}
has_executor.store(false);
size_t domain_id = 0;
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
if (ros_domain_id) {
uint32_t number = strtoul(ros_domain_id, NULL, 0);
if (number == (std::numeric_limits<uint32_t>::max)()) {
// Finalize the interrupt guard condition.
if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
}
throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
}
domain_id = static_cast<size_t>(number);
#ifdef _WIN32
free(ros_domain_id);
#endif
}
rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
node_handle_.reset(rcl_node, [](rcl_node_t * node) {
if (rcl_node_fini(node) != RCL_RET_OK) {
fprintf(
stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe());
}
delete node;
});
rcl_node_options_t options = rcl_node_get_default_options();
// TODO(jacquelinekay): Allocator options
options.domain_id = domain_id;
ret = rcl_node_init(node_handle_.get(), name_.c_str(), &options);
if (ret != RCL_RET_OK) {
// Finalize the interrupt guard condition.
if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
}
throw_from_rcl_error(ret, "failed to initialize rcl node");
}
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = create_callback_group(
CallbackGroupType::MutuallyExclusive);
events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
"parameter_events", rmw_qos_profile_parameter_events);
notify_guard_condition_is_valid_ = true;
}
Node::~Node()
{
// Remove self from graph listener.
// Exchange with false to prevent others from trying to add this node to the
// graph listener after checking that it was not here.
if (!should_add_to_graph_listener_.exchange(false)) {
// If it was already false, then it needs to now be removed.
graph_listener_->remove_node(this);
}
// Finalize the interrupt guard condition after removing self from graph listener.
{
std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
notify_guard_condition_is_valid_ = false;
if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
}
}
}
{}
const std::string &
const char *
Node::get_name() const
{
return name_;
return node_base_->get_name();
}
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;
auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
callback_groups_.push_back(group);
return group;
return node_base_->create_callback_group(group_type);
}
bool
Node::group_in_node(rclcpp::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;
return node_base_->callback_group_in_node(group);
}
// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
// 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);
// }
std::vector<rcl_interfaces::msg::SetParametersResult>
Node::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::vector<rcl_interfaces::msg::SetParametersResult> results;
for (auto p : parameters) {
auto result = set_parameters_atomically({{p}});
results.push_back(result);
}
return results;
return node_parameters_->set_parameters(parameters);
}
rcl_interfaces::msg::SetParametersResult
Node::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
// TODO(jacquelinekay): handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
if (parameters_callback_) {
result = parameters_callback_(parameters);
} else {
result.successful = true;
}
if (!result.successful) {
return result;
}
for (auto p : parameters) {
if (parameters_.find(p.get_name()) == parameters_.end()) {
if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->new_parameters.push_back(p.to_parameter());
}
} else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->changed_parameters.push_back(p.to_parameter());
} else {
parameter_event->deleted_parameters.push_back(p.to_parameter());
}
tmp_map[p.get_name()] = p;
}
// std::map::insert will not overwrite elements, so we'll keep the new
// ones and add only those that already exist in the Node's internal map
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
events_publisher_->publish(parameter_event);
return result;
return node_parameters_->set_parameters_atomically(parameters);
}
std::vector<rclcpp::parameter::ParameterVariant>
Node::get_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rclcpp::parameter::ParameterVariant> results;
for (auto & name : names) {
if (std::any_of(parameters_.cbegin(), parameters_.cend(),
[&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
return name == kv.first;
}))
{
results.push_back(parameters_.at(name));
}
}
return results;
return node_parameters_->get_parameters(names);
}
const rclcpp::parameter::ParameterVariant
rclcpp::parameter::ParameterVariant
Node::get_parameter(const std::string & name) const
{
rclcpp::parameter::ParameterVariant parameter;
if (get_parameter(name, parameter)) {
return parameter;
} else {
throw std::out_of_range("Parameter '" + name + "' not set");
}
return node_parameters_->get_parameter(name);
}
bool Node::get_parameter(const std::string & name,
rclcpp::parameter::ParameterVariant & parameter) const
{
std::lock_guard<std::mutex> lock(mutex_);
if (parameters_.count(name)) {
parameter = parameters_.at(name);
return true;
} else {
return false;
}
return node_parameters_->get_parameter(name, parameter);
}
std::vector<rcl_interfaces::msg::ParameterDescriptor>
Node::describe_parameters(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
parameter_descriptor.name = kv.first;
parameter_descriptor.type = kv.second.get_type();
results.push_back(parameter_descriptor);
}
}
return results;
return node_parameters_->describe_parameters(names);
}
std::vector<uint8_t>
Node::get_parameter_types(
const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<uint8_t> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second.get_type());
} else {
results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
}
}
return results;
return node_parameters_->get_parameter_types(names);
}
rcl_interfaces::msg::ListParametersResult
Node::list_parameters(
const std::vector<std::string> & prefixes, uint64_t depth) const
{
std::lock_guard<std::mutex> lock(mutex_);
rcl_interfaces::msg::ListParametersResult result;
// TODO(esteve): define parameter separator, use "." for now
for (auto & kv : parameters_) {
if (((prefixes.size() == 0) &&
((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
(static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) ||
(std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
if (kv.first == prefix) {
return true;
} else if (kv.first.find(prefix + ".") == 0) {
size_t length = prefix.length();
std::string substr = kv.first.substr(length);
// Cast as unsigned integer to avoid warning
return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
(static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth);
}
return false;
})))
{
result.names.push_back(kv.first);
size_t last_separator = kv.first.find_last_of('.');
if (std::string::npos != last_separator) {
std::string prefix = kv.first.substr(0, last_separator);
if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
result.prefixes.cend())
{
result.prefixes.push_back(prefix);
}
}
}
}
return result;
return node_parameters_->list_parameters(prefixes, depth);
}
std::map<std::string, std::string>
Node::get_topic_names_and_types() const
{
rcl_topic_names_and_types_t topic_names_and_types =
rcl_get_zero_initialized_topic_names_and_types();
auto ret = rcl_get_topic_names_and_types(node_handle_.get(),
&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
std::map<std::string, std::string> topics;
for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
}
ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return topics;
return node_graph_->get_topic_names_and_types();
}
size_t
Node::count_publishers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_handle_.get()),
topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count publishers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
return node_graph_->count_publishers(topic_name);
}
size_t
Node::count_subscribers(const std::string & topic_name) const
{
size_t count;
auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_handle_.get()),
topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count subscribers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
return node_graph_->count_subscribers(topic_name);
}
const Node::CallbackGroupWeakPtrList &
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
Node::get_callback_groups() const
{
return callback_groups_;
}
const rcl_guard_condition_t *
Node::get_notify_guard_condition() const
{
std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
if (!notify_guard_condition_is_valid_) {
return nullptr;
}
return &notify_guard_condition_;
}
const rcl_guard_condition_t *
Node::get_graph_guard_condition() const
{
return rcl_node_get_graph_guard_condition(node_handle_.get());
}
const rcl_node_t *
Node::get_rcl_node_handle() const
{
return node_handle_.get();
}
rcl_node_t *
Node::get_rcl_node_handle()
{
return node_handle_.get();
}
std::shared_ptr<rcl_node_t>
Node::get_shared_node_handle()
{
return node_handle_;
}
void
Node::notify_graph_change()
{
{
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
bool bad_ptr_encountered = false;
for (auto & event_wptr : graph_events_) {
auto event_ptr = event_wptr.lock();
if (event_ptr) {
event_ptr->set();
} else {
bad_ptr_encountered = true;
}
}
if (bad_ptr_encountered) {
// remove invalid pointers with the erase-remove idiom
graph_events_.erase(
std::remove_if(
graph_events_.begin(),
graph_events_.end(),
[](const rclcpp::event::Event::WeakPtr & wptr) {
return wptr.expired();
}),
graph_events_.end());
// update graph_users_count_
graph_users_count_.store(graph_events_.size());
}
}
graph_cv_.notify_all();
{
std::lock_guard<std::mutex> notify_guard_condition_lock(notify_guard_condition_mutex_);
rcl_ret_t ret = rcl_trigger_guard_condition(&notify_guard_condition_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to trigger notify guard condition");
}
}
}
void
Node::notify_shutdown()
{
// notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown().
graph_cv_.notify_all();
return node_base_->get_callback_groups();
}
rclcpp::event::Event::SharedPtr
Node::get_graph_event()
{
auto event = rclcpp::event::Event::make_shared();
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
// on first call, add node to graph_listener_
if (should_add_to_graph_listener_.exchange(false)) {
graph_listener_->add_node(this);
graph_listener_->start_if_not_started();
}
graph_events_.push_back(event);
graph_users_count_++;
return event;
return node_graph_->get_graph_event();
}
void
@ -515,57 +168,41 @@ Node::wait_for_graph_change(
rclcpp::event::Event::SharedPtr event,
std::chrono::nanoseconds timeout)
{
if (!event) {
throw InvalidEventError();
}
{
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
bool event_in_graph_events = false;
for (const auto & event_wptr : graph_events_) {
if (event == event_wptr.lock()) {
event_in_graph_events = true;
break;
}
}
if (!event_in_graph_events) {
throw EventNotRegisteredError();
}
}
std::unique_lock<std::mutex> graph_lock(graph_mutex_);
graph_cv_.wait_for(graph_lock, timeout, [&event]() {
return event->check() || !rclcpp::utilities::ok();
});
node_graph_->wait_for_graph_change(event, timeout);
}
size_t
Node::count_graph_users()
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
Node::get_node_base_interface()
{
return graph_users_count_.load();
return node_base_;
}
// PROTECTED IMPLEMENTATION
void
Node::add_service(rclcpp::service::ServiceBase::SharedPtr serv_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
Node::get_node_graph_interface()
{
if (!serv_base_ptr) {
throw std::runtime_error("Cannot add empty service to group");
}
if (group) {
if (!group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(serv_base_ptr);
} else {
default_callback_group_->add_service(serv_base_ptr);
}
number_of_services_++;
if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on service creation: ") + rmw_get_error_string());
}
return node_graph_;
}
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
Node::get_node_timers_interface()
{
return node_timers_;
}
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr
Node::get_node_topics_interface()
{
return node_topics_;
}
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
Node::get_node_services_interface()
{
return node_services_;
}
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
Node::get_node_parameters_interface()
{
return node_parameters_;
}

215
rclcpp/src/rclcpp/node_interfaces/node_base.cpp Normal file
View file

@ -0,0 +1,215 @@
// Copyright 2016 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.
#include <string>
#include <limits>
#include <memory>
#include <vector>
#include "rclcpp/node_interfaces/node_base.hpp"
#include "rclcpp/exceptions.hpp"
using rclcpp::exceptions::throw_from_rcl_error;
using rclcpp::node_interfaces::NodeBase;
NodeBase::NodeBase(
const std::string & node_name,
rclcpp::context::Context::SharedPtr context)
: context_(context),
node_handle_(nullptr),
default_callback_group_(nullptr),
associated_with_executor_(false),
notify_guard_condition_is_valid_(false)
{
// Setup the guard condition that is notified when changes occur in the graph.
rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
if (ret != RCL_RET_OK) {
throw_from_rcl_error(ret, "failed to create interrupt guard condition");
}
// Setup a safe exit lamda to clean up the guard condition in case of an error here.
auto finalize_notify_guard_condition = [this]() {
// Finalize the interrupt guard condition.
if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
}
};
// Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
size_t domain_id = 0;
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
if (ros_domain_id) {
uint32_t number = strtoul(ros_domain_id, NULL, 0);
if (number == (std::numeric_limits<uint32_t>::max)()) {
// Finalize the interrupt guard condition.
finalize_notify_guard_condition();
#ifdef _WIN32
// free the ros_domain_id before throwing, if getenv was used on Windows
free(ros_domain_id);
#endif
throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
}
domain_id = static_cast<size_t>(number);
#ifdef _WIN32
free(ros_domain_id);
#endif
}
// Create the rcl node and store it in a shared_ptr with a custom destructor.
rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
rcl_node_options_t options = rcl_node_get_default_options();
// TODO(wjwwood): pass the Allocator to the options
options.domain_id = domain_id;
ret = rcl_node_init(rcl_node, node_name.c_str(), &options);
if (ret != RCL_RET_OK) {
// Finalize the interrupt guard condition.
finalize_notify_guard_condition();
throw_from_rcl_error(ret, "failed to initialize rcl node");
}
node_handle_.reset(rcl_node, [](rcl_node_t * node) -> void {
if (rcl_node_fini(node) != RCL_RET_OK) {
fprintf(
stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe());
}
delete node;
});
// Create the default callback group.
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive);
// Indicate the notify_guard_condition is now valid.
notify_guard_condition_is_valid_ = true;
}
NodeBase::~NodeBase()
{
// Finalize the interrupt guard condition after removing self from graph listener.
{
std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
notify_guard_condition_is_valid_ = false;
if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
}
}
}
const char *
NodeBase::get_name() const
{
return rcl_node_get_name(node_handle_.get());
}
rclcpp::context::Context::SharedPtr
NodeBase::get_context()
{
return context_;
}
rcl_node_t *
NodeBase::get_rcl_node_handle()
{
return node_handle_.get();
}
const rcl_node_t *
NodeBase::get_rcl_node_handle() const
{
return node_handle_.get();
}
std::shared_ptr<rcl_node_t>
NodeBase::get_shared_rcl_node_handle()
{
return node_handle_;
}
std::shared_ptr<const rcl_node_t>
NodeBase::get_shared_rcl_node_handle() const
{
return node_handle_;
}
rclcpp::callback_group::CallbackGroup::SharedPtr
NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type)
{
using rclcpp::callback_group::CallbackGroup;
using rclcpp::callback_group::CallbackGroupType;
auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
callback_groups_.push_back(group);
return group;
}
rclcpp::callback_group::CallbackGroup::SharedPtr
NodeBase::get_default_callback_group()
{
return default_callback_group_;
}
bool
NodeBase::callback_group_in_node(rclcpp::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;
}
const std::vector<rclcpp::callback_group::CallbackGroup::WeakPtr> &
NodeBase::get_callback_groups() const
{
return callback_groups_;
}
std::atomic_bool &
NodeBase::get_associated_with_executor_atomic()
{
return associated_with_executor_;
}
rcl_guard_condition_t *
NodeBase::get_notify_guard_condition()
{
std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
if (!notify_guard_condition_is_valid_) {
return nullptr;
}
return &notify_guard_condition_;
}
std::unique_lock<std::recursive_mutex>
NodeBase::acquire_notify_guard_condition_lock() const
{
return std::unique_lock<std::recursive_mutex>(notify_guard_condition_mutex_);
}

209
rclcpp/src/rclcpp/node_interfaces/node_graph.cpp Normal file
View file

@ -0,0 +1,209 @@
// Copyright 2016 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.
#include "rclcpp/node_interfaces/node_graph.hpp"
#include <map>
#include <string>
#include "rcl/graph.h"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/event.hpp"
#include "rclcpp/graph_listener.hpp"
using rclcpp::node_interfaces::NodeGraph;
using rclcpp::exceptions::throw_from_rcl_error;
using rclcpp::graph_listener::GraphListener;
NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)
: node_base_(node_base),
graph_listener_(node_base->get_context()->get_sub_context<GraphListener>()),
should_add_to_graph_listener_(true),
graph_users_count_(0)
{}
NodeGraph::~NodeGraph()
{
// Remove self from graph listener.
// Exchange with false to prevent others from trying to add this node to the
// graph listener after checking that it was not here.
if (!should_add_to_graph_listener_.exchange(false)) {
// If it was already false, then it needs to now be removed.
graph_listener_->remove_node(this);
}
}
std::map<std::string, std::string>
NodeGraph::get_topic_names_and_types() const
{
rcl_topic_names_and_types_t topic_names_and_types =
rcl_get_zero_initialized_topic_names_and_types();
auto ret = rcl_get_topic_names_and_types(node_base_->get_rcl_node_handle(),
&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
std::map<std::string, std::string> topics;
for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
}
ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return topics;
}
size_t
NodeGraph::count_publishers(const std::string & topic_name) const
{
size_t count;
// TODO(wjwwood): use the rcl equivalent methods
auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count publishers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
size_t
NodeGraph::count_subscribers(const std::string & topic_name) const
{
size_t count;
// TODO(wjwwood): use the rcl equivalent methods
auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
topic_name.c_str(), &count);
if (ret != RMW_RET_OK) {
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not count subscribers: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
return count;
}
const rcl_guard_condition_t *
NodeGraph::get_graph_guard_condition() const
{
return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle());
}
void
NodeGraph::notify_graph_change()
{
{
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
bool bad_ptr_encountered = false;
for (auto & event_wptr : graph_events_) {
auto event_ptr = event_wptr.lock();
if (event_ptr) {
event_ptr->set();
} else {
bad_ptr_encountered = true;
}
}
if (bad_ptr_encountered) {
// remove invalid pointers with the erase-remove idiom
graph_events_.erase(
std::remove_if(
graph_events_.begin(),
graph_events_.end(),
[](const rclcpp::event::Event::WeakPtr & wptr) {
return wptr.expired();
}),
graph_events_.end());
// update graph_users_count_
graph_users_count_.store(graph_events_.size());
}
}
graph_cv_.notify_all();
{
auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock();
rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition());
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to trigger notify guard condition");
}
}
}
void
NodeGraph::notify_shutdown()
{
// notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown().
graph_cv_.notify_all();
}
rclcpp::event::Event::SharedPtr
NodeGraph::get_graph_event()
{
auto event = rclcpp::event::Event::make_shared();
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
// on first call, add node to graph_listener_
if (should_add_to_graph_listener_.exchange(false)) {
graph_listener_->add_node(this);
graph_listener_->start_if_not_started();
}
graph_events_.push_back(event);
graph_users_count_++;
return event;
}
void
NodeGraph::wait_for_graph_change(
rclcpp::event::Event::SharedPtr event,
std::chrono::nanoseconds timeout)
{
using rclcpp::exceptions::InvalidEventError;
using rclcpp::exceptions::EventNotRegisteredError;
if (!event) {
throw InvalidEventError();
}
{
std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
bool event_in_graph_events = false;
for (const auto & event_wptr : graph_events_) {
if (event == event_wptr.lock()) {
event_in_graph_events = true;
break;
}
}
if (!event_in_graph_events) {
throw EventNotRegisteredError();
}
}
std::unique_lock<std::mutex> graph_lock(graph_mutex_);
graph_cv_.wait_for(graph_lock, timeout, [&event]() {
return event->check() || !rclcpp::utilities::ok();
});
}
size_t
NodeGraph::count_graph_users()
{
return graph_users_count_.load();
}

230
rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp Normal file
View file

@ -0,0 +1,230 @@
// Copyright 2016 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.
#include "rclcpp/node_interfaces/node_parameters.hpp"
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rclcpp/create_publisher.hpp"
#include "rmw/qos_profiles.h"
using rclcpp::node_interfaces::NodeParameters;
NodeParameters::NodeParameters(
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
bool use_intra_process)
: node_topics_(node_topics)
{
using MessageT = rcl_interfaces::msg::ParameterEvent;
using PublisherT = rclcpp::publisher::Publisher<MessageT>;
using AllocatorT = std::allocator<void>;
// TODO(wjwwood): expose this allocator through the Parameter interface.
auto allocator = std::make_shared<AllocatorT>();
events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
node_topics_,
"parameter_events",
rmw_qos_profile_parameter_events,
use_intra_process,
allocator);
}
NodeParameters::~NodeParameters()
{}
std::vector<rcl_interfaces::msg::SetParametersResult>
NodeParameters::set_parameters(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::vector<rcl_interfaces::msg::SetParametersResult> results;
for (auto p : parameters) {
auto result = set_parameters_atomically({{p}});
results.push_back(result);
}
return results;
}
rcl_interfaces::msg::SetParametersResult
NodeParameters::set_parameters_atomically(
const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
{
std::lock_guard<std::mutex> lock(mutex_);
std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
// TODO(jacquelinekay): handle parameter constraints
rcl_interfaces::msg::SetParametersResult result;
if (parameters_callback_) {
result = parameters_callback_(parameters);
} else {
result.successful = true;
}
if (!result.successful) {
return result;
}
for (auto p : parameters) {
if (parameters_.find(p.get_name()) == parameters_.end()) {
if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->new_parameters.push_back(p.to_parameter());
}
} else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
parameter_event->changed_parameters.push_back(p.to_parameter());
} else {
parameter_event->deleted_parameters.push_back(p.to_parameter());
}
tmp_map[p.get_name()] = p;
}
// std::map::insert will not overwrite elements, so we'll keep the new
// ones and add only those that already exist in the Node's internal map
tmp_map.insert(parameters_.begin(), parameters_.end());
std::swap(tmp_map, parameters_);
events_publisher_->publish(parameter_event);
return result;
}
std::vector<rclcpp::parameter::ParameterVariant>
NodeParameters::get_parameters(const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rclcpp::parameter::ParameterVariant> results;
for (auto & name : names) {
if (std::any_of(parameters_.cbegin(), parameters_.cend(),
[&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
return name == kv.first;
}))
{
results.push_back(parameters_.at(name));
}
}
return results;
}
rclcpp::parameter::ParameterVariant
NodeParameters::get_parameter(const std::string & name) const
{
rclcpp::parameter::ParameterVariant parameter;
if (get_parameter(name, parameter)) {
return parameter;
} else {
throw std::out_of_range("Parameter '" + name + "' not set");
}
}
bool
NodeParameters::get_parameter(
const std::string & name,
rclcpp::parameter::ParameterVariant & parameter) const
{
std::lock_guard<std::mutex> lock(mutex_);
if (parameters_.count(name)) {
parameter = parameters_.at(name);
return true;
} else {
return false;
}
}
std::vector<rcl_interfaces::msg::ParameterDescriptor>
NodeParameters::describe_parameters(const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
parameter_descriptor.name = kv.first;
parameter_descriptor.type = kv.second.get_type();
results.push_back(parameter_descriptor);
}
}
return results;
}
std::vector<uint8_t>
NodeParameters::get_parameter_types(const std::vector<std::string> & names) const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<uint8_t> results;
for (auto & kv : parameters_) {
if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
return name == kv.first;
}))
{
results.push_back(kv.second.get_type());
} else {
results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
}
}
return results;
}
rcl_interfaces::msg::ListParametersResult
NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const
{
std::lock_guard<std::mutex> lock(mutex_);
rcl_interfaces::msg::ListParametersResult result;
// TODO(esteve): define parameter separator, use "." for now
for (auto & kv : parameters_) {
if (((prefixes.size() == 0) &&
((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
(static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) ||
(std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
if (kv.first == prefix) {
return true;
} else if (kv.first.find(prefix + ".") == 0) {
size_t length = prefix.length();
std::string substr = kv.first.substr(length);
// Cast as unsigned integer to avoid warning
return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
(static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth);
}
return false;
})))
{
result.names.push_back(kv.first);
size_t last_separator = kv.first.find_last_of('.');
if (std::string::npos != last_separator) {
std::string prefix = kv.first.substr(0, last_separator);
if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
result.prefixes.cend())
{
result.prefixes.push_back(prefix);
}
}
}
}
return result;
}
void
NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
{
parameters_callback_ = callback;
}

78
rclcpp/src/rclcpp/node_interfaces/node_services.cpp Normal file
View file

@ -0,0 +1,78 @@
// Copyright 2016 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.
#include "rclcpp/node_interfaces/node_services.hpp"
#include <string>
using rclcpp::node_interfaces::NodeServices;
NodeServices::NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base)
: node_base_(node_base)
{}
NodeServices::~NodeServices()
{}
void
NodeServices::add_service(
rclcpp::service::ServiceBase::SharedPtr service_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (group) {
if (!node_base_->callback_group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create service, group not in node.");
}
group->add_service(service_base_ptr);
} else {
node_base_->get_default_callback_group()->add_service(service_base_ptr);
}
// Notify the executor that a new service was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string("Failed to notify waitset on service creation: ") + rmw_get_error_string()
);
}
}
}
void
NodeServices::add_client(
rclcpp::client::ClientBase::SharedPtr client_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
if (group) {
if (!node_base_->callback_group_in_node(group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create client, group not in node.");
}
group->add_client(client_base_ptr);
} else {
node_base_->get_default_callback_group()->add_client(client_base_ptr);
}
// Notify the executor that a new client was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string("Failed to notify waitset on client creation: ") + rmw_get_error_string()
);
}
}
}

47
rclcpp/src/rclcpp/node_interfaces/node_timers.cpp Normal file
View file

@ -0,0 +1,47 @@
// Copyright 2016 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.
#include "rclcpp/node_interfaces/node_timers.hpp"
#include <string>
using rclcpp::node_interfaces::NodeTimers;
NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base)
: node_base_(node_base)
{}
NodeTimers::~NodeTimers()
{}
void
NodeTimers::add_timer(
rclcpp::timer::TimerBase::SharedPtr timer,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
{
if (callback_group) {
if (!node_base_->callback_group_in_node(callback_group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create timer, group not in node.");
}
callback_group->add_timer(timer);
} else {
node_base_->get_default_callback_group()->add_timer(timer);
}
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on timer creation: ") + rmw_get_error_string());
}
}

131
rclcpp/src/rclcpp/node_interfaces/node_topics.cpp Normal file
View file

@ -0,0 +1,131 @@
// Copyright 2016 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.
#include "rclcpp/node_interfaces/node_topics.hpp"
#include <string>
#include "rclcpp/intra_process_manager.hpp"
#include "rclcpp/exceptions.hpp"
using rclcpp::exceptions::throw_from_rcl_error;
using rclcpp::node_interfaces::NodeTopics;
NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base)
: node_base_(node_base)
{}
NodeTopics::~NodeTopics()
{}
rclcpp::publisher::PublisherBase::SharedPtr
NodeTopics::create_publisher(
const std::string & topic_name,
const rclcpp::PublisherFactory & publisher_factory,
rcl_publisher_options_t & publisher_options,
bool use_intra_process)
{
// Create the MessageT specific Publisher using the factory, but store it as PublisherBase.
auto publisher = publisher_factory.create_typed_publisher(
node_base_, topic_name, publisher_options);
// Setup intra process publishing if requested.
if (use_intra_process) {
auto context = node_base_->get_context();
// Get the intra process manager instance for this context.
auto ipm = context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
// Register the publisher with the intra process manager.
uint64_t intra_process_publisher_id =
publisher_factory.add_publisher_to_intra_process_manager(ipm.get(), publisher);
// Create a function to be called when publisher to do the intra process publish.
auto shared_publish_callback = publisher_factory.create_shared_publish_callback(ipm);
publisher->setup_intra_process(
intra_process_publisher_id,
shared_publish_callback,
publisher_options);
}
// Return the completed publisher.
return publisher;
}
void
NodeTopics::add_publisher(
rclcpp::publisher::PublisherBase::SharedPtr publisher)
{
// The publisher is not added to a callback group or anthing like that for now.
// It may be stored within the NodeTopics class or the NodeBase class in the future.
(void)publisher;
// Notify the executor that a new publisher was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on publisher creation: ") + rmw_get_error_string());
}
}
}
rclcpp::subscription::SubscriptionBase::SharedPtr
NodeTopics::create_subscription(
const std::string & topic_name,
const rclcpp::SubscriptionFactory & subscription_factory,
rcl_subscription_options_t & subscription_options,
bool use_intra_process)
{
auto subscription = subscription_factory.create_typed_subscription(
node_base_, topic_name, subscription_options);
// Setup intra process publishing if requested.
if (use_intra_process) {
auto context = node_base_->get_context();
auto intra_process_manager =
context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
subscription_factory.setup_intra_process(
intra_process_manager, subscription, subscription_options);
}
// Return the completed subscription.
return subscription;
}
void
NodeTopics::add_subscription(
rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
{
// Assign to a group.
if (callback_group) {
if (!node_base_->callback_group_in_node(callback_group)) {
// TODO(jacquelinekay): use custom exception
throw std::runtime_error("Cannot create subscription, callback group not in node.");
}
callback_group->add_subscription(subscription);
} else {
node_base_->get_default_callback_group()->add_subscription(subscription);
}
// Notify the executor that a new subscription was created using the parent Node.
{
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
throw std::runtime_error(
std::string(
"Failed to notify waitset on subscription creation: ") + rmw_get_error_string()
);
}
}
}

15
rclcpp/src/rclcpp/parameter_client.cpp
View file

@ -251,7 +251,8 @@ std::vector<rclcpp::parameter::ParameterVariant>
SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
{
auto f = async_parameters_client_->get_parameters(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
using rclcpp::executors::spin_node_until_future_complete;
if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
return f.get();
@ -274,7 +275,8 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
{
auto f = async_parameters_client_->get_parameter_types(parameter_names);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
using rclcpp::executors::spin_node_until_future_complete;
if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
return f.get();
@ -288,7 +290,8 @@ SyncParametersClient::set_parameters(
{
auto f = async_parameters_client_->set_parameters(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
using rclcpp::executors::spin_node_until_future_complete;
if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
return f.get();
@ -302,7 +305,8 @@ SyncParametersClient::set_parameters_atomically(
{
auto f = async_parameters_client_->set_parameters_atomically(parameters);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
using rclcpp::executors::spin_node_until_future_complete;
if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
return f.get();
@ -318,7 +322,8 @@ SyncParametersClient::list_parameters(
{
auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
using rclcpp::executors::spin_node_until_future_complete;
if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
return f.get();

13
rclcpp/src/rclcpp/parameter_service.cpp
View file

@ -16,6 +16,7 @@
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
using rclcpp::parameter_service::ParameterService;
@ -28,7 +29,7 @@ ParameterService::ParameterService(
std::weak_ptr<rclcpp::node::Node> captured_node = node_;
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
node_->get_name() + "__get_parameters",
std::string(node_->get_name()) + "__get_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
@ -47,7 +48,7 @@ ParameterService::ParameterService(
);
get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
node_->get_name() + "__get_parameter_types",
std::string(node_->get_name()) + "__get_parameter_types",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
@ -67,7 +68,7 @@ ParameterService::ParameterService(
);
set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
node_->get_name() + "__set_parameters",
std::string(node_->get_name()) + "__set_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
@ -89,7 +90,7 @@ ParameterService::ParameterService(
set_parameters_atomically_service_ =
node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
node_->get_name() + "__set_parameters_atomically",
std::string(node_->get_name()) + "__set_parameters_atomically",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
@ -113,7 +114,7 @@ ParameterService::ParameterService(
);
describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
node_->get_name() + "__describe_parameters",
std::string(node_->get_name()) + "__describe_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
@ -130,7 +131,7 @@ ParameterService::ParameterService(
);
list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
node_->get_name() + "__list_parameters",
std::string(node_->get_name()) + "__list_parameters",
[captured_node](
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,

60
rclcpp/src/rclcpp/publisher.cpp
View file

@ -29,33 +29,72 @@
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node.hpp"
using rclcpp::publisher::PublisherBase;
PublisherBase::PublisherBase(
std::shared_ptr<rcl_node_t> node_handle,
std::string topic,
size_t queue_size)
: node_handle_(node_handle),
topic_(topic), queue_size_(queue_size),
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic,
const rosidl_message_type_support_t & type_support,
const rcl_publisher_options_t & publisher_options)
: rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
{
if (rcl_publisher_init(
&publisher_handle_, rcl_node_handle_.get(), &type_support,
topic.c_str(), &publisher_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create publisher: ") +
rcl_get_error_string_safe());
}
// Life time of this object is tied to the publisher handle.
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
if (!publisher_rmw_handle) {
throw std::runtime_error(
std::string("failed to get rmw handle: ") + rcl_get_error_string_safe());
}
if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
throw std::runtime_error(
std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
// *INDENT-ON*
}
}
PublisherBase::~PublisherBase()
{
if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
fprintf(
stderr,
"Error in destruction of intra process rcl publisher handle: %s\n",
rcl_get_error_string_safe());
}
if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
fprintf(
stderr,
"Error in destruction of rcl publisher handle: %s\n",
rcl_get_error_string_safe());
}
}
const std::string &
const char *
PublisherBase::get_topic_name() const
{
return topic_;
return rcl_publisher_get_topic_name(&publisher_handle_);
}
size_t
PublisherBase::get_queue_size() const
{
return queue_size_;
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
if (!publisher_options) {
throw std::runtime_error(
std::string("failed to get publisher options: ") + rcl_get_error_string_safe());
}
return publisher_options->qos.depth;
}
const rmw_gid_t &
@ -101,10 +140,11 @@ PublisherBase::setup_intra_process(
StoreMessageCallbackT callback,
const rcl_publisher_options_t & intra_process_options)
{
auto intra_process_topic_name = std::string(this->get_topic_name()) + "__intra";
if (rcl_publisher_init(
&intra_process_publisher_handle_, node_handle_.get(),
&intra_process_publisher_handle_, rcl_node_handle_.get(),
rclcpp::type_support::get_intra_process_message_msg_type_support(),
(topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK)
intra_process_topic_name.c_str(), &intra_process_options) != RCL_RET_OK)
{
throw std::runtime_error(
std::string("could not create intra process publisher: ") +

23
rclcpp/src/rclcpp/subscription.cpp
View file

@ -26,14 +26,21 @@ using rclcpp::subscription::SubscriptionBase;
SubscriptionBase::SubscriptionBase(
std::shared_ptr<rcl_node_t> node_handle,
const rosidl_message_type_support_t & type_support_handle,
const std::string & topic_name,
bool ignore_local_publications)
: node_handle_(node_handle),
topic_name_(topic_name),
ignore_local_publications_(ignore_local_publications)
const rcl_subscription_options_t & subscription_options)
: node_handle_(node_handle)
{
// To avoid unused private member warnings.
(void)ignore_local_publications_;
rcl_ret_t ret = rcl_subscription_init(
&subscription_handle_,
node_handle_.get(),
&type_support_handle,
topic_name.c_str(),
&subscription_options);
if (ret != RCL_RET_OK) {
throw std::runtime_error(
std::string("could not create subscription: ") + rcl_get_error_string_safe());
}
}
SubscriptionBase::~SubscriptionBase()
@ -54,10 +61,10 @@ SubscriptionBase::~SubscriptionBase()
}
}
const std::string &
const char *
SubscriptionBase::get_topic_name() const
{
return this->topic_name_;
return rcl_subscription_get_topic_name(&subscription_handle_);
}
const rcl_subscription_t *

4
rclcpp/src/rclcpp/utilities.cpp
View file

@ -86,7 +86,7 @@ signal_handler(int signal_value)
g_signal_status = signal_value;
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
for (auto const & kv : g_sigint_guard_cond_handles) {
for (auto & kv : g_sigint_guard_cond_handles) {
rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
if (status != RCL_RET_OK) {
fprintf(stderr,
@ -164,7 +164,7 @@ rclcpp::utilities::shutdown()
g_signal_status = SIGINT;
{
std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
for (auto const & kv : g_sigint_guard_cond_handles) {
for (auto & kv : g_sigint_guard_cond_handles) {
if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to trigger sigint guard condition: %s\n",

25
rclcpp/test/test_externally_defined_services.cpp
View file

@ -66,7 +66,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) {
// don't initialize the service
// expect fail
try {
rclcpp::service::Service<rclcpp::srv::Mock>(node_handle->get_shared_node_handle(),
rclcpp::service::Service<rclcpp::srv::Mock>(
node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
&service_handle, cb);
} catch (const std::runtime_error &) {
SUCCEED();
@ -84,9 +85,11 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
rcl_service_options_t service_options = rcl_service_get_default_options();
const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
rclcpp, srv, Mock);
if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(),
ts, "base_node_service", &service_options) != RCL_RET_OK)
{
rcl_ret_t ret = rcl_service_init(
&service_handle,
node_handle->get_node_base_interface()->get_rcl_node_handle(),
ts, "base_node_service", &service_options);
if (ret != RCL_RET_OK) {
FAIL();
return;
}
@ -94,7 +97,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
try {
rclcpp::service::Service<rclcpp::srv::Mock>(node_handle->get_shared_node_handle(),
rclcpp::service::Service<rclcpp::srv::Mock>(
node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
&service_handle, cb);
} catch (const std::runtime_error &) {
FAIL();
@ -112,9 +116,11 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
rcl_service_options_t service_options = rcl_service_get_default_options();
const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
rclcpp, srv, Mock);
if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(),
ts, "base_node_service", &service_options) != RCL_RET_OK)
{
rcl_ret_t ret = rcl_service_init(
&service_handle,
node_handle->get_node_base_interface()->get_rcl_node_handle(),
ts, "base_node_service", &service_options);
if (ret != RCL_RET_OK) {
FAIL();
return;
}
@ -122,7 +128,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
{
// Call constructor
rclcpp::service::Service<rclcpp::srv::Mock> srv_cpp(node_handle->get_shared_node_handle(),
rclcpp::service::Service<rclcpp::srv::Mock> srv_cpp(
node_handle->get_node_base_interface()->get_shared_rcl_node_handle(),
&service_handle, cb);
// Call destructor
}

8
rclcpp/test/test_find_weak_nodes.cpp
View file

@ -37,8 +37,8 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {
auto existing_node = rclcpp::node::Node::make_shared("existing_node");
auto dead_node = rclcpp::node::Node::make_shared("dead_node");
rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes;
weak_nodes.push_back(existing_node);
weak_nodes.push_back(dead_node);
weak_nodes.push_back(existing_node->get_node_base_interface());
weak_nodes.push_back(dead_node->get_node_base_interface());
// AND
// Delete dead_node, creating a dangling pointer in weak_nodes
@ -62,8 +62,8 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
auto existing_node1 = rclcpp::node::Node::make_shared("existing_node1");
auto existing_node2 = rclcpp::node::Node::make_shared("existing_node2");
rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes;
weak_nodes.push_back(existing_node1);
weak_nodes.push_back(existing_node2);
weak_nodes.push_back(existing_node1->get_node_base_interface());
weak_nodes.push_back(existing_node2->get_node_base_interface());
ASSERT_FALSE(weak_nodes[0].expired());
ASSERT_FALSE(weak_nodes[1].expired());

8
rclcpp/test/test_intra_process_manager.cpp
View file

@ -36,9 +36,9 @@ public:
PublisherBase()
: mock_topic_name(""), mock_queue_size(0) {}
const std::string & get_topic_name() const
const char * get_topic_name() const
{
return mock_topic_name;
return mock_topic_name.c_str();
}
size_t get_queue_size() const
{
@ -98,9 +98,9 @@ public:
SubscriptionBase()
: mock_topic_name(""), mock_queue_size(0) {}
const std::string & get_topic_name() const
const char * get_topic_name() const
{
return mock_topic_name;
return mock_topic_name.c_str();
}
size_t get_queue_size() const
{