From 8e6e38e91aaa58e907759f43e8301c7cc6173036 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@manna.aero>
Date: Fri, 21 Jan 2022 23:29:41 +0100
Subject: [PATCH 1/2] Readme: re init readme

---
 INSTALL.md |  56 ------------------
 README.md  | 169 +++++++++++++++++++----------------------------------
 2 files changed, 59 insertions(+), 166 deletions(-)
 delete mode 100644 INSTALL.md

diff --git a/INSTALL.md b/INSTALL.md
deleted file mode 100644
index d384b22b..00000000
--- a/INSTALL.md
+++ /dev/null
@@ -1,56 +0,0 @@
-# Install
-
-## Ubtunu 18.04
-
-Gazebo11 is required.
-
-```bash
-$ sudo apt-get update
-$ sudo apt-get upgrade
-$ sudo apt-get install libgazebo11
-$ sudo apt-get install libgazebo11-dev
-$ sudo apt-get install gazebo11
-```
-
-If you are seeing errors like:
-
-```bash
-$ gazebo --verbose
-Gazebo multi-robot simulator, version 11.3.0
-Copyright (C) 2012 Open Source Robotics Foundation.
-Released under the Apache 2 License.
-http://gazebosim.org
-
-[Msg] Waiting for master.
-Gazebo multi-robot simulator, version 11.3.0
-Copyright (C) 2012 Open Source Robotics Foundation.
-Released under the Apache 2 License.
-http://gazebosim.org
-
-[Msg] Waiting for master.
-[Msg] Connected to gazebo master @ http://127.0.0.1:11345
-[Msg] Publicized address: 192.168.1.170
-[Err] [RTShaderSystem.cc:478] Unable to find shader lib. Shader generating will fail.[Wrn] [SystemPaths.cc:459] File or path does not exist [""] [worlds/empty.world]
-[Err] [Server.cc:444] Could not open file[worlds/empty.world]
-[Wrn] [Server.cc:359] Falling back on worlds/empty.world
-[Wrn] [SystemPaths.cc:459] File or path does not exist [""] [worlds/empty.world]
-[Err] [Server.cc:444] Could not open file[worlds/empty.world]
-[Msg] Connected to gazebo master @ http://127.0.0.1:11345
-[Msg] Publicized address: 192.168.1.170
-[Err] [RTShaderSystem.cc:478] Unable to find shader lib. Shader generating will fail.
-```
-
-You may need to update your `~/.bashrc`:
-
-```bash
-# Gazebo
-- source /usr/share/gazebo-9/setup.sh
-+ source /usr/share/gazebo-11/setup.sh
-```
-
-RapidJSON is required:
-
-```
-$ sudo apt-get update
-$ sudo apt-get install rapidjson-dev
-```
diff --git a/README.md b/README.md
index 1e014360..327affe2 100644
--- a/README.md
+++ b/README.md
@@ -1,135 +1,84 @@
-# Ardupilot Gazebo plugin 
+# ArduPilot Ignition Gazebo plugin
+This is ArduPilot official plugin for Ignition Gazebo.
+It replaces the old Gazebo plugin to bring support of the next gen Gazebo simulator.
+It also brings support for more features :
+- more flexible data exchange between SITL and Ignition with JSON data sharing.
+- more sensors supported.
+- true Simulation lockstepping. It is now possible to use GDB to stop the Ignition time for debugging.
+- Better 3D rendering
 
-## Requirements :
-Native Ubuntu able to run full 3D graphics.
-Gazebo version 8.x or greater
-The dev branch will works on gazebo >= 8.x  
-For Gazebo 7 use branch gazebo7
+The project is composed of an Ignition plugin to connect to ArduPilot SITL (Software In The Loop) and some example models and worlds.
 
-## Disclamer : 
-This is a playground until I get some time to push the correct patch to gazebo master (I got hard time to work with mercurial..)!  
-So you can expect things to not be up-to-date.  
-This assume that your are using Ubuntu 16.04 or Ubuntu 18.04
+## Disclaimer :
+The plugin is currently working, but we are working into bringing support for more feature and refine the API.
 
