plan dispatcher fixed

MERLIN2-ARCH · Apr 17, 2024 · 27a0a67 · 27a0a67
1 parent 4d4e741
commit 27a0a67
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 7 deletions.
diff --git a/...ing_layer/merlin2_plan_dispatcher/merlin2_plan_dispatcher/merlin2_plan_dispatcher_node.py b/...ing_layer/merlin2_plan_dispatcher/merlin2_plan_dispatcher/merlin2_plan_dispatcher_node.py
@@ -183,7 +183,7 @@ def __execute_server(self, goal_handle) -> DispatchPlan.Result:
             self.__action_client = None
             return result
 
-        elif not self.__action_client.is_succeeded():
+        elif self.__action_client and not self.__action_client.is_succeeded():
             goal_handle.abort()
             self.__action_client = None
             return result

diff --git a/merlin2_demo/launch/merlin2_demo2.launch.py b/merlin2_demo/launch/merlin2_demo2.launch.py
@@ -95,12 +95,16 @@ def generate_launch_description():
     #
     merlin2_navigation_action_cmd = Node(
         package="merlin2_demo",
-        executable="merlin2_navigation_fsm_action",
+        executable="merlin2_navigation_bt_action",
         name="navigation",
-        parameters=[{
-            "dao_family": dao_family,
-            "mongo_uri": mongo_uri,
-        }]
+        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
+                     }]
     )
 
     merlin2_check_wp_action_cmd = Node(

diff --git a/simple_node b/simple_node
+4 −0		simple_node/include/simple_node/actions/action_client.hpp
+3 −6		simple_node/include/simple_node/node.hpp
+6 −3		simple_node/src/simple_node/node.cpp