update
This commit is contained in:
parent
9a6b48e2fc
commit
c6b8fb2de0
10 changed files with 23 additions and 22 deletions
|
@ -54,7 +54,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
|
||||||
|
|
||||||
CallbackGroup(CallbackGroupType group_type)
|
CallbackGroup(CallbackGroupType group_type)
|
||||||
: type_(group_type), can_be_taken_from_(true)
|
: type_(group_type), can_be_taken_from_(true)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -45,7 +45,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
|
||||||
|
|
||||||
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
|
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
|
||||||
: client_handle_(client_handle), service_name_(service_name)
|
: client_handle_(client_handle), service_name_(service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
~ClientBase()
|
~ClientBase()
|
||||||
|
@ -92,7 +92,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
|
||||||
|
|
||||||
Client(rmw_client_t * client_handle, const std::string & service_name)
|
Client(rmw_client_t * client_handle, const std::string & service_name)
|
||||||
: ClientBase(client_handle, service_name)
|
: ClientBase(client_handle, service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
std::shared_ptr<void> create_response()
|
std::shared_ptr<void> create_response()
|
||||||
|
|
|
@ -38,7 +38,9 @@ class Executor
|
||||||
public:
|
public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
|
||||||
|
|
||||||
Executor() : interrupt_guard_condition_(rmw_create_guard_condition()) {}
|
Executor()
|
||||||
|
: interrupt_guard_condition_(rmw_create_guard_condition())
|
||||||
|
{}
|
||||||
|
|
||||||
virtual ~Executor()
|
virtual ~Executor()
|
||||||
{
|
{
|
||||||
|
@ -58,8 +60,7 @@ public:
|
||||||
auto node = weak_node.lock();
|
auto node = weak_node.lock();
|
||||||
if (node == node_ptr) {
|
if (node == node_ptr) {
|
||||||
// TODO: Use a different error here?
|
// TODO: Use a different error here?
|
||||||
throw std::runtime_error(
|
throw std::runtime_error("Cannot add node to executor, node already added.");
|
||||||
"Cannot add node to executor, node already added.");
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
weak_nodes_.push_back(node_ptr);
|
weak_nodes_.push_back(node_ptr);
|
||||||
|
@ -100,7 +101,9 @@ public:
|
||||||
protected:
|
protected:
|
||||||
struct AnyExecutable
|
struct AnyExecutable
|
||||||
{
|
{
|
||||||
AnyExecutable() : subscription(0), timer(0), callback_group(0), node(0) {}
|
AnyExecutable()
|
||||||
|
: subscription(0), timer(0), callback_group(0), node(0)
|
||||||
|
{}
|
||||||
// Either the subscription or the timer will be set, but not both
|
// Either the subscription or the timer will be set, but not both
|
||||||
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
|
rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
|
||||||
rclcpp::timer::TimerBase::SharedPtr timer;
|
rclcpp::timer::TimerBase::SharedPtr timer;
|
||||||
|
@ -313,8 +316,7 @@ protected:
|
||||||
std::malloc(sizeof(void *) * number_of_guard_conds));
|
std::malloc(sizeof(void *) * number_of_guard_conds));
|
||||||
if (guard_condition_handles.guard_conditions == NULL) {
|
if (guard_condition_handles.guard_conditions == NULL) {
|
||||||
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
|
// TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
|
||||||
throw std::runtime_error(
|
throw std::runtime_error("Could not malloc for guard condition pointers.");
|
||||||
"Could not malloc for guard condition pointers.");
|
|
||||||
}
|
}
|
||||||
// Put the global ctrl-c guard condition in
|
// Put the global ctrl-c guard condition in
|
||||||
assert(guard_condition_handles.guard_condition_count > 1);
|
assert(guard_condition_handles.guard_condition_count > 1);
|
||||||
|
|
|
@ -34,11 +34,11 @@ using namespace rclcpp::node;
|
||||||
using rclcpp::contexts::default_context::DefaultContext;
|
using rclcpp::contexts::default_context::DefaultContext;
|
||||||
|
|
||||||
Node::Node(std::string node_name)
|
Node::Node(std::string node_name)
|
||||||
: Node(node_name, DefaultContext::make_shared())
|
: Node(node_name, DefaultContext::make_shared())
|
||||||
{}
|
{}
|
||||||
|
|
||||||
Node::Node(std::string node_name, context::Context::SharedPtr context)
|
Node::Node(std::string node_name, context::Context::SharedPtr context)
|
||||||
: name_(node_name), context_(context),
|
: name_(node_name), context_(context),
|
||||||
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
|
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
|
||||||
{
|
{
|
||||||
node_handle_ = rmw_create_node(name_.c_str());
|
node_handle_ = rmw_create_node(name_.c_str());
|
||||||
|
|
|
@ -39,7 +39,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
|
||||||
|
|
||||||
Publisher(rmw_publisher_t * publisher_handle)
|
Publisher(rmw_publisher_t * publisher_handle)
|
||||||
: publisher_handle_(publisher_handle)
|
: publisher_handle_(publisher_handle)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
template<typename MessageT>
|
template<typename MessageT>
|
||||||
|
|
|
@ -48,11 +48,11 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate);
|
||||||
|
|
||||||
GenericRate(double rate)
|
GenericRate(double rate)
|
||||||
: GenericRate<Clock>(
|
: GenericRate<Clock>(
|
||||||
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
|
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
|
||||||
{}
|
{}
|
||||||
GenericRate(std::chrono::nanoseconds period)
|
GenericRate(std::chrono::nanoseconds period)
|
||||||
: period_(period), last_interval_(Clock::now())
|
: period_(period), last_interval_(Clock::now())
|
||||||
{}
|
{}
|
||||||
|
|
||||||
virtual bool
|
virtual bool
|
||||||
|
|
|
@ -46,7 +46,7 @@ public:
|
||||||
ServiceBase(
|
ServiceBase(
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string service_name)
|
const std::string service_name)
|
||||||
: service_handle_(service_handle), service_name_(service_name)
|
: service_handle_(service_handle), service_name_(service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
~ServiceBase()
|
~ServiceBase()
|
||||||
|
@ -94,7 +94,7 @@ public:
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string & service_name,
|
const std::string & service_name,
|
||||||
CallbackType callback)
|
CallbackType callback)
|
||||||
: ServiceBase(service_handle, service_name), callback_(callback)
|
: ServiceBase(service_handle, service_name), callback_(callback)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
std::shared_ptr<void> create_request()
|
std::shared_ptr<void> create_request()
|
||||||
|
|
|
@ -45,7 +45,7 @@ public:
|
||||||
SubscriptionBase(
|
SubscriptionBase(
|
||||||
rmw_subscription_t * subscription_handle,
|
rmw_subscription_t * subscription_handle,
|
||||||
std::string & topic_name)
|
std::string & topic_name)
|
||||||
: subscription_handle_(subscription_handle), topic_name_(topic_name)
|
: subscription_handle_(subscription_handle), topic_name_(topic_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
std::string get_topic_name()
|
std::string get_topic_name()
|
||||||
|
@ -75,7 +75,7 @@ public:
|
||||||
rmw_subscription_t * subscription_handle,
|
rmw_subscription_t * subscription_handle,
|
||||||
std::string & topic_name,
|
std::string & topic_name,
|
||||||
CallbackType callback)
|
CallbackType callback)
|
||||||
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)
|
: SubscriptionBase(subscription_handle, topic_name), callback_(callback)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
std::shared_ptr<void> create_message()
|
std::shared_ptr<void> create_message()
|
||||||
|
|
|
@ -48,7 +48,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase);
|
||||||
|
|
||||||
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
|
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
|
||||||
: period_(period),
|
: period_(period),
|
||||||
callback_(callback),
|
callback_(callback),
|
||||||
canceled_(false)
|
canceled_(false)
|
||||||
{
|
{
|
||||||
|
@ -81,7 +81,7 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer);
|
||||||
|
|
||||||
GenericTimer(std::chrono::nanoseconds period, CallbackType callback)
|
GenericTimer(std::chrono::nanoseconds period, CallbackType callback)
|
||||||
: TimerBase(period, callback), loop_rate_(period)
|
: TimerBase(period, callback), loop_rate_(period)
|
||||||
{
|
{
|
||||||
thread_ = std::thread(&GenericTimer<Clock>::run, this);
|
thread_ = std::thread(&GenericTimer<Clock>::run, this);
|
||||||
}
|
}
|
||||||
|
|
|
@ -109,8 +109,7 @@ init(int argc, char * argv[])
|
||||||
#endif
|
#endif
|
||||||
{
|
{
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("Failed to set SIGINT signal handler: (" +
|
std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
|
||||||
std::to_string(errno) + ")") +
|
|
||||||
// TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows.
|
// TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows.
|
||||||
std::strerror(errno));
|
std::strerror(errno));
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue