ROS2底层机制&源码分析_ros2源码分析

-->topics_interface->add_subscription(subscription, options.callback_group);

   -->void CallbackGroup::add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) {

          std::lock_guard<std::mutex> lock(mutex_);

          subscription_ptrs_.push_back(subscription_ptr); // timer service client waitable一样的逻辑

          subscription_ptrs_.erase(

          std::remove_if(

          subscription_ptrs_.begin(),

          subscription_ptrs_.end(),

          [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}),

          subscription_ptrs_.end());

      }

void spin()

{

  SingleNodeManager::instance().spin();

}

void SingleNodeManager::spin()

{

  if (MOGO_UNLIKELY(!is_start_)) {

    throw std::runtime_error("SingleNodeManager is not running, please create NodeHandle before that!");

  }

  if (!is_join_exec_) {

    std::lock_guard<std::mutex> lock(exec_mutex_);

    if (!is_join_exec_) {

      exec_->add_node(node_ptr_);

      is_join_single_exec_ = true;

      is_join_exec_ = true;

    } else {

      if (!is_join_single_exec_) {

        throw std::runtime_error("Node has been joined in another exec");

      }

    }

  }

  exec_->spin();

  {

    std::lock_guard<std::mutex> lock(exec_mutex_);

    exec_->remove_node(node_ptr_);

    is_join_single_exec_ = false;

    is_join_exec_ = false;

  }

}

// 以subscription为例，以下详细函数调用栈

1. 获取可执行对象

bool

Executor::get_next_ready_executable_from_map(

  AnyExecutable & any_executable,

  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &

  weak_groups_to_nodes)

{

  TRACEPOINT(rclcpp_executor_get_next_ready);

  bool success = false;

  std::lock_guard<std::mutex> guard{mutex_};

  // Check the timers to see if there are any that are ready

  memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes);

  if (any_executable.timer) {

    success = true;

  }

  if (!success) {

    // Check the subscriptions to see if there are any that are ready

    memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes);

    if (any_executable.subscription) {

      success = true;

    }

  }

...

void

get_next_subscription(

    rclcpp::AnyExecutable & any_exec,

    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override

  {

    auto it = subscription_handles_.begin();

    while (it != subscription_handles_.end()) {

      auto subscription = get_subscription_by_handle(*it, weak_groups_to_nodes);

      if (subscription) {

        // Find the group for this handle and see if it can be serviced

        auto group = get_group_by_subscription(subscription, weak_groups_to_nodes);

        if (!group) {

          // Group was not found, meaning the subscription is not valid...

          // Remove it from the ready list and continue looking

          it = subscription_handles_.erase(it);

          continue;

        }

        if (!group->can_be_taken_from().load()) {

          // Group is mutually exclusive and is being used, so skip it for now

          // Leave it to be checked next time, but continue searching

          ++it;

          continue;

        }

        // Otherwise it is safe to set and return the any_exec

        any_exec.subscription = subscription;

        any_exec.callback_group = group;

        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);

        subscription_handles_.erase(it);

        return;

      }

      // Else, the subscription is no longer valid, remove it and continue

      it = subscription_handles_.erase(it);

    }

  }

...

rclcpp::SubscriptionBase::SharedPtr

MemoryStrategy::get_subscription_by_handle(

  const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,

  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)

{

  for (const auto & pair : weak_groups_to_nodes) {

    auto group = pair.first.lock();

    if (!group) {

      continue;

    }

    // check传入的subscriber_handle跟之前创建的是否匹配

    auto match_subscription = group->find_subscription_ptrs_if(

      [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {

        return subscription->get_subscription_handle() == subscriber_handle;

      });

    if (match_subscription) {

      return match_subscription;

    }

  }

  return nullptr;

}

...

template<typename Function>

rclcpp::SubscriptionBase::SharedPtr

find_subscription_ptrs_if(Function func) const

{

  return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);

}

template<typename TypeT, typename Function>

