Review setup instructions

A review of the setup instructions.

1. Dependency on a custom branch of `ardupilot_gazebo`

Suggest that the arena model and world is moved into this repo so it is self contained, and use the released / current version of ardupilot_gazebo.
Replace the section describing the dependencies and setup of ardupilot_gazebo with a link to the repo and comment that the first step is to get the ArduPilotPlugin built following the instructions provided there.

2. Running SITL

cd ardupilot && sim_vehicle.py -D -v ArduCopter -f JSON --add-param-file=$HOME/ardupilot_gazebo_ap/config/gazebo-iris-gimbal-ngps.parm --console --map

Because I run SITL for many types of vehicle, I keep the eeprom.bin, mav.parm, etc files separate by creating a ~/sitl/{vehicle_name} folder for each type, rather than run from the ardupilot source coded directory.
Recommend that Gazebo is run before starting SITL. It should not make a difference, but occasionally there is a no link warning which seems to be less likely if the Gazebo side is running waiting to connect.

3. Console warnings

Expand on the note that because of visual odom the arming checks report the VisOdom is not healthy.

AP: EKF3 IMU0 tilt alignment complete
AP: EKF3 IMU1 tilt alignment complete
AP: EKF3 IMU1 MAG0 initial yaw alignment complete
AP: EKF3 IMU0 MAG0 initial yaw alignment complete
AP: GPS 1: probing for u-blox at 230400 baud
AP: GPS 1: detected u-blox
AP: EKF3 IMU1 origin set
AP: Field Elevation Set: 584m
AP: EKF3 IMU0 origin set
AP: PreArm: VisOdom: not healthy
AP: EKF3 IMU1 is using GPS
AP: EKF3 IMU0 is using GPS
AP: PreArm: VisOdom: not healthy
AP: PreArm: VisOdom: not healthy

4. Point gimbal downdwards

# set roll to centre
GUIDED> rc 6 1500

# set yaw to centre
GUIDED> rc 8 1500

# set pitch directly downwards
GUIDED> rc 7 1300

The PWM for the camera to be directly downwards is 1300 if SERVO7_MIN = 1100 and SERVO7_MAX = 1900. Where MNT1_PITCH_MAX = 45, MNT1_PITCH_MIN = -135, based on the SIYI A8 specs.

PWM	Camera Pitch Angle (deg)
1900	45
1800	22.5
1700	0
1600	-22.5
1500	-45
1400	-67.5
1300	-90
1200	-112.5
1100	-135

5. The `Terminal 3` section

This is the main contribution from this repo! Recommend that the installation step is moved to the beginning.

5a. Install dependencies

These instructions are for Ubuntu:

sudo apt-get install libgirepository1.0-dev libcairo2-dev
sudo apt-get install gobject-introspection

For macOS the dependencies are installed with:

brew install cairo
brew install gobject-introspection

Start streaming

gz topic -t /world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"

The terminal used to launch Gazebo should display the following if the streaming started correctly:

[Msg] GstCameraPlugin:: streaming: started
[Dbg] [GstCameraPlugin.cc:407] GstCameraPlugin: creating generic pipeline
[Msg] GstCameraPlugin: GStreamer element set state returned: 2
[Msg] GstCameraPlugin: starting GStreamer main loop

Missing dependencies on macOS

(ros2-rolling) rhys@MacBookPro2 src % python video_to_feature.py 
Traceback (most recent call last):
  File "/Users/rhys/Code/ros2/rolling/ros2-ardupilot/src/ap_nongps/src/video_to_feature.py", line 4, in <module>
    import feature_match_ardu as feature_match_ardu
  File "/Users/rhys/Code/ros2/rolling/ros2-ardupilot/src/ap_nongps/src/feature_match_ardu.py", line 4, in <module>
    import pyexiv2, json
  File "/Users/rhys/.venv/ros2-rolling/lib/python3.12/site-packages/pyexiv2/__init__.py", line 6, in <module>
    from .core import *
  File "/Users/rhys/.venv/ros2-rolling/lib/python3.12/site-packages/pyexiv2/core.py", line 1, in <module>
    from .lib import exiv2api
  File "/Users/rhys/.venv/ros2-rolling/lib/python3.12/site-packages/pyexiv2/lib/__init__.py", line 23, in <module>
    ctypes.CDLL(os.path.join(lib_dir, 'libexiv2.dylib'))
  File "/opt/homebrew/Cellar/python@3.12/3.12.3/Frameworks/Python.framework/Versions/3.12/lib/python3.12/ctypes/__init__.py", line 379, in __init__
    self._handle = _dlopen(self._name, mode)
                   ^^^^^^^^^^^^^^^^^^^^^^^^^
OSError: dlopen(/Users/rhys/.venv/ros2-rolling/lib/python3.12/site-packages/pyexiv2/lib/libexiv2.dylib, 0x0006): Library not loaded: /opt/homebrew/opt/inih/lib/libinih.0.dylib
  Referenced from: <D78FE009-64A0-3221-B934-BFD4DB7243A6> /Users/rhys/.venv/ros2-rolling/lib/python3.12/site-packages/pyexiv2/lib/libexiv2.dylib
  Reason: tried: '/opt/homebrew/opt/inih/lib/libinih.0.dylib' (no such file), '/System/Volumes/Preboot/Cryptexes/OS/opt/homebrew/opt/inih/lib/libinih.0.dylib' (no such file), '/opt/homebrew/opt/inih/lib/libinih.0.dylib' (no such file)

Needs additional dependency on macOS

brew install inih

and now running!

$ python video_to_feature.py
Heartbeat from system (system 1 component 0)
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944

6. Trying to break things...

If I fly off the map the script exits with a seg fault:

Not enough matches are found - 9/10
Traceback (most recent call last):
  File "/Users/rhys/Code/ros2/rolling/ros2-ardupilot/src/ap_nongps/src/video_to_feature.py", line 84, in <module>
    m.getComp()
  File "/Users/rhys/Code/ros2/rolling/ros2-ardupilot/src/ap_nongps/src/video_to_feature.py", line 61, in getComp
    [bb,x,y] = self.feature.compParam("satellite_image-main.png", self.frame,(self.kp,self.des))
    ^^^^^^^^
TypeError: cannot unpack non-iterable NoneType object
zsh: segmentation fault  python video_to_feature.py
(ros2-rolling) rhys@MacBookPro2 src %

snktshrma / ap_nongps