From ca069cd664f7a72ee9a8b5d52db1c7f14f26e6d0 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Thu, 8 May 2025 23:01:51 +0200
Subject: [PATCH] Added launch_files

---
 mg_bringup/launch/launch-external.py |  35 +++++++
 mg_bringup/launch/launch.py          |  26 ++++-
 mg_bt/behavior_trees/calib_bt.xml    | 145 +++++++++++++++------------
 mg_bt/behavior_trees/tactics.xml     |  87 +++++++++++++++-
 4 files changed, 226 insertions(+), 67 deletions(-)
 create mode 100644 mg_bringup/launch/launch-external.py

diff --git a/mg_bringup/launch/launch-external.py b/mg_bringup/launch/launch-external.py
new file mode 100644
index 0000000..addeef2
--- /dev/null
+++ b/mg_bringup/launch/launch-external.py
@@ -0,0 +1,35 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
+from launch.conditions import UnlessCondition 
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    is_local_test = DeclareLaunchArgument(
+        'local_test',
+        default_value="False",
+        description='Launch with simulated components'
+    )
+
+    return LaunchDescription([
+        is_local_test,
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_bt"),
+                'launch',
+                'launch.py'
+            ]),
+        ),
+        Node(
+            package="mg_planner",
+            executable="mg_planner",
+            name="mg_planner",
+            emulate_tty=True,
+            output='screen',
+        )
+    ])
+    
diff --git a/mg_bringup/launch/launch.py b/mg_bringup/launch/launch.py
index 67ba23d..e032a45 100644
--- a/mg_bringup/launch/launch.py
+++ b/mg_bringup/launch/launch.py
@@ -1,6 +1,6 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
-from launch.conditions import UnlessCondition 
+from launch.conditions import UnlessCondition, IfCondition 
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -26,6 +26,14 @@ def generate_launch_description():
                 'local_test': LaunchConfiguration('local_test')
             }.items()
         ),
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_lidar"),
+                'launch',
+                'launch.py'
+            ]),
+            condition=UnlessCondition(LaunchConfiguration('local_test')),
+        ),
         Node(
             package="mg_odometry",
             executable="mg_odom_publisher",
@@ -39,6 +47,22 @@ def generate_launch_description():
             emulate_tty=True,
             output='screen'
         ),
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare("mg_bt"),
+                'launch',
+                'launch.py'
+            ]),
+            condition=IfCondition(LaunchConfiguration('local_test')),
+        ),
+        Node(
+            package="mg_planner",
+            executable="mg_planner",
+            name="mg_planner",
+            emulate_tty=True,
+            condition=IfCondition(LaunchConfiguration('local_test')),
+            output='screen',
+        ),
         Node(
             package="mg_navigation",
             executable="mg_nav_server",
diff --git a/mg_bt/behavior_trees/calib_bt.xml b/mg_bt/behavior_trees/calib_bt.xml
index 6f82669..a2ba048 100644
--- a/mg_bt/behavior_trees/calib_bt.xml
+++ b/mg_bt/behavior_trees/calib_bt.xml
@@ -3,32 +3,55 @@
   <BehaviorTree ID="calib2_bt">
     <Sequence>
       <ZeroNode service_name="/zero"/>
-      <MovePoint action_name="/MovePoint"
-                 tolerance="0.02"
+      <MovePoint x_goal="0.7"
+                 y_goal="0.0"
+                 max_wheel_speed="3.000000"
                  max_angular="0.1"
-                 x_goal="0.7"
-                 y_goal="0.0"/>
+                 max_vel="2.000000"
+                 ornt_mult="4.000000"
+                 tolerance="0.02"
+                 action_name="/MovePoint"/>
       <RotateNode angle="-2"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
                   action_name="/Rotate"/>
       <RotateNode angle="-3.14"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
                   action_name="/Rotate"/>
-      <MovePoint action_name="/MovePoint"
+      <MovePoint x_goal="0.3"
+                 y_goal="0.0"
+                 max_wheel_speed="3.000000"
+                 max_angular="3.140000"
+                 max_vel="2.000000"
+                 ornt_mult="4.000000"
                  tolerance="0.02"
-                 x_goal="0.3"
-                 y_goal="0.0"/>
+                 action_name="/MovePoint"/>
       <RotateNode angle="-2"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
                   action_name="/Rotate"/>
       <RotateNode angle="0"
+                  max_wheel_speed="3.000000"
+                  max_angular="3.140000"
+                  tolerance="0.001000"
                   action_name="/Rotate"/>
-            <Fallback>
-              <Timeout msec="5000">
-                <MovePoint action_name="/MovePoint"
-                           max_angular="0.1"
-                           x_goal="-0.1"
-                           y_goal="0.0"/>
-              </Timeout>
-              <AlwaysSuccess/>
-            </Fallback>
+      <Fallback>
+        <Timeout msec="5000">
+          <MovePoint x_goal="-0.1"
+                     y_goal="0.0"
+                     max_wheel_speed="3.000000"
+                     max_angular="0.1"
+                     max_vel="2.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Timeout>
+        <AlwaysSuccess/>
+      </Fallback>
       <ZeroNode service_name="/endCalib"/>
     </Sequence>
   </BehaviorTree>
@@ -41,23 +64,39 @@
         <Repeat num_cycles="5">
           <Sequence>
             <ZeroNode service_name="/zero"/>
-            <MovePoint action_name="/MovePoint"
-                       tolerance="0.04"
+            <MovePoint x_goal="0.7"
+                       y_goal="0.0"
+                       max_wheel_speed="3.000000"
                        max_angular="0.1"
-                       x_goal="0.7"
-                       y_goal="0.0"/>
+                       max_vel="2.000000"
+                       ornt_mult="4.000000"
+                       tolerance="0.04"
+                       action_name="/MovePoint"/>
             <RotateNode angle="2"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
                         action_name="/Rotate"/>
             <RotateNode angle="-2"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
                         action_name="/Rotate"/>
             <RotateNode angle="0"
+                        max_wheel_speed="3.000000"
+                        max_angular="3.140000"
+                        tolerance="0.001000"
                         action_name="/Rotate"/>
             <Fallback>
               <Timeout msec="20000">
-                <MovePoint action_name="/MovePoint"
+                <MovePoint x_goal="-0.1"
+                           y_goal="0.0"
+                           max_wheel_speed="3.000000"
                            max_angular="0.1"
-                           x_goal="-0.1"
-                           y_goal="0.0"/>
+                           max_vel="2.000000"
+                           ornt_mult="4.000000"
+                           tolerance="0.001000"
+                           action_name="/MovePoint"/>
               </Timeout>
               <AlwaysSuccess/>
             </Fallback>
@@ -85,59 +124,39 @@
                   type="std::string">Service name</input_port>
     </Action>
     <Action ID="MovePoint">
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-      <input_port name="tolerance"
-                  type="double"/>
-      <input_port name="obst_mult_a"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="pos_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="max_angular"
-                  type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
-      <input_port name="max_wheel_speed"
-                  type="double"/>
       <input_port name="x_goal"
                   type="double"/>
       <input_port name="y_goal"
                   type="double"/>
+      <input_port name="max_wheel_speed"
+                  default="3.000000"
+                  type="double"/>
+      <input_port name="max_angular"
+                  default="3.140000"
+                  type="double"/>
+      <input_port name="max_vel"
+                  default="2.000000"
+                  type="double"/>
+      <input_port name="ornt_mult"
+                  default="4.000000"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.001000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
     </Action>
     <Action ID="RotateNode">
       <input_port name="angle"
                   type="double"/>
-      <input_port name="pos_mult"
-                  type="double"/>
       <input_port name="max_wheel_speed"
+                  default="3.000000"
                   type="double"/>
-      <input_port name="IgnoreList"
-                  type="std::string"/>
       <input_port name="max_angular"
-                  type="double"/>
-      <input_port name="max_vel"
-                  type="double"/>
-      <input_port name="obst_mult_b"
-                  type="double"/>
-      <input_port name="obst_mult_c"
-                  type="double"/>
-      <input_port name="ornt_mult"
-                  type="double"/>
-      <input_port name="t_ornt_mult"
-                  type="double"/>
-      <input_port name="obst_mult_a"
+                  default="3.140000"
                   type="double"/>
       <input_port name="tolerance"
+                  default="0.001000"
                   type="double"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
diff --git a/mg_bt/behavior_trees/tactics.xml b/mg_bt/behavior_trees/tactics.xml
index 445df99..485f5f0 100644
--- a/mg_bt/behavior_trees/tactics.xml
+++ b/mg_bt/behavior_trees/tactics.xml
@@ -257,7 +257,7 @@
 
   <BehaviorTree ID="tactic_base">
     <Sequence>
-      <I2CSignal Address="66"
+      <I2CSignal Address="65"
                  Data="1"
                  Result="{tactic_id}"
                  action_name="/i2c_action"/>
@@ -275,11 +275,79 @@
   </BehaviorTree>
 
   <BehaviorTree ID="tactic_default_yellow">
-    <AlwaysSuccess/>
+    <Sequence>
+      <I2CSignal Address="65"
+                 Data="1"
+                 Result="{i2c_res}"
+                 action_name="/i2c_action"/>
+      <SendPose x="1.0 + 16.5"
+                y="0.45 - 14"
+                angle="90"
+                isDegree="true"
+                service_name="/set_pose"/>
+      <SideObstaclePub tactic="1"
+                       topic_name="/side"/>
+      <Timeout msec="100000">
+        <Sequence>
+          <MoveLocal x_goal="0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+          <MoveLocal x_goal="-0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
   </BehaviorTree>
 
   <BehaviorTree ID="tatic_default_blue">
-    <AlwaysFailure/>
+    <Sequence>
+      <I2CSignal Address="65"
+                 Data="1"
+                 Result="{i2c_res}"
+                 action_name="/i2c_action"/>
+      <SendPose x="0.31"
+                y="1.71"
+                angle="90"
+                isDegree="true"
+                service_name="/set_pose"/>
+      <SideObstaclePub tactic="2"
+                       topic_name="/side"/>
+      <Timeout msec="100000">
+        <Sequence>
+          <MoveLocal x_goal="0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+          <MoveLocal x_goal="-0.20"
+                     y_goal="0"
+                     max_wheel_speed="3.000000"
+                     max_angular="3.140000"
+                     max_vel="2.000000"
+                     pos_mult="15.000000"
+                     ornt_mult="4.000000"
+                     tolerance="0.001000"
+                     action_name="/MovePoint"/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
   </BehaviorTree>
 
   <BehaviorTree ID="testbed">
@@ -439,6 +507,19 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
+    <Action ID="SendPose">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="angle"
+                  type="double"/>
+      <input_port name="isDegree"
+                  default="true"
+                  type="bool"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
     <Condition ID="SideObstaclePub">
       <input_port name="tactic"
                   type="int"/>