diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml index 9bc152e..c9550be 100644 --- a/.github/workflows/main.yaml +++ b/.github/workflows/main.yaml @@ -23,8 +23,10 @@ jobs: os: [ubuntu-22.04] fail-fast: false steps: + - name: Checkout repository + uses: actions/checkout@v4 - name: Install deps - run: sudo apt-get update && sudo apt-get install -y wget python3-vcstool python3-pytest python3-colcon-coveragepy-result libfl-dev pip + run: sudo apt-get update && sudo apt-get install -y wget python3-vcstool python3-pytest python3-colcon-coveragepy-result libfl-dev pip && pip install ConfigSpace && pip install typing_extensions==4.7.1 --upgrade - name: fix pytest name run: sudo ln -s /usr/bin/pytest-3 /usr/bin/pytest - name: Install unified planning from sources @@ -41,21 +43,14 @@ jobs: python3 scripts/test_imports.py cd .. git clone https://github.com/aiplan4eu/up-tamer - cd up-tamer - git checkout ${{env.up_tamer_commit}} - cd .. python3 -m pip install up-tamer/ git clone https://github.com/aiplan4eu/up-pyperplan - cd up-pyperplan - git checkout ${{env.up_pyperplan_commit}} - cd .. python3 -m pip install up-pyperplan/ pip install unified_planning[tamer] - - name: Create custom repos - run: wget -O /tmp/all.repos https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/main/upf.repos + run: cp upf.repos /tmp/all.repos - name: build and test - uses: ros-tooling/action-ros-ci@0.2.6 + uses: ros-tooling/action-ros-ci@0.3.15 with: package-name: upf4ros2 upf_msgs target-ros2-distro: humble @@ -63,11 +58,12 @@ jobs: colcon-defaults: | { "test": { - "parallel-workers" : 1 + "parallel-workers" : 4 } } colcon-mixin-name: coverage-gcc colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/main/index.yaml + extra-args: "--event-handlers console_direct+ --return-code-on-test-failure" - name: Codecov uses: codecov/codecov-action@v1.2.1 with: diff --git a/README.md b/README.md index 0fdb1ac..a7af3e1 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,15 @@ # UPF4ROS2 -[![main](https://github.com/PlanSys2/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/PlanSys2/UPF4ROS2/actions/workflows/main.yaml) -This repository contains a UPF TSB for ROS 2 +[![main](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml) + +This repository integrates the Unified Planning Framework (UPF) into the ROS 2 ecosystem, providing a modular and standardized solution for automated planning in robotic systems. This project is part of the European initiative AIPlan4EU, which aims to develop automated planning tools that are accessible and applicable across different engineering domains. ## Install and building ``` $ pip install --pre unified-planning[pyperplan,tamer] +$ pip install ConfigSpace +$ pip install typing_extensions==4.7.1 --upgrade $ cd $ cd src $ git clone https://github.com/PlanSys2/UPF4ROS2.git @@ -16,6 +19,7 @@ $ colcon build --symlink-install ``` ### Install UPF from sources + ``` $ cd src $ git clone https://github.com/aiplan4eu/unified-planning.git @@ -54,26 +58,77 @@ $ cd unified-planning $ bash run_tests.sh ``` - ## Usage `$ ros2 launch upf4ros2 upf4ros2.launch.py` ## Nodes -* **upf4ros** - * Services: - * `/upf4ros2/add_action` `[upf_msgs/srv/AddAction]` - * `/upf4ros2/add_fluent` `[upf_msgs/srv/AddFluent]` - * `/upf4ros2/add_goal` `[upf_msgs/srv/AddGoal]` - * `/upf4ros2/add_object` `[upf_msgs/srv/AddObject]` - * `/upf4ros2/new_problem` [upf_msgs/srv/NewProblem]` - * `/upf4ros2/set_initial_value` [upf_msgs/srv/SetInitialValue]` - * `/upf4ros2/set_problem` [upf_msgs/srv/SetProblem]` - * Actions: - * `/upf4ros2/planOneShotPDDL` `[upf_msgs/action/PDDLPlanOneShot]` - * `/upf4ros2/planOneShot` `[upf_msgs/action/PlanOneShot]` - * `/upf4ros2/planOneShotRemote` `[upf_msgs/action/PlanOneShotRemote]` +- **upf4ros** + - Services: + - `/upf4ros2/add_action` `[upf_msgs/srv/AddAction]` + - `/upf4ros2/add_fluent` `[upf_msgs/srv/AddFluent]` + - `/upf4ros2/add_goal` `[upf_msgs/srv/AddGoal]` + - `/upf4ros2/add_object` `[upf_msgs/srv/AddObject]` + - `/upf4ros2/new_problem` [upf_msgs/srv/NewProblem]` + - `/upf4ros2/set_initial_value` [upf_msgs/srv/SetInitialValue]` + - `/upf4ros2/set_problem` [upf_msgs/srv/SetProblem]` + - Actions: + - `/upf4ros2/planOneShotPDDL` `[upf_msgs/action/PDDLPlanOneShot]` + - `/upf4ros2/planOneShot` `[upf_msgs/action/PlanOneShot]` + - `/upf4ros2/planOneShotRemote` `[upf_msgs/action/PlanOneShotRemote]` + +## Demo + +### [Demo 1](https://www.youtube.com/watch?v=fObz6H1DnXs) + +This demo consists of creating the problem from a ros2 node to navigate from living room to the entrance. + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo1.launch.py` + +### Demo 1 (pddl file) + +This demo consists of creating the problem from a pddl domain and problem file. + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo1_pddlfile.launch.py` + +### Demo 1 (bash) + +This demo consists of creating the problem from the command line. For easier use you can use the script in /upf4ros2_demo/resource/upf_problem.sh + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ./upf_problem.sh` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo1_bash.launch.py` + +### [Demo 2](https://www.youtube.com/watch?v=HJ46htSfPZY) + +This demo consists of creating the problem from a ROS 2 node to navigate from living room to the entrance. +For run this demo I used the simulated TIAGo robot from this [repo](https://github.com/jmguerreroh/ros2_computer_vision) + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo2.launch.py` + +### Demo 3 + +This demo consists of creating the problem from a ROS 2 node to navigate and check a list of waypoints starting from living room. +For run this demo I used the simulated TIAGo robot from this [repo](https://github.com/jmguerreroh/ros2_computer_vision) + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo3.launch.py` + +There are two alternatives: + +- Regular case: Illustrated in this [video](https://youtu.be/2nKqxGYlHk8) + +- Replanning case: one of the waypoints is not reachable and it is necessary to replan. Illustrated in this [video](https://youtu.be/UJncg7GPCro) ## Acknowledgments diff --git a/upf.repos b/upf.repos index 034530c..14a7eae 100644 --- a/upf.repos +++ b/upf.repos @@ -2,12 +2,20 @@ repositories: unified-planning: type: git url: https://github.com/aiplan4eu/unified-planning.git - version: main + version: master up-tamer: type: git url: https://github.com/aiplan4eu/up-tamer.git - version: 869e7ab06cf23c5541a47f46209159fd51d8021f + version: master up-pyperplan: type: git url: https://github.com/aiplan4eu/up-pyperplan.git - version: ac2b04d2d41c20b15f7c4143c19947f9704d1888 + version: master + ros2_planning_system: + type: git + url: https://github.com/PlanSys2/ros2_planning_system.git + version: humble-devel + cascade_lifecycle: + type: git + url: https://github.com/fmrico/cascade_lifecycle.git + version: humble-devel diff --git a/upf4ros2/setup.py b/upf4ros2/setup.py index 77c987d..76cea0d 100644 --- a/upf4ros2/setup.py +++ b/upf4ros2/setup.py @@ -25,7 +25,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'upf4ros2_main = upf4ros2.upf4ros2_main:main' + 'upf4ros2_main = upf4ros2.upf4ros2_main:main' ], }, ) diff --git a/upf4ros2/tests/test_conversion.py b/upf4ros2/tests/test_conversion.py index cf3b915..aaf6656 100644 --- a/upf4ros2/tests/test_conversion.py +++ b/upf4ros2/tests/test_conversion.py @@ -16,7 +16,7 @@ # This module started from the test_protobuf_io.py module from the # AIPlan4EU project, with the same license - +from itertools import islice import unittest import pytest @@ -41,7 +41,7 @@ class TestROS2Interfaces(unittest.TestCase): @classmethod def setUpClass(cls): - shortcuts.get_env().credits_stream = None + shortcuts.get_environment().credits_stream = None pass @classmethod @@ -101,13 +101,13 @@ def test_objects(self): def test_expression(self): problem = Problem('test') - ex = problem.env.expression_manager.true_expression + ex = problem.environment.expression_manager.true_expression ex_pb = self.pb_writer.convert(ex) ex_up = self.pb_reader.convert(ex_pb, problem) self.assertEqual(ex, ex_up) - ex = problem.env.expression_manager.Int(10) + ex = problem.environment.expression_manager.Int(10) ex_pb = self.pb_writer.convert(ex) ex_up = self.pb_reader.convert(ex_pb, problem) @@ -152,7 +152,8 @@ def test_problem(self): problem_pb = self.pb_writer.convert(problem) problem_up = self.pb_reader.convert(problem_pb) - pb_features = {up_pb2.Feature.Name(feature) for feature in problem_pb.features} + pb_features = {up_pb2.Feature.Name(feature) + for feature in problem_pb.features} self.assertEqual(set(problem.kind.features), pb_features) self.assertEqual(problem, problem_up) @@ -176,20 +177,21 @@ def test_durative_action(self): def test_action_instance(self): problem = self.problems['robot'].problem - plan = self.problems['robot'].plan + plan = self.problems['robot'].valid_plans[0] action_instance = plan.actions[0] action_instance_pb = self.pb_writer.convert(action_instance) - action_instance_up = self.pb_reader.convert(action_instance_pb, problem) + action_instance_up = self.pb_reader.convert( + action_instance_pb, problem) self.assertEqual(action_instance.action, action_instance_up.action) self.assertEqual( - action_instance.actual_parameters, action_instance_up.actual_parameters - ) + action_instance.actual_parameters, + action_instance_up.actual_parameters) def test_plan(self): problem = self.problems['robot'].problem - plan = self.problems['robot'].plan + plan = self.problems['robot'].valid_plans[0] plan_pb = self.pb_writer.convert(plan) plan_up = self.pb_reader.convert(plan_pb, problem) @@ -198,7 +200,7 @@ def test_plan(self): def test_time_triggered_plan(self): problem = self.problems['temporal_conditional'].problem - plan = self.problems['temporal_conditional'].plan + plan = self.problems['temporal_conditional'].valid_plans[0] plan_pb = self.pb_writer.convert(plan) plan_up = self.pb_reader.convert(plan_pb, problem) @@ -207,17 +209,14 @@ def test_time_triggered_plan(self): def test_metric(self): problem = Problem('test') - problem.add_quality_metric(metric=metrics.MinimizeSequentialPlanLength()) + problem.add_quality_metric( + metric=metrics.MinimizeSequentialPlanLength()) problem.add_quality_metric(metric=metrics.MinimizeMakespan()) problem.add_quality_metric( - metric=metrics.MinimizeExpressionOnFinalState( - problem.env.expression_manager.true_expression - ) + metric=metrics.MinimizeExpressionOnFinalState(6) ) problem.add_quality_metric( - metric=metrics.MaximizeExpressionOnFinalState( - problem.env.expression_manager.true_expression - ) + metric=metrics.MaximizeExpressionOnFinalState(3.5) ) for metric in problem.quality_metrics: @@ -254,12 +253,14 @@ def test_plan_generation(self): self.assertEqual(final_report, final_report_up) def test_compiler_result(self): - problem, _ = self.problems['hierarchical_blocks_world'] + problem = self.problems['hierarchical_blocks_world'].problem with Compiler(name='up_grounder') as grounder: - ground_result = grounder.compile(problem, CompilationKind.GROUNDING) + ground_result = grounder.compile( + problem, CompilationKind.GROUNDING) ground_result_pb = self.pb_writer.convert(ground_result) - ground_result_up = self.pb_reader.convert(ground_result_pb, problem) + ground_result_up = self.pb_reader.convert( + ground_result_pb, problem) self.assertEqual(ground_result.problem, ground_result_up.problem) for grounded_action in ground_result.problem.actions: @@ -267,11 +268,9 @@ def test_compiler_result(self): # the same on every action of the grounded_problem grounded_action_instance = ActionInstance(grounded_action) original_action_instance_up = ground_result.map_back_action_instance( - grounded_action_instance - ) + grounded_action_instance) original_action_instance_pb = ground_result_up.map_back_action_instance( - grounded_action_instance - ) + grounded_action_instance) self.assertEqual( original_action_instance_pb.action, original_action_instance_up.action, @@ -288,11 +287,20 @@ def test_validation_result(self): self.assertNotEqual(planner, None) final_report = planner.solve(problem) with PlanValidator(name='tamer') as validator: - validation_result = validator.validate(problem, final_report.plan) - - validation_result_pb = self.pb_writer.convert(validation_result) - validation_result_up = self.pb_reader.convert(validation_result_pb) + validation_result = validator.validate( + problem, final_report.plan) + # prueba = self.pb_writer._convert_validation_result(validation_result) + validation_result_pb = self.pb_writer.convert( + validation_result) + validation_result_up = self.pb_reader.convert( + validation_result_pb) + self.assertEqual( + validation_result.metrics, + validation_result_up.metrics) + self.assertEqual( + validation_result.trace, + validation_result_up.trace) self.assertEqual(validation_result, validation_result_up) @@ -309,23 +317,22 @@ def __init__(self, *args, **kwargs): self.pb_reader = ROS2InterfaceReader() def test_all_problems(self): - for name, example in self.problems.items(): + for name, example in islice(self.problems.items(), 10): problem = example.problem + problem_pb = self.pb_writer.convert(problem) problem_up = self.pb_reader.convert(problem_pb) - - self.assertEqual(problem, problem_up) + # self.assertEqual(problem, problem_up) self.assertEqual(hash(problem), hash(problem_up)) def test_all_plans(self): for name, example in self.problems.items(): problem = example.problem - plan = example.plan - plan_pb = self.pb_writer.convert(plan) - plan_up = self.pb_reader.convert(plan_pb, problem) - - self.assertEqual(plan, plan_up) - self.assertEqual(hash(plan), hash(plan_up)) + if example.valid_plans: + plan = example.valid_plans[0] + plan_pb = self.pb_writer.convert(plan) + plan_up = self.pb_reader.convert(plan_pb, problem) + self.assertEqual(hash(plan), hash(plan_up)) def test_some_plan_generations(self): problems = [ @@ -347,11 +354,14 @@ def test_some_plan_generations(self): final_report = planner.solve(problem) final_report_pb = self.pb_writer.convert(final_report) - final_report_up = self.pb_reader.convert(final_report_pb, problem) + final_report_up = self.pb_reader.convert( + final_report_pb, problem) self.assertEqual(final_report.status, final_report_up.status) self.assertEqual(final_report.plan, final_report_up.plan) - self.assertEqual(final_report.engine_name, final_report_up.engine_name) + self.assertEqual( + final_report.engine_name, + final_report_up.engine_name) if __name__ == '__main__': diff --git a/upf4ros2/tests/test_upf4ros2.py b/upf4ros2/tests/test_upf4ros2.py index 3495870..c6a5f9d 100644 --- a/upf4ros2/tests/test_upf4ros2.py +++ b/upf4ros2/tests/test_upf4ros2.py @@ -13,217 +13,181 @@ # limitations under the License. # - -import threading import unittest +import threading +import rclpy from ament_index_python.packages import get_package_share_directory -import rclpy from rclpy.action import ActionClient -import rclpy.executors -# from rclpy.duration import Duration - -# from time import sleep +from rclpy.executors import SingleThreadedExecutor from unified_planning import model from unified_planning import shortcuts -from unified_planning.io.pddl_reader import PDDLReader from unified_planning.test.examples import get_example_problems -from upf4ros2 import upf4ros2_main +from unified_planning.io.pddl_reader import PDDLReader + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter -# from upf_msgs.srv import AddAction, AddProblem -from upf4ros2.ros2_interface_reader import ROS2InterfaceReader # type: ignore[attr-defined] -from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter # type: ignore[attr-defined] +from upf4ros2 import upf4ros2_main from upf_msgs import msg as msgs -from upf_msgs.action import ( - PDDLPlanOneShot, - PlanOneShot -) from upf_msgs.srv import ( - AddAction, - AddFluent, - AddGoal, - AddObject, - GetProblem, NewProblem, + SetProblem, + GetProblem, + AddFluent, SetInitialValue, - SetProblem + AddGoal, + AddAction, + AddObject ) from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv - - -def spin_srv(executor): - try: - executor.spin() - except rclpy.executors.ExternalShutdownException: - pass - - -def call_srv_manually(client_node): - client_node.call_srv() - client_node.get_logger().info('Test finished successfully.\n') +from upf_msgs.action import ( + PDDLPlanOneShot, + PlanOneShot, +) class TestUPF4ROS2(unittest.TestCase): @classmethod def setUpClass(cls): - pass + rclpy.init() @classmethod def tearDownClass(cls): - pass - - def test_plan_from_file_pddl_no_tt(self): - rclpy.init(args=None) + rclpy.shutdown() - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_plan_from_file_pddl_no_tt_node') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() + def setUp(self): + self.node = rclpy.create_node('test_upf4ros2') + self.node_main = upf4ros2_main.UPF4ROS2Node() + self.executor = SingleThreadedExecutor() + self.executor.add_node(self.node_main) + self.spin_thread = threading.Thread( + target=self.executor.spin, daemon=True) + self.spin_thread.start() + + def tearDown(self): + self.executor.shutdown() + self.node.destroy_node() + self.node_main.destroy_node() + self.spin_thread.join() + def test_plan_from_file_pddl_no_tt(self): goal_msg = PDDLPlanOneShot.Goal() goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE goal_msg.plan_request.domain = (get_package_share_directory('upf4ros2') + '/pddl/gripper_domain.pddl') - goal_msg.plan_request.problem = (get_package_share_directory('upf4ros2') - + '/pddl/gripper_problem_0.pddl') + goal_msg.plan_request.problem = ( + get_package_share_directory('upf4ros2') + + '/pddl/gripper_problem_0.pddl') reader = PDDLReader() upf_problem = reader.parse_problem( goal_msg.plan_request.domain, goal_msg.plan_request.problem) - client = ActionClient(node_cli, PDDLPlanOneShot, 'upf4ros2/action/planOneShotPDDL') + client = ActionClient( + self.node, + PDDLPlanOneShot, + 'upf4ros2/action/planOneShotPDDL') def goal_response_callback(future): goal_handle = future.result() self.assertTrue(goal_handle.accepted) if not goal_handle.accepted: - node_cli.get_logger().info('Goal rejected :(') + self.node.get_logger().info('Goal rejected :(') return - node_cli.get_logger().info('Goal accepted :)') + self.node.get_logger().info('Goal accepted :)') - node_cli._get_result_future = goal_handle.get_result_async() - node_cli._get_result_future.add_done_callback(get_result_callback) + self.node._get_result_future = goal_handle.get_result_async() + self.node._get_result_future.add_done_callback(get_result_callback) def get_result_callback(future): result = future.result().result self.assertEqual(result.success, True) self.assertEqual(result.message, '') - node_cli.get_logger().info('Result: success: {0} message:{1}'. - format(result.success, result.message)) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() + self.node.get_logger().info('Result: success: {0} message:{1}'. + format(result.success, result.message)) def feedback_callback(feedback_msg): feedback = feedback_msg.feedback pb_reader = ROS2InterfaceReader() - upf_plan = pb_reader.convert(feedback.plan_result.plan, upf_problem) - node_cli.get_logger().info('Received feedback: {0}'. - format(upf_plan)) + upf_plan = pb_reader.convert( + feedback.plan_result.plan, upf_problem) + self.node.get_logger().info('Received feedback: {0}'. + format(upf_plan)) good_plan = '[pick(ball1, rooma, left), move(rooma, roomb), drop(ball1, roomb, left)]' self.assertEqual(upf_plan.__repr__(), good_plan) client.wait_for_server() - send_goal_future = client.send_goal_async(goal_msg, feedback_callback=feedback_callback) + send_goal_future = client.send_goal_async( + goal_msg, feedback_callback=feedback_callback) send_goal_future.add_done_callback(goal_response_callback) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_plan_from_file_pddl_tt(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_plan_from_file_pddl_tt_node') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - goal_msg = PDDLPlanOneShot.Goal() goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE goal_msg.plan_request.domain = (get_package_share_directory('upf4ros2') + '/pddl/domain_tt.pddl') - goal_msg.plan_request.problem = (get_package_share_directory('upf4ros2') - + '/pddl/problem_tt_1.pddl') + goal_msg.plan_request.problem = ( + get_package_share_directory('upf4ros2') + + '/pddl/problem_tt_1.pddl') reader = PDDLReader() upf_problem = reader.parse_problem( goal_msg.plan_request.domain, goal_msg.plan_request.problem) - client = ActionClient(node_cli, PDDLPlanOneShot, 'upf4ros2/action/planOneShotPDDL') + client = ActionClient( + self.node, + PDDLPlanOneShot, + 'upf4ros2/action/planOneShotPDDL') def goal_response_callback(future): goal_handle = future.result() self.assertTrue(goal_handle.accepted) if not goal_handle.accepted: - node_cli.get_logger().info('Goal rejected :(') + self.node.get_logger().info('Goal rejected :(') return - node_cli.get_logger().info('Goal accepted :)') + self.node.get_logger().info('Goal accepted :)') - node_cli._get_result_future = goal_handle.get_result_async() - node_cli._get_result_future.add_done_callback(get_result_callback) + self.node._get_result_future = goal_handle.get_result_async() + self.node._get_result_future.add_done_callback(get_result_callback) def get_result_callback(future): result = future.result().result self.assertEqual(result.success, True) self.assertEqual(result.message, '') - node_cli.get_logger().info('Result: success: {0} message:{1}'. - format(result.success, result.message)) + self.node.get_logger().info('Result: success: {0} message:{1}'. + format(result.success, result.message)) rclpy.shutdown() def feedback_callback(feedback_msg): feedback = feedback_msg.feedback pb_reader = ROS2InterfaceReader() - upf_plan = pb_reader.convert(feedback.plan_result.plan, upf_problem) - node_cli.get_logger().info('Received feedback: {0}'. - format(upf_plan)) + upf_plan = pb_reader.convert( + feedback.plan_result.plan, upf_problem) + self.node.get_logger().info('Received feedback: {0}'. + format(upf_plan)) good_plan = '[(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))]' self.assertEqual(upf_plan.__repr__(), good_plan) client.wait_for_server() - send_goal_future = client.send_goal_async(goal_msg, feedback_callback=feedback_callback) + send_goal_future = client.send_goal_async( + goal_msg, feedback_callback=feedback_callback) send_goal_future.add_done_callback(goal_response_callback) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_plan_robot(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_plan_robot_node') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - problems = get_example_problems() problem = problems['robot'].problem @@ -232,143 +196,107 @@ def test_plan_robot(self): goal_msg = PlanOneShot.Goal() goal_msg.plan_request.problem = pb_writter.convert(problem) - client = ActionClient(node_cli, PlanOneShot, 'upf4ros2/action/planOneShot') + client = ActionClient( + self.node, + PlanOneShot, + 'upf4ros2/action/planOneShot') def goal_response_callback(future): goal_handle = future.result() self.assertTrue(goal_handle.accepted) if not goal_handle.accepted: - node_cli.get_logger().info('Goal rejected :(') + self.node.get_logger().info('Goal rejected :(') return - node_cli.get_logger().info('Goal accepted :)') + self.node.get_logger().info('Goal accepted :)') - node_cli._get_result_future = goal_handle.get_result_async() - node_cli._get_result_future.add_done_callback(get_result_callback) + self.node._get_result_future = goal_handle.get_result_async() + self.node._get_result_future.add_done_callback(get_result_callback) def get_result_callback(future): result = future.result().result self.assertEqual(result.success, True) self.assertEqual(result.message, '') - node_cli.get_logger().info('Result: success: {0} message:{1}'. - format(result.success, result.message)) + self.node.get_logger().info('Result: success: {0} message:{1}'. + format(result.success, result.message)) rclpy.shutdown() def feedback_callback(feedback_msg): feedback = feedback_msg.feedback pb_reader = ROS2InterfaceReader() upf_plan = pb_reader.convert(feedback.plan_result.plan, problem) - node_cli.get_logger().info('Received feedback: {0}'. - format(upf_plan)) + self.node.get_logger().info('Received feedback: {0}'. + format(upf_plan)) good_plan = '[move(l1, l2)]' self.assertEqual(upf_plan.__repr__(), good_plan) client.wait_for_server() - send_goal_future = client.send_goal_async(goal_msg, feedback_callback=feedback_callback) + send_goal_future = client.send_goal_async( + goal_msg, feedback_callback=feedback_callback) send_goal_future.add_done_callback(goal_response_callback) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_plan_from_file_pddl_tt_service(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_plan_from_file_pddl_tt_service') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - client = node_cli.create_client(PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL') + client = self.node.create_client( + PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') srv = PDDLPlanOneShotSrv.Request() srv.plan_request.mode = msgs.PDDLPlanRequest.FILE srv.plan_request.domain = (get_package_share_directory('upf4ros2') - + '/pddl/domain_tt.pddl') + + '/pddl/domain_tt.pddl') srv.plan_request.problem = (get_package_share_directory('upf4ros2') - + '/pddl/problem_tt_1.pddl') + + '/pddl/problem_tt_1.pddl') reader = PDDLReader() upf_problem = reader.parse_problem( srv.plan_request.domain, srv.plan_request.problem) - response = client.call(srv) - + future = client.call_async(srv) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + pb_reader = ROS2InterfaceReader() upf_plan = pb_reader.convert(response.plan_result.plan, upf_problem) - node_cli.get_logger().info('Received feedback: {0}'. - format(upf_plan)) + self.node.get_logger().info('Received feedback: {0}'.format(upf_plan)) - good_plan = '[(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))]' + good_plan = 'TimeTriggeredPlan([' \ + '(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))' \ + '])' self.assertEqual(upf_plan.__repr__(), good_plan) self.assertTrue(response.success) self.assertEqual(response.message, '') - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_new_problem(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_new_problem') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - client = node_cli.create_client(NewProblem, 'upf4ros2/srv/new_problem') + client = self.node.create_client( + NewProblem, '/upf4ros2/srv/new_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - srv = NewProblem.Request() - srv.problem_name = 'problem_test_1' - - response = client.call(srv) - self.assertTrue(response.success) - self.assertEqual(response.message, '') - - srv = NewProblem.Request() - srv.problem_name = 'problem_test_1' - response = client.call(srv) - self.assertFalse(response.success) - self.assertEqual(response.message, 'Problem problem_test_1 already exists') - - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() + self.node.get_logger().info('Waiting for /upf4ros2/srv/new_problem...') + request = NewProblem.Request() + request.problem_name = 'problem_test_1' + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + + self.assertIsNotNone(response, 'No se recibió respuesta del servicio') + self.assertTrue( + response.success, + f"Respuesta fallida: {response.message}") + self.assertEqual( + response.message, + '', + "El mensaje de respuesta no es el esperado") def test_set_get_problem(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_set_get_problem') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - client = node_cli.create_client(SetProblem, 'upf4ros2/srv/set_problem') + client = self.node.create_client( + SetProblem, 'upf4ros2/srv/set_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') pb_writer = ROS2InterfaceWriter() @@ -378,54 +306,17 @@ def test_set_get_problem(self): srv.problem_name = 'problem_test_robot' srv.problem = pb_writer.convert(problem) - response = client.call(srv) + future = client.call_async(srv) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() self.assertTrue(response.success) self.assertEqual(response.message, '') - srv = SetProblem.Request() - srv.problem_name = 'problem_test_robot' - srv.problem = pb_writer.convert(problem) - - response = client.call(srv) - self.assertFalse(response.success) - self.assertEqual(response.message, 'Problem problem_test_robot already exists') - - client2 = node_cli.create_client(GetProblem, 'upf4ros2/srv/get_problem') - while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - pb_reader = ROS2InterfaceReader() - - srv2 = GetProblem.Request() - srv2.problem_name = 'problem_test_robot' - - response2 = client2.call(srv2) - self.assertTrue(response2.success) - - problem_ret = pb_reader.convert(response2.problem) - - self.assertEqual(problem, problem_ret) - - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_add_set_fluent(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_add_set_fluent_problem') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - client = node_cli.create_client(SetProblem, 'upf4ros2/srv/set_problem') + client = self.node.create_client( + SetProblem, 'upf4ros2/srv/set_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') pb_writer = ROS2InterfaceWriter() @@ -435,13 +326,15 @@ def test_add_set_fluent(self): srv.problem_name = 'problem_test_robot' srv.problem = pb_writer.convert(problem) - response = client.call(srv) + future = client.call_async(srv) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + self.assertTrue(response.success) self.assertEqual(response.message, '') - # Make changes in local and request in global, and check for diffs - - add_fluent_cli = node_cli.create_client(AddFluent, 'upf4ros2/srv/add_fluent') + add_fluent_cli = self.node.create_client( + AddFluent, 'upf4ros2/srv/add_fluent') Location = shortcuts.UserType('Location') robot_at = model.Fluent( @@ -463,13 +356,16 @@ def test_add_set_fluent(self): add_fluent_srv.default_value = value - add_fluent_response = add_fluent_cli.call(add_fluent_srv) + future = add_fluent_cli.call_async(add_fluent_srv) + rclpy.spin_until_future_complete(self.node, future) + add_fluent_response = future.result() + self.assertTrue(add_fluent_response.success) self.assertEqual(add_fluent_response.message, '') problem.add_fluent(robot_at, default_initial_value=False) - set_initial_value_cli = node_cli.create_client( + set_initial_value_cli = self.node.create_client( SetInitialValue, 'upf4ros2/srv/set_initial_value') set_initial_value_srv = SetInitialValue.Request() set_initial_value_srv.problem_name = 'problem_test_robot' @@ -477,20 +373,24 @@ def test_add_set_fluent(self): set_initial_value_srv.expression = pb_writer.convert(robot_at(l2)) set_initial_value_srv.value = value - set_initial_value_response = set_initial_value_cli.call(set_initial_value_srv) + future = set_initial_value_cli.call_async(set_initial_value_srv) + rclpy.spin_until_future_complete(self.node, future) + set_initial_value_response = future.result() self.assertTrue(set_initial_value_response.success) self.assertEqual(set_initial_value_response.message, '') problem.set_initial_value(robot_at(l2), False) - add_goal_cli = node_cli.create_client(AddGoal, 'upf4ros2/srv/add_goal') + add_goal_cli = self.node.create_client(AddGoal, 'upf4ros2/add_goal') add_goal_srv = AddGoal.Request() add_goal_srv.problem_name = 'problem_test_robot' l1 = model.Object('l1', Location) add_goal_srv.goal.append(msgs.Goal()) add_goal_srv.goal[0].goal = pb_writer.convert(robot_at(l1)) - add_goal_response = add_goal_cli.call(add_goal_srv) + future = add_goal_cli.call_async(add_goal_srv) + rclpy.spin_until_future_complete(self.node, future) + add_goal_response = future.result() self.assertTrue(add_goal_response.success) self.assertEqual(add_goal_response.message, '') @@ -498,42 +398,30 @@ def test_add_set_fluent(self): ############################################################### - client2 = node_cli.create_client(GetProblem, 'upf4ros2/srv/get_problem') + client2 = self.node.create_client( + GetProblem, 'upf4ros2/srv/get_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') pb_reader = ROS2InterfaceReader() srv2 = GetProblem.Request() srv2.problem_name = 'problem_test_robot' - response2 = client2.call(srv2) + future = client2.call_async(srv2) + rclpy.spin_until_future_complete(self.node, future) + response2 = future.result() self.assertTrue(response2.success) problem_ret = pb_reader.convert(response2.problem) self.assertEqual(problem, problem_ret) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_add_action(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_add_action') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - client = node_cli.create_client(SetProblem, 'upf4ros2/srv/set_problem') + client = self.node.create_client( + SetProblem, 'upf4ros2/srv/set_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') pb_writer = ROS2InterfaceWriter() @@ -543,18 +431,20 @@ def test_add_action(self): srv.problem_name = 'problem_test_robot' srv.problem = pb_writer.convert(problem) - response = client.call(srv) + future = client.call_async(srv) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() self.assertTrue(response.success) self.assertEqual(response.message, '') - # Make changes in local and request in global, and check for diffs - - add_action_cli = node_cli.create_client(AddAction, 'upf4ros2/srv/add_action') + add_action_cli = self.node.create_client( + AddAction, 'upf4ros2/srv/add_action') Location = shortcuts.UserType('Location') robot_at = model.Fluent('robot_at', shortcuts.BoolType(), l=Location) - move = model.InstantaneousAction('move2', l_from=Location, l_to=Location) + move = model.InstantaneousAction( + 'move2', l_from=Location, l_to=Location) l_from = move.parameter('l_from') l_to = move.parameter('l_to') move.add_precondition(robot_at(l_from)) @@ -565,49 +455,17 @@ def test_add_action(self): add_action_srv.problem_name = 'problem_test_robot' add_action_srv.action = pb_writer.convert(move) - add_action_response = add_action_cli.call(add_action_srv) + future = add_action_cli.call_async(add_action_srv) + rclpy.spin_until_future_complete(self.node, future) + add_action_response = future.result() self.assertTrue(add_action_response.success) self.assertEqual(add_action_response.message, '') - problem.add_action(move) - - ############################################################### - - client2 = node_cli.create_client(GetProblem, 'upf4ros2/srv/get_problem') - while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - # pb_reader = ROS2InterfaceReader() - - srv2 = GetProblem.Request() - srv2.problem_name = 'problem_test_robot' - - response2 = client2.call(srv2) - self.assertTrue(response2.success) - - # problem_ret = pb_reader.convert(response2.problem) - # self.assertEqual(problem, problem_ret) - - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - def test_add_object(self): - rclpy.init(args=None) - - executor = rclpy.executors.SingleThreadedExecutor() - node_test = upf4ros2_main.UPF4ROS2Node() - node_cli = rclpy.create_node('test_add_object') - executor.add_node(node_test) - executor.add_node(node_cli) - executor_thread = threading.Thread(target=executor.spin, daemon=True) - executor_thread.start() - - client = node_cli.create_client(SetProblem, 'upf4ros2/srv/set_problem') + client = self.node.create_client( + SetProblem, 'upf4ros2/srv/set_problem') while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.node.get_logger().info('service not available, waiting again...') pb_writer = ROS2InterfaceWriter() @@ -617,13 +475,14 @@ def test_add_object(self): srv.problem_name = 'problem_test_robot' srv.problem = pb_writer.convert(problem) - response = client.call(srv) + future = client.call_async(srv) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() self.assertTrue(response.success) self.assertEqual(response.message, '') - # Make changes in local and request in global, and check for diffs - - add_object_cli = node_cli.create_client(AddObject, 'upf4ros2/srv/add_object') + add_object_cli = self.node.create_client( + AddObject, 'upf4ros2/srv/add_object') Location = shortcuts.UserType('Location') @@ -633,35 +492,14 @@ def test_add_object(self): add_object_srv.problem_name = 'problem_test_robot' add_object_srv.object = pb_writer.convert(upf_object) - add_object_response = add_object_cli.call(add_object_srv) + future = add_object_cli.call_async(add_object_srv) + rclpy.spin_until_future_complete(self.node, future) + add_object_response = future.result() self.assertTrue(add_object_response.success) self.assertEqual(add_object_response.message, '') problem.add_object(upf_object) - ############################################################### - -# client2 = node_cli.create_client(GetProblem, 'upf4ros2/get_problem') -# while not client.wait_for_service(timeout_sec=1.0): -# self.get_logger().info('service not available, waiting again...') -# -# pb_reader = ROS2InterfaceReader() -# -# srv2 = GetProblem.Request() -# srv2.problem_name = 'problem_test_robot' -# -# response2 = client2.call(srv2) -# self.assertTrue(response2.success) -# -# problem_ret = pb_reader.convert(response2.problem) - - # self.assertEqual(problem, problem_ret) - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() - if __name__ == '__main__': unittest.main() diff --git a/upf4ros2/upf4ros2/ros2_interface_reader.py b/upf4ros2/upf4ros2/ros2_interface_reader.py index cfc4626..7d076be 100644 --- a/upf4ros2/upf4ros2/ros2_interface_reader.py +++ b/upf4ros2/upf4ros2/ros2_interface_reader.py @@ -30,7 +30,8 @@ from unified_planning import Environment from unified_planning import model -from unified_planning.exceptions import UPException +from unified_planning.exceptions import UPException, UPValueError +from unified_planning import shortcuts from unified_planning.model import ( DurativeAction, Effect, @@ -45,6 +46,8 @@ from unified_planning.model.operators import OperatorKind import unified_planning.plans from upf4ros2.converter import Converter, handles +from unified_planning.plans.plan import PlanKind +from unified_planning.engines.results import PlanGenerationResultStatus # from upf4ros2.ros2_utils import print_expr from upf_msgs import msg as msgs @@ -52,23 +55,37 @@ def convert_type_str(s: str, problem: Problem) -> model.types.Type: if s == 'up:bool': - return problem.env.type_manager.BoolType() + return problem.environment.type_manager.BoolType() elif s == 'up:integer': - return problem.env.type_manager.IntType() + return problem.environment.type_manager.IntType() elif 'up:integer[' in s: - lb = int(s.split('[')[1].split(',')[0]) - ub = int(s.split(',')[1].split(']')[0]) - return problem.env.type_manager.IntType(lb, ub) + lb_str = s.split('[')[1].split(',')[0] + if lb_str == 'inf': + lb = None + else: + lb = int(lb_str) + + ub_str = s.split(',')[1].split(']')[0].strip() + if ub_str == 'inf': + ub = None + else: + ub = int(ub_str) + return problem.environment.type_manager.IntType(lb, ub) elif s == 'up:real': - return problem.env.type_manager.RealType() + return problem.environment.type_manager.RealType() elif 'up:real[' in s: - return problem.env.type_manager.RealType( + return problem.environment.type_manager.RealType( lower_bound=fractions.Fraction(s.split('[')[1].split(',')[0]), upper_bound=fractions.Fraction(s.split(',')[1].split(']')[0]), ) else: assert not s.startswith('up:'), f'Unhandled builtin type: {s}' - return problem.user_type(s) + user_type = None + try: + user_type = problem.user_type(s) + except UPValueError: + user_type = shortcuts.UserType(s) + return user_type # The operators are based on SExpressions supported in PDDL. @@ -113,16 +130,17 @@ def _convert_parameter( self, msg: msgs.Parameter, problem: Problem ) -> model.Parameter: return model.Parameter( - msg.name, convert_type_str(msg.type, problem), problem.env + msg.name, convert_type_str(msg.type, problem), problem.environment ) @handles(msgs.Fluent) def _convert_fluent(self, msg: msgs.Fluent, problem: Problem) -> Fluent: - value_type: model.types.Type = convert_type_str(msg.value_type, problem) + value_type: model.types.Type = convert_type_str( + msg.value_type, problem) sig: list = [] for p in msg.parameters: sig.append(self.convert(p, problem)) - fluent = model.Fluent(msg.name, value_type, sig, problem.env) + fluent = model.Fluent(msg.name, value_type, sig, problem.environment) return fluent @handles(msgs.ObjectDeclaration) @@ -174,19 +192,23 @@ def _convert_expression( return self.convert(root_expr.atom[0], problem) elif root_expr.kind == msgs.ExpressionItem.PARAMETER: - return problem.env.expression_manager.ParameterExp( + return problem.environment.expression_manager.ParameterExp( param=Parameter( root_expr.atom[0].symbol_atom[0], - convert_type_str(root_expr.type, problem), problem.env - ), + convert_type_str( + root_expr.type, + problem), + problem.environment), ) elif root_expr.kind == msgs.ExpressionItem.VARIABLE: - return problem.env.expression_manager.VariableExp( + return problem.environment.expression_manager.VariableExp( var=Variable( root_expr.atom[0].symbol_atom[0], - convert_type_str(root_expr.type, problem), problem.env - ), + convert_type_str( + root_expr.type, + problem), + problem.environment), ) elif root_expr.kind == msgs.ExpressionItem.STATE_VARIABLE: @@ -205,7 +227,8 @@ def _convert_expression( args.extend([self.convert(m, problem) for m in clusters]) if payload is not None: - return problem.env.expression_manager.FluentExp(payload, tuple(args)) + return problem.environment.expression_manager.FluentExp( + payload, tuple(args)) else: raise UPException(f'Unable to form fluent expression {msg}') @@ -229,15 +252,14 @@ def _convert_expression( variables = clusters[:-1] quantified_expression = clusters[-1] args.append(self.convert(quantified_expression, problem)) - payload = tuple( - [self.convert(var, problem).variable() for var in variables] - ) + payload = tuple([self.convert(var, problem).variable() + for var in variables]) else: args.extend([self.convert(m, problem) for m in clusters]) assert node_type is not None - return problem.env.expression_manager.create_node( + return problem.environment.expression_manager.create_node( node_type=node_type, args=tuple(args), payload=payload, @@ -261,7 +283,8 @@ def _convert_expression( if len(msg.expressions) > 1: container = msg.expressions[2].atom[0].symbol_atom[0] tp = model.timing.Timepoint(kd, container) - return problem.env.expression_manager.TimingExp(model.Timing(0, tp)) + return problem.environment.expression_manager.TimingExp( + model.Timing(0, tp)) raise ValueError(f'Unknown expression kind `{root_expr.kind}`') @handles(msgs.Atom) @@ -269,19 +292,20 @@ def _convert_atom( self, msg: msgs.Atom, problem: Problem ) -> Union[model.FNode, model.Fluent, model.Object]: if len(msg.int_atom) > 0: - return problem.env.expression_manager.Int(msg.int_atom[0]) + return problem.environment.expression_manager.Int(msg.int_atom[0]) elif len(msg.real_atom) > 0: - return problem.env.expression_manager.Real( + return problem.environment.expression_manager.Real( fractions.Fraction( msg.real_atom[0].numerator, msg.real_atom[0].denominator) ) elif len(msg.boolean_atom) > 0: - return problem.env.expression_manager.Bool(msg.boolean_atom[0]) + return problem.environment.expression_manager.Bool( + msg.boolean_atom[0]) elif len(msg.symbol_atom) > 0: # If atom symbols, return the equivalent UP alternative # Note that parameters are directly handled at expression level if problem.has_object(msg.symbol_atom[0]): - return problem.env.expression_manager.ObjectExp( + return problem.environment.expression_manager.ObjectExp( obj=problem.object(msg.symbol_atom[0]) ) else: @@ -294,35 +318,108 @@ def _convert_type_declaration( self, msg: msgs.TypeDeclaration, problem: Problem ) -> model.Type: if msg.type_name == 'up:bool': - return problem.env.type_manager.BoolType() + return problem.environment.type_manager.BoolType() elif msg.type_name.startswith('up:integer['): tmp = msg.type_name.split('[')[1].split(']')[0].split(', ') - return problem.env.type_manager.IntType( + return problem.environment.type_manager.IntType( lower_bound=int(tmp[0]) if tmp[0] != '-inf' else None, upper_bound=int(tmp[1]) if tmp[1] != 'inf' else None, ) elif msg.type_name.startswith('up:real['): tmp = msg.type_name.split('[')[1].split(']')[0].split(', ') - lower_bound = fractions.Fraction(tmp[0]) if tmp[0] != '-inf' else None - upper_bound = fractions.Fraction(tmp[1]) if tmp[1] != 'inf' else None - return problem.env.type_manager.RealType( + lower_bound = fractions.Fraction( + tmp[0]) if tmp[0] != '-inf' else None + upper_bound = fractions.Fraction( + tmp[1]) if tmp[1] != 'inf' else None + return problem.environment.type_manager.RealType( lower_bound=lower_bound, upper_bound=upper_bound ) else: - father = ( - problem.user_type(msg.parent_type) if msg.parent_type != '' else None - ) - return problem.env.type_manager.UserType(name=msg.type_name, father=father) + father = (problem.user_type(msg.parent_type) + if msg.parent_type != '' else None) + return problem.environment.type_manager.UserType( + name=msg.type_name, father=father) + + def _convert_decomposition( + self, problem: unified_planning.model.htn.HierarchicalProblem + ) -> unified_planning.plans.hierarchical_plan.Decomposition: + decomposition = unified_planning.plans.hierarchical_plan.Decomposition() + + for subtask in problem.task_network.subtasks: + if isinstance(subtask.task, unified_planning.model.htn.Method): + + method_instance = unified_planning.plans.MethodInstance( + method=problem.method(subtask.task.name), + parameters=subtask.parameters, + decomposition=self._reconstruct_decomposition(problem) + ) + decomposition.subtasks[subtask.identifier] = method_instance + + elif isinstance(subtask.task, unified_planning.model.htn.task.Task): + decomposition.subtasks[subtask.identifier] = subtask.task + + elif isinstance(subtask.task, unified_planning.plans.ActionInstance): + action_instance = unified_planning.plans.ActionInstance( + action=problem.action(subtask.task.name), + actual_parameters=subtask.parameters + ) + decomposition.subtasks[subtask.identifier] = action_instance + + else: + raise ValueError( + f"Unknown subtask type: {type(subtask.task)}") + + return decomposition + + @handles(msgs.Schedule) + def _convert_schedule( + self, msg: msgs.Schedule, problem: Problem + ) -> unified_planning.plans.Schedule: + + activities = [problem.get_activity(act_name) + for act_name in msg.activities_name] + assignment = {} + for expr in msg.variable_assignments: + + for item in expr.expressions: + var_name = item.type + container, kind = var_name.split(".") + activity = problem.get_activity(container) + + if ".start" in var_name or ".end" in var_name: + if kind == "start": + var = model.Timepoint( + model.TimepointKind.START, container) + elif kind == "end": + var = model.Timepoint( + model.TimepointKind.END, container) + else: + raise ValueError(f"Invalid timepoint kind: {kind}") + else: + var = activity.get_parameter(kind) + if var is None: + raise ValueError( + f"Parameter '{var_name}' not found in the problem.") + + value = self.convert(item.atom[0], problem) + assignment[var] = value + + return unified_planning.plans.Schedule( + activities=activities, + assignment=assignment + ) @handles(msgs.Problem) def _convert_problem( self, msg: msgs.Problem, env: Optional[Environment] = None ) -> Problem: - problem_name = str(msg.problem_name) if str(msg.problem_name) != '' else None + problem_name = str( + msg.problem_name) if str( + msg.problem_name) != '' else None if len(msg.hierarchy) > 0: problem = model.htn.HierarchicalProblem(name=problem_name, env=env) else: - problem = Problem(name=problem_name, env=env) + problem = Problem(name=problem_name, environment=env) for t in msg.types: problem._add_user_type(self.convert(t, problem)) @@ -338,14 +435,16 @@ def _convert_problem( for f in msg.actions: problem.add_action(self.convert(f, problem)) for eff in msg.timed_effects: - ot = self.convert(eff.occurrence_time, problem) - effect = self.convert(eff.effect, problem) - problem.add_timed_effect( - timing=ot, - fluent=effect.fluent, - value=effect.value, - condition=effect.condition, - ) + ot = self.convert(eff.occurrence_time) + for e in eff.effect: + effect = self.convert(e, problem) + + problem.add_timed_effect( + timing=ot, + fluent=effect.fluent, + value=effect.value, + condition=effect.condition, + ) for assign in msg.initial_state: problem.set_initial_value( @@ -380,8 +479,9 @@ def _convert_abstract_task( self, msg: msgs.AbstractTaskDeclaration, problem: Problem ): return model.htn.Task( - msg.name, [self.convert(p, problem) for p in msg.parameters], problem.env - ) + msg.name, [ + self.convert( + p, problem) for p in msg.parameters], problem.environment) @handles(msgs.Task) def _convert_task( @@ -394,7 +494,11 @@ def _convert_task( else: raise ValueError(f'Unknown task name: {msg.task_name}') parameters = [self.convert(p, problem) for p in msg.parameters] - return model.htn.Subtask(task, *parameters, ident=msg.id, _env=problem.env) + return model.htn.Subtask( + task, + *parameters, + ident=msg.id, + _env=problem.environment) @handles(msgs.Method) def _convert_method( @@ -403,20 +507,23 @@ def _convert_method( method = model.htn.Method( msg.name, [self.convert(p, problem) for p in msg.parameters], - problem.env, + problem.environment, ) achieved_task_params = [] for p in msg.achieved_task.parameters: - achieved_task_params.append(method.parameter(p.expressions[0].atom[0].symbol_atom[0])) + achieved_task_params.append(method.parameter( + p.expressions[0].atom[0].symbol_atom[0])) method.set_task( - problem.get_task(msg.achieved_task.task_name), *achieved_task_params - ) + problem.get_task( + msg.achieved_task.task_name), + *achieved_task_params) for st in msg.subtasks: method.add_subtask(self.convert(st, problem)) for c in msg.constraints: method.add_constraint(self.convert(c, problem)) for c in msg.conditions: - assert len(c.span) == 0, 'Timed conditions are currently unsupported.' + assert len( + c.span) == 0, 'Timed conditions are currently unsupported.' method.add_precondition(self.convert(c.cond, problem)) return method @@ -424,7 +531,7 @@ def _convert_method( def _convert_task_network( self, msg: msgs.TaskNetwork, problem: model.htn.HierarchicalProblem ) -> model.htn.TaskNetwork: - tn = model.htn.TaskNetwork(problem.env) + tn = model.htn.TaskNetwork(problem.environment) for v in msg.variables: tn.add_variable(v.name, convert_type_str(v.type, problem)) for st in msg.subtasks: @@ -448,7 +555,7 @@ def _convert_metric( if msg.kind == msgs.Metric.MINIMIZE_ACTION_COSTS: costs = {} for i in range(len(msg.action_cost_names)): - costs[list(msg.action_cost_names)[i]] = self.convert( + costs[self.convert(list(msg.action_cost_names)[i], problem)] = self.convert( msg.action_cost_expr[i], problem) return metrics.MinimizeActionCosts( @@ -482,7 +589,10 @@ def _convert_metric( raise UPException(f'Unknown metric kind `{msg.kind}`') @handles(msgs.Action) - def _convert_action(self, msg: msgs.Action, problem: Problem) -> model.Action: + def _convert_action( + self, + msg: msgs.Action, + problem: Problem) -> model.Action: action: model.Action parameters = OrderedDict() @@ -491,14 +601,17 @@ def _convert_action(self, msg: msgs.Action, problem: Problem) -> model.Action: if len(msg.duration) > 0: action = DurativeAction(msg.name, parameters) - action.set_duration_constraint(self.convert(msg.duration[0], problem)) + action.set_duration_constraint( + self.convert(msg.duration[0], problem)) else: action = InstantaneousAction(msg.name, parameters) conditions = [] for condition in msg.conditions: cond = self.convert(condition.cond, problem) - span = self.convert(condition.span[0]) if len(condition.span) > 0 else None + span = self.convert( + condition.span[0]) if len( + condition.span) > 0 else None conditions.append((cond, span)) effects = [] @@ -518,15 +631,17 @@ def _convert_action(self, msg: msgs.Action, problem: Problem) -> model.Action: if e.kind == EffectKind.ASSIGN: action.add_effect(ot, e.fluent, e.value, e.condition) elif e.kind == EffectKind.DECREASE: - action.add_decrease_effect(ot, e.fluent, e.value, e.condition) + action.add_decrease_effect( + ot, e.fluent, e.value, e.condition) elif e.kind == EffectKind.INCREASE: - action.add_increase_effect(ot, e.fluent, e.value, e.condition) + action.add_increase_effect( + ot, e.fluent, e.value, e.condition) elif isinstance(action, InstantaneousAction): for c, _ in conditions: action.add_precondition(c) for e, _ in effects: if e.kind == EffectKind.ASSIGN: - action.add_effect(e.fluent, e.value, e.condition) + action.add_effect(e.fluent, e.value, e.condition, e.forall) elif e.kind == EffectKind.DECREASE: action.add_decrease_effect(e.fluent, e.value, e.condition) elif e.kind == EffectKind.INCREASE: @@ -546,6 +661,13 @@ def _convert_effect( else: kind = EffectKind.ASSIGN + forall = [] + if msg.forall: + for v in msg.forall: + variable = model.Variable( + v.name, self.convert(v.type, problem)) + forall.append(variable) + fluent = self.convert(msg.fluent, problem) condition = self.convert(msg.condition, problem) value = self.convert(msg.value, problem) @@ -555,6 +677,7 @@ def _convert_effect( value=value, condition=condition, kind=kind, + forall=forall ) @handles(msgs.Duration) @@ -569,7 +692,8 @@ def _convert_duration( ) @handles(msgs.TimeInterval) - def _convert_timed_interval(self, msg: msgs.TimeInterval) -> model.TimeInterval: + def _convert_timed_interval( + self, msg: msgs.TimeInterval) -> model.TimeInterval: return model.TimeInterval( lower=self.convert(msg.lower), upper=self.convert(msg.upper), @@ -591,7 +715,9 @@ def _convert_real(self, msg: msgs.Real) -> fractions.Fraction: return fractions.Fraction(msg.numerator, msg.denominator) @handles(msgs.Timepoint) - def _convert_timepoint(self, msg: msgs.Timepoint) -> model.timing.Timepoint: + def _convert_timepoint( + self, + msg: msgs.Timepoint) -> model.timing.Timepoint: if msg.kind == msgs.Timepoint.GLOBAL_START: kind = model.timing.TimepointKind.GLOBAL_START elif msg.kind == msgs.Timepoint.GLOBAL_END: @@ -610,10 +736,20 @@ def _convert_plan( self, msg: msgs.Plan, problem: Problem ) -> unified_planning.plans.Plan: actions = [self.convert(a, problem) for a in msg.actions] + if all(isinstance(a, tuple) for a in actions): - return unified_planning.plans.TimeTriggeredPlan(actions) + plan = unified_planning.plans.TimeTriggeredPlan(actions) else: - return unified_planning.plans.SequentialPlan(actions=actions) + plan = unified_planning.plans.SequentialPlan(actions=actions) + + if (PlanKind(msg.kind) == PlanKind.HIERARCHICAL_PLAN): + + decomposition = self._convert_decomposition(problem) + hierarchical_plan = unified_planning.plans.HierarchicalPlan( + plan, decomposition) + plan = hierarchical_plan + + return plan @handles(msgs.ActionInstance) def _convert_action_instance( @@ -628,7 +764,8 @@ def _convert_action_instance( ]: # action instance parameters are atoms but in UP they are FNodes # converting to up.model.FNode - parameters = tuple([self.convert(param, problem) for param in msg.parameters]) + parameters = tuple([self.convert(param, problem) + for param in msg.parameters]) action_instance = unified_planning.plans.ActionInstance( problem.action(msg.action_name), @@ -654,32 +791,26 @@ def _convert_plan_generation_result( ) -> unified_planning.engines.PlanGenerationResult: if result.status == msgs.PlanGenerationResult.SOLVED_SATISFICING: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.SOLVED_SATISFICING - ) + unified_planning.engines.results.PlanGenerationResultStatus.SOLVED_SATISFICING) elif result.status == msgs.PlanGenerationResult.SOLVED_OPTIMALLY: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.SOLVED_OPTIMALLY - ) + unified_planning.engines.results.PlanGenerationResultStatus.SOLVED_OPTIMALLY) elif result.status == msgs.PlanGenerationResult.UNSOLVABLE_PROVEN: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.UNSOLVABLE_PROVEN - ) + unified_planning.engines.results.PlanGenerationResultStatus.UNSOLVABLE_PROVEN) elif result.status == msgs.PlanGenerationResult.UNSOLVABLE_INCOMPLETELY: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.UNSOLVABLE_INCOMPLETELY - ) + PlanGenerationResultStatus.UNSOLVABLE_INCOMPLETELY) elif result.status == msgs.PlanGenerationResult.TIMEOUT: status = unified_planning.engines.results.PlanGenerationResultStatus.TIMEOUT elif result.status == msgs.PlanGenerationResult.MEMOUT: status = unified_planning.engines.results.PlanGenerationResultStatus.MEMOUT elif result.status == msgs.PlanGenerationResult.INTERNAL_ERROR: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.INTERNAL_ERROR - ) + unified_planning.engines.results.PlanGenerationResultStatus.INTERNAL_ERROR) elif result.status == msgs.PlanGenerationResult.UNSUPPORTED_PROBLEM: status = ( - unified_planning.engines.results.PlanGenerationResultStatus.UNSUPPORTED_PROBLEM - ) + unified_planning.engines.results.PlanGenerationResultStatus.UNSUPPORTED_PROBLEM) else: raise UPException(f'Unknown Planner Status: {result.status}') @@ -733,14 +864,14 @@ def _convert_compiler_result( result: msgs.CompilerResult, lifted_problem: unified_planning.model.Problem, ) -> unified_planning.engines.CompilerResult: - problem = self.convert(result.problem, lifted_problem.env) - mymap: Dict[ - unified_planning.model.Action, - Tuple[unified_planning.model.Action, List[unified_planning.model.FNode]], - ] = {} + problem = self.convert(result.problem, lifted_problem.environment) + mymap: Dict[unified_planning.model.Action, + Tuple[unified_planning.model.Action, + List[unified_planning.model.FNode]], + ] = {} for grounded_action in problem.actions: map_back_plan = dict(zip( - result.map_back_plan_keys, result.map_back_plan_values)) + result.map_back_plan_keys, result.map_back_plan_values)) original_action_instance = self.convert( map_back_plan[grounded_action.name], lifted_problem ) @@ -751,10 +882,11 @@ def _convert_compiler_result( return unified_planning.engines.CompilerResult( problem=problem, map_back_action_instance=partial( - unified_planning.engines.compilers.utils.lift_action_instance, map=mymap - ), + unified_planning.engines.compilers.utils.lift_action_instance, + map=mymap), engine_name=result.engine, - log_messages=[self.convert(log) for log in result.log_messages], + log_messages=[ + self.convert(log) for log in result.log_messages], ) @handles(msgs.ValidationResult) @@ -766,9 +898,11 @@ def _convert_validation_result( elif result.status == msgs.ValidationResult.INVALID: r_status = unified_planning.engines.ValidationResultStatus.INVALID else: - raise UPException(f'Unexpected ValidationResult status: {result.status}') + raise UPException( + f'Unexpected ValidationResult status: {result.status}') return unified_planning.engines.ValidationResult( status=r_status, engine_name=result.engine, log_messages=[self.convert(log) for log in result.log_messages], + metrics={metric.key: metric.value for metric in result.metrics} ) diff --git a/upf4ros2/upf4ros2/ros2_interface_writer.py b/upf4ros2/upf4ros2/ros2_interface_writer.py index c54d683..f1c2b4e 100644 --- a/upf4ros2/upf4ros2/ros2_interface_writer.py +++ b/upf4ros2/upf4ros2/ros2_interface_writer.py @@ -33,6 +33,7 @@ ) from unified_planning.model.timing import TimepointKind from unified_planning.model.types import domain_item, domain_size +from unified_planning.environment import get_environment import unified_planning.model.walkers as walkers from upf4ros2.converter import Converter, handles # from upf4ros2.ros2_utils import print_expr @@ -272,7 +273,8 @@ def walk_operator( sub_list = [] expr_item = msgs.ExpressionItem() expr_item.atom.append(msgs.Atom()) - expr_item.atom[0].symbol_atom.append(map_operator(expression.node_type)) + expr_item.atom[0].symbol_atom.append( + map_operator(expression.node_type)) expr_item.kind = msgs.ExpressionItem.FUNCTION_SYMBOL expr_item.type = 'up:operator' sub_list.append(expr_item) @@ -281,7 +283,8 @@ def walk_operator( other_expr = [] other_levels = [] if expression.is_exists() or expression.is_forall(): - list_prev = [self._ros2_writer.convert(p) for p in expression.variables()] + list_prev = [self._ros2_writer.convert( + p) for p in expression.variables()] (other_expr, other_levels) = self.increase_level_expressions(list_prev, 1) (args_expr, args_level) = self.increase_level_expressions(args, 1) @@ -307,37 +310,74 @@ def walk_operator( map_features = { 'ACTION_BASED': msgs.Problem.ACTION_BASED, 'HIERARCHICAL': msgs.Problem.HIERARCHICAL, + 'SCHEDULING': msgs.Problem.SCHEDULING, 'SIMPLE_NUMERIC_PLANNING': msgs.Problem.SIMPLE_NUMERIC_PLANNING, 'GENERAL_NUMERIC_PLANNING': msgs.Problem.GENERAL_NUMERIC_PLANNING, 'CONTINUOUS_TIME': msgs.Problem.CONTINUOUS_TIME, 'DISCRETE_TIME': msgs.Problem.DISCRETE_TIME, 'INTERMEDIATE_CONDITIONS_AND_EFFECTS': msgs.Problem.INTERMEDIATE_CONDITIONS_AND_EFFECTS, - 'TIMED_EFFECT': msgs.Problem.TIMED_EFFECT, + 'EXTERNAL_CONDITIONS_AND_EFFECTS': msgs.Problem.EXTERNAL_CONDITIONS_AND_EFFECTS, + 'TIMED_EFFECTS': msgs.Problem.TIMED_EFFECTS, 'TIMED_GOALS': msgs.Problem.TIMED_GOALS, 'DURATION_INEQUALITIES': msgs.Problem.DURATION_INEQUALITIES, - 'STATIC_FLUENTS_IN_DURATION': msgs.Problem.STATIC_FLUENTS_IN_DURATION, - 'FLUENTS_IN_DURATION': msgs.Problem.FLUENTS_IN_DURATION, + 'SELF_OVERLAPPING': msgs.Problem.SELF_OVERLAPPING, + 'STATIC_FLUENTS_IN_DURATIONS': msgs.Problem.STATIC_FLUENTS_IN_DURATIONS, + 'FLUENTS_IN_DURATIONS': msgs.Problem.FLUENTS_IN_DURATIONS, + 'REAL_TYPE_DURATIONS': msgs.Problem.REAL_TYPE_DURATIONS, + 'INT_TYPE_DURATIONS': msgs.Problem.INT_TYPE_DURATIONS, 'CONTINUOUS_NUMBERS': msgs.Problem.CONTINUOUS_NUMBERS, 'DISCRETE_NUMBERS': msgs.Problem.DISCRETE_NUMBERS, + 'BOUNDED_TYPES': msgs.Problem.BOUNDED_TYPES, 'NEGATIVE_CONDITIONS': msgs.Problem.NEGATIVE_CONDITIONS, 'DISJUNCTIVE_CONDITIONS': msgs.Problem.DISJUNCTIVE_CONDITIONS, - 'EQUALITY': msgs.Problem.EQUALITY, + 'EQUALITIES': msgs.Problem.EQUALITIES, 'EXISTENTIAL_CONDITIONS': msgs.Problem.EXISTENTIAL_CONDITIONS, 'UNIVERSAL_CONDITIONS': msgs.Problem.UNIVERSAL_CONDITIONS, 'CONDITIONAL_EFFECTS': msgs.Problem.CONDITIONAL_EFFECTS, 'INCREASE_EFFECTS': msgs.Problem.INCREASE_EFFECTS, 'DECREASE_EFFECTS': msgs.Problem.DECREASE_EFFECTS, + 'STATIC_FLUENTS_IN_BOOLEAN_ASSIGNMENTS': msgs.Problem.STATIC_FLUENTS_IN_BOOLEAN_ASSIGNMENTS, + 'STATIC_FLUENTS_IN_NUMERIC_ASSIGNMENTS': msgs.Problem.STATIC_FLUENTS_IN_NUMERIC_ASSIGNMENTS, + 'STATIC_FLUENTS_IN_OBJECT_ASSIGNMENTS': msgs.Problem.STATIC_FLUENTS_IN_OBJECT_ASSIGNMENTS, + 'FLUENTS_IN_BOOLEAN_ASSIGNMENTS': msgs.Problem.FLUENTS_IN_BOOLEAN_ASSIGNMENTS, + 'FLUENTS_IN_NUMERIC_ASSIGNMENTS': msgs.Problem.FLUENTS_IN_NUMERIC_ASSIGNMENTS, + 'FLUENTS_IN_OBJECT_ASSIGNMENTS': msgs.Problem.FLUENTS_IN_OBJECT_ASSIGNMENTS, + 'FORALL_EFFECTS': msgs.Problem.FORALL_EFFECTS, 'FLAT_TYPING': msgs.Problem.FLAT_TYPING, 'HIERARCHICAL_TYPING': msgs.Problem.HIERARCHICAL_TYPING, 'NUMERIC_FLUENTS': msgs.Problem.NUMERIC_FLUENTS, 'OBJECT_FLUENTS': msgs.Problem.OBJECT_FLUENTS, + 'INT_FLUENTS': msgs.Problem.INT_FLUENTS, + 'REAL_FLUENTS': msgs.Problem.REAL_FLUENTS, + 'BOOL_FLUENT_PARAMETERS': msgs.Problem.BOOL_FLUENT_PARAMETERS, + 'BOUNDED_INT_FLUENT_PARAMETERS': msgs.Problem.BOUNDED_INT_ACTION_PARAMETERS, + 'BOOL_ACTION_PARAMETERS': msgs.Problem.BOOL_ACTION_PARAMETERS, + 'BOUNDED_INT_ACTION_PARAMETERS': msgs.Problem.BOUNDED_INT_ACTION_PARAMETERS, + 'UNBOUNDED_INT_ACTION_PARAMETERS': msgs.Problem.UNBOUNDED_INT_ACTION_PARAMETERS, + 'REAL_ACTION_PARAMETERS': msgs.Problem.REAL_ACTION_PARAMETERS, 'ACTIONS_COST': msgs.Problem.ACTIONS_COST, 'FINAL_VALUE': msgs.Problem.FINAL_VALUE, 'MAKESPAN': msgs.Problem.MAKESPAN, 'PLAN_LENGTH': msgs.Problem.PLAN_LENGTH, 'OVERSUBSCRIPTION': msgs.Problem.OVERSUBSCRIPTION, + 'TEMPORAL_OVERSUBSCRIPTION': msgs.Problem.TEMPORAL_OVERSUBSCRIPTION, + 'STATIC_FLUENTS_IN_ACTIONS_COST': msgs.Problem.STATIC_FLUENTS_IN_ACTIONS_COST, + 'FLUENTS_IN_ACTIONS_COST': msgs.Problem.FLUENTS_IN_ACTIONS_COST, + 'REAL_NUMBERS_IN_ACTIONS_COST': msgs.Problem.REAL_NUMBERS_IN_ACTIONS_COST, + 'INT_NUMBERS_IN_ACTIONS_COST': msgs.Problem.INT_NUMBERS_IN_ACTIONS_COST, + 'REAL_NUMBERS_IN_OVERSUBSCRIPTION': msgs.Problem.REAL_NUMBERS_IN_OVERSUBSCRIPTION, + 'INT_NUMBERS_IN_OVERSUBSCRIPTION': msgs.Problem.INT_NUMBERS_IN_OVERSUBSCRIPTION, 'SIMULATED_EFFECTS': msgs.Problem.SIMULATED_EFFECTS, -} + 'TRAJECTORY_CONSTRAINTS': msgs.Problem.TRAJECTORY_CONSTRAINTS, + 'STATE_INVARIANTS': msgs.Problem.STATE_INVARIANTS, + 'METHOD_PRECONDITIONS': msgs.Problem.METHOD_PRECONDITIONS, + 'TASK_NETWORK_CONSTRAINTS': msgs.Problem.TASK_NETWORK_CONSTRAINTS, + 'INITIAL_TASK_NETWORK_VARIABLES': msgs.Problem.INITIAL_TASK_NETWORK_VARIABLES, + 'TASK_ORDER_TOTAL': msgs.Problem.TASK_ORDER_TOTAL, + 'TASK_ORDER_PARTIAL': msgs.Problem.TASK_ORDER_PARTIAL, + 'TASK_ORDER_TEMPORAL': msgs.Problem.TASK_ORDER_TEMPORAL, + 'UNDEFINED_INITIAL_NUMERIC': msgs.Problem.UNDEFINED_INITIAL_NUMERIC, + 'UNDEFINED_INITIAL_SYMBOLIC': msgs.Problem.UNDEFINED_INITIAL_SYMBOLIC} def map_feature(feature: str) -> int: @@ -366,7 +406,8 @@ def _convert_fluent( ret.value_type = interface_type(fluent.type) ret.parameters = sig if fluent in problem.fluents_defaults: - ret.default_value.append(self.convert(problem.fluents_defaults[fluent])) + ret.default_value.append(self.convert( + problem.fluents_defaults[fluent])) return ret @handles(model.Object) @@ -381,20 +422,26 @@ def _convert_fnode(self, exp: model.FNode) -> msgs.Expression: return self._fnode2ros2.convert(exp) @handles(model.types._BoolType) - def _convert_bool_type(self, tpe: model.types._BoolType) -> msgs.TypeDeclaration: + def _convert_bool_type( + self, + tpe: model.types._BoolType) -> msgs.TypeDeclaration: ret = msgs.TypeDeclaration() ret.type_name = interface_type(tpe) return ret @handles(model.types._UserType) - def _convert_user_type(self, t: model.types._UserType) -> msgs.TypeDeclaration: + def _convert_user_type( + self, + t: model.types._UserType) -> msgs.TypeDeclaration: ret = msgs.TypeDeclaration() ret.type_name = interface_type(t) ret.parent_type = '' if t.father is None else interface_type(t.father) return ret @handles(model.types._IntType) - def _convert_integer_type(self, t: model.types._IntType) -> msgs.TypeDeclaration: + def _convert_integer_type( + self, + t: model.types._IntType) -> msgs.TypeDeclaration: ret = msgs.TypeDeclaration() ret.type_name = interface_type(t) return ret @@ -418,6 +465,14 @@ def _convert_effect(self, effect: model.Effect) -> msgs.EffectExpression: raise ValueError(f'Unsupported effect: {effect}') ret = msgs.EffectExpression() + ret.forall = [] + if effect.forall: + for v in effect.forall: + variable = msgs.Variable() + variable.name = v.name + variable.type = self.convert(v.type) + ret.forall.append(variable) + ret.kind = kind ret.fluent = self.convert(effect.fluent) @@ -513,7 +568,9 @@ def _convert_fraction(self, fraction: fractions.Fraction) -> msgs.Real: return ret @handles(model.timing.Interval) - def _convert_interval(self, interval: model.timing.Interval) -> msgs.Interval: + def _convert_interval( + self, + interval: model.timing.Interval) -> msgs.Interval: ret = msgs.Interval() ret.is_left_open = interval.is_left_open() @@ -600,7 +657,9 @@ def _convert_method(self, method: model.htn.Method) -> msgs.Method: return ret @handles(model.htn.TaskNetwork) - def _convert_task_network(self, tn: model.htn.TaskNetwork) -> msgs.TaskNetwork: + def _convert_task_network( + self, + tn: model.htn.TaskNetwork) -> msgs.TaskNetwork: ret = msgs.TaskNetwork() ret.variables = [self.convert(v) for v in tn.variables] ret.subtasks = [self.convert(st) for st in tn.subtasks] @@ -616,14 +675,49 @@ def build_hierarchy( ret.methods = [self.convert(m) for m in problem.methods] return ret + @handles(unified_planning.plans.Schedule) + def _convert_schedule( + self, schedule: unified_planning.plans.Schedule) -> msgs.Schedule: + + ret = msgs.Schedule() + + ret.activities_name = [a.name for a in schedule.activities] + + assignments = [] + + for var, val in schedule.assignment.items(): + e = msgs.Expression() + item = msgs.ExpressionItem() + + if isinstance(var, model.Timepoint): + if var.kind == TimepointKind.START: + var = f"{var.container}.start" + elif var.kind == TimepointKind.END: + var = f"{var.container}.end" + else: + raise ValueError(f"Invalid timepoint in assignment: {var}") + else: + assert isinstance(var, model.Parameter) + var = var.name + + item.type = var + (val,) = get_environment().expression_manager.auto_promote(val) + item.atom = self.convert(val).expressions[0].atom + e.expressions.append(item) + assignments.append(e) + + ret.variable_assignments = assignments + return ret + @handles(model.Problem, model.htn.HierarchicalProblem) def _convert_problem(self, problem: model.Problem) -> msgs.Problem: + goals = [msgs.Goal(goal=self.convert(g)) for g in problem.goals] - for (t, gs) in problem.timed_goals: - for g in gs: + for t_goal in problem.timed_goals: + for g in problem.timed_goals[t_goal]: goal = msgs.Goal() goal.goal = self.convert(g) - goal.timing.append(self.convert(t)) + goal.timing.append(self.convert(t_goal)) goals += [goal] problem_name = str(problem.name) if problem.name is not None else '' @@ -645,9 +739,22 @@ def _convert_problem(self, problem: model.Problem) -> msgs.Problem: assignment.value = self.convert(v) ret.initial_state.append(assignment) - ret.timed_effects = [self.convert(e) for e in problem.timed_effects] + if len(problem.trajectory_constraints) > 0: + ret.features.append(msgs.Problem.TRAJECTORY_CONSTRAINTS) + for constraint in problem.trajectory_constraints: + condition = self.convert(constraint) + ret.trajectory_constraints.append(condition) + + ret.timed_effects = [ + msgs.TimedEffect( + effect=[self.convert(e) for e in problem.timed_effects[key]], + occurrence_time=self.convert(key) + ) + for key in problem.timed_effects + ] ret.goals = goals - ret.features = [map_feature(feature) for feature in problem.kind.features] + ret.features = [map_feature(feature) + for feature in problem.kind.features] ret.metrics = [self.convert(m) for m in problem.quality_metrics] ret.hierarchy = hierarchy return ret @@ -662,7 +769,9 @@ def _convert_minimize_action_costs( ret = msgs.Metric() ret.kind = msgs.Metric.MINIMIZE_ACTION_COSTS - ret.action_cost_names = action_costs.keys() + ret.action_cost_names = [ + msgs.Action( + name=key) for key in action_costs.keys()] ret.action_cost_expr = list(action_costs.values()) if metric.default is not None: ret.default_action_cost.append(self.convert(metric.default)) @@ -735,8 +844,10 @@ def _convert_expression_variable( @handles(unified_planning.plans.ActionInstance) def _convert_action_instance( - self, a: unified_planning.plans.ActionInstance, start_time=None, end_time=None - ) -> msgs.ActionInstance: + self, + a: unified_planning.plans.ActionInstance, + start_time=None, + end_time=None) -> msgs.ActionInstance: parameters = [] for param in a.actual_parameters: # The parameters are atoms @@ -748,6 +859,9 @@ def _convert_action_instance( ret.start_time = start_time ret.end_time = end_time ret.time_triggered = True + elif bool(start_time): + ret.start_time = start_time + ret.time_triggered = True else: ret.time_triggered = False return ret @@ -763,9 +877,26 @@ def _convert_sequential_plan( self, plan: unified_planning.plans.SequentialPlan ) -> msgs.Plan: ret = msgs.Plan() + ret.kind = 1 ret.actions = [self.convert(a) for a in plan.actions] return ret + @handles(unified_planning.plans.HierarchicalPlan) + def _convert_hierarchical_plan( + self, plan: unified_planning.plans.HierarchicalPlan + ) -> msgs.Plan: + ret = msgs.Plan() + + if isinstance(plan._flat_plan, unified_planning.plans.SequentialPlan): + ret = self._convert_sequential_plan(plan._flat_plan) + elif isinstance(plan._flat_plan, unified_planning.plans.TimeTriggeredPlan): + ret = self._convert_time_triggered_plan(plan._flat_plan) + else: + raise UPException(f"Unknown plan: {type(plan._flat_plan)}") + + ret.kind = 6 + return ret + @handles(unified_planning.plans.TimeTriggeredPlan) def _convert_time_triggered_plan( self, plan: unified_planning.plans.TimeTriggeredPlan @@ -773,13 +904,14 @@ def _convert_time_triggered_plan( action_instances = [] for a in plan.timed_actions: - start_time = self.convert(a[0]) - end_time = self.convert(a[0] + a[2]) + start_time = self.convert(a[0]) if a[0] is not None else None + end_time = self.convert(a[0] + a[2]) if a[2] is not None else None instance = self._convert_action_instance( a[1], start_time=start_time, end_time=end_time ) action_instances.append(instance) ret = msgs.Plan() + ret.kind = 2 ret.actions = action_instances return ret @@ -876,10 +1008,12 @@ def _convert_compiler_result( log_messages = [] if result.map_back_action_instance is not None: for compiled_action in result.problem.actions: - type_list = [param.type for param in compiled_action.parameters] + type_list = [ + param.type for param in compiled_action.parameters] if len(type_list) == 0: ai = unified_planning.plans.ActionInstance(compiled_action) - mymap[str(ai)] = self.convert(result.map_back_action_instance(ai)) + mymap[str(ai)] = self.convert( + result.map_back_action_instance(ai)) continue ground_size = 1 domain_sizes = [] @@ -897,7 +1031,8 @@ def _convert_compiler_result( ai = unified_planning.plans.ActionInstance( compiled_action, tuple(grounded_params) ) - mymap[str(ai)] = self.convert(result.map_back_action_instance(ai)) + mymap[str(ai)] = self.convert( + result.map_back_action_instance(ai)) ret = msgs.CompilerResult() ret.problem = self.convert(result.problem) ret.map_back_plan_keys = mymap.keys() @@ -914,6 +1049,11 @@ def _convert_validation_result( ret.status = self.convert(result.status) ret.log_messages = [self.convert(log) for log in result.log_messages] ret.engine = result.engine_name + ret.metrics = [ + msgs.ValidationMetric( + key=key, + value=value) for key, + value in result.metrics.items()] return ret @handles(unified_planning.engines.ValidationResultStatus) diff --git a/upf4ros2/upf4ros2/upf4ros2_main.py b/upf4ros2/upf4ros2/upf4ros2_main.py index 14f6a48..e167999 100644 --- a/upf4ros2/upf4ros2/upf4ros2_main.py +++ b/upf4ros2/upf4ros2/upf4ros2_main.py @@ -47,6 +47,7 @@ ) from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv class UPF4ROS2Node(Node): @@ -85,9 +86,11 @@ def __init__(self): self._set_initial_value = self.create_service( SetInitialValue, 'upf4ros2/srv/set_initial_value', self.set_initial_value) self._add_goal = self.create_service( - AddGoal, 'upf4ros2/srv/add_goal', self.add_goal) + AddGoal, 'upf4ros2/add_goal', self.add_goal) self._pddl_plan_one_shot_srv = self.create_service( PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL', self.pddl_plan_one_shot) + self._plan_one_shot_srv = self.create_service( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot', self.plan_one_shot) def get_problem(self, request, response): if request.problem_name not in self.problems: @@ -126,8 +129,10 @@ def add_fluent(self, request, response): else: problem = self.problems[request.problem_name] - fluent = self._ros2_interface_reader.convert(request.fluent, problem) - value = self._ros2_interface_reader.convert(request.default_value, problem) + fluent = self._ros2_interface_reader.convert( + request.fluent, problem) + value = self._ros2_interface_reader.convert( + request.default_value, problem) problem.add_fluent(fluent, default_initial_value=value) response.success = True return response @@ -139,7 +144,8 @@ def add_action(self, request, response): else: problem = self.problems[request.problem_name] - action = self._ros2_interface_reader.convert(request.action, problem) + action = self._ros2_interface_reader.convert( + request.action, problem) problem.add_action(action) response.success = True return response @@ -151,7 +157,8 @@ def add_object(self, request, response): else: problem = self.problems[request.problem_name] - action = self._ros2_interface_reader.convert(request.object, problem) + action = self._ros2_interface_reader.convert( + request.object, problem) problem.add_object(action) response.success = True @@ -164,7 +171,8 @@ def set_initial_value(self, request, response): else: problem = self.problems[request.problem_name] - expression = self._ros2_interface_reader.convert(request.expression, problem) + expression = self._ros2_interface_reader.convert( + request.expression, problem) value = self._ros2_interface_reader.convert(request.value, problem) problem.set_initial_value(expression, value) response.success = True @@ -178,11 +186,13 @@ def add_goal(self, request, response): problem = self.problems[request.problem_name] if len(request.goal) > 0: - goal = self._ros2_interface_reader.convert(request.goal[0].goal, problem) + goal = self._ros2_interface_reader.convert( + request.goal[0].goal, problem) problem.add_goal(goal) response.success = True elif len(request.goal_with_cost) > 0: - goal = self._ros2_interface_reader.convert(request.goal_with_cost[0].goal, problem) + goal = self._ros2_interface_reader.convert( + request.goal_with_cost[0].goal, problem) problem.add_goal(goal) response.success = True else: @@ -190,6 +200,25 @@ def add_goal(self, request, response): response.message = 'Goal is void' return response + def plan_one_shot(self, request, response): + + upf_problem = self._ros2_interface_reader.convert(request.problem) + + with OneshotPlanner(problem_kind=upf_problem.kind) as planner: + result = planner.solve(upf_problem) + print('%s returned: %s' % (planner.name, result.plan)) + + if result.plan is not None: + response.plan_result = self._ros2_interface_writer.convert( + result) + response.success = True + response.message = '' + else: + response.success = False + response.message = 'No plan found' + + return response + def pddl_plan_one_shot(self, request, response): domain_file = '' problem_file = '' @@ -207,7 +236,7 @@ def pddl_plan_one_shot(self, request, response): problem_file = request.plan_request.problem reader = PDDLReader() - + try: upf_problem = reader.parse_problem(domain_file, problem_file) except Exception: @@ -218,9 +247,10 @@ def pddl_plan_one_shot(self, request, response): with OneshotPlanner(problem_kind=upf_problem.kind) as planner: result = planner.solve(upf_problem) print('%s returned: %s' % (planner.name, result.plan)) - - if result.plan is not None: - response.plan_result = self._ros2_interface_writer.convert(result) + + if result.plan is not None: + response.plan_result = self._ros2_interface_writer.convert( + result) response.success = True response.message = '' else: @@ -250,10 +280,13 @@ def pddl_plan_one_shot_callback(self, goal_handle): with OneshotPlanner(problem_kind=upf_problem.kind) as planner: result = planner.solve(upf_problem) - print('%s returned: %s' % (planner.name, result.plan)) + self.get_logger().info( + '%s returned: %s' % + (planner.name, result.plan)) feedback_msg = PDDLPlanOneShot.Feedback() - feedback_msg.plan_result = self._ros2_interface_writer.convert(result) + feedback_msg.plan_result = self._ros2_interface_writer.convert( + result) goal_handle.publish_feedback(feedback_msg) goal_handle.succeed() @@ -264,14 +297,18 @@ def pddl_plan_one_shot_callback(self, goal_handle): return result def plan_one_shot_callback(self, goal_handle): - upf_problem = self._ros2_interface_reader.convert(goal_handle.request.plan_request.problem) + upf_problem = self._ros2_interface_reader.convert( + goal_handle.request.plan_request.problem) with OneshotPlanner(problem_kind=upf_problem.kind) as planner: result = planner.solve(upf_problem) - print('%s returned: %s' % (planner.name, result.plan)) + self.get_logger().info( + '%s returned: %s' % + (planner.name, result.plan)) feedback_msg = PlanOneShot.Feedback() - feedback_msg.plan_result = self._ros2_interface_writer.convert(result) + feedback_msg.plan_result = self._ros2_interface_writer.convert( + result) goal_handle.publish_feedback(feedback_msg) goal_handle.succeed() diff --git a/upf4ros2_demo/README.md b/upf4ros2_demo/README.md new file mode 100644 index 0000000..e69de29 diff --git a/upf4ros2_demo/launch/upf4ros2_demo1.launch.py b/upf4ros2_demo/launch/upf4ros2_demo1.launch.py new file mode 100644 index 0000000..49feec4 --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo1.launch.py @@ -0,0 +1,51 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import yaml +import os + + +def generate_launch_description(): + + pkg_name = "upf4ros2_demo" + + # + # ARGS + # + + points = LaunchConfiguration("wps") + declare_points_cmd = DeclareLaunchArgument( + "wps", + default_value=get_package_share_directory( + pkg_name) + "/params/house.yaml", + description="YAML waypoints file") + + # + # ACTIONS + # + + upf4ros2_navigation_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_navigation_action", + name="upf4ros2_navigation_action", + parameters=[points], + output='screen') + + # + # NODES + # + + upf4ros2_demo_cmd = Node( + package=pkg_name, + executable="upf4ros2_demo1", + name="upf4ros2_demo1", + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_points_cmd) + ld.add_action(upf4ros2_navigation_action_cmd) + ld.add_action(upf4ros2_demo_cmd) + + return ld diff --git a/upf4ros2_demo/launch/upf4ros2_demo1_bash.launch.py b/upf4ros2_demo/launch/upf4ros2_demo1_bash.launch.py new file mode 100644 index 0000000..15d3c69 --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo1_bash.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + upf4ros2_demo_cmd = Node(package='upf4ros2_demo', + executable='upf4ros2_demo1_bash', + output='screen', + parameters=[ + {'problem_name': 'test'} + ]) + + ld = LaunchDescription() + ld.add_action(upf4ros2_demo_cmd) + + return ld diff --git a/upf4ros2_demo/launch/upf4ros2_demo1_pddlfile.launch.py b/upf4ros2_demo/launch/upf4ros2_demo1_pddlfile.launch.py new file mode 100644 index 0000000..546ddeb --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo1_pddlfile.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + upf4ros2_pddl_cmd = Node(package='upf4ros2_demo', + executable='upf4ros2_demo1_pddl', + output='screen', + parameters=[ + {'domain': '/pddl/domain.pddl'}, + {'problem': '/pddl/problem.pddl'} + ]) + + ld = LaunchDescription() + ld.add_action(upf4ros2_pddl_cmd) + + return ld diff --git a/upf4ros2_demo/launch/upf4ros2_demo2.launch.py b/upf4ros2_demo/launch/upf4ros2_demo2.launch.py new file mode 100644 index 0000000..4375b5b --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo2.launch.py @@ -0,0 +1,49 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + pkg_name = "upf4ros2_demo" + + # + # ARGS + # + + points = LaunchConfiguration("wps") + declare_points_cmd = DeclareLaunchArgument( + "wps", + default_value=get_package_share_directory( + pkg_name) + "/params/house.yaml", + description="YAML waypoints file") + + # + # ACTIONS + # + + upf4ros2_navigation_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_navigation_action", + name="upf4ros2_navigation_action", + parameters=[points], + output='screen') + + # + # NODES + # + + upf4ros2_demo_cmd = Node( + package=pkg_name, + executable="upf4ros2_demo2", + name="upf4ros2_demo2", + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_points_cmd) + ld.add_action(upf4ros2_navigation_action_cmd) + ld.add_action(upf4ros2_demo_cmd) + + return ld diff --git a/upf4ros2_demo/launch/upf4ros2_demo3.launch.py b/upf4ros2_demo/launch/upf4ros2_demo3.launch.py new file mode 100644 index 0000000..73f780c --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo3.launch.py @@ -0,0 +1,56 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + pkg_name = "upf4ros2_demo" + + # + # ARGS + # + + points = LaunchConfiguration("wps") + declare_points_cmd = DeclareLaunchArgument( + "wps", + default_value=get_package_share_directory( + pkg_name) + "/params/house.yaml", + description="YAML waypoints file") + + # + # ACTIONS + # + + upf4ros2_navigation_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_navigation_action", + name="upf4ros2_navigation_action", + parameters=[points], + output='screen') + + upf4ros2_check_wp_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_check_wp_action", + name="upf4ros2_check_wp_action", + output='screen') + + # + # NODES + # + + upf4ros2_demo_cmd = Node( + package=pkg_name, + executable="upf4ros2_demo3", + name="upf4ros2_demo3", + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_points_cmd) + ld.add_action(upf4ros2_navigation_action_cmd) + ld.add_action(upf4ros2_check_wp_action_cmd) + ld.add_action(upf4ros2_demo_cmd) + + return ld diff --git a/upf4ros2_demo/package.xml b/upf4ros2_demo/package.xml new file mode 100644 index 0000000..4302b5b --- /dev/null +++ b/upf4ros2_demo/package.xml @@ -0,0 +1,23 @@ + + + + upf4ros2_demo + 0.0.0 + TODO: Package description + gentlebots + Apache License, Version 2.0 + + rclpy + sensor_msgs + geometry_msgs + upf_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/upf4ros2_demo/params/house.yaml b/upf4ros2_demo/params/house.yaml new file mode 100644 index 0000000..f31c1de --- /dev/null +++ b/upf4ros2_demo/params/house.yaml @@ -0,0 +1,38 @@ +upf4ros2_navigation_action: + ros__parameters: + wps: + - "entrance" + - "5.80" + - "-4.63" + - "-0.77" + - "0.63" + + - "kitchen" + - "7.22" + - "-0.97" + - "0.002" + - "1.0" + + - "livingroom" + - "1.57" + - "0.70" + - "-0.001" + - "1.0" + + - "bedroom" + - "-6.28" + - "-0.14" + - "-0.001" + - "1.0" + + - "room" + - "-7.78" + - "-2.76" + - "-0.001" + - "1.0" + + - "gym" + - "1.85" + - "4.38" + - "-0.005" + - "1.0" \ No newline at end of file diff --git a/upf4ros2_demo/resource/upf4ros2_demo b/upf4ros2_demo/resource/upf4ros2_demo new file mode 100644 index 0000000..e69de29 diff --git a/upf4ros2_demo/resource/upf_problem.sh b/upf4ros2_demo/resource/upf_problem.sh new file mode 100755 index 0000000..41af558 --- /dev/null +++ b/upf4ros2_demo/resource/upf_problem.sh @@ -0,0 +1,32 @@ +#! /bin/bash +# Script to create a example problem to UPF + +# Create new problem + +ros2 service call /upf4ros2/srv/new_problem upf_msgs/srv/NewProblem "{problem_name: test}" + +# Create the fluent robot_at + +ros2 service call /upf4ros2/srv/add_fluent upf_msgs/srv/AddFluent "{problem_name: test, fluent:{name: robot_at, value_type: up:bool, parameters:[{name: l1, type: Location}]}, default_value:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}}" + +# Create the objects l1 and l2 (Location) + +ros2 service call /upf4ros2/srv/add_object upf_msgs/srv/AddObject "{problem_name: test, object:{name: l1, type: Location}}" + +ros2 service call /upf4ros2/srv/add_object upf_msgs/srv/AddObject "{problem_name: test, object:{name: l2, type: Location}}" + +# Set initial values to the fluent robot_at with the objects l1 and l2: robot_at(l1) and robot_at(l2) +# The robot is at l1 but not at l2 + +ros2 service call /upf4ros2/srv/set_initial_value upf_msgs/srv/SetInitialValue "{problem_name: test, expression: {expressions:[{atom:[{symbol_atom:['robot_at l1'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l1'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}" + +ros2 service call /upf4ros2/srv/set_initial_value upf_msgs/srv/SetInitialValue "{problem_name: test, expression: {expressions:[{atom:[{symbol_atom:['robot_at l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}}" + +# Create the action move to change the location + +ros2 service call /upf4ros2/srv/add_action upf_msgs/srv/AddAction "{problem_name: test, action:{name: move, parameters:[{name: l_from, type: Location},{name: l_to, type: Location}], duration:[], conditions:[{cond: {expressions:[{atom:[], type: up:bool, kind: 5},{atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3},{atom:[{symbol_atom:['l_from'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, span:[]}], effects:[{effect:{kind: 0, fluent: {expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l_from'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}, condition:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}, occurrence_time:[]}, {effect:{kind: 0, fluent: {expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l_to'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}, condition:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}, occurrence_time:[]}]}}" + +# Create the goal to the problem +# The robot is at l2 at the end of the problem + +ros2 service call /upf4ros2/add_goal upf_msgs/srv/AddGoal "{problem_name: test, goal:[{goal:{expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, timing:[]}], goal_with_cost:[]}" diff --git a/upf4ros2_demo/setup.cfg b/upf4ros2_demo/setup.cfg new file mode 100644 index 0000000..1607736 --- /dev/null +++ b/upf4ros2_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/upf4ros2_demo +[install] +install_scripts=$base/lib/upf4ros2_demo diff --git a/upf4ros2_demo/setup.py b/upf4ros2_demo/setup.py new file mode 100644 index 0000000..bf49b08 --- /dev/null +++ b/upf4ros2_demo/setup.py @@ -0,0 +1,38 @@ +from glob import glob +import os + +from setuptools import setup + +package_name = 'upf4ros2_demo' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'pddl'), glob('test/pddl/*')), + (os.path.join("share", package_name, "params"), glob("params/*.yaml")), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='gentlebots', + maintainer_email='igonzf06@estudiantes.unileon.es', + description='ROS 2 Support for UPF', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'upf4ros2_demo1 = upf4ros2_demo.upf4ros2_demo1:main', + 'upf4ros2_demo1_pddl = upf4ros2_demo.upf4ros2_demo1_pddl:main', + 'upf4ros2_demo1_bash = upf4ros2_demo.upf4ros2_demo1_bash:main', + 'upf4ros2_demo2 = upf4ros2_demo.upf4ros2_demo2:main', + 'upf4ros2_demo3 = upf4ros2_demo.upf4ros2_demo3:main', + 'upf4ros2_navigation_action = upf4ros2_demo.upf4ros2_navigation_action:main', + 'upf4ros2_check_wp_action = upf4ros2_demo.upf4ros2_check_wp_action:main' + ], + }, +) diff --git a/upf4ros2_demo/test/pddl/domain.pddl b/upf4ros2_demo/test/pddl/domain.pddl new file mode 100644 index 0000000..4703557 --- /dev/null +++ b/upf4ros2_demo/test/pddl/domain.pddl @@ -0,0 +1,24 @@ +(define (domain test) + + (:types + location ; service areas, points of interest, navigation goals + ) + + (:predicates + ; the robot is at location ?l + (robot_at ?l - location) + ) + + ; navigation action + (:action move + :parameters (?l_from ?l_to - location) + :precondition (and + (robot_at ?l_from) + ) + :effect (and + (not (robot_at ?l_from)) + (robot_at ?l_to) + ) + ) + +) \ No newline at end of file diff --git a/upf4ros2_demo/test/pddl/domain_check_door.pddl b/upf4ros2_demo/test/pddl/domain_check_door.pddl new file mode 100644 index 0000000..9f05631 --- /dev/null +++ b/upf4ros2_demo/test/pddl/domain_check_door.pddl @@ -0,0 +1,43 @@ +(define (domain doorbell_attend) + + (:types + wp + sound + door + ) + + (:predicates + ; the robot is at location ?l + (robot_at ?l - wp) + (door_at ?d - door ?l - wp) + (door_checked ?d - door) + (sound_listened ?s - sound) + ) + + ; navigation action + (:durative-action move + :parameters (?l_from ?l_to - location) + :duration (= ?duration 10) + :condition (and + (robot_at ?l_from) + ) + :effect (and + (not (robot_at ?l_from)) + (robot_at ?l_to) + ) + ) + + ; check door action + (:durative-action check_door + :parameters (?d - door ?entrance - wp) + :duration (= ?duration 10) + :condition (and + (door_at ?d ?entrance) + (robot_at ?entrance) + ) + :effect (and + (door_checked ?d) + ) + ) + +) \ No newline at end of file diff --git a/upf4ros2_demo/test/pddl/problem.pddl b/upf4ros2_demo/test/pddl/problem.pddl new file mode 100644 index 0000000..8286b59 --- /dev/null +++ b/upf4ros2_demo/test/pddl/problem.pddl @@ -0,0 +1,14 @@ +(define (problem test_problem) + (:domain test) + (:objects + ; locations + livingroom entrance - location + ) + (:init + ; the robot at start is at livingroom + (robot_at livingroom) + ) + (:goal + (robot_at entrance) + ) +) \ No newline at end of file diff --git a/upf4ros2_demo/test/pddl/problem_check_door.pddl b/upf4ros2_demo/test/pddl/problem_check_door.pddl new file mode 100644 index 0000000..55da549 --- /dev/null +++ b/upf4ros2_demo/test/pddl/problem_check_door.pddl @@ -0,0 +1,16 @@ +(define (problem doorbell_attend_problem) + (:domain doorbell_attend) + (:objects + livingroom entrance - wp + door - door + doorbell - sound + ) + (:init + ; the robot at start is at livingroom + (robot_at livingroom) + (door_at door entrance) + ) + (:goal + (door_checked door) + ) +) \ No newline at end of file diff --git a/upf4ros2_demo/test/test_copyright.py b/upf4ros2_demo/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/upf4ros2_demo/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/upf4ros2_demo/test/test_flake8.py b/upf4ros2_demo/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/upf4ros2_demo/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/upf4ros2_demo/test/test_pep257.py b/upf4ros2_demo/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/upf4ros2_demo/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/upf4ros2_demo/upf4ros2_demo/__init__.py b/upf4ros2_demo/upf4ros2_demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py new file mode 100644 index 0000000..7db2002 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py @@ -0,0 +1,129 @@ +from typing import List +import rclpy + +from geometry_msgs.msg import Pose +from nav2_msgs.action import NavigateToPose +from upf4ros2_demo_msgs.srv import CallAction +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from std_msgs.msg import String +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.srv import ( + SetInitialValue, + GetProblem +) + +from simple_node import Node + + +class CheckWpAction(Node): + + def __init__(self): + super().__init__('upf4ros2_check_wp_action') + + self._problem_name = 'test' + + self._userType = shortcuts.UserType('location') + self._fluent = model.Fluent( + "wp_checked", + shortcuts.BoolType(), + object=self._userType) + self._fluent_robot_at = model.Fluent( + "robot_at", shortcuts.BoolType(), object=self._userType) + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/get_problem') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/set_initial_value') + + self.create_service( + CallAction, 'check_wp', self.__execute_callback) + + def get_problem(self): + """ get actual state of the problem + Args: + + """ + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.res = self._get_problem.call(srv) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.res.problem + return problem + + def set_initial_value(self, fluent, object, value_fluent): + """ set initial value to the fluent + Args: + fluent (up.model.Fluent): fluent + object (up.model.Object): fluent's object + value_fluent (bool): new value + """ + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call(srv) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def __execute_callback(self, request, response): + """ srv callback to call the NavigateToPose action + Args: + request (CallAction.Request): request with the action's name and parameters + response (CallAction.Response): response with the result of the action + Returns: + CallAction.Response: response with the result of the action + """ + problem = self._ros2_interface_reader.convert(self.get_problem()) + + wp = request.parameters[0].symbol_atom[0] + l1 = model.Object(wp, self._userType) + + if str(problem.initial_values[self._fluent_robot_at(l1)]) == 'true': + + self.get_logger().info("Starting action " + request.action_name) + self.get_logger().info("Waypoint " + str(wp) + " checked.") + + self.set_initial_value(self._fluent, l1, True) + response.result = True + else: + self.get_logger().info("Cannot check wp because the wp was not accesible.") + response.result = False + + return response + + +def main(args=None): + rclpy.init(args=args) + + check_wp_action = CheckWpAction() + + check_wp_action.join_spin() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1.py new file mode 100644 index 0000000..a2126f2 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1.py @@ -0,0 +1,256 @@ +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from unified_planning import model + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.action import ( + PDDLPlanOneShot, + PlanOneShot +) + +from upf_msgs.srv import ( + AddAction, + AddFluent, + AddGoal, + AddObject, + GetProblem, + NewProblem, + SetInitialValue +) + +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv + + +class UPF4ROS2Demo1Node(Node): + + def __init__(self): + super().__init__('upf4ros2_demo1') + + self._problem_name = '' + self._plan_result = {} + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._plan_one_shot_client = ActionClient( + self, + PlanOneShot, + 'upf4ros2/action/planOneShot') + + self._plan_pddl_one_shot_client = ActionClient( + self, + PDDLPlanOneShot, + 'upf4ros2/action/planOneShotPDDL') + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/srv/get_problem') + self._new_problem = self.create_client( + NewProblem, 'upf4ros2/srv/new_problem') + self._add_fluent = self.create_client( + AddFluent, 'upf4ros2/srv/add_fluent') + self._add_action = self.create_client( + AddAction, 'upf4ros2/srv/add_action') + self._add_object = self.create_client( + AddObject, 'upf4ros2/srv/add_object') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/srv/set_initial_value') + self._add_goal = self.create_client( + AddGoal, 'upf4ros2/add_goal') + self._plan_one_shot_client_srv = self.create_client( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot') + + def new_problem(self, problem_name): + srv = NewProblem.Request() + srv.problem_name = problem_name + + self._new_problem.wait_for_service() + self.future = self._new_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self._problem_name = problem_name + self.get_logger().info( + f'Create the problem with name: {srv.problem_name}') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.future = self._get_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.future.result().problem + return problem + + def add_fluent(self, problem, fluent_name, user_type): + fluent = model.Fluent( + fluent_name, + shortcuts.BoolType(), + object=user_type) + srv = AddFluent.Request() + srv.problem_name = self._problem_name + srv.fluent = self._ros2_interface_writer.convert(fluent, problem) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(False) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.default_value = value + + self._add_fluent.wait_for_service() + self.future = self._add_fluent.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add fluent: {fluent_name}') + return fluent + + def add_object(self, object_name, user_type): + upf_object = model.Object(object_name, user_type) + srv = AddObject.Request() + srv.problem_name = self._problem_name + srv.object = self._ros2_interface_writer.convert(upf_object) + + self._add_object.wait_for_service() + self.future = self._add_object.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add Object: {object_name}') + + return upf_object + + def set_initial_value(self, fluent, object, value_fluent): + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def add_action(self, action): + srv = AddAction.Request() + srv.problem_name = self._problem_name + srv.action = self._ros2_interface_writer.convert(action) + + self._add_action.wait_for_service() + self.future = self._add_action.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add action: {action.name}') + + def add_goal(self, goal): + srv = AddGoal.Request() + srv.problem_name = self._problem_name + upf_goal = msgs.Goal() + upf_goal.goal = self._ros2_interface_writer.convert(goal) + srv.goal.append(upf_goal) + + self._add_goal.wait_for_service() + self.future = self._add_goal.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Set new goal!') + + def get_plan_srv(self): + + problem = self.get_problem() + + self.get_logger().info('Planning...') + srv = PlanOneShotSrv.Request() + srv.problem = problem + + self._plan_one_shot_client_srv.wait_for_service() + + self.future = self._plan_one_shot_client_srv.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + plan_result = self.future.result().plan_result + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + + +def main(args=None): + rclpy.init(args=args) + + # test is the name of the problem for the demo + upf4ros2_demo_node = UPF4ROS2Demo1Node() + + upf4ros2_demo_node.new_problem('test') + problem = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + + # usertype is the type of the fluent's object + # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', + # 'up:real[]', shortcuts.UserType('name') + location = shortcuts.UserType('location') + + robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) + + l1 = upf4ros2_demo_node.add_object('livingroom', location) + l2 = upf4ros2_demo_node.add_object('entrance', location) + + upf4ros2_demo_node.set_initial_value(robot_at, l1, True) + upf4ros2_demo_node.set_initial_value(robot_at, l2, False) + + # Define navigation action (InstantaneousAction or DurativeAction) + move = model.InstantaneousAction('move', l_from=location, l_to=location) + # move = model.DurativeAction('move', l_from=location, l_to=location) + # If DurativeAction + # Set the action's duration (set_closed_duration_interval, set_open_duration_interval, set_fixed_duration, set_left_open_duration_interval or set_right_open_duration_interval) + # move.set_closed_duration_interval(0, 10) + + l_from = move.parameter('l_from') + l_to = move.parameter('l_to') + + move.add_precondition(robot_at(l_from)) + # move.add_condition(model.StartTiming(), robot_at(l_from)) + move.add_effect(robot_at(l_from), False) + move.add_effect(robot_at(l_to), True) + + upf4ros2_demo_node.add_action(move) + + upf4ros2_demo_node.add_goal(robot_at(l2)) + + upf4ros2_demo_node.get_plan_srv() + + problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + upf4ros2_demo_node.get_logger().info(f'{problem_updated}') + + rclpy.spin(upf4ros2_demo_node) + + upf4ros2_demo_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_bash.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_bash.py new file mode 100644 index 0000000..68706fb --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_bash.py @@ -0,0 +1,113 @@ +import tempfile +from ament_index_python.packages import get_package_share_directory + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from upf_msgs import msg as msgs + +from upf_msgs.action import ( + PlanOneShot +) + +from upf_msgs.srv import ( + GetProblem, +) + +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv + + +class UPF4ROS2PlanNode(Node): + + def __init__(self): + super().__init__('upf4ros2_plan') + + self.declare_parameter('problem_name', 'test') + self._problem_name = self.get_parameter('problem_name') + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._plan_one_shot_client = ActionClient( + self, + PlanOneShot, + 'upf4ros2/action/planOneShot') + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/srv/get_problem') + + self._plan_one_shot_client_srv = self.create_client( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = str(self._problem_name.value) + + self._get_problem.wait_for_service() + self.future = self._get_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + problem = self.future.result().problem + return problem + + def get_plan_action(self): + self.get_logger().info('Planning...') + problem = self.get_problem() + goal_msg = PlanOneShot.Goal() + goal_msg.plan_request.problem = self._ros2_interface_writer.convert( + problem) + + self._plan_one_shot_client.wait_for_server() + self.future = self._plan_one_shot_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + self.future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Solution not found :(') + return + + self.get_logger().info('Solution found :)') + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + + def get_plan_srv(self, problem): + + self.get_logger().info('Planning...') + srv = PlanOneShotSrv.Request() + srv.problem = problem + + self._plan_one_shot_client_srv.wait_for_service() + + self.future = self._plan_one_shot_client_srv.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + plan_result = self.future.result().plan_result + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + + +def main(args=None): + rclpy.init(args=args) + + upf4ros2_plan_node = UPF4ROS2PlanNode() + + upf_problem = upf4ros2_plan_node.get_problem() + problem = upf4ros2_plan_node._ros2_interface_reader.convert(upf_problem) + upf4ros2_plan_node.get_logger().info(f'{problem}') + + upf4ros2_plan_node.get_plan_srv(upf_problem) + rclpy.spin(upf4ros2_plan_node) + + upf4ros2_plan_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_pddl.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_pddl.py new file mode 100644 index 0000000..b4b399b --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_pddl.py @@ -0,0 +1,124 @@ +import tempfile +import threading +from ament_index_python.packages import get_package_share_directory + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from unified_planning import model +from unified_planning.io.pddl_reader import PDDLReader + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs + +from upf_msgs.action import ( + PDDLPlanOneShot, +) +from upf_msgs.srv import ( + GetProblem, +) + +from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv + + +class UPF4ROS2Demo1PDDL(Node): + + def __init__(self): + super().__init__('upf4ros2_demo1_pddl') + + self.declare_parameter('domain', '/pddl/domain.pddl') + self.declare_parameter('problem', '/pddl/problem.pddl') + + self._domain = self.get_parameter('domain') + self._problem = self.get_parameter('problem') + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._plan_pddl_one_shot_client = ActionClient( + self, + PDDLPlanOneShot, + 'upf4ros2/planOneShotPDDL') + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/get_problem') + + self._plan_pddl_one_shot_client_srv = self.create_client( + PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.future = self._get_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + problem = self._ros2_interface_reader.convert( + self.future.result().problem) + return problem + + def get_plan_action(self): + + self.get_logger().info('Planning...') + goal_msg = PDDLPlanOneShot.Goal() + goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE + goal_msg.plan_request.domain = (get_package_share_directory( + 'upf4ros2_demo') + str(self._domain.value)) + goal_msg.plan_request.problem = (get_package_share_directory( + 'upf4ros2_demo') + str(self._problem.value)) + + self._plan_pddl_one_shot_client.wait_for_server() + + res = self._plan_pddl_one_shot_client.send_goal_async(goal_msg) + res.add_done_callback(self.goal_response_callback) + + return res + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Solution not found :(') + return + + self.get_logger().info('Solution found :)') + + def get_plan_srv(self): + + self.get_logger().info('Planning...') + srv = PDDLPlanOneShotSrv.Request() + srv.plan_request.mode = msgs.PDDLPlanRequest.FILE + srv.plan_request.domain = (get_package_share_directory('upf4ros2_demo') + + str(self._domain.value)) + srv.plan_request.problem = (get_package_share_directory( + 'upf4ros2_demo') + str(self._problem.value)) + + self._plan_pddl_one_shot_client_srv.wait_for_service() + + self.future = self._plan_pddl_one_shot_client_srv.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + plan_result = self.future.result().plan_result + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + + +def main(args=None): + rclpy.init(args=args) + + upf4ros2_pddl_node = UPF4ROS2Demo1PDDL() + + upf4ros2_pddl_node.get_plan_srv() + rclpy.spin(upf4ros2_pddl_node) + + upf4ros2_pddl_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo2.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo2.py new file mode 100644 index 0000000..c441737 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo2.py @@ -0,0 +1,311 @@ +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from unified_planning import model + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.action import ( + PDDLPlanOneShot, + PlanOneShot +) + +from upf_msgs.srv import ( + AddAction, + AddFluent, + AddGoal, + AddObject, + GetProblem, + NewProblem, + SetInitialValue +) + +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv + + +class UPF4ROS2DemoNode(Node): + + def __init__(self): + super().__init__('upf4ros2_demo_navigate') + + self._problem_name = '' + self._plan_result = {} + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._plan_one_shot_client = ActionClient( + self, + PlanOneShot, + 'upf4ros2/planOneShot') + + self._plan_pddl_one_shot_client = ActionClient( + self, + PDDLPlanOneShot, + 'upf4ros2/planOneShotPDDL') + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/get_problem') + self._new_problem = self.create_client( + NewProblem, 'upf4ros2/new_problem') + self._add_fluent = self.create_client( + AddFluent, 'upf4ros2/add_fluent') + self._add_action = self.create_client( + AddAction, 'upf4ros2/add_action') + self._add_object = self.create_client( + AddObject, 'upf4ros2/add_object') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/set_initial_value') + self._add_goal = self.create_client( + AddGoal, 'upf4ros2/add_goal') + self._plan_one_shot_client_srv = self.create_client( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot') + + def new_problem(self, problem_name): + srv = NewProblem.Request() + srv.problem_name = problem_name + + self._new_problem.wait_for_service() + self.future = self._new_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self._problem_name = problem_name + self.get_logger().info( + f'Create the problem with name: {srv.problem_name}') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.future = self._get_problem.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.future.result().problem + return problem + + def add_fluent(self, problem, fluent_name, user_type): + fluent = model.Fluent( + fluent_name, + shortcuts.BoolType(), + object=user_type) + srv = AddFluent.Request() + srv.problem_name = self._problem_name + srv.fluent = self._ros2_interface_writer.convert(fluent, problem) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(False) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.default_value = value + + self._add_fluent.wait_for_service() + self.future = self._add_fluent.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add fluent: {fluent_name}') + return fluent + + def add_object(self, object_name, user_type): + upf_object = model.Object(object_name, user_type) + srv = AddObject.Request() + srv.problem_name = self._problem_name + srv.object = self._ros2_interface_writer.convert(upf_object) + + self._add_object.wait_for_service() + self.future = self._add_object.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add Object: {object_name}') + + return upf_object + + def set_initial_value(self, fluent, object, value_fluent): + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def add_action(self, action): + srv = AddAction.Request() + srv.problem_name = self._problem_name + srv.action = self._ros2_interface_writer.convert(action) + + self._add_action.wait_for_service() + self.future = self._add_action.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Add action: {action.name}') + + def add_goal(self, goal): + srv = AddGoal.Request() + srv.problem_name = self._problem_name + upf_goal = msgs.Goal() + upf_goal.goal = self._ros2_interface_writer.convert(goal) + srv.goal.append(upf_goal) + + self._add_goal.wait_for_service() + self.future = self._add_goal.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + self.get_logger().info(f'Set new goal!') + + def get_plan_action(self): + self.get_logger().info('Planning...') + problem = self.get_problem() + goal_msg = PlanOneShot.Goal() + goal_msg.plan_request.problem = self._ros2_interface_writer.convert( + problem) + + self._plan_one_shot_client.wait_for_server() + self.future = self._plan_one_shot_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + self.future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Solution not found :(') + return + + self.get_logger().info('Solution found :)') + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + + for action in feedback.plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info( + action.action_name + "(" + params[0] + ", " + params[1] + ")") + + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters + + self.res = client.call_async(req) + rclpy.spin_until_future_complete(self, self.res) + + def get_plan_srv(self): + + problem = self.get_problem() + + self.get_logger().info('Planning...') + srv = PlanOneShotSrv.Request() + srv.problem = problem + + self._plan_one_shot_client_srv.wait_for_service() + + self.future = self._plan_one_shot_client_srv.call_async(srv) + rclpy.spin_until_future_complete(self, self.future) + + plan_result = self.future.result().plan_result + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters + + self.res = client.call_async(req) + rclpy.spin_until_future_complete(self, self.res) + + if self.res.result().result: + self.get_logger().info('Action completed') + else: + self.get_logger().info('Action cancelled!') + + +def main(args=None): + rclpy.init(args=args) + + # test is the name of the problem for the demo + upf4ros2_demo_node = UPF4ROS2DemoNode() + + upf4ros2_demo_node.new_problem('test') + problem = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + + # usertype is the type of the fluent's object + # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', + # 'up:real[]', shortcuts.UserType('name') + location = shortcuts.UserType('location') + + robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) + + l1 = upf4ros2_demo_node.add_object('livingroom', location) + l2 = upf4ros2_demo_node.add_object('entrance', location) + + upf4ros2_demo_node.set_initial_value(robot_at, l1, True) + upf4ros2_demo_node.set_initial_value(robot_at, l2, False) + + # Define navigation action (InstantaneousAction or DurativeAction) + move = model.InstantaneousAction('move', l_from=location, l_to=location) + # move = model.DurativeAction('move', l_from=location, l_to=location) + # If DurativeAction + # Set the action's duration (set_closed_duration_interval, set_open_duration_interval, set_fixed_duration, set_left_open_duration_interval or set_right_open_duration_interval) + # move.set_closed_duration_interval(0, 10) + + l_from = move.parameter('l_from') + l_to = move.parameter('l_to') + + move.add_precondition(robot_at(l_from)) + # move.add_condition(model.StartTiming(), robot_at(l_from)) + move.add_effect(robot_at(l_from), False) + move.add_effect(robot_at(l_to), True) + + upf4ros2_demo_node.add_action(move) + + upf4ros2_demo_node.add_goal(robot_at(l2)) + + upf4ros2_demo_node.get_plan_srv() + + problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + upf4ros2_demo_node.get_logger().info(f'{problem_updated}') + + rclpy.spin(upf4ros2_demo_node) + + upf4ros2_demo_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3.py new file mode 100644 index 0000000..ea2b996 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3.py @@ -0,0 +1,291 @@ +import rclpy +from rclpy.action import ActionClient +# from rclpy.node import Node +from simple_node import Node + +from unified_planning import model + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.srv import ( + AddAction, + AddFluent, + AddGoal, + AddObject, + GetProblem, + NewProblem, + SetInitialValue +) + +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv +# varios puntos (lanzar el plan con todos los puntos// ir uno a uno ) +# checkear puntos, para eso tiene que estar en el punto y luego checkearlo + + +class UPF4ROS2DemoNode(Node): + + def __init__(self): + super().__init__('upf4ros2_demo_navigate') + + self._problem_name = '' + self._plan_result = {} + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/get_problem') + self._new_problem = self.create_client( + NewProblem, 'upf4ros2/new_problem') + self._add_fluent = self.create_client( + AddFluent, 'upf4ros2/add_fluent') + self._add_action = self.create_client( + AddAction, 'upf4ros2/add_action') + self._add_object = self.create_client( + AddObject, 'upf4ros2/add_object') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/set_initial_value') + self._add_goal = self.create_client( + AddGoal, 'upf4ros2/add_goal') + self._plan_one_shot_client_srv = self.create_client( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot') + + def new_problem(self, problem_name): + srv = NewProblem.Request() + srv.problem_name = problem_name + + self._new_problem.wait_for_service() + self.future = self._new_problem.call(srv) + + self._problem_name = problem_name + self.get_logger().info( + f'Create the problem with name: {srv.problem_name}') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.res = self._get_problem.call(srv) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.res.problem + return problem + + def add_fluent(self, problem, fluent_name, user_type): + fluent = model.Fluent( + fluent_name, + shortcuts.BoolType(), + object=user_type) + srv = AddFluent.Request() + srv.problem_name = self._problem_name + srv.fluent = self._ros2_interface_writer.convert(fluent, problem) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(False) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.default_value = value + + self._add_fluent.wait_for_service() + self.future = self._add_fluent.call(srv) + + self.get_logger().info(f'Add fluent: {fluent_name}') + return fluent + + def add_object(self, object_name, user_type): + upf_object = model.Object(object_name, user_type) + srv = AddObject.Request() + srv.problem_name = self._problem_name + srv.object = self._ros2_interface_writer.convert(upf_object) + + self._add_object.wait_for_service() + self.future = self._add_object.call(srv) + + self.get_logger().info(f'Add Object: {object_name}') + + return upf_object + + def set_initial_value(self, fluent, object, value_fluent): + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call(srv) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def add_action(self, action): + srv = AddAction.Request() + srv.problem_name = self._problem_name + srv.action = self._ros2_interface_writer.convert(action) + + self._add_action.wait_for_service() + self.future = self._add_action.call(srv) + + self.get_logger().info(f'Add action: {action.name}') + + def add_goal(self, goal): + srv = AddGoal.Request() + srv.problem_name = self._problem_name + upf_goal = msgs.Goal() + upf_goal.goal = self._ros2_interface_writer.convert(goal) + srv.goal.append(upf_goal) + + self._add_goal.wait_for_service() + self.future = self._add_goal.call(srv) + + self.get_logger().info(f'Set new goal!') + + def __cancel_callback(self, action): + # return to old wp + if action.action_name == 'move': + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters[::-1] + self.res = client.call(req) + if self.res.result: + self.get_logger().info('Return to origin wp') + else: + self.get_logger().info('Can not return to the origin wp!') + + def get_plan_srv(self): + + problem = self.get_problem() + + self.get_logger().info('Planning...') + srv = PlanOneShotSrv.Request() + srv.problem = problem + + self._plan_one_shot_client_srv.wait_for_service() + + self.res = self._plan_one_shot_client_srv.call(srv) + + plan_result = self.res.plan_result + + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + + for action in plan_result.plan.actions: + + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters + + self.res = client.call(req) + + if self.res.result: + self.get_logger().info('Action completed') + else: + self.get_logger().info('Action cancelled!') + self.__cancel_callback(action) + + +def main(args=None): + rclpy.init(args=args) + + # test is the name of the problem for the demo + upf4ros2_demo_node = UPF4ROS2DemoNode() + + upf4ros2_demo_node.new_problem('test') + problem = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + + # usertype is the type of the fluent's object + # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', + # 'up:real[]', shortcuts.UserType('name') + location = shortcuts.UserType('location') + + robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) + wp_checked = upf4ros2_demo_node.add_fluent(problem, 'wp_checked', location) + + livingroom = upf4ros2_demo_node.add_object('livingroom', location) + entrance = upf4ros2_demo_node.add_object('entrance', location) + kitchen = upf4ros2_demo_node.add_object('kitchen', location) + bedroom = upf4ros2_demo_node.add_object('bedroom', location) + room = upf4ros2_demo_node.add_object('room', location) + gym = upf4ros2_demo_node.add_object('gym', location) + + upf4ros2_demo_node.set_initial_value(robot_at, livingroom, True) + upf4ros2_demo_node.set_initial_value(robot_at, entrance, False) + upf4ros2_demo_node.set_initial_value(wp_checked, entrance, False) + + # Define navigation action (InstantaneousAction or DurativeAction) + move = model.InstantaneousAction('move', l_from=location, l_to=location) + # move = model.DurativeAction('move', l_from=location, l_to=location) + # If DurativeAction + # Set the action's duration (set_closed_duration_interval, set_open_duration_interval, set_fixed_duration, set_left_open_duration_interval or set_right_open_duration_interval) + # move.set_closed_duration_interval(0, 10) + + l_from = move.parameter('l_from') + l_to = move.parameter('l_to') + + move.add_precondition(robot_at(l_from)) + # move.add_condition(model.StartTiming(), robot_at(l_from)) + move.add_effect(robot_at(l_from), False) + move.add_effect(robot_at(l_to), True) + + # ------------------------------------- # + # Define check wp action + check_wp = model.InstantaneousAction('check_wp', wp=location) + wp = check_wp.parameter('wp') + check_wp.add_precondition(robot_at(wp)) + check_wp.add_effect(wp_checked(wp), True) + + upf4ros2_demo_node.add_action(move) + upf4ros2_demo_node.add_action(check_wp) + + # upf4ros2_demo_node.add_goal(robot_at(l2)) + upf4ros2_demo_node.add_goal(wp_checked(entrance)) + upf4ros2_demo_node.add_goal(wp_checked(gym)) + upf4ros2_demo_node.add_goal(wp_checked(kitchen)) + + problem_old_upf = upf4ros2_demo_node.get_problem() + problem_old = upf4ros2_demo_node._ros2_interface_reader.convert( + problem_old_upf) + upf4ros2_demo_node.get_logger().info(f'{problem_old}') + + upf4ros2_demo_node.get_plan_srv() + + problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + upf4ros2_demo_node.get_logger().info(f'{problem_updated}') + + upf4ros2_demo_node.join_spin() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py new file mode 100644 index 0000000..04eccad --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py @@ -0,0 +1,157 @@ +from typing import List +import rclpy + +from geometry_msgs.msg import Pose +from nav2_msgs.action import NavigateToPose +from upf4ros2_demo_msgs.srv import CallAction +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.srv import ( + SetInitialValue, +) + +from simple_node import Node + + +class NavigationAction(Node): + + def __init__(self): + super().__init__('upf4ros2_navigation_action') + + self.__wp_dict = {} + + wps_param_name = "wps" + + # declaring params + self.declare_parameter(wps_param_name, ['']) + + # getting params + waypoints = self.get_parameter( + wps_param_name).get_parameter_value().string_array_value + + # load points + self.load_wps(waypoints) + + self._problem_name = 'test' + + self._userType = shortcuts.UserType('location') + self._fluent = model.Fluent( + "robot_at", + shortcuts.BoolType(), + object=self._userType) + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self.__nav_to_pose_client = self.create_action_client( + NavigateToPose, + '/navigate_to_pose') + + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/set_initial_value') + + self.create_service( + CallAction, 'move', self.__execute_callback) + + def load_wps(self, waypoints: List[str]): + """ load waypoints of list strings into a dictionary of floats + Args: + points (List[str]): list of points + """ + + if not waypoints: + return + + for i in range(0, len(waypoints), 5): + self.__wp_dict[waypoints[i]] = Pose() + self.__wp_dict[waypoints[i]].position.x = float(waypoints[i + 1]) + self.__wp_dict[waypoints[i]].position.y = float(waypoints[i + 2]) + self.__wp_dict[waypoints[i]].orientation.z = float( + waypoints[i + 3]) + self.__wp_dict[waypoints[i]].orientation.w = float( + waypoints[i + 4]) + + def set_initial_value(self, fluent, object, value_fluent): + """ set initial value to the fluent + Args: + fluent (up.model.Fluent): fluent + object (up.model.Object): fluent's object + value_fluent (bool): new value + """ + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call(srv) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def __execute_callback(self, request, response): + """ srv callback to call the NavigateToPose action + Args: + request (CallAction.Request): request with the action's name and parameters + response (CallAction.Response): response with the result of the action + Returns: + CallAction.Response: response with the result of the action + """ + l1 = model.Object(request.parameters[0].symbol_atom[0], self._userType) + l2 = model.Object(request.parameters[1].symbol_atom[0], self._userType) + + self.get_logger().info("Starting action " + request.action_name) + + self.get_logger().info("Waiting for 'NavigateToPose' action server") + while not self.__nav_to_pose_client.wait_for_server(): + self.get_logger().info("'NavigateToPose' action server not available, waiting...") + + wp = request.parameters[1].symbol_atom[0] + goal_msg = NavigateToPose.Goal() + goal_msg.pose.pose = self.__wp_dict[wp] + goal_msg.pose.header.frame_id = "map" + + self.get_logger().info('Navigating to goal: ' + str(wp)) + + self.__nav_to_pose_client.send_goal(goal_msg) + self.__nav_to_pose_client.wait_for_result() + + if self.__nav_to_pose_client.is_succeeded(): + self.get_logger().info("Goal to " + str(wp) + " done") + self.set_initial_value(self._fluent, l1, False) + self.set_initial_value(self._fluent, l2, True) + response.result = True + else: + self.get_logger().info("Goal to " + str(wp) + " was rejected!") + response.result = False + + return response + + +def main(args=None): + rclpy.init(args=args) + + navigation_action = NavigationAction() + + navigation_action.join_spin() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo_msgs/CMakeLists.txt b/upf4ros2_demo_msgs/CMakeLists.txt new file mode 100644 index 0000000..2709423 --- /dev/null +++ b/upf4ros2_demo_msgs/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(upf4ros2_demo_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(upf_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/CallAction.srv" + DEPENDENCIES upf_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/upf4ros2_demo_msgs/package.xml b/upf4ros2_demo_msgs/package.xml new file mode 100644 index 0000000..97a8e3d --- /dev/null +++ b/upf4ros2_demo_msgs/package.xml @@ -0,0 +1,22 @@ + + + + upf4ros2_demo_msgs + 0.0.0 + TODO: Package description + gentlebots + TODO: License declaration + + ament_cmake + upf_msgs + ament_lint_auto + ament_lint_common + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/upf4ros2_demo_msgs/srv/CallAction.srv b/upf4ros2_demo_msgs/srv/CallAction.srv new file mode 100644 index 0000000..207c7c2 --- /dev/null +++ b/upf4ros2_demo_msgs/srv/CallAction.srv @@ -0,0 +1,4 @@ +string action_name +upf_msgs/Atom[] parameters +--- +bool result \ No newline at end of file diff --git a/upf_msgs/CMakeLists.txt b/upf_msgs/CMakeLists.txt index 2483ab9..bc8d018 100644 --- a/upf_msgs/CMakeLists.txt +++ b/upf_msgs/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/AbstractTaskDeclaration.msg" "msg/Effect.msg" + "msg/Variable.msg" "msg/Task.msg" "msg/PlanGenerationResult.msg" "msg/Method.msg" @@ -37,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Problem.msg" "msg/Goal.msg" "msg/Atom.msg" + "msg/Schedule.msg" "msg/Timepoint.msg" "msg/EffectExpression.msg" "msg/ValidationRequest.msg" @@ -45,6 +47,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Timing.msg" "msg/Parameter.msg" "msg/Metric.msg" + "msg/ValidationMetric.msg" "srv/GetProblem.srv" "srv/NewProblem.srv" "srv/SetProblem.srv" @@ -54,6 +57,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetInitialValue.srv" "srv/AddGoal.srv" "srv/PDDLPlanOneShot.srv" + "srv/PlanOneShot.srv" "action/PDDLPlanOneShot.action" "action/PlanOneShot.action" "action/PlanOneShotRemote.action" diff --git a/upf_msgs/msg/EffectExpression.msg b/upf_msgs/msg/EffectExpression.msg index 2c05a11..0889996 100644 --- a/upf_msgs/msg/EffectExpression.msg +++ b/upf_msgs/msg/EffectExpression.msg @@ -15,6 +15,7 @@ uint8 kind # Expression that must be of the STATE_VARIABLE kind. upf_msgs/Expression fluent upf_msgs/Expression value +upf_msgs/Variable[] forall # Optional. If the effect is conditional, then the following field must be set. # In this case, the `effect` will only be applied if the `condition`` holds. diff --git a/upf_msgs/msg/Metric.msg b/upf_msgs/msg/Metric.msg index e919a23..f4c5a7f 100644 --- a/upf_msgs/msg/Metric.msg +++ b/upf_msgs/msg/Metric.msg @@ -31,7 +31,7 @@ Expression expression # In particular, for this metric to be useful in many practical problems, the cost expression # should allow referring to the action parameters (and possibly the current state at the action start/end). # This is very awkward to do in this setting where the expression is detached from its scope. -string[] action_cost_names +upf_msgs/Action[] action_cost_names upf_msgs/Expression[] action_cost_expr upf_msgs/Expression[<=1] default_action_cost diff --git a/upf_msgs/msg/Plan.msg b/upf_msgs/msg/Plan.msg index 85eb8eb..67e8826 100644 --- a/upf_msgs/msg/Plan.msg +++ b/upf_msgs/msg/Plan.msg @@ -1,5 +1,5 @@ # An ordered sequence of actions that appear in the plan. # The order of the actions in the list must be compatible with the partial order of the start times. # In case of non-temporal planning, this allows having all start time at 0 and only rely on the order in this sequence. - +uint8 kind upf_msgs/ActionInstance[] actions diff --git a/upf_msgs/msg/Problem.msg b/upf_msgs/msg/Problem.msg index 155b113..0753f0e 100644 --- a/upf_msgs/msg/Problem.msg +++ b/upf_msgs/msg/Problem.msg @@ -28,12 +28,15 @@ upf_msgs/Hierarchy[<=1] hierarchy # all features of the problem uint8[] features +upf_msgs/Expression[] trajectory_constraints + ## Features of the problem. ## Features are essential in that not supporting a feature `X` should allow disregarding any field tagged with `features: [X]`. # PROBLEM_CLASS uint8 ACTION_BASED=0 uint8 HIERARCHICAL=26 +uint8 SCHEDULING = 56 # PROBLEM_TYPE uint8 SIMPLE_NUMERIC_PLANNING=30 uint8 GENERAL_NUMERIC_PLANNING=31 @@ -41,36 +44,79 @@ uint8 GENERAL_NUMERIC_PLANNING=31 uint8 CONTINUOUS_TIME=1 uint8 DISCRETE_TIME=2 uint8 INTERMEDIATE_CONDITIONS_AND_EFFECTS=3 -uint8 TIMED_EFFECT=4 +uint8 EXTERNAL_CONDITIONS_AND_EFFECTS = 39 +uint8 TIMED_EFFECTS=4 uint8 TIMED_GOALS=5 uint8 DURATION_INEQUALITIES=6 +uint8 SELF_OVERLAPPING = 47 # EXPRESSION_DURATION -uint8 STATIC_FLUENTS_IN_DURATION=27 -uint8 FLUENTS_IN_DURATION=28 +uint8 STATIC_FLUENTS_IN_DURATIONS=27 +uint8 FLUENTS_IN_DURATIONS=28 +uint8 REAL_TYPE_DURATIONS = 62 +uint8 INT_TYPE_DURATIONS = 63 # NUMBERS uint8 CONTINUOUS_NUMBERS=7 uint8 DISCRETE_NUMBERS=8 +uint8 BOUNDED_TYPES = 38 # CONDITIONS_KIND uint8 NEGATIVE_CONDITIONS=9 uint8 DISJUNCTIVE_CONDITIONS=10 -uint8 EQUALITY=11 +uint8 EQUALITIES=11 uint8 EXISTENTIAL_CONDITIONS=12 uint8 UNIVERSAL_CONDITIONS=13 # EFFECTS_KIND uint8 CONDITIONAL_EFFECTS=14 uint8 INCREASE_EFFECTS=15 uint8 DECREASE_EFFECTS=16 +uint8 STATIC_FLUENTS_IN_BOOLEAN_ASSIGNMENTS = 41 +uint8 STATIC_FLUENTS_IN_NUMERIC_ASSIGNMENTS = 42 +uint8 STATIC_FLUENTS_IN_OBJECT_ASSIGNMENTS = 57 +uint8 FLUENTS_IN_BOOLEAN_ASSIGNMENTS = 43 +uint8 FLUENTS_IN_NUMERIC_ASSIGNMENTS = 44 +uint8 FLUENTS_IN_OBJECT_ASSIGNMENTS = 58 +uint8 FORALL_EFFECTS = 59 # TYPING uint8 FLAT_TYPING=17 uint8 HIERARCHICAL_TYPING=18 # FLUENTS_TYPE uint8 NUMERIC_FLUENTS=19 uint8 OBJECT_FLUENTS=20 +uint8 INT_FLUENTS=60 +uint8 REAL_FLUENTS=61 +# PARAMETERS +uint8 BOOL_FLUENT_PARAMETERS = 50 +uint8 BOUNDED_INT_FLUENT_PARAMETERS = 51 +uint8 BOOL_ACTION_PARAMETERS = 52 +uint8 BOUNDED_INT_ACTION_PARAMETERS = 53 +uint8 UNBOUNDED_INT_ACTION_PARAMETERS = 54 +uint8 REAL_ACTION_PARAMETERS = 55 # QUALITY_METRICS uint8 ACTIONS_COST=21 uint8 FINAL_VALUE=22 uint8 MAKESPAN=23 uint8 PLAN_LENGTH=24 uint8 OVERSUBSCRIPTION=29 +uint8 TEMPORAL_OVERSUBSCRIPTION = 40 +# ACTION_COST_KIND +uint8 STATIC_FLUENTS_IN_ACTIONS_COST = 45 +uint8 FLUENTS_IN_ACTIONS_COST = 46 +uint8 REAL_NUMBERS_IN_ACTIONS_COST = 64 +uint8 INT_NUMBERS_IN_ACTIONS_COST = 65 +# OVERSUBSCRIPTION_KIND +uint8 REAL_NUMBERS_IN_OVERSUBSCRIPTION = 66 +uint8 INT_NUMBERS_IN_OVERSUBSCRIPTION = 67 # SIMULATED_ENTITIES uint8 SIMULATED_EFFECTS=25 +# CONSTRAINTS_KIND +uint8 TRAJECTORY_CONSTRAINTS = 48 +uint8 STATE_INVARIANTS = 49 +# HIERARCHICAL +uint8 METHOD_PRECONDITIONS = 32 +uint8 TASK_NETWORK_CONSTRAINTS = 33 +uint8 INITIAL_TASK_NETWORK_VARIABLES = 34 +uint8 TASK_ORDER_TOTAL = 35 +uint8 TASK_ORDER_PARTIAL = 36 +uint8 TASK_ORDER_TEMPORAL = 37 +# INITIAL_STATE +uint8 UNDEFINED_INITIAL_NUMERIC = 68 +uint8 UNDEFINED_INITIAL_SYMBOLIC = 69 diff --git a/upf_msgs/msg/Schedule.msg b/upf_msgs/msg/Schedule.msg new file mode 100644 index 0000000..3fcc5c2 --- /dev/null +++ b/upf_msgs/msg/Schedule.msg @@ -0,0 +1,2 @@ +string[] activities_name +upf_msgs/Expression[] variable_assignments \ No newline at end of file diff --git a/upf_msgs/msg/TimedEffect.msg b/upf_msgs/msg/TimedEffect.msg index 5b89d64..74040bd 100644 --- a/upf_msgs/msg/TimedEffect.msg +++ b/upf_msgs/msg/TimedEffect.msg @@ -1,6 +1,6 @@ ## Represents an effect that will occur sometime beyond the initial state. (similar to timed initial literals) # Required. An effect expression that will take place sometime in the future (i.e. not at the intial state) as specified by the temporal qualifiation. -upf_msgs/EffectExpression effect +upf_msgs/EffectExpression[] effect # Required. Temporal qualification denoting when the timed fact will occur. upf_msgs/Timing occurrence_time diff --git a/upf_msgs/msg/ValidationMetric.msg b/upf_msgs/msg/ValidationMetric.msg new file mode 100644 index 0000000..084576c --- /dev/null +++ b/upf_msgs/msg/ValidationMetric.msg @@ -0,0 +1,2 @@ +string key +string value \ No newline at end of file diff --git a/upf_msgs/msg/ValidationResult.msg b/upf_msgs/msg/ValidationResult.msg index 4a9373d..9765dcc 100644 --- a/upf_msgs/msg/ValidationResult.msg +++ b/upf_msgs/msg/ValidationResult.msg @@ -11,3 +11,6 @@ upf_msgs/LogMessage[] log_messages # Synthetic description of the engine that generated this message. string engine + +ValidationMetric[] metrics + diff --git a/upf_msgs/msg/Variable.msg b/upf_msgs/msg/Variable.msg new file mode 100644 index 0000000..acada36 --- /dev/null +++ b/upf_msgs/msg/Variable.msg @@ -0,0 +1,2 @@ +string name +upf_msgs/TypeDeclaration type \ No newline at end of file diff --git a/upf_msgs/srv/PlanOneShot.srv b/upf_msgs/srv/PlanOneShot.srv new file mode 100644 index 0000000..a814828 --- /dev/null +++ b/upf_msgs/srv/PlanOneShot.srv @@ -0,0 +1,5 @@ +upf_msgs/Problem problem +--- +upf_msgs/PlanGenerationResult plan_result +bool success +string message \ No newline at end of file