Remove declaration of friendship for Executor in callback_group, client, node, service, subscription, timer
This commit is contained in:
		
							parent
							
								
									dba12cba94
								
							
						
					
					
						commit
						0e5094693e
					
				
					 8 changed files with 114 additions and 90 deletions
				
			
		| 
						 | 
					@ -32,10 +32,6 @@ namespace node
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
class Node;
 | 
					class Node;
 | 
				
			||||||
} // namespace node
 | 
					} // namespace node
 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
namespace callback_group
 | 
					namespace callback_group
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
| 
						 | 
					@ -49,7 +45,6 @@ enum class CallbackGroupType
 | 
				
			||||||
class CallbackGroup
 | 
					class CallbackGroup
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::node::Node;
 | 
					  friend class rclcpp::node::Node;
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
 | 
				
			||||||
| 
						 | 
					@ -58,6 +53,40 @@ public:
 | 
				
			||||||
  : type_(group_type), can_be_taken_from_(true)
 | 
					  : type_(group_type), can_be_taken_from_(true)
 | 
				
			||||||
  {}
 | 
					  {}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const std::vector<subscription::SubscriptionBase::WeakPtr> &
 | 
				
			||||||
 | 
					  get_subscription_ptrs() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return subscription_ptrs_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const std::vector<timer::TimerBase::WeakPtr> &
 | 
				
			||||||
 | 
					  get_timer_ptrs() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return timer_ptrs_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const std::vector<service::ServiceBase::SharedPtr> &
 | 
				
			||||||
 | 
					  get_service_ptrs() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return service_ptrs_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const std::vector<client::ClientBase::SharedPtr> &
 | 
				
			||||||
 | 
					  get_client_ptrs() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return client_ptrs_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  std::atomic_bool & can_be_taken_from()
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return can_be_taken_from_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const CallbackGroupType & type() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return type_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
private:
 | 
					private:
 | 
				
			||||||
  RCLCPP_DISABLE_COPY(CallbackGroup);
 | 
					  RCLCPP_DISABLE_COPY(CallbackGroup);
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -31,18 +31,11 @@
 | 
				
			||||||
namespace rclcpp
 | 
					namespace rclcpp
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
// Forward declaration for friend statement
 | 
					 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
namespace client
 | 
					namespace client
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
class ClientBase
 | 
					class ClientBase
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -228,7 +228,7 @@ protected:
 | 
				
			||||||
      execute_client(any_exec->client);
 | 
					      execute_client(any_exec->client);
 | 
				
			||||||
    }
 | 
					    }
 | 
				
			||||||
    // Reset the callback_group, regardless of type
 | 
					    // Reset the callback_group, regardless of type
 | 
				
			||||||
    any_exec->callback_group->can_be_taken_from_.store(true);
 | 
					    any_exec->callback_group->can_be_taken_from().store(true);
 | 
				
			||||||
    // Wake the wait, because it may need to be recalculated or work that
 | 
					    // Wake the wait, because it may need to be recalculated or work that
 | 
				
			||||||
    // was previously blocked is now available.
 | 
					    // was previously blocked is now available.
 | 
				
			||||||
    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
 | 
					    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
 | 
				
			||||||
