plan_dispatcher fixed

merlin2_arch/merlin2_executive_layer/merlin2_action/tests/python/test_merlin2_action.py

@@ -98,14 +98,12 @@ def tearDown(self):
         rclpy.shutdown()
 
     def test_action(self):
-
         self.client_node.call_action()
         self.client_node.wait_action()
         self.assertTrue(self.client_node.is_succeeded())
 
     def test_action_canceled(self):
-
         self.client_node.call_action()
-        self.assertTrue(self.client_node.cancel_action())
+        self.client_node.cancel_action()
         self.client_node.wait_action()
         self.assertTrue(self.client_node.is_canceled())

merlin2_arch/merlin2_planning_layer/merlin2_plan_dispatcher/merlin2_plan_dispatcher/merlin2_plan_dispatcher_node.py

@@ -55,10 +55,12 @@ def __init__(self):
 
         # action server/client
         self.__action_client = None
-        self.__action_server = self.create_action_server(DispatchPlan,
-                                                         "dispatch_plan",
-                                                         execute_callback=self.__execute_server,
-                                                         cancel_callback=self.__cancel_callback)
+        self.__action_server = self.create_action_server(
+            DispatchPlan,
+            "dispatch_plan",
+            execute_callback=self.__execute_server,
+            cancel_callback=self.__cancel_callback
+        )
 
     def __cancel_callback(self):
         if self.__action_server.is_working():
@@ -176,26 +178,27 @@ def __execute_server(self, goal_handle) -> DispatchPlan.Result:
                 if goal_handle.is_cancel_requested:
                     break
 
-            if goal_handle.is_cancel_requested:
-                goal_handle.canceled()
-                self.__action_client = None
-                return result
+        if goal_handle.is_cancel_requested:
+            goal_handle.canceled()
+            self.__action_client = None
+            return result
 
-            elif not self.__action_client.is_succeeded():
-                goal_handle.abort()
-                self.__action_client = None
-                return result
+        elif not self.__action_client.is_succeeded():
+            goal_handle.abort()
+            self.__action_client = None
+            return result
 
         # reset action client
         goal_handle.succeed()
         self.__action_client = None
 
         return result
 
-    def __build_proposition(self,
-                            pddl_effect_dto: PddlPropositionDto,
-                            pddl_objects_dto_dict: Dict[str, PddlObjectDto],
-                            ) -> PddlPropositionDto:
+    def __build_proposition(
+        self,
+        pddl_effect_dto: PddlPropositionDto,
+        pddl_objects_dto_dict: Dict[str, PddlObjectDto],
+    ) -> PddlPropositionDto:
         """ create a pddl proposition from an action effect
 
         Args:
@@ -217,9 +220,11 @@ def __build_proposition(self,
 
         return pddl_proposition_dto
 
-    def __update_knowledge(self,
-                           pddl_proposition_dto: PddlPropositionDto,
-                           deleted: bool) -> bool:
+    def __update_knowledge(
+            self,
+            pddl_proposition_dto: PddlPropositionDto,
+            deleted: bool
+    ) -> bool:
         """ update knowledge using a ppdl proposition
 
         Args:
@@ -240,9 +245,11 @@ def __update_knowledge(self,
 
         return succeed
 
-    def __apply_effect(self,
-                       pddl_effect_dto: PddlConditionEffectDto,
-                       pddl_objects_dto_dict: Dict[str, PddlObjectDto]) -> bool:
+    def __apply_effect(
+        self,
+        pddl_effect_dto: PddlConditionEffectDto,
+        pddl_objects_dto_dict: Dict[str, PddlObjectDto]
+    ) -> bool:
         """ apply an action effect removing or adding knowledge
 
         Args:
@@ -264,11 +271,8 @@ def __apply_effect(self,
 
 def main():
     rclpy.init()
-
     node = Merlin2PlanDispatcherNode()
-
     node.join_spin()
-
     rclpy.shutdown()
 
 

merlin2_arch/merlin2_planning_layer/merlin2_plan_dispatcher/tests/python/test_merlin2_plan_dispatcher.py

@@ -105,7 +105,7 @@ def cancel_plan_dispatcher(self):
         while not self.plan_dispatcher_client.is_working():
             time.sleep(1)
 
-        return self.plan_dispatcher_client.cancel_goal()
+        self.plan_dispatcher_client.cancel_goal()
 
     def is_succeeded(self):
         return self.plan_dispatcher_client.is_succeeded()
@@ -154,7 +154,8 @@ def test_plan_dispatcher_canceled(self):
             time.sleep(1)
 
         self.client_node.call_plan_dispatcher()
-        self.assertTrue(self.client_node.cancel_plan_dispatcher())
+        self.client_node.cancel_plan_dispatcher()
+
         self.client_node.wait_plan_dispatcher()
         self.assertTrue(self.client_node.is_canceled())
 

merlin2_demo/launch/merlin2_demo2.launch.py

@@ -24,7 +24,6 @@
 import ament_index_python
 
 from kant_dao.dao_factory import DaoFamilies
-from merlin2_planner import Merlin2Planners
 
 
 def generate_launch_description():
@@ -58,7 +57,7 @@ def generate_launch_description():
     planner = LaunchConfiguration("planner")
     planner_cmd = DeclareLaunchArgument(
         "planner",
-        default_value=str(int(Merlin2Planners.POPF)),
+        default_value="1",
         description="PDDL planner")
 
     total_points = LaunchConfiguration("total_points")
@@ -96,24 +95,22 @@ def generate_launch_description():
     #
     merlin2_navigation_action_cmd = Node(
         package="merlin2_demo",
-        executable="merlin2_navigation_bt_action",
+        executable="merlin2_navigation_fsm_action",
         name="navigation",
-        parameters=[{"dao_family": dao_family,
-                     "mongo_uri": mongo_uri,
-                     "bt_file_path": ament_index_python.get_package_share_directory(
-                         "merlin2_demo") + "/bt_xml/navigation.xml",
-                     "plugins": ["waypoint_navigation_bt_node"],
-                     "publisher_port": 1668,
-                     "server_port": 1669
-                     }]
+        parameters=[{
+            "dao_family": dao_family,
+            "mongo_uri": mongo_uri,
+        }]
     )
 
     merlin2_check_wp_action_cmd = Node(
         package="merlin2_demo",
         executable="merlin2_check_wp_fsm_action",
         name="check_wp",
-        parameters=[{"dao_family": dao_family,
-                     "mongo_uri": mongo_uri}]
+        parameters=[{
+            "dao_family": dao_family,
+            "mongo_uri": mongo_uri
+        }]
     )
 
     merlin2_demo2_node_cmd = Node(

merlin2_demo/launch/merlin2_demo3.launch.py

@@ -25,7 +25,6 @@
 from ament_index_python.packages import get_package_share_directory
 
 from kant_dao.dao_factory import DaoFamilies
-from merlin2_planner import Merlin2Planners
 
 
 def generate_launch_description():
@@ -61,7 +60,7 @@ def generate_launch_description():
     planner = LaunchConfiguration("planner")
     planner_cmd = DeclareLaunchArgument(
         "planner",
-        default_value=str(int(Merlin2Planners.UP)),
+        default_value="1",
         description="PDDL planner")
 
     #