diff --git a/include/parameter_expression/parameter_expression.hpp b/include/parameter_expression/parameter_expression.hpp index 6bd8586..f071123 100644 --- a/include/parameter_expression/parameter_expression.hpp +++ b/include/parameter_expression/parameter_expression.hpp @@ -16,13 +16,35 @@ #include +#include #include #include #include +#include +#include namespace parameter_expression { -// ROS 2 dynamic typed parameter with mathematical expression +// ROS 2 dynamic typed parameter with mathematical expression. +// +// Value types: +// - int / double : returned as-is +// - string : parsed by muParser, evaluated +// +// Cross-parameter references (feature/cross-param-reference): +// Expressions may reference other parameters on the SAME node by name. +// Example yaml: +// r_wheel: 0.05 +// v_max_rps: 30.0 +// v_max_m_s: "r_wheel * v_max_rps * 2 * _pi" +// +// Referenced parameters may themselves be expressions (recursive). +// Circular dependencies (a->b->a) are detected and throw at eval time. +// When a referenced parameter changes, this expression re-evaluates +// automatically via the shared on_set_parameters_callback. +// +// Built-in muParser constants (_pi, _e) are not treated as parameters. +// Standard functions (sin, cos, atan2, ...) work as before. class ParameterExpression { public: @@ -49,16 +71,54 @@ class ParameterExpression private: rcl_interfaces::msg::SetParametersResult on_parameter(const std::vector &); + // Post-set: re-eval when a parameter we depend on has been committed. + void on_post_parameter(const std::vector &); void eval(const rclcpp::Parameter parameter_value); + // Optional lookup map used during eval to resolve variables from a snapshot + // of pending param values (populated by post-set callback). Fixes the case + // where NodeParametersInterface::get_parameter still returns stale values + // inside post-set for the same batch that triggered the callback. + const std::vector * pending_snapshot_{nullptr}; + void eval_first(); + // muParser variable factory. Called for each unknown variable name in the + // expression. Routes to `this->resolve_variable(name)`. `user_data` is `this`. + static double * var_factory(const mu::char_type * name, void * user_data); + + // Resolve a variable name by looking up a ROS parameter on the same node. + // If the parameter itself is a string expression, evaluate it recursively. + // Throws mu::ParserError if not found or on circular dependency. + double * resolve_variable(const std::string & name); + + // Evaluate an arbitrary expression string using a scratch parser that + // shares this->resolve_variable via var_factory. Used to unpack a + // referenced parameter that is itself a string expression. + double eval_sub_expression(const std::string & expression); + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handle_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_callback_handle_; rclcpp::Parameter parameter_; const std::string name_; mu::Parser parser_; double value_; const double default_value_; + + // Backing storage for muParser DefineVar. muParser holds pointers into + // this map; std::map guarantees pointers to existing elements remain + // valid across insertions. + std::map ref_values_; + + // Names of ROS parameters this expression currently depends on. Populated + // after each successful Eval via GetUsedVar. Used by on_parameter to + // re-evaluate when any of them changes. + std::set ref_names_; + + // Cycle detection stack. thread_local because recursive eval_sub_expression + // can traverse chains of expressions across multiple ParameterExpression + // instances that share the resolve path. + static thread_local std::set resolving_; }; -} // namespace parameter_expression \ No newline at end of file +} // namespace parameter_expression diff --git a/src/parameter_expression.cpp b/src/parameter_expression.cpp index 768e613..0b7c5fe 100644 --- a/src/parameter_expression.cpp +++ b/src/parameter_expression.cpp @@ -18,6 +18,9 @@ namespace parameter_expression { + +thread_local std::set ParameterExpression::resolving_; + ParameterExpression::ParameterExpression( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface, const std::string & name, const double default_value, @@ -33,8 +36,21 @@ ParameterExpression::ParameterExpression( rcl_interfaces::msg::ParameterDescriptor(descriptor).set__dynamic_typing(true); const auto empty_value = rclcpp::ParameterValue(); node_parameters_interface_->declare_parameter(name, empty_value, parameter_descriptor); + + // Route unknown variables in expressions to the ROS parameter registry via + // this->resolve_variable. User data carries `this`. + parser_.SetVarFactory(&ParameterExpression::var_factory, this); + callback_handle_ = node_parameters_interface_->add_on_set_parameters_callback( std::bind(&ParameterExpression::on_parameter, this, _1)); + + // Post-set callback: called AFTER a parameter set commits. Used to catch + // "a param we depend on changed" and re-eval with the fresh value. + // (The pre-set callback above returns stale get_parameter() results for + // params other than the one being set, so it can't handle dep changes.) + post_callback_handle_ = node_parameters_interface_->add_post_set_parameters_callback( + std::bind(&ParameterExpression::on_post_parameter, this, _1)); + eval_first(); } @@ -49,6 +65,10 @@ void ParameterExpression::eval_first() rcl_interfaces::msg::SetParametersResult ParameterExpression::on_parameter( const std::vector & parameters) { + // Pre-set callback: only validate own parameter changes. + // Dep-change re-eval happens in on_post_parameter (post-set) where the + // referenced parameters are already committed and get_parameter returns + // the new value. for (const auto & parameter : parameters) { if (parameter.get_name() == name_) { try { @@ -67,33 +87,161 @@ rcl_interfaces::msg::SetParametersResult ParameterExpression::on_parameter( return rcl_interfaces::msg::SetParametersResult().set__successful(true); } +void ParameterExpression::on_post_parameter(const std::vector & parameters) +{ + // Fires after commit. If any of our current deps just changed, re-eval. + // Pass `parameters` as an override snapshot so resolve_variable sees the + // fresh values for the pending batch (get_parameter can lag inside + // post-set on some rclcpp versions). + bool dep_touched = false; + for (const auto & parameter : parameters) { + if (parameter.get_name() == name_) continue; + if (ref_names_.count(parameter.get_name()) > 0) { + dep_touched = true; + break; + } + } + if (!dep_touched) return; + pending_snapshot_ = ¶meters; + try { + eval(node_parameters_interface_->get_parameter(name_)); + } catch (...) { + // Post-set can't reject; best effort. Prior value_ stays valid. + } + pending_snapshot_ = nullptr; +} + void ParameterExpression::eval(const rclcpp::Parameter parameter_value) { using ParameterType = rcl_interfaces::msg::ParameterType; const auto ty = parameter_value.get_type(); if (ty == ParameterType::PARAMETER_NOT_SET) { value_ = default_value_; + ref_names_.clear(); + ref_values_.clear(); return; } - // If the parameter is double or integer, return the value directly if (ty == ParameterType::PARAMETER_INTEGER) { value_ = static_cast(parameter_value.as_int()); + ref_names_.clear(); + ref_values_.clear(); return; } if (ty == ParameterType::PARAMETER_DOUBLE) { value_ = parameter_value.as_double(); + ref_names_.clear(); + ref_values_.clear(); return; } - // If the parameter is string, parse the expression if (ty != ParameterType::PARAMETER_STRING) { throw std::runtime_error("Parameter type is not string"); } const auto expression = parameter_value.as_string(); - parser_.SetExpr(expression); - value_ = parser_.Eval(); + // Fresh backing storage and dependency set; both re-populated via + // var_factory / resolve_variable during the Eval below. + // ClearVar() forces muParser to re-parse and re-call var_factory even when + // the expression string is unchanged (otherwise it caches the parsed AST + // and keeps pointers to previously returned ref_values_ addresses, which + // we invalidate here). + ref_values_.clear(); + ref_names_.clear(); + parser_.ClearVar(); + + // Cycle guard: mark self as being resolved so any recursion back to name_ + // through resolve_variable trips the check. + resolving_.insert(name_); + try { + parser_.SetExpr(expression); + value_ = parser_.Eval(); + } catch (...) { + resolving_.erase(name_); + throw; + } + resolving_.erase(name_); + + // Note: ref_names_ is populated by resolve_variable during the Eval above + // (via var_factory). GetUsedVar() reports parser-known variables including + // those registered by var_factory, so it would also work; but keeping the + // set built incrementally in resolve_variable avoids relying on that + // implementation detail. +} + +double * ParameterExpression::var_factory(const mu::char_type * name, void * user_data) +{ + auto * self = static_cast(user_data); + return self->resolve_variable(name); +} + +double * ParameterExpression::resolve_variable(const std::string & name) +{ + if (resolving_.count(name) > 0) { + throw mu::ParserError(std::string("Circular dependency in parameter expression: ") + name); + } + + rclcpp::Parameter param; + bool from_snapshot = false; + if (pending_snapshot_ != nullptr) { + // Post-set callback path: the referenced param may be one of the + // parameters currently being committed. get_parameter can return stale + // values in this context, so check the snapshot first. + for (const auto & p : *pending_snapshot_) { + if (p.get_name() == name) { + param = p; + from_snapshot = true; + break; + } + } + } + if (!from_snapshot) { + try { + param = node_parameters_interface_->get_parameter(name); + } catch (const std::exception & e) { + throw mu::ParserError( + std::string("Referenced parameter '") + name + "' not found: " + e.what()); + } + } + + double resolved; + using ParameterType = rcl_interfaces::msg::ParameterType; + const auto ty = param.get_type(); + if (ty == ParameterType::PARAMETER_NOT_SET) { + resolved = 0.0; + } else if (ty == ParameterType::PARAMETER_INTEGER) { + resolved = static_cast(param.as_int()); + } else if (ty == ParameterType::PARAMETER_DOUBLE) { + resolved = param.as_double(); + } else if (ty == ParameterType::PARAMETER_STRING) { + resolving_.insert(name); + try { + resolved = eval_sub_expression(param.as_string()); + } catch (...) { + resolving_.erase(name); + throw; + } + resolving_.erase(name); + } else { + throw mu::ParserError( + std::string("Referenced parameter '") + name + + "' has unsupported type (must be int/double/string)"); + } + + ref_values_[name] = resolved; + ref_names_.insert(name); + return &ref_values_[name]; +} + +double ParameterExpression::eval_sub_expression(const std::string & expression) +{ + // Scratch parser so recursion does not clobber this->parser_'s SetExpr. + // Same var_factory so nested references still route through the cycle + // detection above. + mu::Parser sub; + sub.SetVarFactory(&ParameterExpression::var_factory, this); + sub.SetExpr(expression); + return sub.Eval(); } double ParameterExpression::get() const { return value_; } -} // namespace parameter_expression \ No newline at end of file +} // namespace parameter_expression diff --git a/test/test_parameter_expression.cpp b/test/test_parameter_expression.cpp index d78ee24..c4c5d99 100644 --- a/test/test_parameter_expression.cpp +++ b/test/test_parameter_expression.cpp @@ -162,3 +162,95 @@ TEST_F(OptionsPassedParameterTest, testError) // exception EXPECT_THROW(test("1+", 0), mu::Parser::exception_type); } + +// ---- Cross-parameter reference tests ---- +// Uses per-test node without executor (mirrors OptionsPassedParameterTest). +class CrossParamTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) rclcpp::init(0, nullptr); + } + void TearDown() override + { + exprs_.clear(); + node_.reset(); + } + + void setup( + const std::vector> & overrides, + const std::vector & names) + { + rclcpp::NodeOptions opts; + for (const auto & [n, v] : overrides) opts.append_parameter_override(n, v); + node_ = std::make_shared("cross_param_receiver", opts); + for (const auto & n : names) { + exprs_.push_back(std::make_shared(node_, n, 0.0)); + } + rclcpp::spin_some(node_); + } + + rclcpp::Node::SharedPtr node_; + std::vector exprs_; +}; + +// Simplest cross-ref: y = x * 2, x = 3. +TEST_F(CrossParamTest, resolveOtherParam) +{ + setup( + {{"x", rclcpp::ParameterValue(3.0)}, {"y", rclcpp::ParameterValue(std::string("x * 2"))}}, + {"x", "y"}); + EXPECT_DOUBLE_EQ(exprs_[0]->get(), 3.0); + EXPECT_DOUBLE_EQ(exprs_[1]->get(), 6.0); +} + +// Recursive: c depends on b, b depends on a. +TEST_F(CrossParamTest, transitiveResolution) +{ + setup( + {{"a", rclcpp::ParameterValue(2.0)}, + {"b", rclcpp::ParameterValue(std::string("a + 1"))}, + {"c", rclcpp::ParameterValue(std::string("b * 10"))}}, + {"a", "b", "c"}); + EXPECT_DOUBLE_EQ(exprs_[0]->get(), 2.0); + EXPECT_DOUBLE_EQ(exprs_[1]->get(), 3.0); + EXPECT_DOUBLE_EQ(exprs_[2]->get(), 30.0); +} + +// Change a plain param -> expression that references it re-evaluates. +TEST_F(CrossParamTest, reEvalOnDepChange) +{ + using namespace std::chrono_literals; + setup( + {{"x", rclcpp::ParameterValue(3.0)}, {"y", rclcpp::ParameterValue(std::string("x * 2"))}}, + {"x", "y"}); + EXPECT_DOUBLE_EQ(exprs_[1]->get(), 6.0); + // change x + node_->set_parameter(rclcpp::Parameter("x", 5.0)); + rclcpp::spin_some(node_); + EXPECT_DOUBLE_EQ(exprs_[1]->get(), 10.0); +} + +// Circular dependency raises on set (both params declared with plain values +// first, then set to circular expressions). +TEST_F(CrossParamTest, circularDependencyRejected) +{ + setup({{"a", rclcpp::ParameterValue(1.0)}, {"b", rclcpp::ParameterValue(2.0)}}, {"a", "b"}); + // Now put them into a cycle. First a := "b + 1" (OK, b=2 → a=3), then + // b := "a + 1" (would form a→b→a). + auto r1 = node_->set_parameter(rclcpp::Parameter("a", std::string("b + 1"))); + EXPECT_TRUE(r1.successful); + auto r2 = node_->set_parameter(rclcpp::Parameter("b", std::string("a + 1"))); + EXPECT_FALSE(r2.successful); +} + +// Built-in constants and functions still work when the expression has no +// cross-references (regression check for SetVarFactory not breaking _pi). +// Tolerance is loose (1e-9) because muParser's _pi is not bit-identical to +// M_PI. What we care about is: it's NOT treated as a missing parameter. +TEST_F(CrossParamTest, builtinConstantsStillWork) +{ + setup({{"g", rclcpp::ParameterValue(std::string("2.0 * _pi"))}}, {"g"}); + EXPECT_NEAR(exprs_[0]->get(), 2.0 * M_PI, 1e-9); +}