📑
ROS2を深く理解する：ROSノード編１ 基本構造
解説対象
本記事では、ROS2の中核概念であるROSノードの中身の基本構造について解説します。ROSノードにまつわる諸機能がどの部分で実装されているかを知っていると、コードリーディングが捗ります。
本記事は下記の「ROS2を深く理解する」の記事群の一部ですが、この記事単独でも理解できるようになっています。
目標
本記事の目標は、ROSノードを生成するNodeクラス及びその内部構造であるNodeBaseクラス、及びrclノード/rmwノードを生成する関数の実装を理解することです。
- ROSノード
- ROS2上で実行する処理を担う最も基本的なオブジェクト（論理処理単位）
-
Nodeクラス・LifecycleNodeクラス及びそれらの派生クラス（c++やpython等の各クライアント言語で表現される）をインスタンス化して生成される
- rclノード
- ROSノードの内部構造でありremapの処理など基本機能を提供している部分
- クライアント言語（c++,python等）に依存しないcの関数で実装されている
- rmwノード
- rclノードの内部構造でありDDSという通信規格を用いてROSノード間で互いに通信を行えるようにする機能を提供している部分
- クライアント言語（c++,python等）に依存しないcの関数で実装されている
特にrclノード/rmwノードを理解することが、ROSノードに関わる重要概念（ノード名・ノード名前空間・remap処理・ノードパラメータ等）を理解する上で重要になります。
読むと役に立つと思われる読者
- ROS2を使用したロボット開発を始めて数か月ぐらい、ROS2の主要な概念についてはおおざっぱには理解しておりステップアップしたい方
- チュートリアル通りにやれば確かに動くけど・・・、自分は不適切な/冗長なコード書いているのでは？と自信がない方
- ROS2が期待通りに動いてくれない！ドキュメント読んでも原因がわからない！という人
ROS2を用いた開発では、適切な設計や問題解決の為に遅かれ早かれソースを追って処理を確認する作業が必須になります。ある程度概念を掴めた後は、必要に感じたところからどんどんソースを読み始めましょう。
前提
- ROS2 humble時の実装に基づいています。
- c++側の実装（rclcppのnode.cpp）に基づいていますが、rclpy側も結局はrclで規定される実装につながりますので、大部分は共通です。
- ROSノードには、ライフサイクルを持たないROSノード（
rclcpp::Node）とライフサイクルを持つROSノード（rclcpp_lifecycle::LifecycleNode）の２種類があります。本記事でのソースコードはNodeの実装をもとに記述していますが、LifecycleNodeも同じ振る舞いになっています。
公式ドキュメント
ROSノード周りを理解するのに参考となる公式ドキュメントはこのあたりですが・・・、これだけは深い理解はできません。
- 誰もが最初に読むところ
- Understanding-ROS2-Nodes
- プロンプトからのROSノードの扱いはこれでわかりますね。でも実際にシステム開発していくには物足りないです。
- ここも読むの必須
- Composition
- 実務的にはCompositionとても重要。でもここ読んだだけでは、〇〇どうなってるの？とたくさんの疑問が沸きます。
- APIリファレンス
- Node
- 不親切です。情報量が足りないです。
疑問が沸いたらソースを読むしかないです。
解説
ROSノードとは何か？
- ROSノードは、ROS2上で実行する処理を担う最も基本的なオブジェクト（論理処理単位）です。初学者は勘違いしやすいですが、ROSノードは実行可能なプログラム（=executable）ではなく、executableを実行することで生成されるオブジェクト（論理処理単位）です。
- 通常、ROS2を用いたアプリケーションは非常に多数のROSノードの連係動作によって実現します。
- ROSノードは他のROSノード等と「ROSトピック、ROSサービス、ROSアクション、ノードパラメータ、ros2_controlインタフェース等」を通して情報のやりとりを行い連携動作できることが特徴です。
- あるROSノードと別のROSノードは同じプロセス上で実行することも、異なるプロセス上で実行することも、どちらも可能です
- ROSノードは必ずexecutorを用いて資源（スレッド）が割り当てられ実行されます。明示的にexecutorを使っていないように見えても、裏で必ずexecutorが動いてます。executorはexecutable上で実行されます。
ROSノードとは何か？理解があやふやであれば下記記事も参考にしてください
ROSノードの構造
ROSノードを生成するNodeクラスの基本構造は下記の図のようになります。
この幹の部分がわかっているとソースが非常に読みやすくなります。
NodeBaseより上はオブジェクト指向でコーディングされているが、rcl_node_t以下は関数ベースのコーディングなのでソースは若干読みづらいです
それぞれのざっくりとした役割は下記です。
-
Nodeクラス- クライアント言語（c++）からROSノードを生成・操作する為のIFを提供する
- ROSノードが持つ各種機能（ROSトピック, ROSサービス, ノードパラメータ 等々）の実装はそれぞれ別クラスへ委譲しており、
Nodeクラスはそれら機能を集約するクラスとなっている。
-
NodeBaseクラス- クライアント言語（c++）からROSノードを操作する為の基本機能を提供する
- ROSノードが持つ各種機能の中でも、最も基本となる部分を実装。ノード名やノード名前空間などROSノードを区別する為の値やROSノード間で通信する為の基礎実装をラップする。
-
rcl_node_t構造体,rcl_node_impl_t構造体, 及びrcl_node_init()等のrclの各種関数- クライアント言語（c++,python等）に依存しないROSノードの基本機能（=rclノード）を提供する。重要なのは
rcl_node_init()関数であり、rclノードを生成する処理としてノード名やノード名前空間のバリデーションやremap等の処理を行っている。NodeBaseはこのrcl_node_tをラップする実装になっている。
- クライアント言語（c++,python等）に依存しないROSノードの基本機能（=rclノード）を提供する。重要なのは
-
rmw_node_t構造体及びrmwの各種関数- ROSノードがDDSという通信規格を用いて互いに通信を行えるようにする機能（=rmwノード）を提供する。
rcl_node_tはこのrmw_node_tをラップする実装になっている。
- ROSノードがDDSという通信規格を用いて互いに通信を行えるようにする機能（=rmwノード）を提供する。
ここまでわかっていれば、個々の具体的な事情に応じて気になるところを読んでいくことになります。それらは別記事にしています。
（参考）ソースの確認
上記の解説内容について実際にソースコードを追って確認していきます。
Nodeの実装を理解する
まず、Nodeのprivateメンバを見てみましょう。たくさんの〇〇Interfaceへのスマートポインタが並んでいます。
機能の実装が非常に整理されており分散して定義されてることがわかります。例えばROSノードが他のROSノードや外部のプログラムと連携する為のIFである
- ROSトピック
- ROSサービス
- ノードパラメータ
といった仕組みは、それぞれ別クラスで定義されています。
なお、ROSアクションは、内部的にはROSトピックとROSサービスを組み合わせて実装されるので明示的に登場してきません。
node.hpp抜粋
private:
RCLCPP_DISABLE_COPY(Node)
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
const rclcpp::NodeOptions node_options_;
const std::string sub_namespace_;
const std::string effective_namespace_;
次にNodeのconstructorを見てみましょう。
-
node_〇〇というメンバ変数（スマートポインタ）の参照先がconstruct時に初期化されています。- ちなみにこの参照先はこのタイミングで確定しその後変わることはありません。
- ROSノードと共に必ず１対１の関係で存在しますので、概念的にはROSノードそのものと思ってよいです（機能を分割して定義しているだけ）
- 例えばノードパラメータの仕組みは、
Nodeのメンバ変数node_parameters_から参照できます。-
Nodeをconstructした時にnode_parameters_には実体としてrclcpp::node_interfaces::NodeParametersInterfaceをextendしているrclcpp::node_interfaces::NodeParametersが生成され保持されます
-
node.cpp抜粋
Node::Node(
const std::string & node_name,
const std::string & namespace_,
const NodeOptions & options)
: node_base_(new rclcpp::node_interfaces::NodeBase(
node_name,
namespace_,
options.context(),
*(options.get_rcl_node_options()),
options.use_intra_process_comms(),
options.enable_topic_statistics())),
node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())),
node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
node_clock_(new rclcpp::node_interfaces::NodeClock(
node_base_,
node_topics_,
node_graph_,
node_services_,
node_logging_
)),
node_parameters_(new rclcpp::node_interfaces::NodeParameters(
node_base_,
node_logging_,
node_topics_,
node_services_,
node_clock_,
options.parameter_overrides(),
options.start_parameter_services(),
options.start_parameter_event_publisher(),
// This is needed in order to apply parameter overrides to the qos profile provided in
// options.
get_parameter_events_qos(*node_base_, options),
options.parameter_event_publisher_options(),
options.allow_undeclared_parameters(),
options.automatically_declare_parameters_from_overrides()
)),
node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
node_base_,
node_topics_,
node_graph_,
node_services_,
node_logging_,
node_clock_,
node_parameters_,
options.clock_qos(),
options.use_clock_thread()
)),
node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
node_options_(options),
sub_namespace_(""),
effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
{
// we have got what we wanted directly from the overrides,
// but declare the parameters anyway so they are visible.
rclcpp::detail::declare_qos_parameters(
rclcpp::QosOverridingOptions
{
QosPolicyKind::Depth,
QosPolicyKind::Durability,
QosPolicyKind::History,
QosPolicyKind::Reliability,
},
node_parameters_,
node_topics_->resolve_topic_name("/parameter_events"),
options.parameter_event_qos(),
rclcpp::detail::PublisherQosParametersTraits{});
}
上記constructorを見てわかるのがnode_base_がかなり重要そうということ。node_〇〇を初期化するのに必ずnode_base_.get()が渡されていることからもその重要性が推察できます。
なので次にrclcpp::node_interfaces::NodeBaseを理解しましょう。
なお、node_base_を初期化する際にoptions.get_rcl_node_options()が使用されていることも重要です。optionsはノードオプションであり、コンテキスト情報を保持しているオブジェクトです。詳しくは別記事にて解説しています。
NodeBaseの実装を理解する
NodeBaseのprivateメンバを見てみましょう。重要なのはずばり
std::shared_ptr<rcl_node_t> node_handle_;
これはrcl_node_t型へのスマートポインタです。
node_handle_は、constructor中で作成されセットされます。
node_base.hpp抜粋
private:
RCLCPP_DISABLE_COPY(NodeBase)
rclcpp::Context::SharedPtr context_;
bool use_intra_process_default_;
bool enable_topic_statistics_default_;
std::shared_ptr<rcl_node_t> node_handle_;
rclcpp::CallbackGroup::SharedPtr default_callback_group_;
std::mutex callback_groups_mutex_;
std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
std::atomic_bool associated_with_executor_;
/// Guard condition for notifying the Executor of changes to this node.
mutable std::recursive_mutex notify_guard_condition_mutex_;
rclcpp::GuardCondition notify_guard_condition_;
bool notify_guard_condition_is_valid_;
NodeBaseのconstructorを見ましょう。rclノードが下記の手順で作成され参照が設定されていることがわかります。rclノードとはROSノードの機能の基本部分（ROSノードの本体と言ってよい）であり、クライアント言語（c++やpython）に依存しないcで実装されている部分です。
- rclノードが
new rcl_node_t()で作成される -
rcl_node_init()によってrclノードの変数が設定される- rclノードオプション（引数
rcl_node_optionsで渡される）もrcl_node_init()に渡されます
- rclノードオプション（引数
-
node_handle_.reset()によってnode_handle_に参照が設定される
node_base.cpp抜粋
NodeBase::NodeBase(
const std::string & node_name,
const std::string & namespace_,
rclcpp::Context::SharedPtr context,
const rcl_node_options_t & rcl_node_options,
bool use_intra_process_default,
bool enable_topic_statistics_default)
: context_(context),
use_intra_process_default_(use_intra_process_default),
enable_topic_statistics_default_(enable_topic_statistics_default),
node_handle_(nullptr),
default_callback_group_(nullptr),
associated_with_executor_(false),
notify_guard_condition_(context),
notify_guard_condition_is_valid_(false)
{
// Create the rcl node and store it in a shared_ptr with a custom destructor.
std::unique_ptr<rcl_node_t> rcl_node(new rcl_node_t(rcl_get_zero_initialized_node()));
std::shared_ptr<std::recursive_mutex> logging_mutex = get_global_logging_mutex();
rcl_ret_t ret;
{
std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
// TODO(ivanpauno): /rosout Qos should be reconfigurable.
// TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex,
// rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called
// here directly.
ret = rcl_node_init(
rcl_node.get(),
node_name.c_str(), namespace_.c_str(),
context_->get_rcl_context().get(), &rcl_node_options);
}
// 中略
node_handle_.reset(
rcl_node.release(),
[logging_mutex](rcl_node_t * node) -> void {
std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
// TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
// rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
// here directly.
if (rcl_node_fini(node) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
}
delete node;
});
次にrclノードを表す構造体rcl_node_tを見ていきましょう。
rclノードの実装を理解する
ここからレポジトリが変わります。今まではrclcppレポジトリの中を見てきましたが、ここからはrclレポジトリになります。rclノードはc言語で実装されています。
rcl_node_t構造体の定義を見てみましょう。シンプルなstructです。見てわかるようにrcl_node_sはrcl_node_impl_tのラッパーです。
node.h抜粋（構造体定義部分）
typedef struct rcl_node_impl_s rcl_node_impl_t;
/// Structure which encapsulates a ROS Node.
typedef struct rcl_node_s
{
/// Context associated with this node.
rcl_context_t * context;
/// Private implementation pointer.
rcl_node_impl_t * impl;
} rcl_node_t;
この中で重要なのはrcl_node_impl_t構造体へのポインタimplです。rcl_node_impl_tはrcl_node_impl_sと同義なので、次にrcl_node_impl_sの定義を見ましょう。
node.c抜粋（構造体定義部分）
struct rcl_node_impl_s
{
rcl_node_options_t options;
rmw_node_t * rmw_node_handle;
rcl_guard_condition_t * graph_guard_condition;
const char * logger_name;
const char * fq_name;
};
rcl_node_impl_s構造体で重要なのはrmw_node_t構造体へのポインタrmw_node_handleです。
rmwノードの実装を理解する
rmwノードとはRMW(Ros MiddleWare interface)が提供する実装であり、rclノードの（もっといえばROSノードの）内部実装です。ROSノードがDDSという通信規格を用いてROSノード間で互いに通信を行えるようにしてくれています。
今まで見てきたレポジトリ（rclcpp,rcl）とはまた別のrmwレポジトリで管理されています。
rmw_node_tの定義を見てみましょう。
rmw/types.h抜粋（構造体定義部分）
/// Structure which encapsulates an rmw node
typedef struct RMW_PUBLIC_TYPE rmw_node_s
{
/// Name of the rmw implementation
const char * implementation_identifier;
/// Type erased pointer to this node's data
void * data;
/// A concise name of this rmw node for identification
const char * name;
/// The namespace of this rmw node
const char * namespace_;
/// Context information about node's init specific information
rmw_context_t * context;
} rmw_node_t;
rmwノードが一意に識別する為の情報としてnameとnamespace_を持っていることがわかります。
rclノードオプションを理解する
rclノードオプションとは、ノードオプションの一部です。ノードオプションとは、Nodeのconstructor引数であるoptionで渡される値を指し、rclノードオプションとは、NodeBaseのconstructor引数であるrcl_node_optionsで渡される値を指します。
ノードオプションはROSノードの色々な箇所に影響しますが、その中でもrclノードオプション部分はrclノードの基本的な振る舞いに影響するので重要です。
rclノードオプションは、ノードオプションを表す引数optionからoptions.get_rcl_node_options()によって生成されNodeBaseのconstructorに渡されます。
ではget_rcl_node_options()の実装を見てみましょう。
node_options.cpp
const rcl_node_options_t *
NodeOptions::get_rcl_node_options() const
{
// If it is nullptr, create it on demand.
if (!node_options_) {
node_options_.reset(new rcl_node_options_t);
*node_options_ = rcl_node_get_default_options();
node_options_->allocator = this->allocator_;
node_options_->use_global_arguments = this->use_global_arguments_;
node_options_->enable_rosout = this->enable_rosout_;
node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile();
int c_argc = 0;
std::unique_ptr<const char *[]> c_argv;
if (!this->arguments_.empty()) {
if (this->arguments_.size() > static_cast<size_t>(std::numeric_limits<int>::max())) {
throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
}
c_argc = static_cast<int>(this->arguments_.size());
c_argv.reset(new const char *[c_argc]);
for (std::size_t i = 0; i < this->arguments_.size(); ++i) {
c_argv[i] = this->arguments_[i].c_str();
}
}
rcl_ret_t ret = rcl_parse_arguments(
c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments));
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to parse arguments");
}
std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
c_argc, c_argv.get(), &(node_options_->arguments), this->allocator_);
if (!unparsed_ros_arguments.empty()) {
throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));
}
}
return node_options_.get();
}
注目したいのは、NodeOptions::get_rcl_node_options()がrcl_parse_arguments()関数を呼ぶ点です。ここで、executable起動時に指定できるコマンドラインROS引数からグローバルROS引数を生成する処理がされています。rcl_parse_arguments関数はrcl/src/rcl/arguments.cで定義されていますが、解説は省略します。
get_rcl_node_options()の戻り値の型はrcl_node_options_t構造体へのポインタです。次に、rcl_node_options_tの定義を見てみましょう。
node_options.h抜粋
typedef struct rcl_node_options_s
{
/// Custom allocator used for internal allocations.
rcl_allocator_t allocator;
/// If false then only use arguments in this struct, otherwise use global arguments also.
bool use_global_arguments;
/// Command line arguments that apply only to this node.
rcl_arguments_t arguments;
/// Flag to enable rosout for this node
bool enable_rosout;
/// Middleware quality of service settings for /rosout.
rmw_qos_profile_t rosout_qos;
} rcl_node_options_t;
注目したいのはrcl_arguments_t型であるargumentsです。
rcl_arguments_tの定義を見てみましょう。
rcl_arguments_t構造体はrcl_arguments_impl_s構造体のラッパーあることがわかります。
arguments.h抜粋
typedef struct rcl_arguments_impl_s rcl_arguments_impl_t;
/// Hold output of parsing command line arguments.
typedef struct rcl_arguments_s
{
/// Private implementation pointer.
rcl_arguments_impl_t * impl;
} rcl_arguments_t;
rcl_arguments_impl_s構造体には、下記が入っています。この構造体はROS引数を表す情報であることがわかります。
-
rcl_remap_t型へのポインタremap_rules -
rcl_params_t型へのポインタparameter_overrides- ROS引数の１つ
--params-file <yaml_file_path>で指定されたyamlファイルをparseした結果（ノードパラメータの初期値）が書き込まれている
- ROS引数の１つ
arguments_impl.h
typedef struct rcl_arguments_impl_s
{
/// Array of indices to unknown ROS specific arguments.
int * unparsed_ros_args;
/// Length of unparsed_ros_args.
int num_unparsed_ros_args;
/// Array of indices to non-ROS arguments.
int * unparsed_args;
/// Length of unparsed_args.
int num_unparsed_args;
/// Parameter override rules parsed from arguments.
rcl_params_t * parameter_overrides;
/// Array of yaml parameter file paths
char ** parameter_files;
/// Length of parameter_files.
int num_param_files_args;
/// Array of rules for name remapping.
rcl_remap_t * remap_rules;
/// Length of remap_rules.
int num_remap_rules;
/// Log levels parsed from arguments.
rcl_log_levels_t log_levels;
/// A file used to configure the external logging library
char * external_log_config_file;
/// A boolean value indicating if the standard out handler should be used for log output
bool log_stdout_disabled;
/// A boolean value indicating if the rosout topic handler should be used for log output
bool log_rosout_disabled;
/// A boolean value indicating if the external lib handler should be used for log output
bool log_ext_lib_disabled;
/// Enclave to be used.
char * enclave;
/// Allocator used to allocate objects in this struct
rcl_allocator_t allocator;
} rcl_arguments_impl_t;
ROSノードの深堀はとりあえずここまで見れば十分です。
Discussion