<|start_filename|>plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp<|end_filename|>

#include "plansys2_pddl_parser/Domain.h"

namespace parser { namespace pddl {

void ParamCond::printParams( unsigned first, std::ostream & s, TokenStruct< std::string > & ts, const Domain & d ) const {
	s << "(";
	for ( unsigned i = first; i < params.size(); ++i ) {
		std::stringstream ss;
		ss << "?" << d.types[params[i]]->getName() << ts.size();
		ts.insert( ss.str() );
		s << " " << ss.str();
		if ( d.typed ) s << " - " << d.types[params[i]]->name;
	}
	s << " )\n";
}

} } // namespaces


<|start_filename|>plansys2_core/include/plansys2_core/Utils.hpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PLANSYS2_CORE__UTILS_HPP_
#define PLANSYS2_CORE__UTILS_HPP_

#include <string>
#include <vector>

namespace plansys2
{

std::vector<std::string> tokenize(const std::string & string, const std::string & delim);

/**
 * @brief get a substring without empty lines
 *
 * @param string original string
 * @param init_pos first character in the original string
 * @param end_pos last character in the original string
 * @return a substring without empty lines
 */
std::string substr_without_empty_lines(
  std::string string,
  std::size_t init_pos,
  std::size_t end_pos);

/**
 * @brief remove the comments on a pddl string
 *
 * @param pddl a pddl string
 * @return a pddl string without comments
 */
std::string remove_comments(const std::string & pddl);

}  // namespace plansys2

#endif  // PLANSYS2_CORE__UTILS_HPP_


<|start_filename|>plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp<|end_filename|>
// Copyright 2020 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_
#define PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_

#include <tuple>
#include <memory>
#include <string>
#include <map>
#include <vector>
#include <set>
#include <utility>

#include "plansys2_problem_expert/ProblemExpertClient.hpp"
#include "plansys2_domain_expert/DomainExpertClient.hpp"
#include "plansys2_msgs/msg/tree.hpp"

namespace plansys2
{

/// Evaluate a PDDL expression represented as a tree.
/**
 * \param[in] node The root node of the PDDL expression.
 * \param[in] problem_client The problem expert client.
 * \param[in] predicates Current predicates state.
 * \param[in] functions Current functions state.
 * \param[in] apply Apply result to problem expert or state.
 * \param[in] use_state Use state representation or problem client.
 * \param[in] negate Invert the truth value.
 * \return result <- tuple(bool, bool, double)
 *         result(0) true if success
 *         result(1) truth value of boolen expression
 *         result(2) value of numeric expression
 */
std::tuple<bool, bool, double> evaluate(
  const plansys2_msgs::msg::Tree & tree,
  std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
  std::vector<plansys2::Predicate> & predicates,
  std::vector<plansys2::Function> & functions,
  bool apply = false,
  bool use_state = false,
  uint8_t node_id = 0,
  bool negate = false);

std::tuple<bool, bool, double> evaluate(
  const plansys2_msgs::msg::Tree & tree,
  std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
  bool apply = false,
  uint32_t node_id = 0);

std::tuple<bool, bool, double> evaluate(
  const plansys2_msgs::msg::Tree & tree,
  std::vector<plansys2::Predicate> & predicates,
  std::vector<plansys2::Function> & functions,
  bool apply = false,
  uint32_t node_id = 0);

/// Check a PDDL expression represented as a tree.
/**
* \param[in] node The root node of the PDDL expression.
* \param[in] problem_client The problem expert client.
* \return ret Truth value of the PDDL expression.
*
* This function calls the evaluate function.
*/
bool check(
  const plansys2_msgs::msg::Tree & tree,
  std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
  uint32_t node_id = 0);

bool check(
  const plansys2_msgs::msg::Tree & tree,
  std::vector<plansys2::Predicate> & predicates,
  std::vector<plansys2::Function> & functions,
  uint32_t node_id = 0);

/// Apply a PDDL expression represented as a tree.
/**
 * \param[in] node The root node of the PDDL expression.
 * \param[in] problem_client The problem expert client.
 * \return success Indicates whether the execution was successful.
 *
 * This function calls the evaluate function.
 */
bool apply(
  const plansys2_msgs::msg::Tree & tree,
  std::shared_ptr<plansys2::ProblemExpertClient> problem_client,
  uint32_t node_id = 0);

bool apply(
  const plansys2_msgs::msg::Tree & tree,
  std::vector<plansys2::Predicate> & predicates,
  std::vector<plansys2::Function> & functions,
  uint32_t node_id = 0);

/// Parse the action expression and time (optional) from an input string.
/**
* \param[in] input The input string.
* \return result <- pair(string, int)
*         result(0) The action expression.
*         result(1) The action start time.
*
* The input string can have either of the following formats.
*   "(<name> <param_1> ... <param_n>)"
*   "(<name> <param_1> ... <param_n>):<time>"
 * The output action expression will have the following format.
 *   "<name> <param_1> ... <param_n>"
*/
std::pair<std::string, int> parse_action(const std::string & input);

/// Parse the action expression from an input string.
/**
 * \param[in] input The input string.
 * \return The action expression.
 *
 * The input string can have either of the following formats.
 *   "(<name> <param_1> ... <param_n>)"
 *   "(<name> <param_1> ... <param_n>):<time>"
 */
std::string get_action_expression(const std::string & input);

/// Parse the action time from an input string.
/**
 * \param[in] input The input string.
 * \return The action start time.
 *
 * The input string can have either of the following formats.
 *   "(<name> <param_1> ... <param_n>)"
 *   "(<name> <param_1> ... <param_n>):<time>"
 */
int get_action_time(const std::string & input);

/// Parse the action name from an input string.
/**
 * \param[in] input The input string.
 * \return The name of the action.
 *
 * The input string can have either of the following formats.
 *   "(<name> <param_1> ... <param_n>)"
 *   "(<name> <param_1> ... <param_n>):<time>"
 */
std::string get_action_name(const std::string & input);

/// Parse the action parameter names from an input string.
/**
 * \param[in] input The input string.
 * \return A list of the action parameter names.
 *
 * The input string can have either of the following formats.
 *   "(<name> <param_1> ... <param_n>)"
 *   "(<name> <param_1> ... <param_n>):<time>"
 */
std::vector<std::string> get_action_params(const std::string & action_expr);

}  // namespace plansys2

#endif  // PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.h<|end_filename|>

#pragma once

#include "plansys2_msgs/msg/node.hpp"
#include "plansys2_msgs/msg/tree.hpp"

#include "plansys2_pddl_parser/Lifted.h"

namespace parser { namespace pddl {

class Ground : public ParamCond {

public:

	Lifted * lifted;

	Ground()
		: ParamCond(), lifted( 0 ) {}

	Ground( const std::string s, const IntVec & p = IntVec() )
		: ParamCond( s, p ), lifted( 0 ) {}

	Ground( Lifted * l, const IntVec & p = IntVec() )
		: ParamCond( l->name, p ), lifted( l ) {}

	Ground( const Ground * g, Domain & d );

	void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override;

	plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace = {} ) const override;

	void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d );

	void addParams( int m, unsigned n ) {
		for ( unsigned i = 0; i < params.size(); ++i )
			if ( params[i] >= m ) params[i] += n;
	}