typename TypeT::SharedPtr _find_ptrs_if_impl(Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const

{

  std::lock_guard<std::mutex> lock(mutex_);

  for (auto & weak_ptr : vect_ptrs) {

    auto ref_ptr = weak_ptr.lock();

    if (ref_ptr && func(ref_ptr)) {

      return ref_ptr;

    }

  }

  return typename TypeT::SharedPtr();

}

至此就能匹配到对应的timer service client waitable subscription

2. 构造执行器

 auto it = subscription_handles_.begin();

    while (it != subscription_handles_.end()) {

      auto subscription = get_subscription_by_handle(*it, weak_groups_to_nodes);

      if (subscription) {

        // Find the group for this handle and see if it can be serviced

        auto group = get_group_by_subscription(subscription, weak_groups_to_nodes);

        if (!group) {

          // Group was not found, meaning the subscription is not valid...

          // Remove it from the ready list and continue looking

          it = subscription_handles_.erase(it);

          continue;

        }

        if (!group->can_be_taken_from().load()) {

          // Group is mutually exclusive and is being used, so skip it for now

          // Leave it to be checked next time, but continue searching

          ++it;

          continue;

        }

        // Otherwise it is safe to set and return the any_exec

        any_exec.subscription = subscription;

        any_exec.callback_group = group;

        any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);

        subscription_handles_.erase(it);

        return;

      }

      // Else, the subscription is no longer valid, remove it and continue

      it = subscription_handles_.erase(it);

    }

3. 执行

void

Executor::execute_any_executable(AnyExecutable & any_exec)

{

  if (!spinning.load()) {

    return;

  }

  if (any_exec.timer) {

    TRACEPOINT(

      rclcpp_executor_execute,

      static_cast<const void *>(any_exec.timer->get_timer_handle().get()));

    execute_timer(any_exec.timer);

  }

  if (any_exec.subscription) {

    TRACEPOINT(

      rclcpp_executor_execute,

      static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));

    execute_subscription(any_exec.subscription);

  }

  if (any_exec.service) {

    execute_service(any_exec.service);

  }

  if (any_exec.client) {

    execute_client(any_exec.client);

  }

  if (any_exec.waitable) {

    any_exec.waitable->execute(any_exec.data);

  }

  // Reset the callback_group, regardless of type

  any_exec.callback_group->can_be_taken_from().store(true);

  // Wake the wait, because it may need to be recalculated or work that

  // was previously blocked is now available.

  try {

    interrupt_guard_condition_.trigger();

  } catch (const rclcpp::exceptions::RCLError & ex) {

    throw std::runtime_error(

            std::string(

              "Failed to trigger guard condition from execute_any_executable: ") + ex.what());

  }

}

callback如何传入？

业务代码订阅

subscription = node_handle_.get_node()->create_generic_subscription(

                topic_meta.name,

                topic_meta.type,

                rosbag2_transport::Rosbag2QoS(queue_size),

                [this, topic_meta](std::shared_ptr<mogo::SerializedMessage> message) { // TODO 超过Xs没有回调加事件上报

                    if (!mogo::ok())

                        return;

                    count_++;

                    static double now_timestamp = mogo::TimeHelper::to_sec(mogo::Time::now());

                    // calc hz every second

                    if (mogo::TimeHelper::to_sec(mogo::Time::now()) - now_timestamp >= mogo::TimeHelper::to_sec(mogo::Time::create(1))) {

                        MOGO_INFO_STREAM_THROTTLE(10, "current callback frequency: " << count_);

                        count_ = 0;

                        now_timestamp = mogo::TimeHelper::to_sec(mogo::Time::now());

                    }

                    pushQueue(OutgoingMessage(message, topic_meta.name, topic_meta.type, mogo::Time::now()));

                },

                subscription_options

            );

内层调用注册callback

