From 5483a573763fcb3d5f3d00beabf92df466b5d917 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Irene=20Gonz=C3=A1lez=20Fern=C3=A1ndez?= Date: Thu, 12 Dec 2024 13:35:19 +0100 Subject: [PATCH] fix test_upf4ros2 --- .github/workflows/main.yaml | 2 +- README.md | 2 +- upf4ros2/tests/test_upf4ros2.py | 435 ++++++++++---------------------- 3 files changed, 141 insertions(+), 298 deletions(-) diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml index e1f883f..d33d46e 100644 --- a/.github/workflows/main.yaml +++ b/.github/workflows/main.yaml @@ -49,7 +49,7 @@ jobs: - name: Create custom repos run: wget -O /tmp/all.repos https://raw.githubusercontent.com/igonzf/UPF4ROS2/main/upf.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 diff --git a/README.md b/README.md index 818b0f6..a7af3e1 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [![main](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml) -This repository contains a UPF TSB for ROS 2 +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 diff --git a/upf4ros2/tests/test_upf4ros2.py b/upf4ros2/tests/test_upf4ros2.py index fccb6e3..c6a5f9d 100644 --- a/upf4ros2/tests/test_upf4ros2.py +++ b/upf4ros2/tests/test_upf4ros2.py @@ -13,67 +13,68 @@ # 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.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 upf_msgs.srv import AddFluent, SetInitialValue, AddGoal, AddAction, AddObject from upf4ros2.ros2_interface_reader import ROS2InterfaceReader from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter + +from upf4ros2 import upf4ros2_main from upf_msgs import msg as msgs +from upf_msgs.srv import ( + NewProblem, + SetProblem, + GetProblem, + AddFluent, + SetInitialValue, + AddGoal, + AddAction, + AddObject +) +from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv from upf_msgs.action import ( PDDLPlanOneShot, PlanOneShot, ) -from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv -from upf_msgs.srv import NewProblem, SetProblem, GetProblem - - -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') 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') @@ -88,7 +89,7 @@ def test_plan_from_file_pddl_no_tt(self): goal_msg.plan_request.problem) client = ActionClient( - node_cli, + self.node, PDDLPlanOneShot, 'upf4ros2/action/planOneShotPDDL') @@ -96,34 +97,29 @@ 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)) + 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) @@ -134,23 +130,7 @@ def feedback_callback(feedback_msg): 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') @@ -165,7 +145,7 @@ def test_plan_from_file_pddl_tt(self): goal_msg.plan_request.problem) client = ActionClient( - node_cli, + self.node, PDDLPlanOneShot, 'upf4ros2/action/planOneShotPDDL') @@ -173,21 +153,21 @@ 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): @@ -195,8 +175,8 @@ def feedback_callback(feedback_msg): 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)) + 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) @@ -207,23 +187,7 @@ def feedback_callback(feedback_msg): 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 @@ -233,7 +197,7 @@ def test_plan_robot(self): goal_msg.plan_request.problem = pb_writter.convert(problem) client = ActionClient( - node_cli, + self.node, PlanOneShot, 'upf4ros2/action/planOneShot') @@ -241,29 +205,29 @@ 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) @@ -274,27 +238,11 @@ def feedback_callback(feedback_msg): 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( + 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 @@ -308,12 +256,13 @@ def test_plan_from_file_pddl_tt_service(self): 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 = 'TimeTriggeredPlan([' \ '(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))' \ @@ -322,62 +271,32 @@ def test_plan_from_file_pddl_tt_service(self): 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.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, - 'Problem problem_test_1 already exists') - - node_cli.destroy_node() - node_test.destroy_node() - executor.shutdown() - rclpy.shutdown() - executor_thread.join() + '', + "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() @@ -387,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() @@ -444,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( @@ -472,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' @@ -486,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/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, '') @@ -507,45 +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 normalize_text(self, text): - return "\n".join(line.strip() for line in text.strip().splitlines()) - 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() @@ -555,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)) @@ -577,32 +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) - - 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() @@ -612,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') @@ -628,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/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() - if __name__ == '__main__': unittest.main()