	Condition * copy( Domain & d ) {
		return new Ground( this, d );
	}

};

typedef std::vector< Ground * > GroundVec;

} } // namespaces


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.h<|end_filename|>

#pragma once

#include "plansys2_pddl_parser/Type.h"

namespace parser { namespace pddl {

class EitherType : public Type {

public:

	EitherType( const std::string & s )
		: Type( s ) {}

	EitherType( const EitherType * t )
		: Type( t ) {}
	
	std::string getName() const {
		std::string out = "either";
		for ( unsigned i = 0; i < subtypes.size(); ++i )
			out += "_" + subtypes[i]->getName();
		return out;
	}

	void PDDLPrint( std::ostream & s ) const override {}

	Type * copy() {
		return new EitherType( this );
	}

};

} } // namespaces


<|start_filename|>plansys2_domain_expert/test/unit/types_test.cpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <vector>
#include <memory>

#include "gtest/gtest.h"
#include "plansys2_pddl_parser/Utils.h"

#include "plansys2_msgs/msg/node.hpp"
#include "plansys2_msgs/msg/param.hpp"

TEST(domain_types, basic_types)
{
  plansys2_msgs::msg::Node predicate_1;
  predicate_1.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_1.name = "robot_at";
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));

  ASSERT_EQ(parser::pddl::toString(predicate_1), "(robot_at r2d2 bedroom)");
}

TEST(domain_types, predicate_tree_to_string)
{
  plansys2_msgs::msg::Node predicate_1;
  predicate_1.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_1.node_id = 0;
  predicate_1.name = "robot_at";
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_1.negate = false;

  plansys2_msgs::msg::Node predicate_2;
  predicate_2.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_2.node_id = 0;
  predicate_2.name = "robot_at";
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_2.negate = false;

  plansys2_msgs::msg::Node predicate_3;
  predicate_3.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_3.node_id = 0;
  predicate_3.name = "person_at";
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_3.negate = false;

  plansys2_msgs::msg::Node predicate_4;
  predicate_4.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_4.node_id = 0;
  predicate_4.name = "person_at";
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_4.negate = false;

  plansys2_msgs::msg::Tree pn_1;
  pn_1.nodes.push_back(predicate_1);

  ASSERT_EQ(parser::pddl::toString(pn_1), "(robot_at r2d2 bedroom)");

  plansys2_msgs::msg::Tree pn_2;
  pn_2.nodes.push_back(predicate_2);

  ASSERT_EQ(parser::pddl::toString(pn_2), "(robot_at r2d2 kitchen)");

  plansys2_msgs::msg::Tree pn_3;
  pn_3.nodes.push_back(predicate_3);

  ASSERT_EQ(parser::pddl::toString(pn_3), "(person_at paco bedroom)");

  plansys2_msgs::msg::Tree pn_4;
  pn_4.nodes.push_back(predicate_4);

  ASSERT_EQ(parser::pddl::toString(pn_4), "(person_at paco kitchen)");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees;
  not_subtrees.push_back(pn_2);
  auto not_tree = parser::pddl::fromSubtrees(not_subtrees, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(parser::pddl::toString(*not_tree), "(not (robot_at r2d2 kitchen))");

  std::vector<plansys2_msgs::msg::Tree> or_subtrees;
  or_subtrees.push_back(pn_3);
  or_subtrees.push_back(pn_4);
  auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR);

  ASSERT_EQ(
    parser::pddl::toString(
      *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))");

  std::vector<plansys2_msgs::msg::Tree> and_subtrees;
  and_subtrees.push_back(pn_1);
  and_subtrees.push_back(*not_tree);
  and_subtrees.push_back(*or_tree);
  auto and_tree = parser::pddl::fromSubtrees(and_subtrees, plansys2_msgs::msg::Node::AND);

  ASSERT_EQ(
    parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)(not ") +
    std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))"));
}

TEST(domain_types, predicate_tree_to_string_2)
{
  plansys2_msgs::msg::Node predicate_1;
  predicate_1.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_1.node_id = 0;
  predicate_1.name = "robot_at";
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_1.negate = false;

  plansys2_msgs::msg::Node predicate_2;
  predicate_2.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_2.node_id = 0;
  predicate_2.name = "robot_at";
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_2.negate = false;

  plansys2_msgs::msg::Node predicate_3;
  predicate_3.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_3.node_id = 0;
  predicate_3.name = "person_at";
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_3.negate = false;

  plansys2_msgs::msg::Node predicate_4;
  predicate_4.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_4.node_id = 0;
  predicate_4.name = "person_at";
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_4.negate = false;

  plansys2_msgs::msg::Tree pn_1;
  pn_1.nodes.push_back(predicate_1);

  ASSERT_EQ(parser::pddl::toString(pn_1), "(robot_at r2d2 bedroom)");

  plansys2_msgs::msg::Tree pn_2;
  pn_2.nodes.push_back(predicate_2);

  ASSERT_EQ(parser::pddl::toString(pn_2), "(robot_at r2d2 kitchen)");

  plansys2_msgs::msg::Tree pn_3;
  pn_3.nodes.push_back(predicate_3);

  ASSERT_EQ(parser::pddl::toString(pn_3), "(person_at paco bedroom)");

  plansys2_msgs::msg::Tree pn_4;
  pn_4.nodes.push_back(predicate_4);

  ASSERT_EQ(parser::pddl::toString(pn_4), "(person_at paco kitchen)");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees;
  not_subtrees.push_back(pn_2);
  auto not_tree = parser::pddl::fromSubtrees(not_subtrees, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(parser::pddl::toString(*not_tree), "(not (robot_at r2d2 kitchen))");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees_2;
  not_subtrees_2.push_back(*not_tree);
  auto not_tree_2 = parser::pddl::fromSubtrees(not_subtrees_2, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(parser::pddl::toString(*not_tree_2), "(robot_at r2d2 kitchen)");

  std::vector<plansys2_msgs::msg::Tree> or_subtrees;
  or_subtrees.push_back(pn_3);
  or_subtrees.push_back(pn_4);
  auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR);

  ASSERT_EQ(
    parser::pddl::toString(
      *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))");

  std::vector<plansys2_msgs::msg::Tree> and_subtrees;
  and_subtrees.push_back(pn_1);
  and_subtrees.push_back(*not_tree_2);
  and_subtrees.push_back(*or_tree);
  auto and_tree = parser::pddl::fromSubtrees(and_subtrees, plansys2_msgs::msg::Node::AND);

  ASSERT_EQ(
    parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)") +
    std::string("(robot_at r2d2 kitchen)(or (person_at paco bedroom)(person_at paco kitchen)))"));
}

