From d6f2dd8b5d716d704a27445a61574f560638cd87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 4 Mar 2025 10:25:24 +0100 Subject: [PATCH 01/13] Merge pull request #283 from PickNikRobotics/add-imply-parsing Add Imply to PDDL domain parsing (cherry picked from commit 30e3e266886542100f84a2be1c82db78450b538a) --- .../include/plansys2_pddl_parser/Imply.h | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h new file mode 100644 index 000000000..cb27ffec3 --- /dev/null +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h @@ -0,0 +1,49 @@ + +#pragma once + +#include "plansys2_msgs/msg/node.hpp" +#include "plansys2_msgs/msg/tree.hpp" + +#include "plansys2_pddl_parser/ParamCond.h" + +namespace parser { namespace pddl { + +class Imply : public ParamCond { + +public: + Condition * cond; + + Imply() + : cond( 0 ) {} + + Imply( const Imply * f, Domain & d ) + : ParamCond( f ), cond( 0 ) { + if ( f->cond ) cond = f->cond->copy( d ); + } + + ~Imply() { + if ( cond ) delete cond; + } + + void print( std::ostream & s ) const { + s << "Imply" << params << ":\n"; + if ( cond ) cond->print( s ); + } + + 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; + + void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + + void addParams( int m, unsigned n ) { + cond->addParams( m, n ); + } + + Condition * copy( Domain & d ) { + return new Imply( this, d ); + } + +}; + +} } // namespaces From 5ecb07407e2394a7320c3332bbd9ea6f0b527251 Mon Sep 17 00:00:00 2001 From: Eshan Savla Date: Tue, 4 Mar 2025 10:28:15 +0100 Subject: [PATCH 02/13] refactor imply class to support two conditions --- .../include/plansys2_pddl_parser/Imply.h | 19 ++++++---- .../src/plansys2_pddl_parser/Imply.cpp | 37 +++++++++---------- 2 files changed, 30 insertions(+), 26 deletions(-) diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h index cb27ffec3..615890180 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h @@ -11,23 +11,27 @@ namespace parser { namespace pddl { class Imply : public ParamCond { public: - Condition * cond; + Condition * cond_1; // The initial condition of imply + Condition * cond_2; // The latter condition of imply Imply() - : cond( 0 ) {} + : cond_1( 0 ), cond_2( 0 ) {} Imply( const Imply * f, Domain & d ) - : ParamCond( f ), cond( 0 ) { - if ( f->cond ) cond = f->cond->copy( d ); + : ParamCond( f ), cond_1( 0 ), cond_2( 0 ) { + if ( f->cond_1 ) cond_1 = f->cond_1->copy( d ); + if ( f->cond_2 ) cond_2 = f->cond_2->copy( d ); } ~Imply() { - if ( cond ) delete cond; + if ( cond_1 ) delete cond_1; // bc new creation + if ( cond_2 ) delete cond_2; } void print( std::ostream & s ) const { s << "Imply" << params << ":\n"; - if ( cond ) cond->print( s ); + if ( cond_1 ) cond_1->print( s ); + if( cond_2 ) cond_2->print( s ); } void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; @@ -37,7 +41,8 @@ class Imply : public ParamCond { void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); void addParams( int m, unsigned n ) { - cond->addParams( m, n ); + cond_1->addParams( m, n ); + cond_2->addParams( m, n ); } Condition * copy( Domain & d ) { diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index aed6529e0..a475ec027 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -29,12 +29,12 @@ void Imply::PDDLPrint( tabindent(s, indent + 1); printParams(0, s, fstruct, d); - if (cond) { - cond->PDDLPrint(s, indent + 1, fstruct, d); - } else { - tabindent(s, indent + 1); - s << "()"; - } + if ( cond_1 ) cond_1->PDDLPrint( s, indent + 1, fstruct, d ); + if (cond_2) cond_2->PDDLPrint( s, indent + 1, fstruct, d ); + else { + tabindent( s, indent + 1 ); + s << "()"; + } s << "\n"; tabindent(s, indent); @@ -52,20 +52,19 @@ void Imply::parse(Stringreader & f, TokenStruct & ts, Domain & d) f.next(); f.assert_token("("); - TokenStruct fs = f.parseTypedList(true, d.types); - params = d.convertTypes(fs.types); + if (f.getChar() != ')' ) { + cond_1 = d.createCondition( f ); + cond_1->parse( f, ts, d ); + } + else ++f.c; - TokenStruct fstruct(ts); - fstruct.append(fs); - - f.next(); - f.assert_token("("); - if (f.getChar() != ')') { - cond = d.createCondition(f); - cond->parse(f, fstruct, d); - } else { - ++f.c; - } + f.next(); + f.assert_token( "(" ); + if ( f.getChar() != ')' ) { + cond_2 = d.createCondition( f ); + cond_2->parse( f, ts, d ); + } + else ++f.c; f.next(); f.assert_token(")"); From d20fd977d88d733a9907568d395532ce1d74920a Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:06:43 +0100 Subject: [PATCH 03/13] reworked method to optionally accept instances --- .../include/plansys2_pddl_parser/Action.hpp | 2 +- .../include/plansys2_pddl_parser/And.hpp | 2 +- .../plansys2_pddl_parser/Condition.hpp | 2 +- .../include/plansys2_pddl_parser/Derived.hpp | 2 +- .../include/plansys2_pddl_parser/Exists.hpp | 2 +- .../plansys2_pddl_parser/Expression.hpp | 6 ++-- .../include/plansys2_pddl_parser/Forall.hpp | 2 +- .../include/plansys2_pddl_parser/Function.hpp | 2 +- .../plansys2_pddl_parser/FunctionModifier.hpp | 2 +- .../include/plansys2_pddl_parser/Ground.hpp | 2 +- .../plansys2_pddl_parser/GroundFunc.hpp | 2 +- .../include/plansys2_pddl_parser/Imply.h | 2 +- .../include/plansys2_pddl_parser/Imply.hpp | 2 +- .../include/plansys2_pddl_parser/Lifted.hpp | 2 +- .../include/plansys2_pddl_parser/Not.hpp | 2 +- .../include/plansys2_pddl_parser/Oneof.hpp | 2 +- .../include/plansys2_pddl_parser/Or.hpp | 2 +- .../include/plansys2_pddl_parser/Task.hpp | 2 +- .../plansys2_pddl_parser/TemporalAction.hpp | 2 +- .../plansys2_pddl_parser/TypeGround.hpp | 2 +- .../include/plansys2_pddl_parser/When.hpp | 2 +- .../src/plansys2_pddl_parser/Action.cpp | 30 ++++++++-------- .../src/plansys2_pddl_parser/And.cpp | 20 +++++------ .../src/plansys2_pddl_parser/Derived.cpp | 6 ++-- .../src/plansys2_pddl_parser/Exists.cpp | 4 +-- .../src/plansys2_pddl_parser/Expression.cpp | 34 +++++++++---------- .../src/plansys2_pddl_parser/Forall.cpp | 6 ++-- .../src/plansys2_pddl_parser/Function.cpp | 6 ++-- .../plansys2_pddl_parser/FunctionModifier.cpp | 33 +++++++++--------- .../src/plansys2_pddl_parser/Ground.cpp | 2 +- .../src/plansys2_pddl_parser/GroundFunc.cpp | 24 ++++++------- .../src/plansys2_pddl_parser/Imply.cpp | 6 ++-- .../src/plansys2_pddl_parser/Lifted.cpp | 6 ++-- .../src/plansys2_pddl_parser/Not.cpp | 22 ++++++------ .../src/plansys2_pddl_parser/Oneof.cpp | 6 ++-- .../src/plansys2_pddl_parser/Or.cpp | 8 ++--- .../plansys2_pddl_parser/TemporalAction.cpp | 6 ++-- .../src/plansys2_pddl_parser/TypeGround.cpp | 6 ++-- .../src/plansys2_pddl_parser/When.cpp | 6 ++-- 39 files changed, 121 insertions(+), 156 deletions(-) diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp index 5e4ba105b..c0624784a 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp @@ -65,7 +65,7 @@ class Action : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parseConditions(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp index da46de5c3..9ab12de66 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp @@ -54,7 +54,7 @@ class And : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp index d223c3a2a..ffcf8c589 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp @@ -52,7 +52,7 @@ class Condition virtual plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const = 0; + const std::vector & replace = {}, const std::map> & instances_map = {}) const = 0; virtual void parse(Stringreader & f, TokenStruct & ts, Domain & d) = 0; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp index 8792b1c4b..cd6c44463 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp @@ -53,7 +53,7 @@ class Derived : public Lifted plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp index 63b9a7548..7043e87e8 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp @@ -57,7 +57,7 @@ class Exists : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp index 14f9a919b..85fabe23e 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp @@ -98,7 +98,7 @@ class CompositeExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override + const std::vector & replace = {}, const std::map> & instances_map = {}) const override { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::EXPRESSION; @@ -106,10 +106,10 @@ class CompositeExpression : public Expression node->node_id = tree.nodes.size(); tree.nodes.push_back(*node); - plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace); + plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace, instances_map); tree.nodes[node->node_id].children.push_back(left_child->node_id); - plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace); + plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace, instances_map); tree.nodes[node->node_id].children.push_back(right_child->node_id); return node; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp index 9540e3fd6..1236b166b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp @@ -57,7 +57,7 @@ class Forall : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp index 3a2a95b02..cf96b788b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp @@ -45,7 +45,7 @@ class Function : public Lifted plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); }; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp index b7e8f706d..6cc365794 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp @@ -63,7 +63,7 @@ class FunctionModifier : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp index 674cccc9f..e3546962b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp @@ -48,7 +48,7 @@ class Ground : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp index 192438c89..dbd506213 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp @@ -43,7 +43,7 @@ class GroundFunc : public TypeGround plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void print(std::ostream & stream) const {stream << name << params << value << "\n";} diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h index 615890180..2b806dbfc 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h @@ -36,7 +36,7 @@ class Imply : public ParamCond { 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; + plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {}, const std::map> & instances_map = {} ) const override; void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp index 7cd3a4cb0..a2f20d181 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp @@ -57,7 +57,7 @@ class Imply : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp index 237576c82..240cb34c3 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp @@ -43,7 +43,7 @@ class Lifted : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp index 554059fa9..06a268ee6 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp @@ -60,7 +60,7 @@ class Not : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp index 1f3b0ea56..1061942e1 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp @@ -54,7 +54,7 @@ class Oneof : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp index f676d5104..6da39a781 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp @@ -60,7 +60,7 @@ class Or : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp index 7cdc10afd..8fa11b2b9 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp @@ -45,7 +45,7 @@ class Task : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override + const std::vector & replace = {}, const std::map> & instances_map = {}) const override { throw UnsupportedConstruct("Task"); } diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp index f2f6368a5..1acf04fe7 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp @@ -81,7 +81,7 @@ class TemporalAction : public Action plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parseCondition(Stringreader & f, TokenStruct & ts, Domain & d, And * a); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp index 028f9e2b2..1004b63e2 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp @@ -43,7 +43,7 @@ class TypeGround : public Ground plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void insert(Domain & d, const StringVec & v); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp index d74438656..bf70c703f 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp @@ -60,7 +60,7 @@ class When : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp index b9f677509..e16d46526 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp @@ -48,23 +48,21 @@ void Action::PDDLPrint( s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr Action::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::ACTION; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (pre) { - plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } +plansys2_msgs::msg::Node::SharedPtr Action::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::ACTION; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (pre) { + plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } - if (eff) { - plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + if (eff) { + plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp index 01d5c8fd1..5bdce4cfa 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp @@ -31,18 +31,16 @@ void And::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr And::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::AND; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); +plansys2_msgs::msg::Node::SharedPtr And::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::AND; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - for (unsigned i = 0; i < conds.size(); ++i) { - plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + for ( unsigned i = 0; i < conds.size(); ++i) { + plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp index 9102ce10f..db4c89e8c 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp @@ -45,10 +45,8 @@ void Derived::PDDLPrint( s << "\n)\n"; } -plansys2_msgs::msg::Node::SharedPtr Derived::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Derived"); +plansys2_msgs::msg::Node::SharedPtr Derived::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Derived"); } void Derived::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp index b47f8d2c1..d339bcbf1 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp @@ -41,9 +41,7 @@ void Exists::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Exists::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ +plansys2_msgs::msg::Node::SharedPtr Exists::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::EXISTS; node->node_id = tree.nodes.size(); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp index d0b92cee4..56559986d 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp @@ -39,25 +39,23 @@ void FunctionExpression::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionExpression::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::FUNCTION; - node->node_id = tree.nodes.size(); - node->name = fun->name; - for (unsigned i = 0; i < fun->params.size(); ++i) { - plansys2_msgs::msg::Param param; - if (i < replace.size()) { - param.name = replace[fun->params[i]]; - } else { - param.name = "?" + std::to_string(fun->params[i]); +plansys2_msgs::msg::Node::SharedPtr FunctionExpression::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION; + node->node_id = tree.nodes.size(); + node->name = fun->name; + for ( unsigned i = 0; i < fun->params.size(); ++i ) { + plansys2_msgs::msg::Param param; + if (i < replace.size()) { + param.name = replace[fun->params[i]]; + } else { + param.name = "?" + std::to_string(fun->params[i]); + } + param.type = d.types[fun->params[i]]->name; + node->parameters.push_back(param); } - param.type = d.types[fun->params[i]]->name; - node->parameters.push_back(param); - } - tree.nodes.push_back(*node); - return node; + tree.nodes.push_back(*node); + return node; } double FunctionExpression::evaluate(Instance & ins, const StringVec & par) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp index 4503c37e5..ec8613978 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp @@ -41,10 +41,8 @@ void Forall::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Forall::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Forall"); +plansys2_msgs::msg::Node::SharedPtr Forall::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Forall"); } void Forall::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp index 4a5388ba3..80b01fb04 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp @@ -25,10 +25,8 @@ void Function::PDDLPrint( if (returnType >= 0) {s << " - " << d.types[returnType]->name;} } -plansys2_msgs::msg::Node::SharedPtr Function::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Function"); +plansys2_msgs::msg::Node::SharedPtr Function::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Function"); } void Function::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp index 11a52d58b..45fb78f4e 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp @@ -62,24 +62,23 @@ void FunctionModifier::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionModifier::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::FUNCTION_MODIFIER; - node->modifier_type = getFunModType(name); - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (modifiedGround) { - plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); +plansys2_msgs::msg::Node::SharedPtr FunctionModifier::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION_MODIFIER; + node->modifier_type = getFunModType(name); + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (modifiedGround) { + plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); // Is type Ground, so should not need instances + tree.nodes[node->node_id].children.push_back(child->node_id); + } + else { + std::cerr << "function modifier for total-cost not supported" << std::endl; + } + + plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace, instances_map); // Does it need instances? tree.nodes[node->node_id].children.push_back(child->node_id); - } else { - std::cerr << "function modifier for total-cost not supported" << std::endl; - } - - plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp index d469afee3..86d4106dd 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp @@ -47,7 +47,7 @@ void Ground::PDDLPrint( } plansys2_msgs::msg::Node::SharedPtr Ground::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map) const { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); if (d.funcs.index(name) >= 0) { diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp index 232d68119..dc0b05a9b 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp @@ -40,22 +40,18 @@ void GroundFunc::PDDLPrint( s << " " << d.types[(reinterpret_cast(lifted))->returnType]->object(value) << " )"; } -template<> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - auto node = TypeGround::getTree(tree, d, replace); - node->value = value; - return node; +template <> +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + auto node = TypeGround::getTree(tree, d, replace); // Ground should not need instances, so is not passed + node->value = value; + return node; } -template<> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - auto node = TypeGround::getTree(tree, d, replace); - node->value = value; - return node; +template <> +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + auto node = TypeGround::getTree(tree, d, replace); // Ground should not need instances, so is not passed + node->value = value; + return node; } template<> diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index a475ec027..55b8c47cf 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -41,10 +41,8 @@ void Imply::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Imply::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Imply"); +plansys2_msgs::msg::Node::SharedPtr Imply::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Imply"); } void Imply::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp index 12a27bee6..b55e9cc75 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp @@ -34,10 +34,8 @@ void Lifted::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Lifted"); +plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Lifted"); } void Lifted::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp index b4cc71c32..fc64ee6c2 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp @@ -27,18 +27,16 @@ void Not::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Not::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::NOT; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (cond) { - plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } +plansys2_msgs::msg::Node::SharedPtr Not::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::NOT; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (cond) { + plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp index 9e104fc95..e6d3c460c 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp @@ -31,10 +31,8 @@ void Oneof::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("Oneof"); +plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("Oneof"); } void Oneof::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp index 3f4c2396b..19a89c543 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp @@ -41,17 +41,15 @@ void Or::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Or::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ +plansys2_msgs::msg::Node::SharedPtr Or::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::OR; node->node_id = tree.nodes.size(); tree.nodes.push_back(*node); - plansys2_msgs::msg::Node::SharedPtr child_f = first->getTree(tree, d, replace); + plansys2_msgs::msg::Node::SharedPtr child_f = first->getTree(tree, d, replace, instances_map); tree.nodes[node->node_id].children.push_back(child_f->node_id); - plansys2_msgs::msg::Node::SharedPtr child_s = second->getTree(tree, d, replace); + plansys2_msgs::msg::Node::SharedPtr child_s = second->getTree(tree, d, replace, instances_map); tree.nodes[node->node_id].children.push_back(child_s->node_id); return node; diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp index 958a8b755..1d31f71f9 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp @@ -70,10 +70,8 @@ void TemporalAction::PDDLPrint( s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("TemporalAction"); +plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("TemporalAction"); } void TemporalAction::parseCondition( diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp index 812b40c6b..5b2ba4f0a 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp @@ -29,10 +29,8 @@ void TypeGround::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - return Ground::getTree(tree, d, replace); +plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + return Ground::getTree(tree, d, replace); // Ground should not need instances, so is not passed } void TypeGround::insert(Domain & d, const StringVec & v) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp index 248d9d91e..62e92784b 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp @@ -41,10 +41,8 @@ void When::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr When::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const -{ - throw UnsupportedConstruct("When"); +plansys2_msgs::msg::Node::SharedPtr When::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { + throw UnsupportedConstruct("When"); } void When::parse(Stringreader & f, TokenStruct & ts, Domain & d) From cd1ea8b9b3e3a85427b9ca768c01d0bc6895ffe0 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:07:10 +0100 Subject: [PATCH 04/13] added params to parsing --- plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index 55b8c47cf..5bc96d1f5 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -45,11 +45,10 @@ plansys2_msgs::msg::Node::SharedPtr Imply::getTree( plansys2_msgs::msg::Tree & t throw UnsupportedConstruct("Imply"); } -void Imply::parse(Stringreader & f, TokenStruct & ts, Domain & d) -{ - f.next(); - f.assert_token("("); - +void Imply::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { + f.next(); + f.assert_token( "(" ); + params = d.convertTypes(ts.types); if (f.getChar() != ')' ) { cond_1 = d.createCondition( f ); cond_1->parse( f, ts, d ); From a8c69ff4fb48d1bb5b345e3627a9f36e63e12503 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:13:24 +0100 Subject: [PATCH 05/13] added option to pass instances from executor to domain expert --- .../include/plansys2_domain_expert/DomainExpert.hpp | 3 ++- .../include/plansys2_domain_expert/DomainExpertClient.hpp | 3 ++- .../plansys2_domain_expert/DomainExpertInterface.hpp | 2 +- .../src/plansys2_domain_expert/DomainExpert.cpp | 2 +- .../src/plansys2_domain_expert/DomainExpertClient.cpp | 6 ++++-- .../src/plansys2_domain_expert/DomainExpertNode.cpp | 2 +- plansys2_executor/src/plansys2_executor/ExecutorNode.cpp | 5 +++-- plansys2_msgs/srv/GetDomainDurativeActionDetails.srv | 1 + 8 files changed, 15 insertions(+), 9 deletions(-) diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp index 7fee4c338..b41560905 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp @@ -142,7 +142,8 @@ class DomainExpert : public DomainExpertInterface */ plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( const std::string & action, - const std::vector & params = {}); + const std::vector & params = {}, + const std::vector & instances = {}); /// Get the current domain, ready to be saved to file, or to initialize another domain. /** diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp index d336808d8..8ae0c6c4e 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp @@ -151,7 +151,8 @@ class DomainExpertClient : public DomainExpertInterface */ plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( const std::string & action, - const std::vector & params = {}); + const std::vector & params = {}, + const std::vector & instances = {}); /// Get the current domain, ready to be saved to file, or to initialize another domain. /** diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp index 5686705d4..d4fcecb6e 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp @@ -132,7 +132,7 @@ class DomainExpertInterface * effects. If the action does not exist, the value returned has not value. */ virtual plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( - const std::string & durative_action, const std::vector & params) = + const std::string & durative_action, const std::vector & params, const std::vector& instances) = 0; /// Get the current domain, ready to be saved to file, or to initialize another domain. diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp index a5ae16b39..9863085a2 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp @@ -303,7 +303,7 @@ DomainExpert::getDurativeActions() } plansys2_msgs::msg::DurativeAction::SharedPtr -DomainExpert::getDurativeAction(const std::string & action, const std::vector & params) +DomainExpert::getDurativeAction(const std::string &action, const std::vector ¶ms, const std::vector &instances) { std::string action_search = action; std::transform( diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp index c8cca5b2b..14b96c0b2 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp @@ -492,8 +492,9 @@ DomainExpertClient::getDurativeActions() plansys2_msgs::msg::DurativeAction::SharedPtr DomainExpertClient::getDurativeAction( - const std::string & action, - const std::vector & params) + const std::string &action, + const std::vector ¶ms, + const std::vector &instances) { while (!get_durative_action_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -509,6 +510,7 @@ DomainExpertClient::getDurativeAction( request->durative_action = action; request->parameters = params; + request->instances = instances; auto future_result = get_durative_action_details_client_->async_send_request(request); diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp index 2bde1c11e..38b2157b1 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp @@ -337,7 +337,7 @@ DomainExpertNode::get_domain_durative_action_details_service_callback( RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); } else { - auto action = domain_expert_->getDurativeAction(request->durative_action, request->parameters); + auto action = domain_expert_->getDurativeAction(request->durative_action, request->parameters, request->instances); if (action) { response->durative_action = *action; diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index ba1e3d5ed..a874ad1b7 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -347,7 +347,8 @@ ExecutorNode::create_plan_runtime_info(PlanRuntineInfo & runtime_info) { runtime_info.action_map = std::make_shared>(); auto action_timeout_actions = this->get_parameter("action_timeouts.actions").as_string_array(); - + auto instances = plansys2::convertVector( + problem_client_->getInstances()); (*runtime_info.action_map)[":0"] = ActionExecutionInfo(); (*runtime_info.action_map)[":0"].action_executor = ActionExecutor::make_shared("(INIT)", shared_from_this()); @@ -372,7 +373,7 @@ ExecutorNode::create_plan_runtime_info(PlanRuntineInfo & runtime_info) action_name, get_action_params(plan_item.action)); } else { (*runtime_info.action_map)[index].action_info = domain_client_->getDurativeAction( - action_name, get_action_params(plan_item.action)); + action_name, get_action_params(plan_item.action), instances); } action_name = (*runtime_info.action_map)[index].action_info.get_action_name(); diff --git a/plansys2_msgs/srv/GetDomainDurativeActionDetails.srv b/plansys2_msgs/srv/GetDomainDurativeActionDetails.srv index 471dc4197..457ccb3ca 100644 --- a/plansys2_msgs/srv/GetDomainDurativeActionDetails.srv +++ b/plansys2_msgs/srv/GetDomainDurativeActionDetails.srv @@ -1,5 +1,6 @@ string durative_action string[] parameters +plansys2_msgs/Param[] instances --- bool success plansys2_msgs/DurativeAction durative_action From f837f99ed896a119ebd2a926ba2c533d20bb9467 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:14:14 +0100 Subject: [PATCH 06/13] create Tree msgs --- .../plansys2_domain_expert/DomainExpert.cpp | 12 ++++--- plansys2_msgs/msg/Node.msg | 2 ++ .../src/plansys2_pddl_parser/Forall.cpp | 24 +++++++++++++- .../src/plansys2_pddl_parser/Imply.cpp | 33 ++++++++++++++++++- 4 files changed, 65 insertions(+), 6 deletions(-) diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp index 9863085a2..d69604ba3 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp @@ -335,20 +335,24 @@ DomainExpert::getDurativeAction(const std::string &action, const std::vectortypes[action_obj->params[j]]->getSubTypesNames(param.sub_types); ret->parameters.push_back(param); } - + // Instances + std::map> instances_map; + for (const auto & instance : instances) { + instances_map[instance.type].push_back(instance.name); + } // Preconditions AtStart if (action_obj->pre) { - action_obj->pre->getTree(ret->at_start_requirements, *domain_, params); + action_obj->pre->getTree(ret->at_start_requirements, *domain_, params, instances_map); } // Preconditions OverAll if (action_obj->pre_o) { - action_obj->pre_o->getTree(ret->over_all_requirements, *domain_, params); + action_obj->pre_o->getTree(ret->over_all_requirements, *domain_, params, instances_map); } // Preconditions AtEnd if (action_obj->pre_e) { - action_obj->pre_e->getTree(ret->at_end_requirements, *domain_, params); + action_obj->pre_e->getTree(ret->at_end_requirements, *domain_, params, instances_map); } // Effects AtStart diff --git a/plansys2_msgs/msg/Node.msg b/plansys2_msgs/msg/Node.msg index cc969106e..2164bd085 100644 --- a/plansys2_msgs/msg/Node.msg +++ b/plansys2_msgs/msg/Node.msg @@ -14,6 +14,8 @@ uint8 NUMBER = 9 uint8 CONSTANT = 10 uint8 PARAMETER = 11 uint8 EXISTS = 12 +uint8 FORALL = 13 +uint8 IMPLY = 14 # Expression types uint8 COMP_EQ = 13 diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp index ec8613978..1b8d7ab61 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp @@ -42,7 +42,29 @@ void Forall::PDDLPrint( } plansys2_msgs::msg::Node::SharedPtr Forall::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Forall"); + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FORALL; + node->node_id = tree.nodes.size(); + node->name = name; + tree.nodes.push_back(*node); + + std::string type = d.types[params[0]]->name; + + for (const auto& instance: instances_map.at(type)) { + plansys2_msgs::msg::Param param; + param.name = instance; + param.type = type; + tree.nodes[node->node_id].parameters.push_back(param); + + std::vector new_replace = replace; + new_replace.push_back(instance); + if(cond) { + plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, new_replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + } + + return node; } void Forall::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index 5bc96d1f5..fccf460bd 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -42,7 +42,38 @@ void Imply::PDDLPrint( } plansys2_msgs::msg::Node::SharedPtr Imply::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Imply"); + + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::IMPLY; + node->node_id = tree.nodes.size(); + node->name = name; + + for (unsigned i = 0; i < params.size(); ++i) { + plansys2_msgs::msg::Param param; + if (i < replace.size()){ + if (params[i] >= 0) // param has a variable value; replace by action-args + { + param.name = replace[i]; + } + } else { + param.name = "?" + std::to_string(params[i]); + } + param.type = d.types[params[i]]->name; + node->parameters.push_back(param); + } + + tree.nodes.push_back(*node); + + if (cond_1){ + plansys2_msgs::msg::Node::SharedPtr child_1 = cond_1->getTree(tree, d, replace); // Could conditions need instances? + tree.nodes[node->node_id].children.push_back(child_1->node_id); + } + + if(cond_2){ + plansys2_msgs::msg::Node::SharedPtr child_2 = cond_2->getTree(tree, d, replace); // Could conditions need instances? + tree.nodes[node->node_id].children.push_back(child_2->node_id); + } + return node; } void Imply::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { From 55668cc2f61679edc159c593a7d65d110952d2fa Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:16:10 +0100 Subject: [PATCH 07/13] finished BT creation for forall and imply --- .../src/plansys2_executor/ExecutorNode.cpp | 5 +++- .../bt_builder_plugins/simple_bt_builder.cpp | 4 +-- .../src/plansys2_problem_expert/Utils.cpp | 30 +++++++++++++++++++ 3 files changed, 36 insertions(+), 3 deletions(-) diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index a874ad1b7..5002107cb 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -268,6 +268,7 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) auto goal = problem_client_->getGoal(); auto local_predicates = problem_client_->getPredicates(); auto local_functions = problem_client_->getFunctions(); + auto instances = plansys2::convertVector(problem_client_->getInstances()); std::vector unordered_subgoals = parser::pddl::getSubtreeIds(goal); @@ -285,6 +286,8 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) for (const auto & plan_item : runtime_info.complete_plan.items) { auto actions = domain_client_->getActions(); + auto instances = plansys2::convertVector( + problem_client_->getInstances()); std::string action_name = get_action_name(plan_item.action); if (std::find(actions.begin(), actions.end(), action_name) != actions.end()) { std::shared_ptr action = @@ -294,7 +297,7 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) } else { std::shared_ptr action = domain_client_->getDurativeAction( - action_name, get_action_params(plan_item.action)); + action_name, get_action_params(plan_item.action), instances); apply(action->at_start_effects, local_predicates, local_functions); apply(action->at_end_effects, local_predicates, local_functions); } diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp index 49e3f229b..15eca32fe 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp @@ -824,7 +824,7 @@ std::vector SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) { std::vector ret; - + auto instances = plansys2::convertVector(problem_client_->getInstances()); for (auto & item : plan.items) { ActionStamped action_stamped; @@ -838,7 +838,7 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) } else { action_stamped.action.action = domain_client_->getDurativeAction( - get_action_name(item.action), get_action_params(item.action)); + get_action_name(item.action), get_action_params(item.action), instances); } ret.push_back(action_stamped); diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp index b14dbd7d2..ebb6b2e9b 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp @@ -314,6 +314,36 @@ std::tuple evaluate( case plansys2_msgs::msg::Node::NUMBER: { return std::make_tuple(true, true, tree.nodes[node_id].value); } + + case plansys2_msgs::msg::Node::FORALL: { + bool success = true; + bool truth_value = true; + + for (auto & child_id : tree.nodes[node_id].children) { + std::tuple result = + evaluate( + tree, problem_client, predicates, functions, apply, use_state, child_id, + negate); + success = success && std::get<0>(result); + truth_value = truth_value && std::get<1>(result); + } + return std::make_tuple(success, truth_value, 0); + } + + case plansys2_msgs::msg::Node::IMPLY: { + std::tuple left = evaluate( + tree, problem_client, predicates, + functions, false, use_state, tree.nodes[node_id].children[0], !negate); // negated as imply evaluates to !left || right + // apply set to false as imply can't be used in effects. Maybe raise error if true? + std::tuple right = evaluate( + tree, problem_client, + predicates, functions, false, use_state, tree.nodes[node_id].children[1], + negate); + + bool eval = std::get<1>(left) || std::get<1>(right); + + return std::make_tuple(std::get<0>(left) && std::get<0>(right), std::get<1>(left) || std::get<1>(right), 0); + } case plansys2_msgs::msg::Node::CONSTANT: { if (tree.nodes[node_id].name.size() > 0) { From 27e2a97c24a954f412f06f7c34d046cc2b7c1957 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:17:24 +0100 Subject: [PATCH 08/13] refactor: improve code formatting to pass testing --- .../DomainExpertInterface.hpp | 4 +- .../plansys2_domain_expert/DomainExpert.cpp | 4 +- .../DomainExpertClient.cpp | 6 +-- .../DomainExpertNode.cpp | 3 +- .../src/plansys2_executor/ExecutorNode.cpp | 3 +- .../bt_builder_plugins/simple_bt_builder.cpp | 3 +- .../src/plansys2_problem_expert/Utils.cpp | 47 ++++++++++--------- 7 files changed, 39 insertions(+), 31 deletions(-) diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp index d4fcecb6e..998e4496d 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp @@ -132,8 +132,8 @@ class DomainExpertInterface * effects. If the action does not exist, the value returned has not value. */ virtual plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( - const std::string & durative_action, const std::vector & params, const std::vector& instances) = - 0; + const std::string & durative_action, const std::vector & params, + const std::vector & instances) = 0; /// Get the current domain, ready to be saved to file, or to initialize another domain. /** diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp index d69604ba3..42210de74 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp @@ -303,7 +303,9 @@ DomainExpert::getDurativeActions() } plansys2_msgs::msg::DurativeAction::SharedPtr -DomainExpert::getDurativeAction(const std::string &action, const std::vector ¶ms, const std::vector &instances) +DomainExpert::getDurativeAction( + const std::string & action, const std::vector & params, + const std::vector & instances) { std::string action_search = action; std::transform( diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp index 14b96c0b2..036acc273 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp @@ -492,9 +492,9 @@ DomainExpertClient::getDurativeActions() plansys2_msgs::msg::DurativeAction::SharedPtr DomainExpertClient::getDurativeAction( - const std::string &action, - const std::vector ¶ms, - const std::vector &instances) + const std::string & action, + const std::vector & params, + const std::vector & instances) { while (!get_durative_action_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp index 38b2157b1..08dba81f1 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp @@ -337,7 +337,8 @@ DomainExpertNode::get_domain_durative_action_details_service_callback( RCLCPP_WARN(get_logger(), "Requesting service in non-active state"); } else { - auto action = domain_expert_->getDurativeAction(request->durative_action, request->parameters, request->instances); + auto action = domain_expert_->getDurativeAction( + request->durative_action, request->parameters, request->instances); if (action) { response->durative_action = *action; diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index 5002107cb..17e091efc 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -268,7 +268,8 @@ ExecutorNode::get_ordered_subgoals(PlanRuntineInfo & runtime_info) auto goal = problem_client_->getGoal(); auto local_predicates = problem_client_->getPredicates(); auto local_functions = problem_client_->getFunctions(); - auto instances = plansys2::convertVector(problem_client_->getInstances()); + auto instances = plansys2::convertVector( + problem_client_->getInstances()); std::vector unordered_subgoals = parser::pddl::getSubtreeIds(goal); diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp index 15eca32fe..7e4e3d991 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp @@ -824,7 +824,8 @@ std::vector SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) { std::vector ret; - auto instances = plansys2::convertVector(problem_client_->getInstances()); + auto instances = plansys2::convertVector( + problem_client_->getInstances()); for (auto & item : plan.items) { ActionStamped action_stamped; diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp index ebb6b2e9b..6b0a7d507 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp @@ -314,36 +314,39 @@ std::tuple evaluate( case plansys2_msgs::msg::Node::NUMBER: { return std::make_tuple(true, true, tree.nodes[node_id].value); } - + case plansys2_msgs::msg::Node::FORALL: { - bool success = true; - bool truth_value = true; + bool success = true; + bool truth_value = true; - for (auto & child_id : tree.nodes[node_id].children) { - std::tuple result = - evaluate( - tree, problem_client, predicates, functions, apply, use_state, child_id, - negate); - success = success && std::get<0>(result); - truth_value = truth_value && std::get<1>(result); + for (auto & child_id : tree.nodes[node_id].children) { + std::tuple result = + evaluate( + tree, problem_client, predicates, functions, apply, use_state, child_id, + negate); + success = success && std::get<0>(result); + truth_value = truth_value && std::get<1>(result); + } + return std::make_tuple(success, truth_value, 0); } - return std::make_tuple(success, truth_value, 0); - } case plansys2_msgs::msg::Node::IMPLY: { - std::tuple left = evaluate( - tree, problem_client, predicates, - functions, false, use_state, tree.nodes[node_id].children[0], !negate); // negated as imply evaluates to !left || right + std::tuple left = evaluate( + tree, problem_client, predicates, + functions, false, use_state, tree.nodes[node_id].children[0], !negate); + // negated as imply evaluates to !left || right // apply set to false as imply can't be used in effects. Maybe raise error if true? - std::tuple right = evaluate( - tree, problem_client, - predicates, functions, false, use_state, tree.nodes[node_id].children[1], - negate); + std::tuple right = evaluate( + tree, problem_client, + predicates, functions, false, use_state, tree.nodes[node_id].children[1], + negate); - bool eval = std::get<1>(left) || std::get<1>(right); + bool eval = std::get<1>(left) || std::get<1>(right); - return std::make_tuple(std::get<0>(left) && std::get<0>(right), std::get<1>(left) || std::get<1>(right), 0); - } + return std::make_tuple( + std::get<0>(left) && std::get<0>(right), std::get<1>( + left) || std::get<1>(right), 0); + } case plansys2_msgs::msg::Node::CONSTANT: { if (tree.nodes[node_id].name.size() > 0) { From f4fb89d5042a9a8458d6512c1409c80d1bf63531 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:17:24 +0100 Subject: [PATCH 09/13] refactor conds to distinguish them --- .../include/plansys2_pddl_parser/Imply.h | 24 +++++++++---------- .../src/plansys2_pddl_parser/Imply.cpp | 20 ++++++++-------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h index 2b806dbfc..aa286d7d9 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h @@ -11,27 +11,27 @@ namespace parser { namespace pddl { class Imply : public ParamCond { public: - Condition * cond_1; // The initial condition of imply - Condition * cond_2; // The latter condition of imply + Condition * left; // The initial condition of imply + Condition * right; // The latter condition of imply Imply() - : cond_1( 0 ), cond_2( 0 ) {} + : left( 0 ), right( 0 ) {} Imply( const Imply * f, Domain & d ) - : ParamCond( f ), cond_1( 0 ), cond_2( 0 ) { - if ( f->cond_1 ) cond_1 = f->cond_1->copy( d ); - if ( f->cond_2 ) cond_2 = f->cond_2->copy( d ); + : ParamCond( f ), left( 0 ), right( 0 ) { + if ( f->left ) left = f->left->copy( d ); + if ( f->right ) right = f->right->copy( d ); } ~Imply() { - if ( cond_1 ) delete cond_1; // bc new creation - if ( cond_2 ) delete cond_2; + if ( left ) delete left; // bc new creation + if ( right ) delete right; } void print( std::ostream & s ) const { s << "Imply" << params << ":\n"; - if ( cond_1 ) cond_1->print( s ); - if( cond_2 ) cond_2->print( s ); + if ( left ) left->print( s ); + if( right ) right->print( s ); } void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; @@ -41,8 +41,8 @@ class Imply : public ParamCond { void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); void addParams( int m, unsigned n ) { - cond_1->addParams( m, n ); - cond_2->addParams( m, n ); + left->addParams( m, n ); + right->addParams( m, n ); } Condition * copy( Domain & d ) { diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index fccf460bd..e9cb986fa 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -29,8 +29,8 @@ void Imply::PDDLPrint( tabindent(s, indent + 1); printParams(0, s, fstruct, d); - if ( cond_1 ) cond_1->PDDLPrint( s, indent + 1, fstruct, d ); - if (cond_2) cond_2->PDDLPrint( s, indent + 1, fstruct, d ); + if ( left ) left->PDDLPrint( s, indent + 1, fstruct, d ); + if (right) right->PDDLPrint( s, indent + 1, fstruct, d ); else { tabindent( s, indent + 1 ); s << "()"; @@ -64,13 +64,13 @@ plansys2_msgs::msg::Node::SharedPtr Imply::getTree( plansys2_msgs::msg::Tree & t tree.nodes.push_back(*node); - if (cond_1){ - plansys2_msgs::msg::Node::SharedPtr child_1 = cond_1->getTree(tree, d, replace); // Could conditions need instances? + if (left){ + plansys2_msgs::msg::Node::SharedPtr child_1 = left->getTree(tree, d, replace); // Could conditions need instances? tree.nodes[node->node_id].children.push_back(child_1->node_id); } - if(cond_2){ - plansys2_msgs::msg::Node::SharedPtr child_2 = cond_2->getTree(tree, d, replace); // Could conditions need instances? + if(right){ + plansys2_msgs::msg::Node::SharedPtr child_2 = right->getTree(tree, d, replace); // Could conditions need instances? tree.nodes[node->node_id].children.push_back(child_2->node_id); } return node; @@ -81,16 +81,16 @@ void Imply::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d f.assert_token( "(" ); params = d.convertTypes(ts.types); if (f.getChar() != ')' ) { - cond_1 = d.createCondition( f ); - cond_1->parse( f, ts, d ); + left = d.createCondition( f ); + left->parse( f, ts, d ); } else ++f.c; f.next(); f.assert_token( "(" ); if ( f.getChar() != ')' ) { - cond_2 = d.createCondition( f ); - cond_2->parse( f, ts, d ); + right = d.createCondition( f ); + right->parse( f, ts, d ); } else ++f.c; From 08d99c775ff3b19c8c65f6b1f4aa36cf0121b056 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:17:24 +0100 Subject: [PATCH 10/13] added test case for forall and imply --- .../test/pddl/test_forall_imply.pddl | 27 +++++ plansys2_executor/test/unit/bt_node_test.cpp | 111 ++++++++++++++++++ 2 files changed, 138 insertions(+) create mode 100644 plansys2_executor/test/pddl/test_forall_imply.pddl diff --git a/plansys2_executor/test/pddl/test_forall_imply.pddl b/plansys2_executor/test/pddl/test_forall_imply.pddl new file mode 100644 index 000000000..89cb067e2 --- /dev/null +++ b/plansys2_executor/test/pddl/test_forall_imply.pddl @@ -0,0 +1,27 @@ +(define (domain test_forall_imply) + (:requirements :strips :adl :typing :durative-actions :fluents) + (:types + object + item - object + location - object + ) + (:predicates + (item_at ?item - item ?location - location) + (item_checked ?item - item) + (all_items_checked ?location - location) + ) + (:durative-action check_items + :parameters (?location - location) + :duration (= ?duration 1) + :condition + (and (over all + (forall (?item - item) + (imply (item_at ?item ?location) (item_checked ?item)) + ) + )) + :effect + (and + (at end (all_items_checked ?location)) + ) + ) +) \ No newline at end of file diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index 5db2da010..342889d03 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/plansys2_executor/test/unit/bt_node_test.cpp @@ -761,6 +761,117 @@ TEST(problem_expert, at_end_effect_test) t.join(); } +TEST(problem_expert, forall_imply_test) +{ + auto test_node = rclcpp::Node::make_shared("test_node"); + auto test_lc_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lc_node"); + auto domain_node = std::make_shared(); + auto problem_node = std::make_shared(); + + auto domain_client = std::make_shared(); + auto problem_client = std::make_shared(); + + std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor"); + + domain_node->set_parameter({"model_file", pkgpath + "/pddl/test_forall_imply.pddl"}); + problem_node->set_parameter({"model_file", pkgpath + "/pddl/test_forall_imply.pddl"}); + + rclcpp::executors::MultiThreadedExecutor exe(rclcpp::ExecutorOptions(), 8); + + exe.add_node(domain_node->get_node_base_interface()); + exe.add_node(problem_node->get_node_base_interface()); + + bool finish = false; + std::thread t([&]() { + while (!finish) {exe.spin_some();} + }); + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + + { + rclcpp::Rate rate(10); + auto start = test_node->now(); + while ((test_node->now() - start).seconds() < 0.5) { + rate.sleep(); + } + } + + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("location1", "location"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("item1", "item"))); + ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("item2", "item"))); + + auto instances = plansys2::convertVector( + problem_client->getInstances()); + auto action_map = std::make_shared>(); + (*action_map)["(check_items location1):1"] = plansys2::ActionExecutionInfo(); + (*action_map)["(check_items location1):1"].durative_action_info = + domain_client->getDurativeAction( + plansys2::get_action_name("(check_items location1)"), + plansys2::get_action_params("(check_items location1)"), instances); + + ASSERT_NE( + (*action_map)["(check_items location1):1"].durative_action_info, + nullptr); + + std::string bt_xml_tree = + R"( + + + + + + + + )"; + + auto blackboard = BT::Blackboard::create(); + + blackboard->set("action_map", action_map); + blackboard->set("node", test_lc_node); + blackboard->set("problem_client", problem_client); + + BT::BehaviorTreeFactory factory; + factory.registerNodeType("ExecuteAction"); + factory.registerNodeType("CheckOverAllReq"); + + + std::vector predicates = { + "(item_at item1 location1)", + "(item_at item2 location1)", + "(item_checked item1)", + "(item_checked item2)" + }; + + try { + for (const auto & pred : predicates) { + ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); + } + + auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); + + auto status = BT::NodeStatus::RUNNING; + status = tree.tickRoot(); + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + } catch (std::exception & e) { + std::cerr << e.what() << std::endl; + } + + finish = true; + t.join(); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 38a811e7462173093108d828d1a6dab086364ce5 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 11:18:32 +0100 Subject: [PATCH 11/13] remove useless line --- plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp index 6b0a7d507..0e5aad0c1 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/Utils.cpp @@ -341,8 +341,6 @@ std::tuple evaluate( predicates, functions, false, use_state, tree.nodes[node_id].children[1], negate); - bool eval = std::get<1>(left) || std::get<1>(right); - return std::make_tuple( std::get<0>(left) && std::get<0>(right), std::get<1>( left) || std::get<1>(right), 0); From 518324305ae54e99219b070b0837a4a2829694db Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 15:49:33 +0100 Subject: [PATCH 12/13] fixed test --- plansys2_executor/test/unit/bt_node_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index 342889d03..a814e7463 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/plansys2_executor/test/unit/bt_node_test.cpp @@ -816,14 +816,14 @@ TEST(problem_expert, forall_imply_test) problem_client->getInstances()); auto action_map = std::make_shared>(); (*action_map)["(check_items location1):1"] = plansys2::ActionExecutionInfo(); - (*action_map)["(check_items location1):1"].durative_action_info = + (*action_map)["(check_items location1):1"].action_info = domain_client->getDurativeAction( plansys2::get_action_name("(check_items location1)"), plansys2::get_action_params("(check_items location1)"), instances); ASSERT_NE( - (*action_map)["(check_items location1):1"].durative_action_info, - nullptr); + (*action_map)["(check_items location1):1"].action_info.action.index(), + std::variant_npos); std::string bt_xml_tree = R"( @@ -862,7 +862,7 @@ TEST(problem_expert, forall_imply_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); } catch (std::exception & e) { std::cerr << e.what() << std::endl; From 663b3ef98141cdf2153e100304169fd60ec3e0f4 Mon Sep 17 00:00:00 2001 From: eshan-savla Date: Tue, 4 Mar 2025 15:50:05 +0100 Subject: [PATCH 13/13] refactor for readability --- .../include/plansys2_pddl_parser/Action.hpp | 4 +- .../include/plansys2_pddl_parser/And.hpp | 4 +- .../plansys2_pddl_parser/Condition.hpp | 4 +- .../include/plansys2_pddl_parser/Derived.hpp | 4 +- .../include/plansys2_pddl_parser/Exists.hpp | 4 +- .../plansys2_pddl_parser/Expression.hpp | 25 ++-- .../include/plansys2_pddl_parser/Forall.hpp | 4 +- .../include/plansys2_pddl_parser/Function.hpp | 4 +- .../plansys2_pddl_parser/FunctionModifier.hpp | 4 +- .../include/plansys2_pddl_parser/Ground.hpp | 4 +- .../plansys2_pddl_parser/GroundFunc.hpp | 4 +- .../include/plansys2_pddl_parser/Imply.h | 54 --------- .../include/plansys2_pddl_parser/Imply.hpp | 26 +++-- .../include/plansys2_pddl_parser/Instance.hpp | 1 + .../include/plansys2_pddl_parser/Lifted.hpp | 4 +- .../include/plansys2_pddl_parser/Not.hpp | 4 +- .../include/plansys2_pddl_parser/Oneof.hpp | 4 +- .../include/plansys2_pddl_parser/Or.hpp | 4 +- .../include/plansys2_pddl_parser/Task.hpp | 4 +- .../plansys2_pddl_parser/TemporalAction.hpp | 4 +- .../plansys2_pddl_parser/TypeGround.hpp | 4 +- .../include/plansys2_pddl_parser/When.hpp | 4 +- .../src/plansys2_pddl_parser/Action.cpp | 32 +++--- .../src/plansys2_pddl_parser/And.cpp | 22 ++-- .../src/plansys2_pddl_parser/Derived.cpp | 8 +- .../src/plansys2_pddl_parser/Exists.cpp | 6 +- .../src/plansys2_pddl_parser/Expression.cpp | 40 ++++--- .../src/plansys2_pddl_parser/Forall.cpp | 53 +++++---- .../src/plansys2_pddl_parser/Function.cpp | 8 +- .../plansys2_pddl_parser/FunctionModifier.cpp | 37 +++--- .../src/plansys2_pddl_parser/Ground.cpp | 3 +- .../src/plansys2_pddl_parser/GroundFunc.cpp | 30 +++-- .../src/plansys2_pddl_parser/Imply.cpp | 108 +++++++++--------- .../src/plansys2_pddl_parser/Lifted.cpp | 8 +- .../src/plansys2_pddl_parser/Not.cpp | 24 ++-- .../src/plansys2_pddl_parser/Oneof.cpp | 8 +- .../src/plansys2_pddl_parser/Or.cpp | 6 +- .../plansys2_pddl_parser/TemporalAction.cpp | 8 +- .../src/plansys2_pddl_parser/TypeGround.cpp | 8 +- .../src/plansys2_pddl_parser/When.cpp | 8 +- 40 files changed, 337 insertions(+), 258 deletions(-) delete mode 100644 plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp index c0624784a..184141dbd 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -65,7 +66,8 @@ class Action : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parseConditions(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp index 9ab12de66..a95bcdb44 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -54,7 +55,8 @@ class And : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp index ffcf8c589..679d9a4f0 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -52,7 +53,8 @@ class Condition virtual plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const = 0; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const = 0; virtual void parse(Stringreader & f, TokenStruct & ts, Domain & d) = 0; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp index cd6c44463..4db66210a 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -53,7 +54,8 @@ class Derived : public Lifted plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp index 7043e87e8..f0895547d 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -57,7 +58,8 @@ class Exists : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp index 85fabe23e..f4406171f 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -98,7 +99,8 @@ class CompositeExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::EXPRESSION; @@ -106,10 +108,12 @@ class CompositeExpression : public Expression node->node_id = tree.nodes.size(); tree.nodes.push_back(*node); - plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace, instances_map); + plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace, + instances_map); tree.nodes[node->node_id].children.push_back(left_child->node_id); - plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace, instances_map); + plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace, + instances_map); tree.nodes[node->node_id].children.push_back(right_child->node_id); return node; @@ -186,7 +190,8 @@ class FunctionExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; double evaluate() {return 1;} @@ -224,7 +229,8 @@ class ValueExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::NUMBER; @@ -254,7 +260,8 @@ class DurationExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override { throw UnsupportedConstruct("DurationExpression"); } @@ -294,7 +301,8 @@ class ParamExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::PARAMETER; @@ -341,7 +349,8 @@ class ConstExpression : public Expression plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; double evaluate() {return -1;} diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp index 1236b166b..573ac8d4b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -57,7 +58,8 @@ class Forall : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp index cf96b788b..8897ae46a 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp @@ -15,6 +15,7 @@ #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -45,7 +46,8 @@ class Function : public Lifted plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); }; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp index 6cc365794..8df492aba 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -63,7 +64,8 @@ class FunctionModifier : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp index e3546962b..c50b61e3d 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -48,7 +49,8 @@ class Ground : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp index dbd506213..2d1d27f1d 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp @@ -15,6 +15,7 @@ #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -43,7 +44,8 @@ class GroundFunc : public TypeGround plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void print(std::ostream & stream) const {stream << name << params << value << "\n";} diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h deleted file mode 100644 index aa286d7d9..000000000 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.h +++ /dev/null @@ -1,54 +0,0 @@ - -#pragma once - -#include "plansys2_msgs/msg/node.hpp" -#include "plansys2_msgs/msg/tree.hpp" - -#include "plansys2_pddl_parser/ParamCond.h" - -namespace parser { namespace pddl { - -class Imply : public ParamCond { - -public: - Condition * left; // The initial condition of imply - Condition * right; // The latter condition of imply - - Imply() - : left( 0 ), right( 0 ) {} - - Imply( const Imply * f, Domain & d ) - : ParamCond( f ), left( 0 ), right( 0 ) { - if ( f->left ) left = f->left->copy( d ); - if ( f->right ) right = f->right->copy( d ); - } - - ~Imply() { - if ( left ) delete left; // bc new creation - if ( right ) delete right; - } - - void print( std::ostream & s ) const { - s << "Imply" << params << ":\n"; - if ( left ) left->print( s ); - if( right ) right->print( s ); - } - - 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 std::map> & instances_map = {} ) const override; - - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - - void addParams( int m, unsigned n ) { - left->addParams( m, n ); - right->addParams( m, n ); - } - - Condition * copy( Domain & d ) { - return new Imply( this, d ); - } - -}; - -} } // namespaces diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp index a2f20d181..f0870160b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -29,26 +30,30 @@ namespace pddl class Imply : public ParamCond { public: - Condition * cond; + Condition * left; + Condition * right; Imply() - : cond(0) {} + : left(0), right(0) {} Imply(const Imply * f, Domain & d) - : ParamCond(f), cond(0) + : ParamCond(f), left(0), right(0) { - if (f->cond) {cond = f->cond->copy(d);} + if (f->left) {left = f->left->copy(d);} + if (f->right) {right = f->right->copy(d);} } ~Imply() { - if (cond) {delete cond;} + if (left) {delete left;} + if (right) {delete right;} } void print(std::ostream & s) const { s << "Imply" << params << ":\n"; - if (cond) {cond->print(s);} + if (left) {left->print(s);} + if (right) {right->print(s);} } void PDDLPrint( @@ -57,11 +62,16 @@ class Imply : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); - void addParams(int m, unsigned n) {cond->addParams(m, n);} + void addParams(int m, unsigned n) + { + left->addParams(m, n); + right->addParams(m, n); + } Condition * copy(Domain & d) {return new Imply(this, d);} }; diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.hpp index 7846b48e0..573c11aea 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.hpp @@ -15,6 +15,7 @@ #include #include +#include #include "plansys2_pddl_parser/Domain.hpp" // #undef DOMAIN_DEBUG diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp index 240cb34c3..c25744354 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -43,7 +44,8 @@ class Lifted : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp index 06a268ee6..961be8b87 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -60,7 +61,8 @@ class Not : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp index 1061942e1..15b20598d 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -54,7 +55,8 @@ class Oneof : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp index 6da39a781..795978b8a 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -60,7 +61,8 @@ class Or : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp index 8fa11b2b9..1d579d2fe 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -45,7 +46,8 @@ class Task : public ParamCond plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override { throw UnsupportedConstruct("Task"); } diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp index 1acf04fe7..7e2da0f6f 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.hpp @@ -15,6 +15,7 @@ #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -81,7 +82,8 @@ class TemporalAction : public Action plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parseCondition(Stringreader & f, TokenStruct & ts, Domain & d, And * a); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp index 1004b63e2..cfc30f327 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp @@ -15,6 +15,7 @@ #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -43,7 +44,8 @@ class TypeGround : public Ground plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void insert(Domain & d, const StringVec & v); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp index bf70c703f..4680c7e85 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -60,7 +61,8 @@ class When : public Condition plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, - const std::vector & replace = {}, const std::map> & instances_map = {}) const override; + const std::vector & replace = {}, + const std::map> & instances_map = {}) const override; void parse(Stringreader & f, TokenStruct & ts, Domain & d); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp index e16d46526..930c484be 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp @@ -48,21 +48,25 @@ void Action::PDDLPrint( s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr Action::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::ACTION; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (pre) { - plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace, instances_map); - tree.nodes[node->node_id].children.push_back(child->node_id); - } +plansys2_msgs::msg::Node::SharedPtr Action::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::ACTION; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - if (eff) { - plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace, instances_map); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + if (pre) { + plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + + if (eff) { + plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp index 5bdce4cfa..e4300d4d4 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp @@ -31,16 +31,20 @@ void And::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr And::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::AND; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); +plansys2_msgs::msg::Node::SharedPtr And::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::AND; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - for ( unsigned i = 0; i < conds.size(); ++i) { - plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace, instances_map); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + for ( unsigned i = 0; i < conds.size(); ++i) { + plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp index db4c89e8c..10be1d66f 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp @@ -45,8 +45,12 @@ void Derived::PDDLPrint( s << "\n)\n"; } -plansys2_msgs::msg::Node::SharedPtr Derived::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Derived"); +plansys2_msgs::msg::Node::SharedPtr Derived::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("Derived"); } void Derived::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp index d339bcbf1..5425d90e9 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp @@ -41,7 +41,11 @@ void Exists::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Exists::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { +plansys2_msgs::msg::Node::SharedPtr Exists::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace, + const std::map> & instances_map) const +{ plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::EXISTS; node->node_id = tree.nodes.size(); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp index 56559986d..f64076d07 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp @@ -39,23 +39,27 @@ void FunctionExpression::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionExpression::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::FUNCTION; - node->node_id = tree.nodes.size(); - node->name = fun->name; - for ( unsigned i = 0; i < fun->params.size(); ++i ) { - plansys2_msgs::msg::Param param; - if (i < replace.size()) { - param.name = replace[fun->params[i]]; - } else { - param.name = "?" + std::to_string(fun->params[i]); - } - param.type = d.types[fun->params[i]]->name; - node->parameters.push_back(param); +plansys2_msgs::msg::Node::SharedPtr FunctionExpression::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION; + node->node_id = tree.nodes.size(); + node->name = fun->name; + for ( unsigned i = 0; i < fun->params.size(); ++i ) { + plansys2_msgs::msg::Param param; + if (i < replace.size()) { + param.name = replace[fun->params[i]]; + } else { + param.name = "?" + std::to_string(fun->params[i]); } - tree.nodes.push_back(*node); - return node; + param.type = d.types[fun->params[i]]->name; + node->parameters.push_back(param); + } + tree.nodes.push_back(*node); + return node; } double FunctionExpression::evaluate(Instance & ins, const StringVec & par) @@ -92,7 +96,9 @@ void ConstExpression::PDDLPrint( } plansys2_msgs::msg::Node::SharedPtr ConstExpression::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::CONSTANT; diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp index 1b8d7ab61..def703491 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp @@ -41,30 +41,35 @@ void Forall::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Forall::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::FORALL; - node->node_id = tree.nodes.size(); - node->name = name; - tree.nodes.push_back(*node); - - std::string type = d.types[params[0]]->name; - - for (const auto& instance: instances_map.at(type)) { - plansys2_msgs::msg::Param param; - param.name = instance; - param.type = type; - tree.nodes[node->node_id].parameters.push_back(param); - - std::vector new_replace = replace; - new_replace.push_back(instance); - if(cond) { - plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, new_replace, instances_map); - tree.nodes[node->node_id].children.push_back(child->node_id); - } - } - - return node; +plansys2_msgs::msg::Node::SharedPtr Forall::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FORALL; + node->node_id = tree.nodes.size(); + node->name = name; + tree.nodes.push_back(*node); + + std::string type = d.types[params[0]]->name; + + for (const auto & instance : instances_map.at(type)) { + plansys2_msgs::msg::Param param; + param.name = instance; + param.type = type; + tree.nodes[node->node_id].parameters.push_back(param); + + std::vector new_replace = replace; + new_replace.push_back(instance); + if(cond) { + plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, new_replace, + instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + } + + return node; } void Forall::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp index 80b01fb04..4576fbe72 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp @@ -25,8 +25,12 @@ void Function::PDDLPrint( if (returnType >= 0) {s << " - " << d.types[returnType]->name;} } -plansys2_msgs::msg::Node::SharedPtr Function::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Function"); +plansys2_msgs::msg::Node::SharedPtr Function::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("Function"); } void Function::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp index 45fb78f4e..c907fd644 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp @@ -62,23 +62,28 @@ void FunctionModifier::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionModifier::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::FUNCTION_MODIFIER; - node->modifier_type = getFunModType(name); - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (modifiedGround) { - plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); // Is type Ground, so should not need instances - tree.nodes[node->node_id].children.push_back(child->node_id); - } - else { - std::cerr << "function modifier for total-cost not supported" << std::endl; - } - - plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace, instances_map); // Does it need instances? +plansys2_msgs::msg::Node::SharedPtr FunctionModifier::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION_MODIFIER; + node->modifier_type = getFunModType(name); + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (modifiedGround) { +// Is type Ground, so should not need instances + plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); tree.nodes[node->node_id].children.push_back(child->node_id); + } else { + std::cerr << "function modifier for total-cost not supported" << std::endl; + } + + plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace, + instances_map); // Does it need instances? + tree.nodes[node->node_id].children.push_back(child->node_id); return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp index 86d4106dd..b9f142bd4 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp @@ -47,7 +47,8 @@ void Ground::PDDLPrint( } plansys2_msgs::msg::Node::SharedPtr Ground::getTree( - plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map) const + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, + const std::map> & instances_map) const { plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); if (d.funcs.index(name) >= 0) { diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp index dc0b05a9b..347ff9338 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp @@ -40,18 +40,28 @@ void GroundFunc::PDDLPrint( s << " " << d.types[(reinterpret_cast(lifted))->returnType]->object(value) << " )"; } -template <> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - auto node = TypeGround::getTree(tree, d, replace); // Ground should not need instances, so is not passed - node->value = value; - return node; +template<> +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + auto node = TypeGround::getTree(tree, d, replace); + // Ground doesn't need instances, so is not passed + node->value = value; + return node; } -template <> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - auto node = TypeGround::getTree(tree, d, replace); // Ground should not need instances, so is not passed - node->value = value; - return node; +template<> +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + auto node = TypeGround::getTree(tree, d, replace); + // Ground doesn't need instances, so is not passed + node->value = value; + return node; } template<> diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp index e9cb986fa..2e1feff89 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Imply.cpp @@ -29,70 +29,72 @@ void Imply::PDDLPrint( tabindent(s, indent + 1); printParams(0, s, fstruct, d); - if ( left ) left->PDDLPrint( s, indent + 1, fstruct, d ); - if (right) right->PDDLPrint( s, indent + 1, fstruct, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } + if (left) {left->PDDLPrint(s, indent + 1, fstruct, d);} + if (right) {right->PDDLPrint(s, indent + 1, fstruct, d);} else { + tabindent(s, indent + 1); + s << "()"; + } s << "\n"; tabindent(s, indent); s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Imply::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::IMPLY; - node->node_id = tree.nodes.size(); - node->name = name; - - for (unsigned i = 0; i < params.size(); ++i) { - plansys2_msgs::msg::Param param; - if (i < replace.size()){ - if (params[i] >= 0) // param has a variable value; replace by action-args - { - param.name = replace[i]; - } - } else { - param.name = "?" + std::to_string(params[i]); - } - param.type = d.types[params[i]]->name; - node->parameters.push_back(param); - } - - tree.nodes.push_back(*node); +plansys2_msgs::msg::Node::SharedPtr Imply::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::IMPLY; + node->node_id = tree.nodes.size(); + node->name = name; + + for (unsigned i = 0; i < params.size(); ++i) { + plansys2_msgs::msg::Param param; + if (i < replace.size()) { + if (params[i] >= 0) { // param has a variable value; replace by action-args + param.name = replace[i]; + } + } else { + param.name = "?" + std::to_string(params[i]); + } + param.type = d.types[params[i]]->name; + node->parameters.push_back(param); + } + + tree.nodes.push_back(*node); - if (left){ - plansys2_msgs::msg::Node::SharedPtr child_1 = left->getTree(tree, d, replace); // Could conditions need instances? - tree.nodes[node->node_id].children.push_back(child_1->node_id); - } + if (left) { + plansys2_msgs::msg::Node::SharedPtr child_1 = left->getTree(tree, d, replace); + // Could conditions need instances? + tree.nodes[node->node_id].children.push_back(child_1->node_id); + } - if(right){ - plansys2_msgs::msg::Node::SharedPtr child_2 = right->getTree(tree, d, replace); // Could conditions need instances? - tree.nodes[node->node_id].children.push_back(child_2->node_id); - } - return node; + if(right) { + plansys2_msgs::msg::Node::SharedPtr child_2 = right->getTree(tree, d, replace); + // Could conditions need instances? + tree.nodes[node->node_id].children.push_back(child_2->node_id); + } + return node; } -void Imply::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); - params = d.convertTypes(ts.types); - if (f.getChar() != ')' ) { - left = d.createCondition( f ); - left->parse( f, ts, d ); - } - else ++f.c; +void Imply::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); + params = d.convertTypes(ts.types); + if (f.getChar() != ')') { + left = d.createCondition(f); + left->parse(f, ts, d); + } else {++f.c;} - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - right = d.createCondition( f ); - right->parse( f, ts, d ); - } - else ++f.c; + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + right = d.createCondition(f); + right->parse(f, ts, d); + } else {++f.c;} f.next(); f.assert_token(")"); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp index b55e9cc75..892156e84 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp @@ -34,8 +34,12 @@ void Lifted::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Lifted"); +plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("Lifted"); } void Lifted::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp index fc64ee6c2..823a549ad 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp @@ -27,16 +27,20 @@ void Not::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Not::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::NOT; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (cond) { - plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace, instances_map); - tree.nodes[node->node_id].children.push_back(child->node_id); - } +plansys2_msgs::msg::Node::SharedPtr Not::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::NOT; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (cond) { + plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace, instances_map); + tree.nodes[node->node_id].children.push_back(child->node_id); + } return node; } diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp index e6d3c460c..95f01401e 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp @@ -31,8 +31,12 @@ void Oneof::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("Oneof"); +plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("Oneof"); } void Oneof::parse(Stringreader & f, TokenStruct & ts, Domain & d) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp index 19a89c543..22e06e7e2 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp @@ -41,7 +41,11 @@ void Or::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Or::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { +plansys2_msgs::msg::Node::SharedPtr Or::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::OR; node->node_id = tree.nodes.size(); diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp index 1d31f71f9..482db94cf 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp @@ -70,8 +70,12 @@ void TemporalAction::PDDLPrint( s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("TemporalAction"); +plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("TemporalAction"); } void TemporalAction::parseCondition( diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp index 5b2ba4f0a..8233ab877 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp @@ -29,8 +29,12 @@ void TypeGround::PDDLPrint( s << " )"; } -plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - return Ground::getTree(tree, d, replace); // Ground should not need instances, so is not passed +plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + return Ground::getTree(tree, d, replace); // Ground doesn't instances, so is not passed } void TypeGround::insert(Domain & d, const StringVec & v) diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp index 62e92784b..a2b857a3d 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp @@ -41,8 +41,12 @@ void When::PDDLPrint( s << ")"; } -plansys2_msgs::msg::Node::SharedPtr When::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace, const std::map> & instances_map ) const { - throw UnsupportedConstruct("When"); +plansys2_msgs::msg::Node::SharedPtr When::getTree( + plansys2_msgs::msg::Tree & tree, + const Domain & d, const std::vector & replace, + const std::map> & instances_map) const +{ + throw UnsupportedConstruct("When"); } void When::parse(Stringreader & f, TokenStruct & ts, Domain & d)