template<typename AllocatorT = std::allocator<void>>

  GenericSubscription(

    rclcpp::node_interfaces::NodeBaseInterface * node_base,

    const std::shared_ptr<rcpputils::SharedLibrary> ts_lib,

    const std::string & topic_name,

    const std::string & topic_type,

    const rclcpp::QoS & qos,

    // TODO(nnmm): Add variant for callback with message info. See issue #1604.

    std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,

    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)

  : SubscriptionBase(

      node_base,

      *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),

      topic_name,

      options.template to_rcl_subscription_options<rclcpp::SerializedMessage>(qos),

      true),

    callback_(callback),

    ts_lib_(ts_lib)

  {

    // This is unfortunately duplicated with the code in subscription.hpp.

    // TODO(nnmm): Deduplicate by moving this into SubscriptionBase.

    if (options.event_callbacks.deadline_callback) {

      this->add_event_handler(

        options.event_callbacks.deadline_callback,

        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);

    }

    if (options.event_callbacks.liveliness_callback) {

      this->add_event_handler(

        options.event_callbacks.liveliness_callback,

        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);

    }

    if (options.event_callbacks.incompatible_qos_callback) {

      this->add_event_handler(

        options.event_callbacks.incompatible_qos_callback,

        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);

    } else if (options.use_default_callbacks) {

      // Register default callback when not specified

      try {

        this->add_event_handler(

          [this](QOSRequestedIncompatibleQoSInfo & info) {

            this->default_incompatible_qos_callback(info);

          },

          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);

      } catch (UnsupportedEventTypeException & /*exc*/) {

        // pass

      }

    }

    if (options.event_callbacks.message_lost_callback) {

      this->add_event_handler(

        options.event_callbacks.message_lost_callback,

        RCL_SUBSCRIPTION_MESSAGE_LOST);

    }

  }

处理消息

void

GenericSubscription::handle_serialized_message(

  const std::shared_ptr<rclcpp::SerializedMessage> & message,

  const rclcpp::MessageInfo &)

{

  callback_(message);

}

消息从哪里来？---

bool

SubscriptionBase::take_serialized(

  rclcpp::SerializedMessage & message_out,

  rclcpp::MessageInfo & message_info_out)

{

  rcl_ret_t ret = rcl_take_serialized_message(

    this->get_subscription_handle().get(),

    &message_out.get_rcl_serialized_message(),

    &message_info_out.get_rmw_message_info(),

    nullptr);

  if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {

    return false;

  } else if (RCL_RET_OK != ret) {

    rclcpp::exceptions::throw_from_rcl_error(ret);

  }

  return true;

}

rcl_ret_t

rcl_take_serialized_message(

  const rcl_subscription_t * subscription,

  rcl_serialized_message_t * serialized_message,

  rmw_message_info_t * message_info,

  rmw_subscription_allocation_t * allocation

)

{

  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message");

  if (!rcl_subscription_is_valid(subscription)) {

    return RCL_RET_SUBSCRIPTION_INVALID;  // error already set

  }

  RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT);

  // If message_info is NULL, use a place holder which can be discarded.

  rmw_message_info_t dummy_message_info;

  rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;

  *message_info_local = rmw_get_zero_initialized_message_info();

  // Call rmw_take_with_info.

  bool taken = false;

  rmw_ret_t ret = rmw_take_serialized_message_with_info(

    subscription->impl->rmw_handle, serialized_message, &taken, message_info_local, allocation);

  if (ret != RMW_RET_OK) {

    RCL_SET_ERROR_MSG(rmw_get_error_string().str);

    return rcl_convert_rmw_ret_to_rcl_ret(ret);

  }

  RCUTILS_LOG_DEBUG_NAMED(

    ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false");

  if (!taken) {

    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;

  }

  return RCL_RET_OK;

}

注意：这里已经到rmw层了（DDS的封装层）

rmw_ret_t

rmw_take_serialized_message_with_info(

  const rmw_subscription_t * subscription,

  rmw_serialized_message_t * serialized_message,

  bool * taken,

  rmw_message_info_t * message_info,

  rmw_subscription_allocation_t * allocation)

{

  return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message_with_info(

    eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info,

    allocation);

}

核心代码---循环通过data_reader_->take(data_values, info_seq, 1)获取数据，最终内存拷贝到serialized_message中带出

rmw_ret_t