TEST(domain_types, predicate_tree_to_string_3)
{
  plansys2_msgs::msg::Node predicate_1;
  predicate_1.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_1.node_id = 0;
  predicate_1.name = "robot_at";
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_1.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_1.negate = false;

  plansys2_msgs::msg::Node predicate_2;
  predicate_2.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_2.node_id = 0;
  predicate_2.name = "robot_at";
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("r2d2", "robot"));
  predicate_2.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_2.negate = false;

  plansys2_msgs::msg::Node predicate_3;
  predicate_3.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_3.node_id = 0;
  predicate_3.name = "person_at";
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_3.parameters.push_back(parser::pddl::fromStringParam("bedroom", "room"));
  predicate_3.negate = false;

  plansys2_msgs::msg::Node predicate_4;
  predicate_4.node_type = plansys2_msgs::msg::Node::PREDICATE;
  predicate_4.node_id = 0;
  predicate_4.name = "person_at";
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("paco", "person"));
  predicate_4.parameters.push_back(parser::pddl::fromStringParam("kitchen", "room"));
  predicate_4.negate = false;

  plansys2_msgs::msg::Tree pn_1;
  pn_1.nodes.push_back(predicate_1);

  ASSERT_EQ(parser::pddl::toString(pn_1), "(robot_at r2d2 bedroom)");

  plansys2_msgs::msg::Tree pn_2;
  pn_2.nodes.push_back(predicate_2);

  ASSERT_EQ(parser::pddl::toString(pn_2), "(robot_at r2d2 kitchen)");

  plansys2_msgs::msg::Tree pn_3;
  pn_3.nodes.push_back(predicate_3);

  ASSERT_EQ(parser::pddl::toString(pn_3), "(person_at paco bedroom)");

  plansys2_msgs::msg::Tree pn_4;
  pn_4.nodes.push_back(predicate_4);

  ASSERT_EQ(parser::pddl::toString(pn_4), "(person_at paco kitchen)");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees;
  not_subtrees.push_back(pn_2);
  auto not_tree = parser::pddl::fromSubtrees(not_subtrees, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(parser::pddl::toString(*not_tree), "(not (robot_at r2d2 kitchen))");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees_2;
  not_subtrees_2.push_back(*not_tree);
  auto not_tree_2 = parser::pddl::fromSubtrees(not_subtrees_2, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(parser::pddl::toString(*not_tree_2), "(robot_at r2d2 kitchen)");

  std::vector<plansys2_msgs::msg::Tree> or_subtrees;
  or_subtrees.push_back(pn_3);
  or_subtrees.push_back(pn_4);
  auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR);

  ASSERT_EQ(
    parser::pddl::toString(
      *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))");

  std::vector<plansys2_msgs::msg::Tree> not_subtrees_3;
  not_subtrees_3.push_back(*or_tree);
  auto not_tree_3 = parser::pddl::fromSubtrees(not_subtrees_3, plansys2_msgs::msg::Node::NOT);

  ASSERT_EQ(
    parser::pddl::toString(*not_tree_3),
    "(and (not (person_at paco bedroom))(not (person_at paco kitchen)))");

  std::vector<plansys2_msgs::msg::Tree> and_subtrees;
  and_subtrees.push_back(pn_1);
  and_subtrees.push_back(*not_tree_2);
  and_subtrees.push_back(*not_tree_3);
  auto and_tree = parser::pddl::fromSubtrees(and_subtrees, plansys2_msgs::msg::Node::AND);

  ASSERT_EQ(
    parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)") +
    std::string("(robot_at r2d2 kitchen)(and (not (person_at paco ") +
    std::string("bedroom))(not (person_at paco kitchen))))"));
}

TEST(domain_types, predicate_tree_from_string)
{
  std::string expresion = std::string("(and (robot_at r2d2 bedroom)(not ") +
    std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))");

  plansys2_msgs::msg::Tree tree;
  parser::pddl::fromString(tree, expresion);

  plansys2_msgs::msg::Node and_node = tree.nodes[0];
  ASSERT_EQ(and_node.node_type, plansys2_msgs::msg::Node::AND);
  ASSERT_FALSE(and_node.negate);

  plansys2_msgs::msg::Node p1_node = tree.nodes[1];
  ASSERT_EQ(p1_node.node_type, plansys2_msgs::msg::Node::PREDICATE);
  ASSERT_EQ(p1_node.name, "robot_at");
  ASSERT_EQ(p1_node.parameters[0].name, "r2d2");
  ASSERT_EQ(p1_node.parameters[1].name, "bedroom");
  ASSERT_FALSE(p1_node.negate);

  plansys2_msgs::msg::Node pnot_node = tree.nodes[2];
  ASSERT_EQ(pnot_node.node_type, plansys2_msgs::msg::Node::NOT);
  ASSERT_FALSE(pnot_node.negate);

  plansys2_msgs::msg::Node p2_node = tree.nodes[3];
  ASSERT_EQ(p2_node.node_type, plansys2_msgs::msg::Node::PREDICATE);
  ASSERT_EQ(p2_node.name, "robot_at");
  ASSERT_EQ(p2_node.parameters[0].name, "r2d2");
  ASSERT_EQ(p2_node.parameters[1].name, "kitchen");
  ASSERT_TRUE(p2_node.negate);

  ASSERT_EQ(parser::pddl::toString(tree), expresion);

  std::string expresion2 = std::string("(and (person_at ?0 ?2)(not (person_at ?0 ?1)))");
  plansys2_msgs::msg::Tree tree2;
  parser::pddl::fromString(tree2, expresion2);

  ASSERT_EQ(parser::pddl::toString(tree2), expresion2);
}

TEST(domain_types, predicate_tree_from_string_2)
{
  std::string expresion = std::string("(not (and (robot_at r2d2 bedroom)") +
    std::string("(robot_at r2d2 kitchen)))");

  plansys2_msgs::msg::Tree tree;
  parser::pddl::fromString(tree, expresion);

  plansys2_msgs::msg::Node not_node = tree.nodes[0];
  ASSERT_EQ(not_node.node_type, plansys2_msgs::msg::Node::NOT);
  ASSERT_FALSE(not_node.negate);

  plansys2_msgs::msg::Node and_node = tree.nodes[1];
  ASSERT_EQ(and_node.node_type, plansys2_msgs::msg::Node::AND);
  ASSERT_TRUE(and_node.negate);

  plansys2_msgs::msg::Node p1_node = tree.nodes[2];
  ASSERT_EQ(p1_node.node_type, plansys2_msgs::msg::Node::PREDICATE);
  ASSERT_EQ(p1_node.name, "robot_at");
  ASSERT_EQ(p1_node.parameters[0].name, "r2d2");
  ASSERT_EQ(p1_node.parameters[1].name, "bedroom");
  ASSERT_TRUE(and_node.negate);

  plansys2_msgs::msg::Node p2_node = tree.nodes[3];
  ASSERT_EQ(p2_node.node_type, plansys2_msgs::msg::Node::PREDICATE);
  ASSERT_EQ(p2_node.name, "robot_at");
  ASSERT_EQ(p2_node.parameters[0].name, "r2d2");
  ASSERT_EQ(p2_node.parameters[1].name, "kitchen");
  ASSERT_TRUE(p2_node.negate);

  std::string expresion_eq = std::string("(or (not (robot_at r2d2 bedroom))") +
    std::string("(not (robot_at r2d2 kitchen)))");

  ASSERT_EQ(parser::pddl::toString(tree), expresion_eq);

  std::string expresion2 = std::string("(and (person_at ?0 ?2)(not (person_at ?0 ?1)))");
  plansys2_msgs::msg::Tree tree2;
  parser::pddl::fromString(tree2, expresion2);

  ASSERT_EQ(parser::pddl::toString(tree2), expresion2);
}