-## Usage :
-I assume you already have Gazebo installed with ROS (or without).  
-If you don't have it yet, install ROS with `sudo apt install ros-melodic-desktop-full`
-(follow instruction here http://wiki.ros.org/melodic/Installation/Ubuntu).  
-Due to a bug in current gazebo release from ROS, please update gazebo with OSRF version from http://gazebosim.org/tutorials?tut=install_ubuntu
+## Prerequisites :
+Ignition Fortress is supported on Ubuntu Bionic, Focal and Jammy. If you are running Ubuntu as a virtual machine you will need at least Ubuntu 20.04 (Focal) in order to have the OpenGL support required for the `ogre2` render engine.
 
-libgazeboX-dev must be installed, X be your gazebo version (9 on ROS melodic).
+Follow the instructions for a [binary install of ignition fortress](https://ignitionrobotics.org/docs/fortress/install) and verify that ignition gazebo is running correctly.
 
-For Gazebo X
-````
-sudo apt-get install libgazeboX-dev
-````
+Set up an [ArduPilot development environment](https://ardupilot.org/dev/index.html). In the following it is assumed that you are able to
+run ArduPilot SITL using the [MAVProxy GCS](https://ardupilot.org/mavproxy/index.html).
 
-````
-git clone https://github.com/ArduPilot/ardupilot_gazebo
-cd ardupilot_gazebo
-mkdir build
-cd build
-cmake ..
-make -j4
-sudo make install
-````
+## Installation :
 
+Install Ignition Gazebo Fortress development libs and rapidjson:
 ````
-echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc
+sudo apt install rapidjson-dev libignition-gazebo6-dev
 ````
 
-Set Path of Gazebo Models (Adapt the path to where to clone the repo)
-````
-echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models' >> ~/.bashrc
+Clone the repo and build with:
+````bash
+git clone https://github.com/ArduPilot/ardupilot_gazebo -b ignition-fortress
+cd ardupilot_gazebo
+mkdir build && cd build
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
+make -j4
 ````
 
-Set Path of Gazebo Worlds (Adapt the path to where to clone the repo)
-````
-echo 'export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc
-````
+## Running :
 
-````
-source ~/.bashrc
-````
+Set the ignition environment variables in your `.bashrc` or `.zshrc` or in  the terminal used to run gazebo:
 
-DONE !
+### In terminal
+Assuming that you have clone the repository in `$HOME/ardupilot_gazebo`:
+```bash
+export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$IGN_GAZEBO_SYSTEM_PLUGIN_PATH
+export IGN_GAZEBO_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:IGN_GAZEBO_RESOURCE_PATH
+```
 
-Now launch a world file with a copter/rover/plane and ardupilot plugin, and it should work! 
-(I will try to add some world file and model later)
+### In .bashrc
+Assuming that you have clone the repository in `$HOME/ardupilot_gazebo`:
+```bash
+echo 'export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:${IGN_GAZEBO_SYSTEM_PLUGIN_PATH}' >> ~/.bashrc
+echo 'export IGN_GAZEBO_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:${IGN_GAZEBO_RESOURCE_PATH}' >> ~/.bashrc
+```
 
-## HELP
+Reload your terminal with source ~/.bashrc
 
-How to Launch :  
-Launch Ardupilot Software In the Loop Simulation for each vehicle.
-On new terminal, launch Gazebo with basic demo world.
+### Run Gazebo
 
-#####ROVER (no model provided for now)
+```bash
+ign gazebo -v 4 -r iris_arducopter_runway.world
+```
 
-On 1st Terminal (Launch Ardupilot SITL)
-````
-sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console
-````
+The `-v 4` parameter is not mandatory, it shows the debug informations.
 
-On 2nd Terminal (Launch Gazebo with demo Rover model)
-````
-gazebo --verbose worlds/ (Please Add if there is one.)
-````
+### Run ArduPilot SITL
+To run an ArduPilot simulation with Gazebo, the frame should have `gazebo-` in it and have `JSON` as model. Other commandline parameters are the same as usal on SITL.
+```bash
+sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console
+```
 
-##### COPTER
+### Arm and takeoff
 
-On 1st Terminal (Launch Ardupilot SITL)
-````
-sim_vehicle.py -v ArduCopter -f gazebo-iris --map --console
-````
-
-On 2nd Terminal (Launch Gazebo with demo 3DR Iris model)
-````
-gazebo --verbose worlds/iris_arducopter_runway.world
-````
-
-##### PLANE
-
-On 1st Terminal (Launch Ardupilot SITL)
-````
-sim_vehicle.py -v ArduPlane -f gazebo-zephyr --map --console
-````
-
-On 2nd Terminal (Launch Gazebo with demo Zephyr flying wing model)
-````
-gazebo --verbose worlds/zephyr_ardupilot_demo.world
-````
-
-In addition, you can use any GCS of Ardupilot locally or remotely (will require connection setup).
-If MAVProxy Developer GCS is uncomportable. Omit --map --console arguments out of SITL launch and use APMPlanner 2 or QGroundControl instead.
-Local connection with APMPlanner2/QGroundControl is automatic, and recommended.
+```bash
+STABILIZE> mode guided
+GUIDED> arm throttle
+GUIDED> takeoff 5
+```
 
 ## Troubleshooting
 
-### Missing libArduPilotPlugin.so... etc 
-
-In case you see this message when you launch gazebo with demo worlds, check you have no error after sudo make install.  
-If no error use "ls" on the install path given to see if the plugin is really here.  
-If this is correct, check with `cat /usr/share/gazebo/setup.sh` the variable `GAZEBO_PLUGIN_PATH`. It should be the same as the install path. If not use `cp` to copy the lib to right path. 
-
-For Example
-
-````
-sudo cp -a /usr/lib/x86_64-linux-gnu/gazebo-7.0/plugins/ /usr/lib/x86_64-linux-gnu/gazebo-7/
-````
-
-path mismatch is confirmed as ROS's glitch. It will be fixed.
-
-### Future(not activated yet)
-To use Gazebo gps, you must offset the heading of +90° as gazebo gps is NWU and ardupilot is NED 
-(I don't use GPS altitude for now)  
-example : for SITL default location
-````
-    <spherical_coordinates>
-      <surface_model>EARTH_WGS84</surface_model>
-      <latitude_deg>-35.363261</latitude_deg>
-      <longitude_deg>149.165230</longitude_deg>
-      <elevation>584</elevation>
-      <heading_deg>87</heading_deg>
-    </spherical_coordinates>
-````
-Rangefinder
+### Gazebo issues
+Ignition documentation is already providing some help on most frequents issues https://ignitionrobotics.org/docs/fortress/troubleshooting#ubuntu

From 2cdcb06747246e004b1b17b31a2662569c011d88 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Fri, 25 Feb 2022 23:03:24 +0100
Subject: [PATCH 2/2] IrlockPluging: fix port as per
 https://github.com/khancyr/ardupilot_gazebo/pull/48

---
 src/ArduCopterIRLockPlugin.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ArduCopterIRLockPlugin.cc b/src/ArduCopterIRLockPlugin.cc
index 5b98eef1..72526989 100755
--- a/src/ArduCopterIRLockPlugin.cc
+++ b/src/ArduCopterIRLockPlugin.cc
@@ -169,7 +169,7 @@ void ArduCopterIRLockPlugin::Load(sensors::SensorPtr _sensor,
   }
   this->dataPtr->irlock_addr =
           _sdf->Get("irlock_addr", static_cast<std::string>("127.0.0.1")).first;
-  this->dataPtr->irlock_addr =
+  this->dataPtr->irlock_port =
           _sdf->Get("irlock_port", 9005).first;
 
   this->dataPtr->parentSensor->SetActive(true);