| 
						 | 
					@ -245,7 +245,8 @@ protected:
 | 
				
			||||||
    bool taken = false;
 | 
					    bool taken = false;
 | 
				
			||||||
    rmw_message_info_t message_info;
 | 
					    rmw_message_info_t message_info;
 | 
				
			||||||
    auto ret =
 | 
					    auto ret =
 | 
				
			||||||
      rmw_take_with_info(subscription->subscription_handle_, message.get(), &taken, &message_info);
 | 
					      rmw_take_with_info(subscription->get_subscription_handle(),
 | 
				
			||||||
 | 
					        message.get(), &taken, &message_info);
 | 
				
			||||||
    if (ret == RMW_RET_OK) {
 | 
					    if (ret == RMW_RET_OK) {
 | 
				
			||||||
      if (taken) {
 | 
					      if (taken) {
 | 
				
			||||||
        message_info.from_intra_process = false;
 | 
					        message_info.from_intra_process = false;
 | 
				
			||||||
| 
						 | 
					@ -267,7 +268,7 @@ protected:
 | 
				
			||||||
    bool taken = false;
 | 
					    bool taken = false;
 | 
				
			||||||
    rmw_message_info_t message_info;
 | 
					    rmw_message_info_t message_info;
 | 
				
			||||||
    rmw_ret_t status = rmw_take_with_info(
 | 
					    rmw_ret_t status = rmw_take_with_info(
 | 
				
			||||||
      subscription->intra_process_subscription_handle_,
 | 
					      subscription->get_intra_process_subscription_handle(),
 | 
				
			||||||
      &ipm,
 | 
					      &ipm,
 | 
				
			||||||
      &taken,
 | 
					      &taken,
 | 
				
			||||||
      &message_info);
 | 
					      &message_info);
 | 
				
			||||||
| 
						 | 
					@ -287,7 +288,7 @@ protected:
 | 
				
			||||||
  execute_timer(
 | 
					  execute_timer(
 | 
				
			||||||
    rclcpp::timer::TimerBase::SharedPtr timer)
 | 
					    rclcpp::timer::TimerBase::SharedPtr timer)
 | 
				
			||||||
  {
 | 
					  {
 | 
				
			||||||
    timer->callback_();
 | 
					    timer->execute_callback();
 | 
				
			||||||
  }
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
  static void
 | 
					  static void
 | 
				
			||||||
| 
						 | 
					@ -298,7 +299,7 @@ protected:
 | 
				
			||||||
    std::shared_ptr<void> request = service->create_request();
 | 
					    std::shared_ptr<void> request = service->create_request();
 | 
				
			||||||
    bool taken = false;
 | 
					    bool taken = false;
 | 
				
			||||||
    rmw_ret_t status = rmw_take_request(
 | 
					    rmw_ret_t status = rmw_take_request(
 | 
				
			||||||
      service->service_handle_,
 | 
					      service->get_service_handle(),
 | 
				
			||||||
      request_header.get(),
 | 
					      request_header.get(),
 | 
				
			||||||
      request.get(),
 | 
					      request.get(),
 | 
				
			||||||
      &taken);
 | 
					      &taken);
 | 
				
			||||||
| 
						 | 
					@ -321,7 +322,7 @@ protected:
 | 
				
			||||||
    std::shared_ptr<void> response = client->create_response();
 | 
					    std::shared_ptr<void> response = client->create_response();
 | 
				
			||||||
    bool taken = false;
 | 
					    bool taken = false;
 | 
				
			||||||
    rmw_ret_t status = rmw_take_response(
 | 
					    rmw_ret_t status = rmw_take_response(
 | 
				
			||||||
      client->client_handle_,
 | 
					      client->get_client_handle(),
 | 
				
			||||||
      request_header.get(),
 | 
					      request_header.get(),
 | 
				
			||||||
      response.get(),
 | 
					      response.get(),
 | 
				
			||||||
      &taken);
 | 
					      &taken);
 | 
				
			||||||
| 
						 | 
					@ -353,21 +354,21 @@ protected:
 | 
				
			||||||
        has_invalid_weak_nodes = false;
 | 
					        has_invalid_weak_nodes = false;
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group || !group->can_be_taken_from_.load()) {
 | 
					        if (!group || !group->can_be_taken_from().load()) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & weak_subscription : group->subscription_ptrs_) {
 | 
					        for (auto & weak_subscription : group->get_subscription_ptrs()) {
 | 
				
			||||||
          auto subscription = weak_subscription.lock();
 | 
					          auto subscription = weak_subscription.lock();
 | 
				
			||||||
          if (subscription) {
 | 
					          if (subscription) {
 | 
				
			||||||
            memory_strategy_->subs.push_back(subscription);
 | 
					            memory_strategy_->subs.push_back(subscription);
 | 
				
			||||||
          }
 | 
					          }
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & service : group->service_ptrs_) {
 | 
					        for (auto & service : group->get_service_ptrs()) {
 | 
				
			||||||
          memory_strategy_->services.push_back(service);
 | 
					          memory_strategy_->services.push_back(service);
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & client : group->client_ptrs_) {
 | 
					        for (auto & client : group->get_client_ptrs()) {
 | 
				
			||||||
          memory_strategy_->clients.push_back(client);
 | 
					          memory_strategy_->clients.push_back(client);
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
| 
						 | 
					@ -397,10 +398,10 @@ protected:
 | 
				
			||||||
    // Then fill the SubscriberHandles with ready subscriptions
 | 
					    // Then fill the SubscriberHandles with ready subscriptions
 | 
				
			||||||
    for (auto & subscription : memory_strategy_->subs) {
 | 
					    for (auto & subscription : memory_strategy_->subs) {
 | 
				
			||||||
      subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
 | 
					      subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
 | 
				
			||||||
        subscription->subscription_handle_->data;
 | 
					        subscription->get_subscription_handle()->data;
 | 
				
			||||||
      if (subscription->intra_process_subscription_handle_) {
 | 
					      if (subscription->get_intra_process_subscription_handle()) {
 | 
				
			||||||
        subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
 | 
					        subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
 | 
				
			||||||
          subscription->intra_process_subscription_handle_->data;
 | 
					          subscription->get_intra_process_subscription_handle()->data;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
    }
 | 
					    }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					@ -420,7 +421,7 @@ protected:
 | 
				
			||||||
    size_t service_handle_index = 0;
 | 
					    size_t service_handle_index = 0;
 | 
				
			||||||
    for (auto & service : memory_strategy_->services) {
 | 
					    for (auto & service : memory_strategy_->services) {
 | 
				
			||||||
      service_handles.services[service_handle_index] = \
 | 
					      service_handles.services[service_handle_index] = \
 | 
				
			||||||
        service->service_handle_->data;
 | 
					        service->get_service_handle()->data;
 | 
				
			||||||
      service_handle_index += 1;
 | 
					      service_handle_index += 1;
 | 
				
			||||||
    }
 | 
					    }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					@ -438,7 +439,7 @@ protected:
 | 
				
			||||||
    size_t client_handle_index = 0;
 | 
					    size_t client_handle_index = 0;
 | 
				
			||||||
    for (auto & client : memory_strategy_->clients) {
 | 
					    for (auto & client : memory_strategy_->clients) {
 | 
				
			||||||
      client_handles.clients[client_handle_index] = \
 | 
					      client_handles.clients[client_handle_index] = \
 | 
				
			||||||
        client->client_handle_->data;
 | 
					        client->get_client_handle()->data;
 | 
				
			||||||
      client_handle_index += 1;
 | 
					      client_handle_index += 1;
 | 
				
			||||||
    }
 | 
					    }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					@ -556,19 +557,19 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & weak_subscription : group->subscription_ptrs_) {
 | 
					        for (auto & weak_subscription : group->get_subscription_ptrs()) {
 | 
				
			||||||
          auto subscription = weak_subscription.lock();
 | 
					          auto subscription = weak_subscription.lock();
 | 
				
			||||||
          if (subscription) {
 | 
					          if (subscription) {
 | 
				
			||||||
            if (subscription->subscription_handle_->data == subscriber_handle) {
 | 
					            if (subscription->get_subscription_handle()->data == subscriber_handle) {
 | 
				
			||||||
              return subscription;
 | 
					              return subscription;
 | 
				
			||||||
            }
 | 
					            }
 | 
				
			||||||
            if (subscription->intra_process_subscription_handle_ &&
 | 
					            if (subscription->get_intra_process_subscription_handle() &&
 | 
				
			||||||
              subscription->intra_process_subscription_handle_->data == subscriber_handle)
 | 
					              subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
 | 
				
			||||||
            {
 | 
					            {
 | 
				
			||||||
              return subscription;
 | 
					              return subscription;
 | 
				
			||||||
            }
 | 
					            }
 | 
				
			||||||
| 
						 | 
					@ -587,13 +588,13 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & service : group->service_ptrs_) {
 | 
					        for (auto & service : group->get_service_ptrs()) {
 | 
				
			||||||
          if (service->service_handle_->data == service_handle) {
 | 
					          if (service->get_service_handle()->data == service_handle) {
 | 
				
			||||||
            return service;
 | 
					            return service;
 | 
				
			||||||
          }
 | 
					          }
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
| 
						 | 
					@ -610,13 +611,13 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & client : group->client_ptrs_) {
 | 
					        for (auto & client : group->get_client_ptrs()) {
 | 
				
			||||||
          if (client->client_handle_->data == client_handle) {
 | 
					          if (client->get_client_handle()->data == client_handle) {
 | 
				
			||||||
            return client;
 | 
					            return client;
 | 
				
			||||||
          }
 | 
					          }
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
| 
						 | 
					@ -636,7 +637,7 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto callback_group = weak_group.lock();
 | 
					        auto callback_group = weak_group.lock();
 | 
				
			||||||
        if (callback_group == group) {
 | 
					        if (callback_group == group) {
 | 
				
			||||||
          return node;
 | 
					          return node;
 | 
				
			||||||
| 
						 | 
					@ -655,12 +656,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & weak_timer : group->timer_ptrs_) {
 | 
					        for (auto & weak_timer : group->get_timer_ptrs()) {
 | 
				
			||||||
          auto t = weak_timer.lock();
 | 
					          auto t = weak_timer.lock();
 | 
				
			||||||
          if (t == timer) {
 | 
					          if (t == timer) {
 | 
				
			||||||
            return group;
 | 
					            return group;
 | 
				
			||||||
| 
						 | 
					@ -679,12 +680,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group || !group->can_be_taken_from_.load()) {
 | 
					        if (!group || !group->can_be_taken_from().load()) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & timer_ref : group->timer_ptrs_) {
 | 
					        for (auto & timer_ref : group->get_timer_ptrs()) {
 | 
				
			||||||
          auto timer = timer_ref.lock();
 | 
					          auto timer = timer_ref.lock();
 | 
				
			||||||
          if (timer && timer->check_and_trigger()) {
 | 
					          if (timer && timer->check_and_trigger()) {
 | 
				
			||||||
            any_exec->timer = timer;
 | 
					            any_exec->timer = timer;
 | 
				
			||||||
| 
						 | 
					@ -707,12 +708,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group || !group->can_be_taken_from_.load()) {
 | 
					        if (!group || !group->can_be_taken_from().load()) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & timer_ref : group->timer_ptrs_) {
 | 
					        for (auto & timer_ref : group->get_timer_ptrs()) {
 | 
				
			||||||
          timers_empty = false;
 | 
					          timers_empty = false;
 | 
				
			||||||
          // Check the expected trigger time
 | 
					          // Check the expected trigger time
 | 
				
			||||||
          auto timer = timer_ref.lock();
 | 
					          auto timer = timer_ref.lock();
 | 
				
			||||||
| 
						 | 
					@ -737,12 +738,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & weak_sub : group->subscription_ptrs_) {
 | 
					        for (auto & weak_sub : group->get_subscription_ptrs()) {
 | 
				
			||||||
          auto sub = weak_sub.lock();
 | 
					          auto sub = weak_sub.lock();
 | 
				
			||||||
          if (sub == subscription) {
 | 
					          if (sub == subscription) {
 | 
				
			||||||
            return group;
 | 
					            return group;
 | 
				
			||||||
| 
						 | 
					@ -762,8 +763,8 @@ protected:
 | 
				
			||||||
      if (subscription) {
 | 
					      if (subscription) {
 | 
				
			||||||
        // Figure out if this is for intra-process or not.
 | 
					        // Figure out if this is for intra-process or not.
 | 
				
			||||||
        bool is_intra_process = false;
 | 
					        bool is_intra_process = false;
 | 
				
			||||||
        if (subscription->intra_process_subscription_handle_) {
 | 
					        if (subscription->get_intra_process_subscription_handle()) {
 | 
				
			||||||
          is_intra_process = subscription->intra_process_subscription_handle_->data == *it;
 | 
					          is_intra_process = subscription->get_intra_process_subscription_handle()->data == *it;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        // Find the group for this handle and see if it can be serviced
 | 
					        // Find the group for this handle and see if it can be serviced
 | 
				
			||||||
        auto group = get_group_by_subscription(subscription);
 | 
					        auto group = get_group_by_subscription(subscription);
 | 
				
			||||||
| 
						 | 
					@ -773,7 +774,7 @@ protected:
 | 
				
			||||||
          subscriber_handles_.erase(it++);
 | 
					          subscriber_handles_.erase(it++);
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        if (!group->can_be_taken_from_.load()) {
 | 
					        if (!group->can_be_taken_from().load()) {
 | 
				
			||||||
          // Group is mutually exclusive and is being used, so skip it for now
 | 
					          // Group is mutually exclusive and is being used, so skip it for now
 | 
				
			||||||
          // Leave it to be checked next time, but continue searching
 | 
					          // Leave it to be checked next time, but continue searching
 | 
				
			||||||
          ++it;
 | 
					          ++it;
 | 
				
			||||||
| 
						 | 
					@ -804,12 +805,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & serv : group->service_ptrs_) {
 | 
					        for (auto & serv : group->get_service_ptrs()) {
 | 
				
			||||||
          if (serv == service) {
 | 
					          if (serv == service) {
 | 
				
			||||||
            return group;
 | 
					            return group;
 | 
				
			||||||
          }
 | 
					          }
 | 
				
			||||||
| 
						 | 
					@ -834,7 +835,7 @@ protected:
 | 
				
			||||||
          service_handles_.erase(it++);
 | 
					          service_handles_.erase(it++);
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        if (!group->can_be_taken_from_.load()) {
 | 
					        if (!group->can_be_taken_from().load()) {
 | 
				
			||||||
          // Group is mutually exclusive and is being used, so skip it for now
 | 
					          // Group is mutually exclusive and is being used, so skip it for now
 | 
				
			||||||
          // Leave it to be checked next time, but continue searching
 | 
					          // Leave it to be checked next time, but continue searching
 | 
				
			||||||
          ++it;
 | 
					          ++it;
 | 
				
			||||||
| 
						 | 
					@ -861,12 +862,12 @@ protected:
 | 
				
			||||||
      if (!node) {
 | 
					      if (!node) {
 | 
				
			||||||
        continue;
 | 
					        continue;
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
      for (auto & weak_group : node->callback_groups_) {
 | 
					      for (auto & weak_group : node->get_callback_groups()) {
 | 
				
			||||||
        auto group = weak_group.lock();
 | 
					        auto group = weak_group.lock();
 | 
				
			||||||
        if (!group) {
 | 
					        if (!group) {
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        for (auto & cli : group->client_ptrs_) {
 | 
					        for (auto & cli : group->get_client_ptrs()) {
 | 
				
			||||||
          if (cli == client) {
 | 
					          if (cli == client) {
 | 
				
			||||||
            return group;
 | 
					            return group;
 | 
				
			||||||
          }
 | 
					          }
 | 
				
			||||||
| 
						 | 
					@ -891,7 +892,7 @@ protected:
 | 
				
			||||||
          client_handles_.erase(it++);
 | 
					          client_handles_.erase(it++);
 | 
				
			||||||
          continue;
 | 
					          continue;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
        if (!group->can_be_taken_from_.load()) {
 | 
					        if (!group->can_be_taken_from().load()) {
 | 
				
			||||||
          // Group is mutually exclusive and is being used, so skip it for now
 | 
					          // Group is mutually exclusive and is being used, so skip it for now
 | 
				
			||||||
          // Leave it to be checked next time, but continue searching
 | 
					          // Leave it to be checked next time, but continue searching
 | 
				
			||||||
          ++it;
 | 
					          ++it;
 | 
				
			||||||
| 
						 | 
					@ -957,13 +958,13 @@ protected:
 | 
				
			||||||
    if (any_exec) {
 | 
					    if (any_exec) {
 | 
				
			||||||
      // If it is valid, check to see if the group is mutually exclusive or
 | 
					      // If it is valid, check to see if the group is mutually exclusive or
 | 
				
			||||||
      // not, then mark it accordingly
 | 
					      // not, then mark it accordingly
 | 
				
			||||||
      if (any_exec->callback_group && any_exec->callback_group->type_ == \
 | 
					      if (any_exec->callback_group && any_exec->callback_group->type() == \
 | 
				
			||||||
        callback_group::CallbackGroupType::MutuallyExclusive)
 | 
					        callback_group::CallbackGroupType::MutuallyExclusive)
 | 
				
			||||||
      {
 | 
					      {
 | 
				
			||||||
        // It should not have been taken otherwise
 | 
					        // It should not have been taken otherwise
 | 
				
			||||||
        assert(any_exec->callback_group->can_be_taken_from_.load());
 | 
					        assert(any_exec->callback_group->can_be_taken_from().load());
 | 
				
			||||||
        // Set to false to indicate something is being run from this group
 | 
					        // Set to false to indicate something is being run from this group
 | 
				
			||||||
        any_exec->callback_group->can_be_taken_from_.store(false);
 | 
					        any_exec->callback_group->can_be_taken_from().store(false);
 | 
				
			||||||
      }
 | 
					      }
 | 
				
			||||||
    }
 | 
					    }
 | 
				
			||||||
    return any_exec;
 | 
					    return any_exec;
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -47,18 +47,12 @@ struct rmw_node_t;
 | 
				
			||||||
namespace rclcpp
 | 
					namespace rclcpp
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
// Forward declaration for friend statement
 | 
					 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
namespace node
 | 
					namespace node
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
/// Node is the single point of entry for creating publishers and subscribers.
 | 
					/// Node is the single point of entry for creating publishers and subscribers.
 | 
				
			||||||
class Node
 | 
					class Node
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS(Node);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS(Node);
 | 
				
			||||||
| 
						 | 
					@ -232,6 +226,8 @@ public:
 | 
				
			||||||
 | 
					
 | 
				
			||||||
  size_t count_subscribers(const std::string & topic_name) const;
 | 
					  size_t count_subscribers(const std::string & topic_name) const;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const CallbackGroupWeakPtrList & get_callback_groups() const;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
private:
 | 
					private:
 | 
				
			||||||
  RCLCPP_DISABLE_COPY(Node);
 | 
					  RCLCPP_DISABLE_COPY(Node);
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -643,4 +643,11 @@ Node::count_subscribers(const std::string & topic_name) const
 | 
				
			||||||
  return count;
 | 
					  return count;
 | 
				
			||||||
}
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					const Node::CallbackGroupWeakPtrList &
 | 
				
			||||||
 | 
					Node::get_callback_groups() const
 | 
				
			||||||
 | 
					{
 | 
				
			||||||
 | 
					  return callback_groups_;
 | 
				
			||||||
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
 | 
					#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -30,18 +30,11 @@
 | 
				
			||||||
namespace rclcpp
 | 
					namespace rclcpp
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
// Forward declaration for friend statement
 | 
					 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
namespace service
 | 
					namespace service
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
class ServiceBase
 | 
					class ServiceBase
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -32,12 +32,6 @@
 | 
				
			||||||
namespace rclcpp
 | 
					namespace rclcpp
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
// Forward declaration is for friend statement in SubscriptionBase
 | 
					 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
namespace node
 | 
					namespace node
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
class Node;
 | 
					class Node;
 | 
				
			||||||
| 
						 | 
					@ -50,7 +44,6 @@ namespace subscription
 | 
				
			||||||
/// specializations of Subscription, among other things.
 | 
					/// specializations of Subscription, among other things.
 | 
				
			||||||
class SubscriptionBase
 | 
					class SubscriptionBase
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
 | 
				
			||||||
| 
						 | 
					@ -104,6 +97,16 @@ public:
 | 
				
			||||||
    return this->topic_name_;
 | 
					    return this->topic_name_;
 | 
				
			||||||
  }
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const rmw_subscription_t * get_subscription_handle() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return subscription_handle_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const rmw_subscription_t * get_intra_process_subscription_handle() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return intra_process_subscription_handle_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
  /// Borrow a new message.
 | 
					  /// Borrow a new message.
 | 
				
			||||||
  // \return Shared pointer to the fresh message.
 | 
					  // \return Shared pointer to the fresh message.
 | 
				
			||||||
  virtual std::shared_ptr<void> create_message() = 0;
 | 
					  virtual std::shared_ptr<void> create_message() = 0;
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
| 
						 | 
					@ -31,12 +31,6 @@
 | 
				
			||||||
namespace rclcpp
 | 
					namespace rclcpp
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
// Forward declaration is for friend statement in GenericTimer
 | 
					 | 
				
			||||||
namespace executor
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
class Executor;
 | 
					 | 
				
			||||||
} // namespace executor
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
namespace timer
 | 
					namespace timer
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 | 
					
 | 
				
			||||||
| 
						 | 
					@ -44,7 +38,6 @@ using CallbackType = std::function<void()>;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
class TimerBase
 | 
					class TimerBase
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
 | 
				
			||||||
| 
						 | 
					@ -66,6 +59,16 @@ public:
 | 
				
			||||||
    this->canceled_ = true;
 | 
					    this->canceled_ = true;
 | 
				
			||||||
  }
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  void execute_callback() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    callback_();
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					  const CallbackType & get_callback() const
 | 
				
			||||||
 | 
					  {
 | 
				
			||||||
 | 
					    return callback_;
 | 
				
			||||||
 | 
					  }
 | 
				
			||||||
 | 
					
 | 
				
			||||||
  /// Check how long the timer has until its next scheduled callback.
 | 
					  /// Check how long the timer has until its next scheduled callback.
 | 
				
			||||||
  // \return A std::chrono::duration representing the relative time until the next callback.
 | 
					  // \return A std::chrono::duration representing the relative time until the next callback.
 | 
				
			||||||
  virtual std::chrono::nanoseconds
 | 
					  virtual std::chrono::nanoseconds
 | 
				
			||||||
| 
						 | 
					@ -95,7 +98,6 @@ protected:
 | 
				
			||||||
template<class Clock = std::chrono::high_resolution_clock>
 | 
					template<class Clock = std::chrono::high_resolution_clock>
 | 
				
			||||||
class GenericTimer : public TimerBase
 | 
					class GenericTimer : public TimerBase
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
  friend class rclcpp::executor::Executor;
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
 | 
					  RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);
 | 
				
			||||||
| 
						 | 
					
 | 
				
			||||||
		Loading…
	
	Add table
		Add a link
		
	
		Reference in a new issue