TEST(domain_types, predicate_tree_from_string_3)
{
  std::string expresion = std::string("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))");
  plansys2_msgs::msg::Tree tree;
  parser::pddl::fromString(tree, expresion);

  ASSERT_EQ(parser::pddl::toString(tree), "(and (patrolled ro1)(patrolled ro2)(patrolled ro3))");
}

TEST(domain_types, predicate_tree_from_string_4)
{
  std::string expresion = std::string("  (  and (patrolled ro1) (patrolled ro2) (patrolled ro3))");
  plansys2_msgs::msg::Tree tree;
  parser::pddl::fromString(tree, expresion);

  ASSERT_EQ(parser::pddl::toString(tree), "(and (patrolled ro1)(patrolled ro2)(patrolled ro3))");
}

TEST(domain_types, predicate_tree_from_string_negative)
{
  std::string expresion = std::string("(and (robot_at r2d2 bedroom)(not ") +
    std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))");

  plansys2_msgs::msg::Tree tree;
  parser::pddl::fromString(tree, expresion);

  ASSERT_EQ(parser::pddl::toString(tree), expresion);

  std::string expresion2 = std::string("(and (person_at ?0 ?2)(not (person_at ?0 ?1)))");
  plansys2_msgs::msg::Tree tree2;
  parser::pddl::fromString(tree2, expresion2);

  ASSERT_EQ(parser::pddl::toString(tree2), expresion2);
}

TEST(domain_types, get_predicates)
{
  std::string expresion_1 = std::string("(and (robot_at r2d2 bedroom)(not ") +
    std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))");

  plansys2_msgs::msg::Tree tree_1;
  parser::pddl::fromString(tree_1, expresion_1);

  std::vector<plansys2_msgs::msg::Node> predicates_1;
  parser::pddl::getPredicates(predicates_1, tree_1);

  ASSERT_EQ(4u, predicates_1.size());
  ASSERT_EQ(predicates_1[0].name, "robot_at");
  ASSERT_EQ(predicates_1[0].parameters[0].name, "r2d2");
  ASSERT_EQ(predicates_1[0].parameters[1].name, "bedroom");
  ASSERT_FALSE(predicates_1[0].negate);

  ASSERT_EQ(predicates_1[1].name, "robot_at");
  ASSERT_EQ(predicates_1[1].parameters[0].name, "r2d2");
  ASSERT_EQ(predicates_1[1].parameters[1].name, "kitchen");
  ASSERT_TRUE(predicates_1[1].negate);

  ASSERT_EQ(predicates_1[2].name, "person_at");
  ASSERT_EQ(predicates_1[2].parameters[0].name, "paco");
  ASSERT_EQ(predicates_1[2].parameters[1].name, "bedroom");
  ASSERT_FALSE(predicates_1[2].negate);

  ASSERT_EQ(predicates_1[3].name, "person_at");
  ASSERT_EQ(predicates_1[3].parameters[0].name, "paco");
  ASSERT_EQ(predicates_1[3].parameters[1].name, "kitchen");
  ASSERT_FALSE(predicates_1[3].negate);
}

int main(int argc, char ** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


<|start_filename|>plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp<|end_filename|>

#include "plansys2_pddl_parser/Domain.h"

namespace parser { namespace pddl {

void Or::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const {
	tabindent( s, indent );
	s << "( or\n";
	if ( first ) first->PDDLPrint( s, indent + 1, ts, d );
	else {
		tabindent( s, indent + 1 );
		s << "()";
	}
	s << "\n";
	if ( second ) second->PDDLPrint( s, indent + 1, ts, d );
	else {
		tabindent( s, indent + 1 );
		s << "()";
	}
	s << "\n";
	tabindent( s, indent );
	s << ")";
}

plansys2_msgs::msg::Node::SharedPtr Or::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace ) const {
    throw UnsupportedConstruct("Or");
}

void Or::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {
	f.next();
	f.assert_token( "(" );
	if ( f.getChar() != ')' ) {
		first = d.createCondition( f );
		first->parse( f, ts, d );
	}
	else ++f.c;

	f.next();
	f.assert_token( "(" );
	if ( f.getChar() != ')' ) {
		second = d.createCondition( f );
		second->parse( f, ts, d );
	}
	else ++f.c;

	f.next();
	f.assert_token( ")" );
}


} } // namespaces


<|start_filename|>plansys2_core/include/plansys2_core/Types.hpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PLANSYS2_CORE__TYPES_HPP_
#define PLANSYS2_CORE__TYPES_HPP_

#include <string>
#include <vector>

#include "plansys2_msgs/msg/node.hpp"
#include "plansys2_msgs/msg/param.hpp"
#include "plansys2_msgs/msg/tree.hpp"

#include "plansys2_pddl_parser/Utils.h"

namespace plansys2
{

class Instance : public plansys2_msgs::msg::Param
{
public:
  Instance()
  : plansys2_msgs::msg::Param() {}
  explicit Instance(const std::string & name, const std::string & type = {})
  : plansys2_msgs::msg::Param(parser::pddl::fromStringParam(name, type)) {}
  Instance(const plansys2_msgs::msg::Param & instance)  // NOLINT(runtime/explicit)
  : plansys2_msgs::msg::Param(instance) {}
};

class Predicate : public plansys2_msgs::msg::Node
{
public:
  Predicate()
  : plansys2_msgs::msg::Node() {}
  explicit Predicate(const std::string & pred)
  : plansys2_msgs::msg::Node(parser::pddl::fromStringPredicate(pred)) {}
  Predicate(const plansys2_msgs::msg::Node & pred)  // NOLINT(runtime/explicit)
  : plansys2_msgs::msg::Node(pred) {}
};

class Function : public plansys2_msgs::msg::Node
{
public:
  Function()
  : plansys2_msgs::msg::Node() {}
  explicit Function(const std::string & func)
  : plansys2_msgs::msg::Node(parser::pddl::fromStringFunction(func)) {}
  Function(const plansys2_msgs::msg::Node & func)  // NOLINT(runtime/explicit)
  : plansys2_msgs::msg::Node(func) {}
};

class Goal : public plansys2_msgs::msg::Tree
{
public:
  Goal()
  : plansys2_msgs::msg::Tree() {}
  explicit Goal(const std::string & goal)
  : plansys2_msgs::msg::Tree(parser::pddl::fromString(goal)) {}
  Goal(const plansys2_msgs::msg::Tree & goal)  // NOLINT(runtime/explicit)
  : plansys2_msgs::msg::Tree(goal) {}
};

template<class toT, class fromT>
std::vector<toT>
convertVector(const std::vector<fromT> & in_vector)
{
  std::vector<toT> ret;
  for (const auto & item : in_vector) {
    ret.push_back(item);
  }
  return ret;
}

}  // namespace plansys2

#endif  // PLANSYS2_CORE__TYPES_HPP_


<|start_filename|>plansys2_tests/src/plansys2_tests/test_action_node.cpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>

#include "plansys2_tests/test_action_node.hpp"

#include "plansys2_executor/ActionExecutorClient.hpp"

#include "rclcpp/rclcpp.hpp"


