diff --git a/include/behaviortree_cpp/scripting/operators.hpp b/include/behaviortree_cpp/scripting/operators.hpp index 483bc7f4d..28f5065eb 100644 --- a/include/behaviortree_cpp/scripting/operators.hpp +++ b/include/behaviortree_cpp/scripting/operators.hpp @@ -15,9 +15,18 @@ #include "behaviortree_cpp/scripting/any_types.hpp" #include "behaviortree_cpp/scripting/script_parser.hpp" +#include +#include #include +#include +#include +#include +#include #include +#include +#include #include +#include #include // Naive implementation of an AST with simple evaluation function. @@ -27,6 +36,279 @@ using SimpleString = SafeAny::SimpleString; using expr_ptr = std::shared_ptr; +namespace detail +{ +// 2^53: every integer up to here is exact as a double. +constexpr double kMaxExactDouble = + static_cast(int64_t{ 1 } << std::numeric_limits::digits); + +inline std::string_view TrimSpaces(std::string_view str) +{ + const auto is_space = [](char c) { + return std::isspace(static_cast(c)) != 0; + }; + while(!str.empty() && is_space(str.front())) + { + str.remove_prefix(1); + } + while(!str.empty() && is_space(str.back())) + { + str.remove_suffix(1); + } + return str; +} + +// Whether str spells a decimal number: digits, an optional leading '-', one '.', and an +// exponent whose sign may be '+'. Checked before parsing so every standard library rejects the +// same strings ("0x10", "nan", "+5"). +inline bool IsDecimalNumberText(std::string_view str) +{ + bool digit_seen = false; + bool dot_seen = false; + bool exponent_seen = false; + for(size_t i = 0; i < str.size(); ++i) + { + const char c = str[i]; + const bool after_exponent = i > 0 && (str[i - 1] == 'e' || str[i - 1] == 'E'); + if(std::isdigit(static_cast(c)) != 0) + { + digit_seen = true; + } + else if(c == '-' && (i == 0 || after_exponent)) + { + } + else if(c == '+' && after_exponent) + { + } + else if(c == '.' && !dot_seen && !exponent_seen) + { + dot_seen = true; + } + else if((c == 'e' || c == 'E') && digit_seen && !exponent_seen) + { + exponent_seen = true; + digit_seen = false; + } + else + { + return false; + } + } + return digit_seen; +} + +// Locale-independent parsing for standard libraries without floating-point std::from_chars. +inline std::optional StreamParseNumber(std::string_view str) +{ + double out = 0; + std::istringstream stream{ std::string(str) }; + stream.imbue(std::locale::classic()); + stream >> out; + if(stream.fail() || stream.peek() != std::char_traits::eof()) + { + return std::nullopt; + } + return out; +} + +// Locale-independent shortest %g form that reads back exactly. +inline std::string StreamFormatNumber(double number) +{ + std::string text; + for(int precision = std::numeric_limits::digits10; + precision <= std::numeric_limits::max_digits10; ++precision) + { + std::ostringstream stream; + stream.imbue(std::locale::classic()); + stream << std::setprecision(precision) << number; + text = stream.str(); + if(StreamParseNumber(text) == number) + { + break; + } + } + return text; +} + +// The number a string holds, if all of it apart from surrounding spaces is a finite decimal +// number: "12", " 5 " and "-1.5e3" are numbers; "3 apples", "+5", "0x10", "nan" and "" are +// not. Any::stringToNumber accepts a numeric prefix, so scripts don't use it. +inline std::optional ParseWholeNumber(std::string_view text) +{ + const auto str = TrimSpaces(text); + if(!IsDecimalNumberText(str)) + { + return std::nullopt; + } + double out = 0; +#if __cpp_lib_to_chars >= 201611L + const char* end = str.data() + str.size(); + const auto [ptr, err] = std::from_chars(str.data(), end, out); + if(err != std::errc() || ptr != end) + { + return std::nullopt; + } +#else + if(auto parsed = StreamParseNumber(str)) + { + out = *parsed; + } + else + { + return std::nullopt; + } +#endif + if(!std::isfinite(out)) + { + return std::nullopt; + } + // Too small for a double: reject it on every standard library, as from_chars does. + const auto mantissa = str.substr(0, str.find_first_of("eE")); + if(out == 0 && mantissa.find_first_of("123456789") != std::string_view::npos) + { + return std::nullopt; + } + return out; +} + +// An integer string parsed exactly, so values above 2^53 keep every digit. +inline std::optional ParseWholeInteger(std::string_view text) +{ + const auto str = TrimSpaces(text); + int64_t out = 0; + const char* end = str.data() + str.size(); + const auto [ptr, err] = std::from_chars(str.data(), end, out); + if(str.empty() || err != std::errc() || ptr != end) + { + return std::nullopt; + } + return out; +} + +// The number a string holds for arithmetic. Integer text beyond 2^53 is an error, as it is for +// an int64 entry, because a double would silently round it. +inline std::optional ArithmeticString(std::string_view text) +{ + const auto number = ParseWholeNumber(text); + const auto str = TrimSpaces(text); + if(number && str.find_first_not_of("-0123456789") == std::string_view::npos) + { + // Checked on the exact integer: the double has already rounded it. + constexpr auto kMaxExactInteger = static_cast(kMaxExactDouble); + const auto exact = ParseWholeInteger(str); + if(!exact || *exact > kMaxExactInteger || *exact < -kMaxExactInteger) + { + throw RuntimeError(StrCat("Number ['", std::string(str), + "'] is too large to compute with exactly")); + } + } + return number; +} + +inline std::string_view StringView(const SimpleString& str) +{ + return { str.data(), str.size() }; +} + +inline std::string ShownOperand(const Any& operand) +{ + return operand.isString() ? StrCat("'", operand.cast(), "'") : + demangle(operand.type()); +} + +inline std::string NotANumberError(const Any& operand, const char* op_str) +{ + auto msg = StrCat("Operand [", ShownOperand(operand), "] of operator [", op_str, + "] is not a number"); + if(std::string_view(op_str) == "+" || std::string_view(op_str) == "+=") + { + msg += ". Use [..] to join a string with a number"; + } + return msg; +} + +// A number as a double, reported as a script error when a double can't hold it exactly +// (for example an int64 above 2^53). +inline double NumberToDouble(const Any& value) +{ + try + { + return value.cast(); + } + catch(const std::exception& err) + { + throw RuntimeError(err.what()); + } +} + +// The value of an arithmetic operand: a number, or a string that is entirely a number. +inline std::optional ArithmeticOperand(const Any& value) +{ + if(value.isString()) + { + return ArithmeticString(StringView(value.cast())); + } + if(value.isNumber()) + { + return NumberToDouble(value); + } + if(auto number = value.tryCast()) + { + return number.value(); + } + return std::nullopt; +} + +// The value of a bitwise operand as an exact integer. +inline int64_t BitwiseOperand(const Any& value, const char* op_str) +{ + try + { + if(value.isString()) + { + const auto owned = value.cast(); + const auto str = StringView(owned); + if(auto exact = ParseWholeInteger(str)) + { + return *exact; + } + if(auto number = ParseWholeNumber(str); + number && std::abs(*number) <= kMaxExactDouble) + { + return Any(*number).cast(); + } + } + else if(value.isNumber()) + { + return value.cast(); + } + } + catch(const std::exception&) + { + throw RuntimeError("Binary operators are not allowed if " + "one of the operands is not an integer"); + } + throw RuntimeError(NotANumberError(value, op_str)); +} + +// A string or number as `..` joins it. Whole doubles print as integers ("2", not "2.000000") +// up to 2^53; other doubles in the shortest %g form that reads back exactly ("0.0001"), the +// same on every standard library. +inline std::string ConcatOperandString(const Any& value) +{ + if(value.castedType() != typeid(double)) + { + return value.cast(); + } + const auto number = value.cast(); + if(std::trunc(number) == number && std::abs(number) <= kMaxExactDouble) + { + return std::to_string(static_cast(number)); + } + return StreamFormatNumber(number); +} +} // namespace detail + // extended string to number that consider enums and booleans inline double StringToDouble(const Any& value, const Environment& env) { @@ -47,7 +329,11 @@ inline double StringToDouble(const Any& value, const Environment& env) return it->second; } } - return value.cast(); + if(auto number = detail::ArithmeticString(str)) + { + return *number; + } + throw RuntimeError(StrCat("String ['", str, "'] is not a number")); } struct ExprBase @@ -121,9 +407,25 @@ struct ExprUnaryArithmetic : ExprBase Any evaluate(Environment& env) const override { auto rhs_v = rhs->evaluate(env); + if(op == complement && (rhs_v.isString() || rhs_v.isIntegral())) + { + return Any(static_cast(~detail::BitwiseOperand(rhs_v, "~"))); + } + if(rhs_v.isString()) + { + // A numeric string, for example from a SubTree port, takes part as the number it holds. + const auto number = + detail::ArithmeticString(detail::StringView(rhs_v.cast())); + if(!number) + { + throw RuntimeError(StrCat("Operand [", detail::ShownOperand(rhs_v), + "] of a unary operator is not a number")); + } + rhs_v = Any(*number); + } if(rhs_v.isNumber()) { - const double rv = rhs_v.cast(); + const double rv = detail::NumberToDouble(rhs_v); switch(op) { case negate: @@ -139,10 +441,6 @@ struct ExprUnaryArithmetic : ExprBase return Any(static_cast(!static_cast(rv))); } } - else if(rhs_v.isString()) - { - throw RuntimeError("Invalid operator for std::string"); - } throw RuntimeError("ExprUnaryArithmetic: undefined"); } }; @@ -213,93 +511,87 @@ struct ExprBinaryArithmetic : ExprBase throw RuntimeError(ErrorNotInit("right", opStr())); } - // If just one value is strongly typed as a number, and the other can be casted to one, do arithmetic. If both are not strongly typed, continue. In the case of `+`, non-strongly typed values will result in concatenation. - bool are_numbers = (rhs_v.isNumber() && lhs_v.isNumber()) || - ((rhs_v.tryCast().has_value() && lhs_v.isNumber()) != - (rhs_v.isNumber() && lhs_v.tryCast().has_value())); - if(are_numbers) + if(op == concat) { - auto lv = lhs_v.cast(); - auto rv = rhs_v.cast(); - - switch(op) + if((lhs_v.isString() || lhs_v.isNumber()) && (rhs_v.isString() || rhs_v.isNumber())) { - case plus: - return Any(lv + rv); - case minus: - return Any(lv - rv); - case times: - return Any(lv * rv); - case div: - return Any(lv / rv); - default: { - } + return Any(detail::ConcatOperandString(lhs_v) + + detail::ConcatOperandString(rhs_v)); } + const auto& bad = (lhs_v.isString() || lhs_v.isNumber()) ? rhs_v : lhs_v; + throw RuntimeError(StrCat("Operand [", detail::ShownOperand(bad), + "] of operator [..] is neither a string nor a number")); + } - if(op == bit_and || op == bit_or || op == bit_xor) - { - try - { - int64_t li = lhs_v.cast(); - int64_t ri = rhs_v.cast(); - switch(op) - { - case bit_and: - return Any(static_cast(li & ri)); - case bit_or: - return Any(static_cast(li | ri)); - case bit_xor: - return Any(static_cast(li ^ ri)); - default: { - } - } - } - catch(...) - { - throw RuntimeError("Binary operators are not allowed if " - "one of the operands is not an integer"); - } - } + // `+` joins two strings until PickNikRobotics/moveit_pro#24181 removes it. + if(op == plus && lhs_v.isString() && rhs_v.isString()) + { + return Any(lhs_v.cast() + rhs_v.cast()); + } - if(op == logic_or || op == logic_and) + if(op == bit_and || op == bit_or || op == bit_xor) + { + const int64_t li = detail::BitwiseOperand(lhs_v, opStr()); + const int64_t ri = detail::BitwiseOperand(rhs_v, opStr()); + switch(op) { - try - { - auto lb = lhs_v.cast(); - auto rb = rhs_v.cast(); - switch(op) - { - case logic_or: - return Any(static_cast(lb || rb)); - case logic_and: - return Any(static_cast(lb && rb)); - default: { - } - } - } - catch(...) - { - throw RuntimeError("Logic operators are not allowed if " - "one of the operands is not castable to bool"); - } + case bit_and: + return Any(static_cast(li & ri)); + case bit_or: + return Any(static_cast(li | ri)); + default: + return Any(static_cast(li ^ ri)); } } - else if(rhs_v.isString() && lhs_v.isString() && op == plus) + + const auto lv = detail::ArithmeticOperand(lhs_v); + if(!lv) { - return Any(lhs_v.cast() + rhs_v.cast()); + throw RuntimeError(detail::NotANumberError(lhs_v, opStr())); } - else if(op == concat && ((rhs_v.isString() && lhs_v.isString()) || - (rhs_v.isString() && lhs_v.isNumber()) || - (rhs_v.isNumber() && lhs_v.isString()))) + const auto rv = detail::ArithmeticOperand(rhs_v); + if(!rv) { - return Any(lhs_v.cast() + rhs_v.cast()); + throw RuntimeError(detail::NotANumberError(rhs_v, opStr())); } - else + switch(op) { - throw RuntimeError("Operation not permitted"); + case plus: + return Any(*lv + *rv); + case minus: + return Any(*lv - *rv); + case times: + return Any(*lv * *rv); + case div: + return Any(*lv / *rv); + default: { + } + } + + if(op == logic_or || op == logic_and) + { + try + { + auto lb = (lhs_v.isNumber() ? lhs_v : Any(*lv)).cast(); + auto rb = (rhs_v.isNumber() ? rhs_v : Any(*rv)).cast(); + switch(op) + { + case logic_or: + return Any(static_cast(lb || rb)); + case logic_and: + return Any(static_cast(lb && rb)); + default: { + } + } + } + catch(...) + { + throw RuntimeError("Logic operators are not allowed if " + "one of the operands is not castable to bool"); + } } - return {}; // unreachable + throw RuntimeError("Operation not permitted"); } }; @@ -402,8 +694,8 @@ struct ExprComparison : ExprBase if(lhs_v.isNumber() && rhs_v.isNumber()) { - auto lv = lhs_v.cast(); - auto rv = rhs_v.cast(); + auto lv = detail::NumberToDouble(lhs_v); + auto rv = detail::NumberToDouble(rhs_v); if(!SwitchImpl(lv, rv, ops[i])) { return False; @@ -413,7 +705,37 @@ struct ExprComparison : ExprBase { auto lv = lhs_v.cast(); auto rv = rhs_v.cast(); - if(!SwitchImpl(lv, rv, ops[i])) + // Two strings that are both numbers, for example from SubTree ports, compare as numbers, + // exactly: integers as int64, others as long double, which IsSame compares without the + // tolerance it applies to double. Strings compare as text only if one isn't a number. + const auto lview = detail::StringView(lv); + const auto rview = detail::StringView(rv); + const auto li = detail::ParseWholeInteger(lview); + const auto ri = detail::ParseWholeInteger(rview); + const auto ln = detail::ParseWholeNumber(lview); + const auto rn = detail::ParseWholeNumber(rview); + bool holds = false; + if(li && ri) + { + holds = SwitchImpl(*li, *ri, ops[i]); + } + else if(ln && rn && std::abs(*ln) <= detail::kMaxExactDouble && + std::abs(*rn) <= detail::kMaxExactDouble) + { + holds = SwitchImpl(static_cast(*ln), static_cast(*rn), + ops[i]); + } + else if(ln && rn) + { + throw RuntimeError(StrCat("Numbers ['", lv.toStdString(), "'] and ['", + rv.toStdString(), + "'] are too large to compare exactly")); + } + else + { + holds = SwitchImpl(lv, rv, ops[i]); + } + if(!holds) { return False; } @@ -421,7 +743,7 @@ struct ExprComparison : ExprBase else if(lhs_v.isString() && rhs_v.isNumber()) { auto lv = StringToDouble(lhs_v, env); - auto rv = rhs_v.cast(); + auto rv = detail::NumberToDouble(rhs_v); if(!SwitchImpl(lv, rv, ops[i])) { return False; @@ -429,7 +751,7 @@ struct ExprComparison : ExprBase } else if(lhs_v.isNumber() && rhs_v.isString()) { - auto lv = lhs_v.cast(); + auto lv = detail::NumberToDouble(lhs_v); auto rv = StringToDouble(rhs_v, env); if(!SwitchImpl(lv, rv, ops[i])) { @@ -460,9 +782,23 @@ struct ExprIf : ExprBase Any evaluate(Environment& env) const override { const auto& v = condition->evaluate(env); - bool valid = (v.isType() && v.cast().size() > 0) || - (v.cast() != 0.0); - if(valid) + double number = 0; + if(v.isString()) + { + const auto parsed = + detail::ArithmeticString(detail::StringView(v.cast())); + if(!parsed) + { + throw RuntimeError( + StrCat("Condition [", detail::ShownOperand(v), "] of [?:] is not a number")); + } + number = *parsed; + } + else + { + number = detail::NumberToDouble(v); + } + if(number != 0.0) { return then->evaluate(env); } @@ -580,7 +916,15 @@ struct ExprAssignment : ExprBase } else if(dst_ptr->isNumber()) { - auto num_value = StringToDouble(value, env); + double num_value = 0; + try + { + num_value = StringToDouble(value, env); + } + catch(const RuntimeError& err) + { + throw RuntimeError(StrCat(errorPrefix(), err.what())); + } *dst_ptr = Any(num_value); } else @@ -619,16 +963,28 @@ struct ExprAssignment : ExprBase // temporary use Any temp_variable = *dst_ptr; - if(value.isNumber()) + // `+=` joins two strings until PickNikRobotics/moveit_pro#24181 removes it. + if(op == assign_plus && temp_variable.isString() && value.isString()) + { + temp_variable = Any(temp_variable.cast() + value.cast()); + } + else { - if(!temp_variable.isNumber()) + const auto current = detail::ArithmeticOperand(temp_variable); + if(!current) + { + throw RuntimeError(StrCat(errorPrefix(), "Its value is not a number, so [", + opStr(), "] can't be applied")); + } + const auto operand = detail::ArithmeticOperand(value); + if(!operand) { - throw RuntimeError("This Assignment operator can't be used " - "with a non-numeric type"); + throw RuntimeError( + StrCat(errorPrefix(), detail::NotANumberError(value, opStr()))); } - auto lv = temp_variable.cast(); - auto rv = value.cast(); + const auto lv = *current; + const auto rv = *operand; switch(op) { case assign_plus: @@ -647,21 +1003,16 @@ struct ExprAssignment : ExprBase } } } - else if(value.isString()) + + if(temp_variable.isNumber() && dst_ptr->isString()) { - if(op == assign_plus) - { - auto lv = temp_variable.cast(); - auto rv = value.cast(); - temp_variable = Any(lv + rv); - } - else - { - throw RuntimeError("Operator not supported for strings"); - } + // The entry held a numeric string, so it keeps holding a string. + *dst_ptr = Any(detail::ConcatOperandString(temp_variable)); + } + else + { + temp_variable.copyInto(*dst_ptr); } - - temp_variable.copyInto(*dst_ptr); entry->sequence_id++; entry->stamp = std::chrono::steady_clock::now().time_since_epoch(); return *dst_ptr; diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp index f543de7bf..5adf48620 100644 --- a/tests/gtest_subtree.cpp +++ b/tests/gtest_subtree.cpp @@ -406,6 +406,41 @@ TEST(SubTree, ScriptingTypingIssue_StringConcatWorks) EXPECT_EQ(blackboard->get("y"), "1234"); } +// https://github.com/PickNikRobotics/moveit_pro/issues/24179 +TEST(SubTree, ScriptingTypingIssue_SubtractNumericStringPort) +{ + static const char* xml_text = R"( + + + + + + + + + +