-
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());
}
}
为什么没有pub对象?pub是对外的,spin是对向内的数据
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();
}
调用回调,注意这个callback_就是订阅传进去的回调函数
void
GenericSubscription::handle_serialized_message(
const
std::shared_ptr<rclcpp::SerializedMessage> & message,
const
rclcpp::MessageInfo &)
{
callback_(message);
}
-
create_publisher&&publish
ReturnCode_t DataWriterImpl::perform_create_new_change(
ChangeKind_t change_kind,
void* data,
WriteParams& wparams,
const InstanceHandle_t& handle)
{
// Block lowlevel writer
auto max_blocking_time = steady_clock::now() +
microseconds(::TimeConv::Time_t2MicroSecondsInt64(qos_.reliability().max_blocking_time));
#if HAVE_STRICT_REALTIME
std::unique_lock<RecursiveTimedMutex> lock(writer_->getMutex(), std::defer_lock);
if (!lock.try_lock_until(max_blocking_time))
{
return ReturnCode_t::RETCODE_TIMEOUT;
}
#else
std::unique_lock<RecursiveTimedMutex> lock(writer_->getMutex());
#endif // if HAVE_STRICT_REALTIME
PayloadInfo_t payload;
bool was_loaned = check_and_remove_loan(data, payload);
if (!was_loaned)
{
if (!get_free_payload_from_pool(type_->getSerializedSizeProvider(data), payload))
{
return ReturnCode_t::RETCODE_OUT_OF_RESOURCES;
}
if ((ALIVE == change_kind) && !type_->serialize(data, &payload.payload))
{
logWarning(DATA_WRITER, "Data serialization returned false");
return_payload_to_pool(payload);
return ReturnCode_t::RETCODE_ERROR;
}
}
// 这里是写数据的地方
CacheChange_t* ch = writer_->new_change(change_kind, handle);
if (ch != nullptr)
{
payload.move_into_change(*ch);
mogo_recorder::MogoRecorder::GetInstance()->AddNewMessage(*ch, topic_->get_name());
bool added = false;
if (reader_filters_)
{
auto related_sample_identity = wparams.related_sample_identity();
auto filter_hook = [&related_sample_identity, this](CacheChange_t& ch)
{
reader_filters_->update_filter_info(static_cast<DataWriterFilteredChange&>(ch),
related_sample_identity);
};
added = history_.add_pub_change_with_commit_hook(ch, wparams, filter_hook, lock, max_blocking_time);
}
else
{
added = history_.add_pub_change(ch, wparams, lock, max_blocking_time);
}
if (!added)
{
if (was_loaned)
{
payload.move_from_change(*ch);
add_loan(data, payload);
}
writer_->release_change(ch);
return ReturnCode_t::RETCODE_TIMEOUT;
}
if (qos_.deadline().period != c_TimeInfinite)
{
if (!history_.set_next_deadline(
handle,
steady_clock::now() + duration_cast<system_clock::duration>(deadline_duration_us_)))
{
logError(DATA_WRITER, "Could not set the next deadline in the history");
}
else
{
if (timer_owner_ == handle || timer_owner_ == InstanceHandle_t())
{
if (deadline_timer_reschedule())
{
deadline_timer_->cancel_timer();
deadline_timer_->restart_timer();
}
}
}
}
if (qos_.lifespan().duration != c_TimeInfinite)
{
lifespan_duration_us_ = duration<double, std::ratio<1, 1000000>>(
qos_.lifespan().duration.to_ns() * 1e-3);
lifespan_timer_->update_interval_millisec(qos_.lifespan().duration.to_ns() * 1e-6);
lifespan_timer_->restart_timer();
}
return ReturnCode_t::RETCODE_OK;
}
return ReturnCode_t::RETCODE_OUT_OF_RESOURCES;
}
-
mogo::shutdown
bool shutdown()
{
return
rclcpp::shutdown();
}