namespace plansys2_tests
{

int TestAction::node_name_counter_ = 0;

TestAction::TestAction(
  const std::string & action_name, const std::chrono::seconds & rate, float increment)
: plansys2::ActionExecutorClient(action_name + std::to_string(node_name_counter_++),
    rate), increment_(increment)
{
  progress_ = 0.0;
}

void
TestAction::do_work()
{
  if (progress_ < 1.0) {
    progress_ += increment_;
    send_feedback(progress_, "Action running");
  } else {
    finish(true, 1.0, "Action completed");
    progress_ = 0.0;
  }
}

}  // namespace plansys2_tests


<|start_filename|>plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <iostream>

#include "CloseGripper.hpp"

#include "behaviortree_cpp_v3/behavior_tree.h"

namespace plansys2_bt_tests
{

CloseGripper::CloseGripper(
  const std::string & xml_tag_name,
  const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), counter_(0)
{
}

void
CloseGripper::halt()
{
  std::cout << "CloseGripper halt" << std::endl;
}

BT::NodeStatus
CloseGripper::tick()
{
  std::cout << "CloseGripper tick " << counter_ << std::endl;

  if (counter_++ < 5) {
    return BT::NodeStatus::RUNNING;
  } else {
    counter_ = 0;
    return BT::NodeStatus::SUCCESS;
  }
}

}  // namespace plansys2_bt_tests

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<plansys2_bt_tests::CloseGripper>("CloseGripper");
}


<|start_filename|>plansys2_tests/test_3/test_3.cpp<|end_filename|>
// Copyright 2021 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <string>

#include "ament_index_cpp/get_package_share_directory.hpp"

#include "gtest/gtest.h"
#include "plansys2_domain_expert/DomainExpertNode.hpp"
#include "plansys2_domain_expert/DomainExpertClient.hpp"
#include "plansys2_problem_expert/ProblemExpertNode.hpp"
#include "plansys2_problem_expert/ProblemExpertClient.hpp"
#include "plansys2_planner/PlannerNode.hpp"
#include "plansys2_planner/PlannerClient.hpp"
#include "plansys2_executor/ExecutorNode.hpp"
#include "plansys2_executor/ExecutorClient.hpp"
#include "plansys2_pddl_parser/Utils.h"

#include "plansys2_tests/test_action_node.hpp"
#include "plansys2_tests/execution_logger.hpp"

TEST(test_3, test_3)
{
  auto test_node = rclcpp::Node::make_shared("test_node");
  auto domain_node = std::make_shared<plansys2::DomainExpertNode>();
  auto problem_node = std::make_shared<plansys2::ProblemExpertNode>();
  auto planner_node = std::make_shared<plansys2::PlannerNode>();
  auto executor_node = std::make_shared<plansys2::ExecutorNode>();

  auto domain_client = std::make_shared<plansys2::DomainExpertClient>();
  auto problem_client = std::make_shared<plansys2::ProblemExpertClient>();
  auto planner_client = std::make_shared<plansys2::PlannerClient>();
  auto executor_client = std::make_shared<plansys2::ExecutorClient>();

  auto move_1 = plansys2_tests::TestAction::make_shared("move");
  auto move_2 = plansys2_tests::TestAction::make_shared("move");
  auto move_3 = plansys2_tests::TestAction::make_shared("move");
  auto transport_1 = plansys2_tests::TestAction::make_shared("transport");
  auto transport_2 = plansys2_tests::TestAction::make_shared("transport");
  auto transport_3 = plansys2_tests::TestAction::make_shared("transport");
  auto assemble_1 = plansys2_tests::TestAction::make_shared("assemble");
  auto assemble_2 = plansys2_tests::TestAction::make_shared("assemble");
  auto assemble_3 = plansys2_tests::TestAction::make_shared("assemble");

  auto execution_logger = plansys2_tests::ExecutionLogger::make_shared();

  std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_tests");

  domain_node->set_parameter({"model_file", pkgpath + "/test_3/pddl/test_3.pddl"});
  problem_node->set_parameter({"model_file", pkgpath + "/test_3/pddl/test_3.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());
  exe.add_node(planner_node->get_node_base_interface());
  exe.add_node(executor_node->get_node_base_interface());
  exe.add_node(execution_logger->get_node_base_interface());

  exe.add_node(move_1->get_node_base_interface());
  exe.add_node(move_2->get_node_base_interface());
  exe.add_node(move_3->get_node_base_interface());
  exe.add_node(transport_1->get_node_base_interface());
  exe.add_node(transport_2->get_node_base_interface());
  exe.add_node(transport_3->get_node_base_interface());
  exe.add_node(assemble_1->get_node_base_interface());
  exe.add_node(assemble_2->get_node_base_interface());
  exe.add_node(assemble_3->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);
  planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
  executor_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);
  planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
  executor_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();
    }
  }

  problem_client->addInstance(plansys2::Instance("robot1", "robot"));
  problem_client->addInstance(plansys2::Instance("robot2", "robot"));
  problem_client->addInstance(plansys2::Instance("robot3", "robot"));
  problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"));
  problem_client->addInstance(plansys2::Instance("sterwheel_zone", "zone"));
  problem_client->addInstance(plansys2::Instance("body_car_zone", "zone"));
  problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"));
  problem_client->addInstance(plansys2::Instance("recharge_zone", "zone"));
  problem_client->addInstance(plansys2::Instance("wheel_1", "piece"));
  problem_client->addInstance(plansys2::Instance("wheel_2", "piece"));
  problem_client->addInstance(plansys2::Instance("wheel_3", "piece"));
  problem_client->addInstance(plansys2::Instance("body_car_1", "piece"));
  problem_client->addInstance(plansys2::Instance("body_car_2", "piece"));
  problem_client->addInstance(plansys2::Instance("body_car_3", "piece"));
  problem_client->addInstance(plansys2::Instance("sterwheel_1", "piece"));
  problem_client->addInstance(plansys2::Instance("sterwheel_2", "piece"));
  problem_client->addInstance(plansys2::Instance("sterwheel_3", "piece"));
  problem_client->addInstance(plansys2::Instance("car1", "car"));
  problem_client->addInstance(plansys2::Instance("car2", "car"));
  problem_client->addInstance(plansys2::Instance("car3", "car"));
  problem_client->addPredicate(plansys2::Predicate("(robot_at robot1 assembly_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(robot_at robot2 assembly_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(robot_at robot3 assembly_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(is_assembly_zone assembly_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(robot_available robot1)"));
  problem_client->addPredicate(plansys2::Predicate("(robot_available robot2)"));
  problem_client->addPredicate(plansys2::Predicate("(robot_available robot3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at wheel_1 wheels_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at body_car_1 body_car_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at sterwheel_1 sterwheel_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_wheel wheel_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_body_car body_car_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_steering_wheel sterwheel_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at wheel_2 wheels_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at body_car_2 body_car_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at sterwheel_2 sterwheel_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_wheel wheel_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_body_car body_car_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_steering_wheel sterwheel_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at wheel_3 wheels_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at body_car_3 body_car_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_at sterwheel_3 sterwheel_zone)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_wheel wheel_3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_body_car body_car_3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_is_steering_wheel sterwheel_3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used wheel_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used wheel_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used wheel_3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used body_car_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used body_car_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used body_car_3)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used sterwheel_1)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used sterwheel_2)"));
  problem_client->addPredicate(plansys2::Predicate("(piece_not_used sterwheel_3)"));

  problem_client->setGoal(
    plansys2::Goal(
      "(and(car_assembled car1) (car_assembled car2) (car_assembled car3))"));

  auto domain = domain_client->getDomain();
  auto problem = problem_client->getProblem();
  auto plan = planner_client->getPlan(domain, problem);

  ASSERT_FALSE(domain.empty());
  ASSERT_FALSE(problem.empty());
  ASSERT_TRUE(plan.has_value());

  ASSERT_TRUE(executor_client->start_plan_execution(plan.value()));

  rclcpp::Rate rate(5);
  while (executor_client->execute_and_check_plan()) {
    rate.sleep();
  }

  auto result = executor_client->getResult();

  ASSERT_TRUE(result.value().success);

  ASSERT_TRUE(
    execution_logger->sorted(
  {
    "(move robot1 assembly_zone body_car_zone):0",
    "(transport robot1 body_car_1 body_car_zone assembly_zone):2000",
    "(assemble robot1 assembly_zone wheel_1 body_car_1 sterwheel_1 car1):5002",
    "(move robot1 assembly_zone wheels_zone):10003",
    "(transport robot1 wheel_2 wheels_zone assembly_zone):12004",
    "(move robot1 assembly_zone sterwheel_zone):15005",
    "(transport robot1 sterwheel_3 sterwheel_zone assembly_zone):17006",
    "(assemble robot1 assembly_zone wheel_3 body_car_3 sterwheel_3 car3):20008"
  }));

  ASSERT_TRUE(
    execution_logger->sorted(
  {
    "(move robot2 assembly_zone sterwheel_zone):0",
    "(transport robot2 sterwheel_1 sterwheel_zone assembly_zone):2000",
    "(move robot2 assembly_zone body_car_zone):5002",
    "(transport robot2 body_car_2 body_car_zone assembly_zone):7003",
    "(assemble robot2 assembly_zone wheel_2 body_car_2 sterwheel_2 car2):15005"
  }));

  ASSERT_TRUE(
    execution_logger->sorted(
  {
    "(move robot3 assembly_zone wheels_zone):0",
    "(transport robot3 wheel_1 wheels_zone assembly_zone):2000",
    "(move robot3 assembly_zone sterwheel_zone):5002",
    "(transport robot3 sterwheel_2 sterwheel_zone assembly_zone):7003",
    "(move robot3 assembly_zone body_car_zone):10004",
    "(transport robot3 body_car_3 body_car_zone assembly_zone):12005",
    "(move robot3 assembly_zone wheels_zone):15006",
    "(transport robot3 wheel_3 wheels_zone assembly_zone):17007"
  }));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot1 body_car_1 body_car_zone assembly_zone):2000",
      "(assemble robot1 assembly_zone wheel_1 body_car_1 sterwheel_1 car1):5002"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot2 sterwheel_1 sterwheel_zone assembly_zone):2000",
      "(assemble robot1 assembly_zone wheel_1 body_car_1 sterwheel_1 car1):5002"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot3 wheel_1 wheels_zone assembly_zone):2000",
      "(assemble robot1 assembly_zone wheel_1 body_car_1 sterwheel_1 car1):5002"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot1 wheel_2 wheels_zone assembly_zone):12004",
      "(assemble robot2 assembly_zone wheel_2 body_car_2 sterwheel_2 car2):15005"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot2 body_car_2 body_car_zone assembly_zone):7003",
      "(assemble robot2 assembly_zone wheel_2 body_car_2 sterwheel_2 car2):15005"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot3 sterwheel_2 sterwheel_zone assembly_zone):7003",
      "(assemble robot2 assembly_zone wheel_2 body_car_2 sterwheel_2 car2):15005"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot1 sterwheel_3 sterwheel_zone assembly_zone):17006",
      "(assemble robot1 assembly_zone wheel_3 body_car_3 sterwheel_3 car3):20008"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot3 body_car_3 body_car_zone assembly_zone):12005",
      "(assemble robot1 assembly_zone wheel_3 body_car_3 sterwheel_3 car3):20008"
  ));

  ASSERT_TRUE(
    execution_logger->before(
      "(transport robot3 wheel_3 wheels_zone assembly_zone):17007",
      "(assemble robot1 assembly_zone wheel_3 body_car_3 sterwheel_3 car3):20008"
  ));
  finish = true;
  t.join();
}

