ROS 2 param expression の cross-reference 実装 (muParser)

要点: ROS 2 param の値が数式 ("a * 2 + b") で、その式が同 node の他 param を参照できる仕組み。実装には 4 つの落とし穴 (muParser の AST cache、post-set callback で get_parameter が古い値を返す、cycle detection、built-in constants) がある。

動く形#

node_name:
  ros__parameters:
    r_wheel: 0.05
    v_max_rps: 30.0
    v_max_m_s: "r_wheel * v_max_rps * 2 * _pi"   # ← 他 param 参照

r_wheel を ros2 param set で変えると v_max_m_s も自動で再計算される。

前提: parameter_expression (ForteFibre)#

ROS 2 の param 値に muParser で数式を書けるようにする既存 package parameter_expression。ここでは cross-reference (他 param 参照) を追加する pattern を扱う。

実装の 4 ポイント#

① muParser::SetVarFactory で未知変数を param に解決#

muParser は expression 内の未知識別子を「変数」として扱う。SetVarFactory に factory 関数を登録すると、未知識別子を見つけた時に呼ばれる。ここで node_parameters_interface_->get_parameter(name) を呼んで param 値を返せば、expression が同 node の他 param を参照できる。

parser_.SetVarFactory(&var_factory, this);

static double * var_factory(const mu::char_type * name, void * user_data) {
  return static_cast<ParameterExpression*>(user_data)->resolve_variable(name);
}

muParser の built-in 定数 (_pi, _e) は factory を経由せず直接展開されるので影響しない。

② Parser::ClearVar() で AST cache を強制無効化#

muParser は同じ expression string に対して parse 結果 (AST) を cache し、次回 Eval では var_factory を再呼び出ししない。この場合、cache された variable pointer が古い値を指し続けて再評価が効かない。

対策: 各 Eval の前に ClearVar() を呼んで variable table を空にする。SetExpr で再 parse され、var_factory が呼ばれ直す。

ref_values_.clear();           // ref values を invalidate
parser_.ClearVar();            // ← これが無いと変数が古いまま
parser_.SetExpr(expression);
value_ = parser_.Eval();

③ 依存 param 変更時の再評価は post_set_parameters_callback#

add_on_set_parameters_callback (pre-set) は commit 前に呼ばれるため、参照先 param の get_parameter は古い値を返す。cross-ref の再評価は commit 後の add_post_set_parameters_callback を使う。

// pre-set: 自分の expression の validation にだけ使う
callback_handle_ = node->add_on_set_parameters_callback(&on_pre);

// post-set: 依存 param 変更時の再評価
post_callback_handle_ = node->add_post_set_parameters_callback(&on_post);

④ post-set 内でも get_parameter は古い場合がある#

同じ batch で set された param について、post-set callback 内から get_parameter(name) を呼んでも古い値が返るケースが観測される (rclcpp 実装の詳細による)。

対策: post-set callback の引数 std::vector<rclcpp::Parameter> を snapshot として持ち、resolve_variable がまずそこを見てから get_parameter に fallback する。

void on_post_parameter(const std::vector<rclcpp::Parameter> & parameters) {
  pending_snapshot_ = &parameters;  // resolve_variable が優先参照
  eval(node_parameters_interface_->get_parameter(name_));
  pending_snapshot_ = nullptr;
}

double * resolve_variable(const std::string & name) {
  if (pending_snapshot_ != nullptr) {
    for (const auto & p : *pending_snapshot_) {
      if (p.get_name() == name) return &(ref_values_[name] = to_double(p));
    }
  }
  // fallback: get_parameter
  auto param = node_parameters_interface_->get_parameter(name);
  ...
}

Cycle detection#

a = "b + 1", b = "a + 1" のような循環を検出する。thread_local の std::set<std::string> で「今解決中の param 名」を track、resolve_variable の入口で自分の名前が既にあれば mu::ParserError を throw。

thread_local にする理由: recursive resolve (expression 内から string 型 param を再帰評価) が複数 ParameterExpression インスタンスを跨いで走る可能性があり、instance ごとの stack だと cycle を見逃す。

static thread_local std::set<std::string> resolving_;

double * resolve_variable(const std::string & name) {
  if (resolving_.count(name)) throw mu::ParserError("Circular dep: " + name);
  // ... resolve ...
}

参照 param が string (recursive)#

a: 5, b: "a + 1", c: "b * 10" の chain を許す。resolve_variable が値を取り出す時、param 型が string なら scratch parser で eval して数値化:

if (ty == PARAMETER_STRING) {
  resolving_.insert(name);
  mu::Parser sub;
  sub.SetVarFactory(&var_factory, this);  // 同じ factory 共有
  sub.SetExpr(param.as_string());
  resolved = sub.Eval();
  resolving_.erase(name);
}

sub-parser を分けるのは、自 instance の parser の SetExpr を上書きしないため。

muParser 変数ポインタの寿命#

SetVarFactory が返す double * は muParser の内部変数 map に登録され、以後の Eval で参照される。stable な pointer を返す必要がある。std::map の要素へのポインタは insertion で無効化されないので、std::map<std::string, double> ref_values_ を backing store にして &ref_values_[name] を返すのが安全。

ref_values_[name] = resolved;
return &ref_values_[name];

std::vector は resize で pointer 無効化するので不可。

依存#

• muParser (libmuparser-dev) — mathematical expression parser
• rclcpp — parameter interface

適用対象#

• 車輪半径・減速比・エンコーダ ppr など、同 node 内で相互依存する param
• URDF joint limit を param に流し込んだ後、それを他 param が参照する場合

他 node の param 参照 はこの pattern の範囲外。cross-node は ParameterEventHandler + AsyncParametersClient で別途構築 (起動順・fault 対応が複雑)。多くは launch-time injection (URDF から Python launch で計算 → 各 node に配布) で回避できる。

関連#

• muParser 公式ドキュメント — SetVarFactory / ClearVar / GetUsedVar
• ROS 2 の add_post_set_parameters_callback — pre-set と post-set の使い分け