Sim fine tune

go2_auto_dock/go2_auto_dock/go2_dock.py

@@ -78,7 +78,7 @@ def trigger_charging_navigation(self):
 
         try:
             # Launch the navigation script
-            subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_dock"])
+            subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_nav_to_charger"])
             self.charging_navigation_triggered = True
             self.get_logger().info(
                 "Navigation to charger script launched successfully."

go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py

@@ -35,8 +35,8 @@ def __init__(self):
         self.nav_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
 
         # Define goal position and orientation  (bearing angle)
-        self.goal_position = {"x": 0.01612, "y": 0.1392, "z": 0.0}
-        self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.9512, "w": 0.3085}
+        self.goal_position = {"x": -0.9578, "y": -1.952, "z": 0.0}
+        self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.99, "w": -0.067}
 
         # Precompute goal yaw (deg)
         self.goal_yaw_deg = yaw_from_quaternion(

go2_auto_dock/setup.py

@@ -32,7 +32,8 @@
             "go2_apriltag_detector = go2_auto_dock.go2_apriltag_detector:main",
             "go2_tag_follower = go2_auto_dock.go2_tag_follower:main",
             "go2_tag_charger = go2_auto_dock.go2_tag_charger:main",
-            "go2_dock = go2_auto_dock.go2_nav_to_charger_final:main",
+            "go2_battery_monitor = go2_auto_dock.go2_dock:main",
+            "go2_nav_to_charger = go2_auto_dock.go2_nav_to_charger_final:main",
             "go2_camera_publisher = go2_auto_dock.go2_camera_with_adjustable_publisher:main",
         ],
     },

go2_gazebo_sim/go2_description/CMakeLists.txt

@@ -26,6 +26,16 @@ install(DIRECTORY
   DESTINATION
   share/${PROJECT_NAME})
 
+install(DIRECTORY
+  models
+  DESTINATION
+  share/${PROJECT_NAME})
+
+install(DIRECTORY
+  tags
+  DESTINATION
+  share/${PROJECT_NAME})
+
 
 
 ament_package()

go2_gazebo_sim/go2_description/models/aruco_marker/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+  <name>aruco_marker</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <author>
+    <name>OpenMind</name>
+    <email>hello@openmind.com</email>
+  </author>
+  <description>
+    An ArUco marker for auto docking.
+  </description>
+</model>

go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf

@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="aruco_marker">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.2 0.01 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.2 0.01 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+          <diffuse>1 1 1 1</diffuse>
+          <specular>0 0 0 1</specular>
+          <pbr>
+            <metal>
+              <albedo_map>../../tags/apriltags.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>