Multiple auv simulation

README_old.md

@@ -0,0 +1,158 @@
+# Orca4 ![ROS2 CI](https://github.com/clydemcqueen/orca4/actions/workflows/build_test.yml/badge.svg?branch=main)
+
+Orca4 is a set of [ROS2](http://www.ros.org/) packages that provide basic AUV (Autonomous Underwater
+Vehicle) functionality for the [BlueRobotics BlueROV2](https://www.bluerobotics.com).
+
+Orca4 uses [ArduSub](http://www.ardusub.com/) as the flight controller and
+[mavros](https://github.com/mavlink/mavros) as the GCS.
+
+Orca4 runs in [Gazebo Harmonic](https://gazebosim.org/home) using the standard buoyancy, hydrodynamics and thruster
+plugins. The connection between ArduSub and Gazebo is provided by [ardupilot_gazebo](https://github.com/ArduPilot/ardupilot_gazebo).
+
+## Sensors
+
+The BlueROV2 provides the following interesting sensors:
+* An [external barometer](https://bluerobotics.com/product-category/sensors-sonars-cameras/sensors/) provides depth
+* An IMU provides attitude
+
+Orca4 adds a simulated down-facing stereo camera and [ORB_SLAM2](https://github.com/clydemcqueen/orb_slam_2_ros/tree/orca4_galactic)
+to generate a 3D pose as long as the camera has a good view of the seafloor.
+The pose is sent to ArduSub and fused with the other sensor information.
+
+If there is no view of the seafloor a synthetic pose is generated based on the last good pose and a simple motion model.
+
+See [orca_base](orca_base/README.md) for details.
+
+## Navigation
+
+Orca4 uses the [Navigation2](https://navigation.ros.org/index.html) framework for mission
+planning and navigation. Several simple Nav2 plugins are provided to work in a 3D environment:
+* straight_line_planner_3d
+* pure_pursuit_3d
+* progress_checker_3d
+* goal_checker_3d 
+
+See [orca_nav2](orca_nav2/README.md) for details.
+
+## Installation
+
+See the [Dockerfile](docker/Dockerfile) for installation details.
+
+Install these packages:
+* [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html)
+* [Gazebo Harmonic](https://gazebosim.org/docs/harmonic/install)
+* [ros_gz for Humble + Harmonic (ros-humble-gzharmonic)](https://gazebosim.org/docs/latest/ros_installation/)
+* [ardupilot_gazebo for Harmonic](https://github.com/ArduPilot/ardupilot_gazebo)
+* [ArduSub](https://ardupilot.org/dev/docs/building-setup-linux.html)
+
+Build ArduSub for SITL:
+~~~
+cd ~/ardupilot
+./waf configure --board sitl
+./waf sub
+~~~
+
+Populate the workspace:
+~~~
+mkdir -p ~/colcon_ws/src
+cd colcon_ws/src
+git clone https://github.com/clydemcqueen/orca4
+vcs import < orca4/workspace.repos
+~~~
+
+
+Get dependencies:
+~~~
+rosdep update
+rosdep install -y --from-paths . --ignore-src
+~~~
+
+MAVROS depends on GeographicLib, and [GeographicLib needs some datasets](https://ardupilot.org/dev/docs/ros-install.html):
+~~~
+wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
+chmod a+x install_geographiclib_datasets.sh
+sudo ./install_geographiclib_datasets.sh
+~~~
+
+Build the workspace:
+~~~
+cd ~/colcon_ws
+colcon build
+~~~
+
+## Simulation
+
+In a terminal run:
+~~~
+source src/orca4/setup.bash
+ros2 launch orca_bringup sim_launch.py
+~~~
+
+This will bring up all of the components, including the Gazebo UI.
+The surface of the water is at Z=0 and the sub will be sitting at the surface.
+The world contains a sandy seafloor 10 meters below the surface.
+
+![GAZEBO GUI](images/gazebo.png)
+
+You should see ArduSub establish a connection to the ardupilot_gazebo plugin:
+~~~
+[ardusub-2] JSON received:
+[ardusub-2] 	timestamp
+[ardusub-2] 	imu: gyro
+[ardusub-2] 	imu: accel_body
+[ardusub-2] 	position
+[ardusub-2] 	quaternion
+[ardusub-2] 	velocity
+~~~
+
+At this point SLAM is not running because the seafloor is too far away,
+but the sub can still move using dead-reckoning.
+The [base_controller](orca_base/src/base_controller.cpp) node will send default camera poses to ArduSub
+to warm up the EKF and the [manager](orca_base/src/manager.cpp) node will request attitude information at 20Hz.
+Initialization completes when there is a good pose from the EKF:
+
+~~~
+[mavros_node-8] [INFO] [mavros.imu/handle_attitude_quaternion]: IMU: Attitude quaternion IMU detected!
+[manager-9] [INFO] [manager/operator()]: EKF is running
+[base_controller-10] [INFO] [base_controller/change_state]: EKF is running, state => RUN_NO_MAP
+[base_controller-10] [INFO] [base_controller/UnderwaterMotion]: initialize odometry to {{-2.52304e-05, -3.28182e-05, -0.228547}, {0, 0, -5.00936e-05}}
+~~~
+
+Execute a mission in a second terminal:
+~~~
+source src/orca4/setup.bash
+ros2 run orca_bringup mission_runner.py
+~~~ 
+
+![RVIZ2_GUI](images/rviz2.png)
+
+The default mission will dive to -7m and move in a large rectangle.
+At -6m the cameras will pick up a view of the seafloor at and ORB_SLAM2 will start:
+~~~
+[orb_slam2_ros_stereo-13] New map created with 571 points
+[base_controller-10] [INFO] [base_controller/change_state]: map created, state => RUN_LOCALIZED
+~~~
+
+
+You should notice a loop closure sometime during the 2nd run around the rectangle. The adjustment is very small.
+
+~~~
+[orb_slam2_ros_stereo-13] Loop detected!
+[orb_slam2_ros_stereo-13] Local Mapping STOP
+[orb_slam2_ros_stereo-13] Local Mapping RELEASE
+[orb_slam2_ros_stereo-13] Starting Global Bundle Adjustment
+[orb_slam2_ros_stereo-13] Global Bundle Adjustment finished
+[orb_slam2_ros_stereo-13] Updating map ...
+[orb_slam2_ros_stereo-13] Local Mapping STOP
+[orb_slam2_ros_stereo-13] Local Mapping RELEASE
+[orb_slam2_ros_stereo-13] Map updated!
+~~~
+
+## Packages
+
+* [`orca_base` Base controller, localization, frames](orca_base)
+* [`orca_bringup` Launch files](orca_bringup)
+* [`orca_description` SDF files](orca_description)
+* [`orca_msgs` Custom messages](orca_msgs)
+* [`orca_nav2` Nav2 plugins](orca_nav2)
+* [`orca_shared` Dynamics model, shared utilities](orca_shared)

check_tf_tree.sh

@@ -0,0 +1,65 @@
+#!/bin/bash
+
+# Script to check the TF tree for the multi-AUV simulation
+echo "=== Checking TF tree for multi-AUV simulation ==="
+echo "This script checks if all required TF transforms are available"
+echo ""
+
+echo "--- Checking if tf2_tools is available ---"
+which ros2 >/dev/null 2>&1 || { echo "ROS2 not found! Make sure ROS2 is sourced."; exit 1; }
+
+echo "--- Waiting for TF tree to populate (5 seconds) ---"
+sleep 5
+
+echo "--- TF Tree Structure ---"
+ros2 run tf2_tools view_frames.py --no-gui
+
+echo ""
+echo "--- Checking specific transforms ---"
+
+# Function to check if a transform exists
+check_transform() {
+    local parent=$1
+    local child=$2
+    echo -n "Checking $parent -> $child: "
+    if ros2 run tf2_ros tf2_echo $parent $child --timeout 1.0 >/dev/null 2>&1; then
+        echo "✓ Available"
+    else
+        echo "✗ Missing"
+    fi
+}
+
+echo "Global transforms:"
+check_transform "map" "auv1/map"
+check_transform "map" "auv2/map" 
+check_transform "map" "auv3/map"
+
+echo ""
+echo "AUV1 transforms:"
+check_transform "auv1/map" "auv1/slam"
+check_transform "auv1/slam" "auv1/down"
+check_transform "auv1/map" "auv1/odom"
+check_transform "auv1/odom" "auv1/base_link"
+check_transform "auv1/base_link" "auv1/left_camera_link"
+
+echo ""
+echo "AUV2 transforms:"
+check_transform "auv2/map" "auv2/slam"
+check_transform "auv2/slam" "auv2/down"
+check_transform "auv2/map" "auv2/odom"
+check_transform "auv2/odom" "auv2/base_link"
+check_transform "auv2/base_link" "auv2/left_camera_link"
+
+echo ""
+echo "AUV3 transforms:"
+check_transform "auv3/map" "auv3/slam"
+check_transform "auv3/slam" "auv3/down"
+check_transform "auv3/map" "auv3/odom"
+check_transform "auv3/odom" "auv3/base_link"
+check_transform "auv3/base_link" "auv3/left_camera_link"
+
+echo ""
+echo "--- TF Tree File Generated ---"
+echo "Check frames.pdf for a visual representation of the TF tree"
+echo ""
+echo "--- Multi-AUV TF Check Complete ---"

debug_tf_live.sh

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+# Script to create TF tree debugging output during simulation
+echo "=== Multi-AUV TF Debugging Script ==="
+echo "This script helps debug TF transform issues"
+echo ""
+
+echo "--- Checking active static transform publishers ---"
+ros2 node list | grep static_transform_publisher | sort
+
+echo ""
+echo "--- Checking all TF frames ---"
+ros2 run tf2_ros tf2_monitor --all-frames
+
+echo ""
+echo "--- Checking transform chain from map to each AUV base_link ---"
+echo "AUV1 transform chain:"
+ros2 run tf2_ros tf2_echo map auv1/base_link --timeout 2.0
+
+echo ""
+echo "AUV2 transform chain:"
+ros2 run tf2_ros tf2_echo map auv2/base_link --timeout 2.0
+
+echo ""
+echo "AUV3 transform chain:"
+ros2 run tf2_ros tf2_echo map auv3/base_link --timeout 2.0
+
+echo ""
+echo "--- Checking base controller states ---"
+echo "Looking for base controller log messages..."
+ros2 topic echo /rosout --once | grep -E "(auv[1-3]_base_controller|manager)" | head -10
+
+echo ""
+echo "=== TF Debug Complete ==="