当前位置:   article > 正文

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

ros2源码分析
  • init
    ->init_and_remove_ros_arguments
        ->init
            ->Context::init 保存初始化传入的信号
            ->install_signal_handlers→SignalHandler::install 开线程响应信号
        ->_remove_ros_arguments 移除ros参数
    ->SingleNodeManager::instance().init 
    ->mogo_recorder::MogoRecorder::instance().Init 中间件录包初始化
    ->创建全局静态的NodeHandle
     

  • NodeHandle

    • 构造nodehandle,并校验命名空间,内部调用

      void NodeHandle::construct()

      {

          SingleNodeManager::instance().increase_ref();

      }

    • param->从redis获取配置信息

  • create_generic_subscription 订阅接口

    -->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());

          }

  • mogo::AsyncSpinner

    /**
    * AsyncSpinner 用来异步spin 某一个 callback group
    * 如果thread_num =1,将创建一个独立的线程,进行指定callback group的spin
    * 如果thread_num >1,将使用multithead spinner 执行callback group的spin
    *
    * 提示:若只是想要进行整个node的多线程的spin,请使用 mogo::multithread_spin
    */

    • 构造

    • start

      创建执行器,将构造中传入的callback_group对象传入执行器,传入node;单独开个线程spin;多线程spin就是开指定多个线程并行spin动作

      void AsyncSpinner::start()

      {

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

        if (is_started_) {

          return;

        }

        mogo::spin();

        if (thread_num_ == 1) {

          exec_ = rclcpp::executors::SingleThreadedExecutor::make_shared();

        else {

          exec_ = rclcpp::executors::MultiThreadedExecutor::make_shared(rclcpp::ExecutorOptions(), thread_num_);

        }

        exec_->add_callback_group(callback_group_,

                                  SingleNodeManager::instance().get_node()->get_node_base_interface()); // 将回调组跟节点对象存入map

        th_ = std::thread([this] {

                        long tid = syscall(SYS_gettid);

                        if (tid > 0) {

                          this->th_id_.store((int)tid);

                        }

                        exec_->spin();

                      });

        is_started_ = true;

      }

      void

      MultiThreadedExecutor::spin()

      {

        if (spinning.exchange(true)) {

          throw std::runtime_error("spin() called while already spinning");

        }

        RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );

        std::vector<std::thread> threads;

        size_t thread_id = 0;

        {

          std::lock_guard wait_lock{wait_mutex_};

          for (; thread_id < number_of_threads_ - 1; ++thread_id) {

            auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);

            threads.emplace_back(func);

          }

        }

        run(thread_id);

        for (auto & thread : threads) {

          thread.join();

        }

      }

      void

      SingleThreadedExecutor::spin()

      {

        if (spinning.exchange(true)) {

          throw std::runtime_error("spin() called while already spinning");

        }

        RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );

        while (rclcpp::ok(this->context_) && spinning.load()) {

          rclcpp::AnyExecutable any_executable;

          if (get_next_executable(any_executable)) { // 内部从map中取

            execute_any_executable(any_executable);

          }

        }

      }

  • mogo::spin

    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();

    }

  • create_publisher

  • publish

  • mogo::shutdown

    bool shutdown()

    {

      return rclcpp::shutdown();

    }

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/在线问答5/article/detail/887284
推荐阅读
相关标签
  

闽ICP备14008679号