int main(int argc, char ** argv)
{
  testing::InitGoogleTest(&argc, argv);
  rclcpp::init(argc, argv);

  return RUN_ALL_TESTS();
}


<|start_filename|>plansys2_core/src/plansys2_core/Utils.cpp<|end_filename|>
// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <sstream>
#include <vector>

#include "plansys2_core/Utils.hpp"

namespace plansys2
{

std::vector<std::string> tokenize(const std::string & string, const std::string & delim)
{
  std::string::size_type lastPos = 0, pos = string.find_first_of(delim, lastPos);
  std::vector<std::string> tokens;

  while (lastPos != std::string::npos) {
    if (pos != lastPos) {
      tokens.push_back(string.substr(lastPos, pos - lastPos));
    }
    lastPos = pos;
    if (lastPos == std::string::npos || lastPos + 1 == string.length()) {
      break;
    }
    pos = string.find_first_of(delim, ++lastPos);
  }

  return tokens;
}

std::string substr_without_empty_lines(
  std::string string,
  std::size_t init_pos,
  std::size_t end_pos)
{
  std::stringstream stream_in(string.substr(init_pos, end_pos - init_pos));
  std::stringstream stream_out;
  std::string line;
  bool first = true;
  while (std::getline(stream_in, line)) {
    if (!(line.empty() || line.find_first_not_of(' ') == std::string::npos)) {
      if (first) {
        first = false;
      } else {
        stream_out << "\n";
      }
      stream_out << line;
    }
  }
  return stream_out.str();
}

std::string remove_comments(const std::string & pddl)
{
  std::stringstream uncomment;

  std::size_t pddl_length = pddl.length();
  std::size_t start_pos = 0;
  std::size_t end_pos = 0;
  bool commented = false;
  while (end_pos < pddl_length) {
    if ((!commented) && (pddl.at(end_pos) == ';')) {
      commented = true;
      uncomment << pddl.substr(start_pos, end_pos - start_pos);
    }

    if (commented && (pddl.at(end_pos) == '\r' || pddl.at(end_pos) == '\n')) {
      commented = false;
      start_pos = end_pos;
    }
    end_pos++;
  }
  if (!commented) {
    uncomment << pddl.substr(start_pos, end_pos - start_pos);
  }
  return std::string(uncomment.str());
}
}  // namespace plansys2


<|start_filename|>plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp<|end_filename|>

#include "plansys2_pddl_parser/Instance.h"

