From 0f5a8d506c3112639e67ea0aeb3e3908059fd646 Mon Sep 17 00:00:00 2001
From: Alastair <albradford2468@gmail.com>
Date: Thu, 10 Oct 2024 00:26:52 +1000
Subject: [PATCH] Some quick things to test for EBS control - using global
 frame over local

---
 .../vehicle_urdf/urdf/sensors.urdf.xacro      | 27 +++----------------
 .../launch/ebs_test.launch.py                 | 11 ++++++--
 .../node_ebs_test_handler.py                  |  2 +-
 tools/play_bag.sh                             |  3 ++-
 4 files changed, 16 insertions(+), 27 deletions(-)

diff --git a/src/common/vehicle_urdf/urdf/sensors.urdf.xacro b/src/common/vehicle_urdf/urdf/sensors.urdf.xacro
index 398b21ca0..fb63e1c69 100644
--- a/src/common/vehicle_urdf/urdf/sensors.urdf.xacro
+++ b/src/common/vehicle_urdf/urdf/sensors.urdf.xacro
@@ -2,38 +2,19 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:macro name="velodyne" params="*origin parent name">
     <!-- base frame to velodyne base -->
-    <joint name="${name}_base_mount_joint" type="fixed">
+      <joint name="${name}_base_mount_joint" type="fixed">
       <xacro:insert_block name="origin" />
       <parent link="${parent}"/>
-      <child link="${name}_base_link"/>
-    </joint>
-
-    <link name="${name}_base_link">
-      <visual>
-        <geometry>
-          <mesh filename="file://$(find vehicle_urdf)/meshes/VLP16_base_1.dae" />
-        </geometry>
-      </visual>
-      <visual>
-        <geometry>
-          <mesh filename="file://$(find vehicle_urdf)/meshes/VLP16_base_2.dae" />
-        </geometry>
-      </visual>
-    </link>
-
-    <!-- velodyne base to velodyne scan -->
-    <joint name="${name}_base_scan_joint" type="fixed" >
-      <origin xyz="0 0 0.0377" rpy="0 0 0" />
-      <parent link="${name}_base_link" />
       <child link="${name}"/>
     </joint>
 
     <link name="${name}">
       <visual>
-        <origin xyz="0 0 -0.0377" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="file://$(find vehicle_urdf)/meshes/VLP16_scan.dae" />
+          <cylinder radius="0.0516" length="0.0717"/>
         </geometry>
+        <material name="black"/>
       </visual>
     </link>
   </xacro:macro>
diff --git a/src/operations/mission_controller/launch/ebs_test.launch.py b/src/operations/mission_controller/launch/ebs_test.launch.py
index f97b3439f..061f9dc12 100644
--- a/src/operations/mission_controller/launch/ebs_test.launch.py
+++ b/src/operations/mission_controller/launch/ebs_test.launch.py
@@ -22,6 +22,13 @@ def generate_launch_description():
                 )
             ),
             # mapping/planning
+            Node(
+                package="map_creation",
+                executable="cone_placement_node",
+                parameters=[
+                    get_package_share_path("map_creation") / "config" / "cone_placement.yaml",
+                ],
+            ),
             Node(
                 package="planners",
                 executable="ft_planner_node",
@@ -30,9 +37,9 @@ def generate_launch_description():
                 ],
                 ros_arguments=[
                     "-p",
-                    "topic_name:=lidar/cone_detection",
+                    "topic_name:=slam/global_map",
                     "-p",
-                    "target_frame:=base_footprint",
+                    "target_frame:=track",
                 ],
             ),
             # guidance/control
diff --git a/src/operations/mission_controller/mission_controller/node_ebs_test_handler.py b/src/operations/mission_controller/mission_controller/node_ebs_test_handler.py
index 3265d4b04..c5a9a1265 100644
--- a/src/operations/mission_controller/mission_controller/node_ebs_test_handler.py
+++ b/src/operations/mission_controller/mission_controller/node_ebs_test_handler.py
@@ -177,5 +177,5 @@ def main(args=None):
     rclpy.init(args=args)
     node = EBSTestHandler()
     rclpy.spin(node)
-    # node.destroy_node()
+    node.destroy_node()
     rclpy.shutdown()
diff --git a/tools/play_bag.sh b/tools/play_bag.sh
index 0d7fab630..01791d0e0 100755
--- a/tools/play_bag.sh
+++ b/tools/play_bag.sh
@@ -8,8 +8,9 @@ source install/setup.bash
 # BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-03_24_19/ --clock -p" # lidar on roll hoop test
 # BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-05_04_46/ --clock -p" # half lap
 # BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-05_18_42/ --clock -p" # other half of lap
-BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_04-05_14_43/ --clock -p" # 2.5 laps
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_04-05_14_43/ --clock -p" # 2.5 laps
 # BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_24-06_08_59/ --clock -p" # 1 lap, loop close crashing
+BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_24-02_16_55/ --clock -p" # ebs test run
 
 REMAPPINGS=(
     # /tf:=/tf_old