diff --git a/plansys2_msgs/msg/Node.msg b/plansys2_msgs/msg/Node.msg index 469b03c3..cc969106 100644 --- a/plansys2_msgs/msg/Node.msg +++ b/plansys2_msgs/msg/Node.msg @@ -11,25 +11,27 @@ uint8 FUNCTION = 6 uint8 EXPRESSION = 7 uint8 FUNCTION_MODIFIER = 8 uint8 NUMBER = 9 -uint8 EXISTS = 10 +uint8 CONSTANT = 10 +uint8 PARAMETER = 11 +uint8 EXISTS = 12 # Expression types -uint8 COMP_EQ = 11 -uint8 COMP_GE = 12 -uint8 COMP_GT = 13 -uint8 COMP_LE = 14 -uint8 COMP_LT = 15 -uint8 ARITH_MULT = 16 -uint8 ARITH_DIV = 17 -uint8 ARITH_ADD = 18 -uint8 ARITH_SUB = 19 +uint8 COMP_EQ = 13 +uint8 COMP_GE = 14 +uint8 COMP_GT = 15 +uint8 COMP_LE = 16 +uint8 COMP_LT = 17 +uint8 ARITH_MULT = 18 +uint8 ARITH_DIV = 19 +uint8 ARITH_ADD = 20 +uint8 ARITH_SUB = 21 # Function modifier types -uint8 ASSIGN = 20 -uint8 INCREASE = 21 -uint8 DECREASE = 22 -uint8 SCALE_UP = 23 -uint8 SCALE_DOWN = 24 +uint8 ASSIGN = 22 +uint8 INCREASE = 23 +uint8 DECREASE = 24 +uint8 SCALE_UP = 25 +uint8 SCALE_DOWN = 26 uint8 node_type uint8 expression_type diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h index 10d2593c..f376b2f8 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h @@ -250,11 +250,23 @@ class ParamExpression : public Expression { } void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override { - s << ts[param]; + s << "?" < & replace = {} ) const override { - throw UnsupportedConstruct("ParamExpression"); + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::PARAMETER; + node->node_id = tree.nodes.size(); + plansys2_msgs::msg::Param node_param; + if (replace.size() > param) { + node->name = replace[param]; + } else { + node->name = "?" + std::to_string(param); + } + node_param.name = node->name; + node->parameters.push_back(node_param); + tree.nodes.push_back(*node); + return node; } double evaluate() { return -1; } @@ -276,8 +288,8 @@ class ConstExpression : public Expression { public: - int constant; - int tid; + int constant; + int tid; ConstExpression( int c, int t ) : constant( c ), tid (t) {} std::string info() const { @@ -288,14 +300,12 @@ class ConstExpression : public Expression { void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - throw UnsupportedConstruct("ConstExpression"); - } + plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; double evaluate() { return -1; } double evaluate( Instance & ins, const StringVec & par ) { - return evaluate(); + return evaluate(); } IntSet params() { diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h index c8a74af7..f0449801 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h @@ -100,6 +100,10 @@ std::string toStringExpression(const plansys2_msgs::msg::Tree & tree, uint32_t n std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); +std::string toStringConstant(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); + +std::string toStringParameter(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); + std::string toStringExists(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); /// This function creates a complete tree. diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp index d4e13e03..185152e5 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp @@ -60,8 +60,17 @@ double FunctionExpression::evaluate( Instance & ins, const StringVec & par ) { void ConstExpression::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - s << d.types[tid]->constants[constant]; - } + s << d.types[tid]->constants[constant]; +} + +plansys2_msgs::msg::Node::SharedPtr ConstExpression::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::CONSTANT; + node->node_id = tree.nodes.size(); + node->name = d.types[tid]->constants[constant]; + tree.nodes.push_back(*node); + return node; +} Expression * createExpression( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { f.next(); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp index 0efd960c..3d2488a1 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp @@ -4,7 +4,7 @@ namespace parser { namespace pddl { void ParamCond::printParams( unsigned first, std::ostream & s, TokenStruct< std::string > & ts, const Domain & d ) const { - s << "("; + s << "("; for ( unsigned i = first; i < params.size(); ++i ) { std::stringstream ss; ss << "?" << d.types[params[i]]->getName() << ts.size(); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp index ed702350..e76ed32f 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp @@ -448,6 +448,11 @@ std::string toString(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bo case plansys2_msgs::msg::Node::FUNCTION_MODIFIER: ret = toStringFunctionModifier(tree, node_id, negate); break; + case plansys2_msgs::msg::Node::CONSTANT: + ret = toStringConstant(tree, node_id, negate); + break; + case plansys2_msgs::msg::Node::PARAMETER: + ret = toStringParameter(tree, node_id, negate); case plansys2_msgs::msg::Node::EXISTS: ret = toStringExists(tree, node_id, negate); break; @@ -693,6 +698,24 @@ std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint return ret; } +std::string toStringConstant(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) +{ + if (node_id >= tree.nodes.size()) { + return {}; + } + + return tree.nodes[node_id].name; +} + +std::string toStringParameter(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) +{ + if (node_id >= tree.nodes.size()) { + return {}; + } + + return tree.nodes[node_id].parameters[0].name; +} + std::string toStringExists(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) { if (node_id >= tree.nodes.size()) { diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp index 6adf5a18..ac7f7419 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp @@ -41,7 +41,7 @@ namespace plansys2 * \param[in] negate Invert the truth value. * \return result <- tuple(bool, bool, double) * result(0) true if success - * result(1) truth value of boolen expression + * result(1) truth value of boolean expression * result(2) value of numeric expression */ std::tuple evaluate( diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp index 4c969694..e2ed0886 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp @@ -200,14 +200,27 @@ std::tuple evaluate( return std::make_tuple(true, negate ^ false, 0); } break; - case plansys2_msgs::msg::Node::COMP_EQ: - if (std::get<2>(left) == std::get<2>(right)) { - return std::make_tuple(true, negate ^ true, 0); - } else { - return std::make_tuple(true, negate ^ false, 0); + case plansys2_msgs::msg::Node::COMP_EQ: { + auto c_t = plansys2_msgs::msg::Node::CONSTANT; + auto p_t = plansys2_msgs::msg::Node::PARAMETER; + auto n_t = plansys2_msgs::msg::Node::NUMBER; + auto c0 = tree.nodes[tree.nodes[node_id].children[0]]; + auto c1 = tree.nodes[tree.nodes[node_id].children[1]]; + auto c0_type = c0.node_type; + auto c1_type = c1.node_type; + if ((c0_type == c_t || c0_type == p_t) && (c1_type == c_t || c1_type == p_t)) { + std::string c0_name = (c0_type == p_t) ? c0.parameters[0].name : c0.name; + std::string c1_name = (c1_type == p_t) ? c1.parameters[0].name : c1.name; + return std::make_tuple( + true, + negate ^ ( c1_name == c0_name), + 0); + } + if (c0_type == n_t && c1_type == n_t) { + return std::make_tuple(true, negate ^ std::get<2>(left) == std::get<2>(right), 0); + } + break; } - break; - case plansys2_msgs::msg::Node::ARITH_MULT: return std::make_tuple(true, false, std::get<2>(left) * std::get<2>(right)); break; @@ -302,6 +315,21 @@ std::tuple evaluate( return std::make_tuple(true, true, tree.nodes[node_id].value); } + case plansys2_msgs::msg::Node::CONSTANT: { + if (tree.nodes[node_id].name.size() > 0) { + return std::make_tuple(true, true, 0); + } + return std::make_tuple(true, false, 0); + } + + case plansys2_msgs::msg::Node::PARAMETER: { + if (tree.nodes[node_id].parameters.size() > 0 && + tree.nodes[node_id].parameters[0].name.front() != '?') + { + return std::make_tuple(true, true, 0); + } + } + case plansys2_msgs::msg::Node::EXISTS: { std::vector instances; if (use_state == false) {