namespace parser { namespace pddl {

Expression * TemporalAction::parseDuration( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {
	return createExpression( f, ts, d );
}

void TemporalAction::printCondition( std::ostream & s, const TokenStruct< std::string > & ts, const Domain & d,
									 const std::string & t, And * a ) const {
	for ( unsigned i = 0; a && i < a->conds.size(); ++i ) {
		s << "\t\t( " << t << " ";
		a->conds[i]->PDDLPrint( s, 0, ts, d );
		s << " )\n";
	}
}

void TemporalAction::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const {
	s << "( :durative-action " << name << "\n";

	s << "  :parameters ";

	TokenStruct< std::string > astruct;

	printParams( 0, s, astruct, d );

	s << "  :duration ( = ?duration ";
	if ( durationExpr ) durationExpr->PDDLPrint( s, 0, astruct, d );
	else s << "1";
	s << " )\n";

	s << "  :condition\n";
	s << "\t( and\n";
	printCondition( s, astruct, d, "at start", (And *)pre );
	printCondition( s, astruct, d, "over all", pre_o );
	printCondition( s, astruct, d, "at end", pre_e );
	s << "\t)\n";

	s << "  :effect\n";
	s << "\t( and\n";
	printCondition( s, astruct, d, "at start", (And *)eff );
	printCondition( s, astruct, d, "at end", eff_e );
	s << "\t)\n";

	s << ")\n";
}

plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace ) const {
    throw UnsupportedConstruct("TemporalAction");
}

void TemporalAction::parseCondition( Stringreader & f, TokenStruct< std::string > & ts, Domain & d, And * a ) {
	f.next();
	f.assert_token( "(" );
	Condition * c = d.createCondition( f );
	c->parse( f, ts, d );
	a->conds.push_back( c );
}

void TemporalAction::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {
	f.next();
	f.assert_token( ":parameters" );
	f.assert_token( "(" );

	TokenStruct< std::string > astruct = f.parseTypedList( true, d.types );
	params = d.convertTypes( astruct.types );

	f.next();
	f.assert_token( ":duration" );
	f.assert_token( "(" );
	f.assert_token( "=" );
	f.assert_token( "?duration" );
	durationExpr = parseDuration( f, astruct, d );
	f.next();
	f.assert_token( ")" );

	f.next();
	f.assert_token( ":" );
	std::string s = f.getToken();
	if ( s == "condition" ) {
		pre = new And;
		pre_o = new And;
		pre_e = new And;
		f.next();
		f.assert_token( "(" );
		if ( f.getChar() != ')' ) {
			s = f.getToken();
			if ( s == "and" ) {
				for ( f.next(); f.getChar() != ')'; f.next() ) {
					f.assert_token( "(" );
					s = f.getToken();
					f.next();
					std::string t = f.getToken();

					if ( s == "at" && t == "start" )
						parseCondition( f, astruct, d, (And *)pre );
					else if ( s == "over" && t == "all" )
						parseCondition( f, astruct, d, pre_o );
					else if ( s == "at" && t == "end" )
						parseCondition( f, astruct, d, pre_e );
					else f.tokenExit( s + " " + t );

					f.next();
					f.assert_token( ")" );
				}
				++f.c;
			}
			else {
				f.next();
				std::string t = f.getToken();

				if ( s == "at" && t == "start" )
					parseCondition( f, astruct, d, (And *)pre );
				else if ( s == "over" && t == "all" )
					parseCondition( f, astruct, d, pre_o );
				else if ( s == "at" && t == "end" )
					parseCondition( f, astruct, d, pre_e );
				else f.tokenExit( s + " " + t );

				f.next();
				f.assert_token( ")" );
			}
		}
		else ++f.c;

		f.next();
		f.assert_token( ":" );
		s = f.getToken();
	}
	if ( s != "effect" ) f.tokenExit( s );

	f.next();
	f.assert_token( "(" );
	if ( f.getChar() != ')' ) {
		eff = new And;
		eff_e = new And;

		s = f.getToken();
		if ( s == "and" ) {
			for ( f.next(); f.getChar() != ')'; f.next() ) {
				f.assert_token( "(" );
				s = f.getToken();
				f.next();
				std::string t = f.getToken();

				if ( s == "at" && t == "start" )
					parseCondition( f, astruct, d, (And *)eff );
				else if ( s == "at" && t == "end" )
					parseCondition( f, astruct, d, eff_e );
				else f.tokenExit( s + " " + t );

				f.next();
				f.assert_token( ")" );
			}
			++f.c;
		}
		else {
			f.next();
			std::string t = f.getToken();

			if ( s == "at" && t == "start" )
				parseCondition( f, astruct, d, (And *)eff );
			else if ( s == "at" && t == "end" )
				parseCondition( f, astruct, d, eff_e );
			else f.tokenExit( s + " " + t );

			f.next();
			f.assert_token( ")" );
		}
	}
	else ++f.c;

	f.next();
	f.assert_token( ")" );
}

GroundVec TemporalAction::preconsStart() {
	return getGroundsFromCondition( pre, false );
}

GroundVec TemporalAction::preconsOverall() {
	return getGroundsFromCondition( pre_o, false );
}

GroundVec TemporalAction::preconsEnd() {
	return getGroundsFromCondition( pre_e, false );
}

CondVec TemporalAction::endEffects() {
	return getSubconditionsFromCondition( eff_e );
}

GroundVec TemporalAction::addEndEffects() {
	return getGroundsFromCondition( eff_e, false );
}

GroundVec TemporalAction::deleteEndEffects() {
	return getGroundsFromCondition( eff_e, true );
}

} } // namespaces


<|start_filename|>plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp<|end_filename|>
// Copyright 2020 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <map>
#include <memory>
#include <tuple>

#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp"

namespace plansys2
{

WaitAtStartReq::WaitAtStartReq(
  const std::string & xml_tag_name,
  const BT::NodeConfiguration & conf)
: ActionNodeBase(xml_tag_name, conf)
{
  action_map_ =
    config().blackboard->get<std::shared_ptr<std::map<std::string, ActionExecutionInfo>>>(
    "action_map");

  problem_client_ =
    config().blackboard->get<std::shared_ptr<plansys2::ProblemExpertClient>>(
    "problem_client");
}

BT::NodeStatus
WaitAtStartReq::tick()
{
  std::string action;
  getInput("action", action);

  auto node = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");

  auto reqs_as = (*action_map_)[action].durative_action_info->at_start_requirements;
  auto reqs_oa = (*action_map_)[action].durative_action_info->over_all_requirements;

  bool check_as = check(reqs_as, problem_client_);
  if (!check_as) {
    (*action_map_)[action].execution_error_info = "Error checking at start reqs";

    RCLCPP_ERROR_STREAM(
      node->get_logger(),
      "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
        parser::pddl::toString((*action_map_)[action].durative_action_info->at_start_requirements));

    return BT::NodeStatus::RUNNING;
  }

  bool check_oa = check(reqs_oa, problem_client_);
  if (!check_oa) {
    (*action_map_)[action].execution_error_info = "Error checking over all reqs";

    RCLCPP_ERROR_STREAM(
      node->get_logger(),
      "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " <<
        parser::pddl::toString((*action_map_)[action].durative_action_info->over_all_requirements));

    return BT::NodeStatus::RUNNING;
  }

  return BT::NodeStatus::SUCCESS;
}

}  // namespace plansys2


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.h<|end_filename|>

#pragma once

#include "plansys2_pddl_parser/Condition.h"
#include "plansys2_pddl_parser/Basic.h"

namespace parser { namespace pddl {


// This is necessary for adl
using ::operator<<;

class ParamCond : public Condition {

public:
	std::string name;
	IntVec params;

	ParamCond() {}