_take_serialized_message(

  const char * identifier,

  const rmw_subscription_t * subscription,

  rmw_serialized_message_t * serialized_message,

  bool * taken,

  rmw_message_info_t * message_info,

  rmw_subscription_allocation_t * allocation)

{

  (void) allocation;

  *taken = false;

  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(

    subscription handle,

    subscription->implementation_identifier, identifier,

    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION)

  auto info = static_cast<CustomSubscriberInfo *>(subscription->data);

  RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR);

  eprosima::fastcdr::FastBuffer buffer;

  eprosima::fastdds::dds::SampleInfo sinfo;

  rmw_fastrtps_shared_cpp::SerializedData data;

  data.is_cdr_buffer = true;

  data.data = &buffer;

  data.impl = nullptr;    // not used when is_cdr_buffer is true

  eprosima::fastdds::dds::StackAllocatedSequence<void *, 1> data_values;

  const_cast<void **>(data_values.buffer())[0] = &data;

  eprosima::fastdds::dds::SampleInfoSeq info_seq{1};

  while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(data_values, info_seq, 1)) {

    auto reset = rcpputils::make_scope_exit(

      [&]()

      {

        data_values.length(0);

        info_seq.length(0);

      });

    if (info_seq[0].valid_data) {

      auto buffer_size = static_cast<size_t>(buffer.getBufferSize());

      if (serialized_message->buffer_capacity < buffer_size) {

        auto ret = rmw_serialized_message_resize(serialized_message, buffer_size);

        if (ret != RMW_RET_OK) {

          return ret;           // Error message already set

        }

      }

      serialized_message->buffer_length = buffer_size;

      memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length);

      if (message_info) {

        _assign_message_info(identifier, message_info, &info_seq[0]);

      }

      *taken = true;

      break;

    }

  }

  return RMW_RET_OK;

}

fastrtps-fastdds---查数据

ReturnCode_t DataReaderImpl::read_or_take(

        LoanableCollection& data_values,

        SampleInfoSeq& sample_infos,

        int32_t max_samples,

        const InstanceHandle_t& handle,

        SampleStateMask sample_states,

        ViewStateMask view_states,

        InstanceStateMask instance_states,

        bool exact_instance,

        bool single_instance,

        bool should_take)

{

    if (reader_ == nullptr)

    {

        return ReturnCode_t::RETCODE_NOT_ENABLED;

    }

    ReturnCode_t code = check_collection_preconditions_and_calc_max_samples(data_values, sample_infos, max_samples);

    if (!code)

    {

        return code;

    }

#if HAVE_STRICT_REALTIME

    auto max_blocking_time = std::chrono::steady_clock::now() +

            std::chrono::microseconds(::TimeConv::Time_t2MicroSecondsInt64(qos_.reliability().max_blocking_time));

    std::unique_lock<RecursiveTimedMutex> lock(reader_->getMutex(), std::defer_lock);

    if (!lock.try_lock_until(max_blocking_time))

    {

        return ReturnCode_t::RETCODE_TIMEOUT;

    }

#else

    std::lock_guard<RecursiveTimedMutex> _(reader_->getMutex());

#endif // if HAVE_STRICT_REALTIME

    set_read_communication_status(false);

    auto it = history_.lookup_available_instance(handle, exact_instance);

    if (!it.first)

    {

        if (exact_instance && !history_.is_instance_present(handle))

        {

            return ReturnCode_t::RETCODE_BAD_PARAMETER;

        }

        else

        {

            return ReturnCode_t::RETCODE_NO_DATA;

        }

    }

    code = prepare_loan(data_values, sample_infos, max_samples);

    if (!code)

    {

        return code;

    }

    detail::StateFilter states{ sample_states, view_states, instance_states };

    detail::ReadTakeCommand cmd(*this, data_values, sample_infos, max_samples, states, it.second, single_instance);

    while (!cmd.is_finished())

    {

        cmd.add_instance(should_take);

    }

    return cmd.return_value();

}

bool shutdown()

{

  return rclcpp::shutdown();

}