Merge pull request #324 from Rezenders/param_const_expression

Add support for ParamExpression and ConstExpression
PlanSys2 · Oct 2, 2024 · 8610ec6 · 8610ec6
2 parents b9b14c9 + 7c8f73e
commit 8610ec6
Show file tree

Hide file tree

Showing 8 changed files with 110 additions and 34 deletions.
diff --git 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
@@ -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 << "?" <<param;
 	}
 
 	plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace = {} ) const override {
-		throw UnsupportedConstruct("ParamExpression");
+    plansys2_msgs::msg::Node::SharedPtr node = std::make_shared<plansys2_msgs::msg::Node>();
+    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<std::string> & replace = {} ) const override {
-		throw UnsupportedConstruct("ConstExpression");
-	}
+	plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & 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
@@ -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
@@ -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<std::string> & replace) const {
+  plansys2_msgs::msg::Node::SharedPtr node = std::make_shared<plansys2_msgs::msg::Node>();
+  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
@@ -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
@@ -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
@@ -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<bool, bool, double> evaluate(

diff --git 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<bool, bool, double> 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<bool, bool, double> 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<plansys2::Instance> instances;
         if (use_state == false) {