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