	ParamCond( const std::string & s, const IntVec & p = IntVec() )
		: name( s ), params( p ) {}

	ParamCond( const ParamCond * c )
		: name( c->name ), params( c->params ) {}

	std::string c_str() const {
		return name;
	}

	void print( std::ostream & stream ) const {
		stream << name << params << "\n";
	}

	void printParams( unsigned first, std::ostream & s, TokenStruct< std::string > & ts, const Domain & d ) const;
};

typedef std::vector< ParamCond * > ParamCondVec;

} } // namespaces


<|start_filename|>plansys2_tests/include/plansys2_tests/execution_logger.hpp<|end_filename|>
// Copyright 2021 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PLANSYS2_TESTS__EXECUTION_LOGGER_HPP_
#define PLANSYS2_TESTS__EXECUTION_LOGGER_HPP_

#include <list>
#include <memory>
#include <string>

#include "plansys2_msgs/msg/action_execution_info.hpp"
#include "plansys2_msgs/msg/action_execution.hpp"

#include "rclcpp/rclcpp.hpp"


namespace plansys2_tests
{

class ExecutionLogger : public rclcpp::Node
{
public:
  using Ptr = std::shared_ptr<ExecutionLogger>;
  static Ptr make_shared()
  {
    return std::make_shared<ExecutionLogger>();
  }

  ExecutionLogger();

  const std::list<plansys2_msgs::msg::ActionExecution> & get_action_execution_log()
  {
    return action_execution_log_;
  }

  const std::list<plansys2_msgs::msg::ActionExecutionInfo> & get_action_execution_info_log()
  {
    return action_execution_info_log_;
  }

  bool finished_before(const std::string & op1, const std::string & op2);
  bool started_before(const std::string & op1, const std::string & op2);
  bool before(const std::string & op1, const std::string & op2);
  bool sorted(const std::list<std::string> & actions);
  bool is_executed(const std::string & action_full_name);

private:
  rclcpp::Subscription<plansys2_msgs::msg::ActionExecution>::SharedPtr action_execution_sub_;
  rclcpp::Subscription<plansys2_msgs::msg::ActionExecutionInfo>::SharedPtr
    action_execution_info_sub_;

  std::list<plansys2_msgs::msg::ActionExecution> action_execution_log_;
  std::list<plansys2_msgs::msg::ActionExecutionInfo> action_execution_info_log_;
};

}  // namespace plansys2_tests

#endif  // PLANSYS2_TESTS__EXECUTION_LOGGER_HPP_


<|start_filename|>plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp<|end_filename|>

#include "plansys2_pddl_parser/Domain.h"

namespace parser { namespace pddl {

void Not::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const {
	tabindent( s, indent );
	s << "( not ";
	if ( cond ) cond->PDDLPrint( s, 0, ts, d );
	s << " )";
}

plansys2_msgs::msg::Node::SharedPtr Not::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace ) const {
    plansys2_msgs::msg::Node::SharedPtr node = std::make_shared<plansys2_msgs::msg::Node>();
    node->node_type = plansys2_msgs::msg::Node::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);
    }

    return node;
}

void Not::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {
	f.next();
	f.assert_token( "(" );

	cond = dynamic_cast< Ground * >( d.createCondition( f ) );

	if ( !cond ) {
		f.tokenExit( f.getToken() );
	}

	cond->parse( f, ts, d );

	f.next();
	f.assert_token( ")" );
}


} } // namespaces


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.h<|end_filename|>

#pragma once

#include "plansys2_pddl_parser/Condition.h"

namespace parser { namespace pddl {

typedef std::list< std::pair< Condition *, unsigned > > CondList;

class CondIter : public std::iterator< std::input_iterator_tag, Condition * > {

public:

	CondList condStack;

	CondIter( Condition * c ) {
		condStack.push_back( std::make_pair( c, 0 ) );
		(*this)++;
	}

	CondIter( const CondIter & ci )
		: condStack( ci.condStack ) {}

	MyIterator & operator++() {
		while ( condStack().size() && condStack.last().done() )
			condStack.pop_back();

		if ( condStack().size() ) {
		}

		return *this;
	}

	bool hasNext() {
		return condStack.size();
	}

	Condition * operator*() {
		return condStack.last();
	}

};

} } // namespaces


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/TokenStruct.h<|end_filename|>

#pragma once
#include <map>
#include "plansys2_pddl_parser/Basic.h"
/*
	T is either a pointer or a string
	A token structure stores pointers but does not delete them!
*/

namespace parser { namespace pddl {

typedef std::map< std::string, unsigned > TokenMap;

template <typename T>
inline std::string getName( const T & t ) {
	return t->name;
}

template <>
inline std::string getName<std::string>( const std::string & s ) {
	return s;
}

template <typename T>
class TokenStruct {

public:

	std::vector< T > tokens;
	TokenMap tokenMap;

	// represents the types of a typed list
	StringVec types;

	TokenStruct() {}

	TokenStruct( const TokenStruct & ts )
		: tokens( ts.tokens ), tokenMap( ts.tokenMap ), types( ts.types ) {}

	void append( const TokenStruct & ts ) {
		for ( unsigned i = 0; i < ts.size(); ++i )
			insert( ts[i] );
		types.insert( types.end(), ts.types.begin(), ts.types.end() );
	}

	unsigned size() const {
		return tokens.size();
	}

	T & operator[]( std::size_t i ) {
		return tokens[i];
	}

	const T & operator[]( std::size_t i ) const {
		return tokens[i];
	}

	void clear() {
		for ( unsigned i = 0; i < size(); ++i )
			delete tokens[i];

		tokens.clear();
		tokenMap.clear();
	}

	unsigned insert( const T & t ) {
		TokenMap::iterator i = tokenMap.insert( tokenMap.begin(), std::make_pair( getName( t ), size() ) );
		tokens.push_back( t );
		return i->second;
	}

	int index( const std::string & s ) const {
		TokenMap::const_iterator i = tokenMap.find( s );
		return i == tokenMap.end() ? -1 : i->second;
	}

	T get( const std::string & s ) const {
		return tokens[index( s )];
	}

};

} } // namespaces


<|start_filename|>plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.h<|end_filename|>

#pragma once

#include <stdexcept>

#include "plansys2_msgs/msg/node.hpp"
#include "plansys2_msgs/msg/tree.hpp"

#include "plansys2_pddl_parser/Basic.h"
#include "plansys2_pddl_parser/Stringreader.h"
#include "plansys2_pddl_parser/Utils.h"
#include "plansys2_pddl_parser/Type.h"

namespace parser { namespace pddl {

class UnsupportedConstruct : public std::runtime_error {
public:
    UnsupportedConstruct(const std::string& construct) : std::runtime_error(construct +  " is not currently supported by plansys2") {}
};

class Condition {

public:

	virtual ~Condition() {}

	virtual void print( std::ostream & stream ) const = 0;

	virtual void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const = 0;

	virtual plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace = {} ) const = 0;

	virtual void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) = 0;

	virtual void addParams( int m, unsigned n ) = 0;

	virtual Condition * copy( Domain & d ) = 0;
};

inline std::ostream & operator<<( std::ostream & stream, const Condition * c ) {
	c->print( stream );
	return stream;
}

typedef std::vector< Condition * > CondVec;

} } // namespaces
