UAV Simulation with Gazebo + PX4

# UAV Simulation with Gazebo + PX4 The origin project is at [here](https://github.com/matlabbe/rtabmap_drone_example.git), we will show how to install the corresponding packages and start the process like so. The origin repository provides a `Docker` file, and we will be using the commands in it. After we can successfully start the navigation, we will change the control of drone to KeyBoard, and then apply to our own world file created at [Simulation using ROS + Gazebo with ArduPilot](https://hackmd.io/@dANNy1209/rkBAPYyqA). ## Install packages Use the following command to install ROS packages: ```shell sudo apt update sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-geographic-msgs ros-noetic-dwa-local-planner ros-noetic-rtabmap-ros ros-noetic-mavlink geographiclib-tools libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev python3-pip ``` Use the following command to install Python packages: ```shell sudo pip3 install numpy toml packaging jinja2 empy pyros-genmsg ``` ## Install PX4 We will be using PX4 for controling drone in Gazebo and communicate with ROS. Install PX4 using the following commands: ```shell git clone https://github.com/PX4/PX4-Autopilot.git cd PX4-Autopilot git checkout v1.12.3 git submodule update --init --recursive sudo HEADLESS=1 make -j8 px4_sitl_default gazebo sudo make px4_sitl_default install source ~/.bashrc ``` >[!Note] >If you found it stuck at the command `HEADLESS` (it will automatically start the PX4 for testing), just hit `enter` and `CTRL`+`C` to >escape it and keep going on the next command. ## Create Workspace Next, create a ROS workspace with the following command, it also contains the repository we need inside: ```shell cd ~/Documents #You can decide where to put your workspace by yourself, I put it in ~/Documents mkdir -p catkin_ws/src cd catkin_ws/src catkin_init_workspace git clone https://github.com/mavlink/mavros.git git clone https://github.com/SyrianSpock/realsense_gazebo_plugin.git echo "install(DIRECTORY models DESTINATION \${CATKIN_PACKAGE_SHARE_DESTINATION})" >> realsense_gazebo_plugin/CMakeLists.txt cd mavros git checkout 1.9.0 sudo ./mavros/scripts/install_geographiclib_datasets.sh cd ../../ catkin_make -j8 ``` ## Build up the SLAM with ROS Clone the repository (we are currently at the workspace root directory) and change some settings then build it. We need to change those settings because we wan't to control the drone using keyboard, so the corresponding packages needs to be removed to avoid errors: ```shell git clone https://github.com/matlabbe/rtabmap_drone_example.git src/rtabmap_drone_example source ~/.bashrc ``` Open `src/rtabmap_drone_example/launch/slam.launch` with your preference text editor. Delete all code in `` section: ![image](https://hackmd.io/_uploads/SkjC4pra0.png) Open `src/rtabmap_drone_example/src/offboard_node.cpp` with your preference text editor. Delete the following lines: ![image](https://hackmd.io/_uploads/rkRCH6BpC.png) ![image](https://hackmd.io/_uploads/BkigLpBpC.png) ![image](https://hackmd.io/_uploads/HJPMLaH6R.png) CMake it again (Currently at your workspace root dir): ```shell catkin_make -j8 echo 'export NVIDIA_VISIBLE_DEVICES=${NVIDIA_VISIBLE_DEVICES:-all}' >> ~/.bashrc echo 'export NVIDIA_DRIVER_CAPABILITIES=${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics' >> ~/.bashrc source ~/.bashrc ``` ## Final setup Finally, we need to setup some source and export so that we don't need to do such things every time we open terminal. You will see `{YOUR_WORKSPACE_PATH}` in the commands, you need to change them to where you put the workspace at: ```shell echo "source /usr/local/px4/Tools/setup_gazebo.bash /usr/local/px4 /usr/local/px4/build/px4_sitl_default" >> ~/.bashrc echo "source {YOUR_WORKSPACE_PATH}/devel/setup.sh" >> ~/.bashrc echo "export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:{YOUR_WORKSPACE_PATH}/src/rtabmap_drone_example/models" >> ~/.bashrc echo "export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:{YOUR_WORKSPACE_PATH}/src/realsense_gazebo_plugin/models" >> ~/.bashrc echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:/usr/local/px4:/usr/local/px4/Tools/sitl_gazebo" >> ~/.bashrc source ~/.bashrc ``` Change to root privilege and delete those two files: ```shell sudo su rm /tmp/px4-sock-0 /tmp/px4_lock-0 # Go back to normal user using CTRL+D ``` Change the package imported in `patrol.py`: Use your preference text editor to open `/opt/ros/noetic/lib/rtabmap_util/patrol.py` (with sudo), change ``` from rtabmap_ros.msg import Goal ``` to ``` from rtabmap_msgs.msg import Goal ``` This is for making sure `patrol.py` works in later SLAM navigation. ## Running the origin project To run the project, we need a total of 6 terminal sessions. Below I'll show you how to open and execute them. 1. Start Gazebo simulation We can start the simulation using the command in first session: ```shell roslaunch rtabmap_drone_example gazebo.launch ``` You should see the rooms just like those in the origin repository. 2. Start SLAM Next, use the below command to start the rtabmap and SLAM for map in the second session: ```shell roslaunch rtabmap_drone_example slam.launch ``` You should see the rtabmap application pop out and can see the drone's sight. 3. Start SLAM result view Start the rviz using the below command to view the result of SLAM in 2D view in third session: ```shell roslaunch rtabmap_drone_example rviz.launch ``` The result should look like a lidar but create with VSLAM. 4. Drone takeoff Use the following command to let the drone takeoff in the forth session: ```shell rosrun rtabmap_drone_example offboard ``` Wait for the drone to takeoff, the rtabmap and SLAM view should follow it. 5. Start drone control We've mentioned to control the drone with keyboard, this is the place. We can use the following command to send instructions to drone in Gazebo in fifth session: ```shell rosrun teleop_twist_keyboard teleop_twist_keyboard.py ``` You should get the following result that tells you how to control the drone: ```shell Reading from the keyboard and Publishing to Twist! --------------------------- Moving around: u i o j k l m , . For Holonomic mode (strafing), hold down the shift key: --------------------------- U I O J K L M < > t : up (+z) b : down (-z) anything else : stop ``` 6. Set label of position The repository provides a way to set position marking for later navigation. We can use the following command in the sixth session: ``` rosservice call /rtabmap/set_label "node_id: 0 node_label: 'home' frame_id: ''" ``` This should be a one liner. Change the `home` to your label name, for example `kitchen`, `bedroom`...etc. At this point, we already have everything we need. Start controling the drone to fly around and use the command in step 6 to set up some labels. After you finish them, you can stop the simulation, and start them all again, with the guidence: 1. Start Gazebo simulation Remain the same, just open the simulation. 2. Start SLAM Change the command to: ``` roslaunch rtabmap_drone_example slam.launch localization:=true ``` This would start SLAM that enable us to use the recorded map. 3. Start SLAM result view Remain the same, open rviz. You should now see the track of drone you go through during the previous simulation. 4. Drone takeoff Same, takeoff the drone. 5. Set the map for SLAM Go to rtabmap application. Click the reload button on the left top, and just click ok to let it load the record. If you accidently start the SLAM again without adding `localization:=true`, you will found that it overwrites the map result. Just do the map record again and carefully dealing with the launch command 6. Automatic navigation Finally, provide some points and the wait time using the following command, it should automatically start the navigation: ``` rosrun rtabmap_util patrol.py _time:=5 kitchen room1 ``` The above command means that the drone will keep navigating between `kitchen` and `room1` with a `5 secs` wait time when reaching the target point. Change them to fit your requirement. Upon sending the above commands, you should now see the drone keep flying around the points and are following the track you do when creating SLAM map. Congrats! Next, we will be changing the map to our own one. ## Replace the Gazebo world with our own world Finally, to replace the world, we can simply create a new `.world` file in `{YOUR_WORKSPACE_PATH}/src/rtabmap_drone_example/worlds`, and edit the line: ``` <arg name="world_name" default="apt"/> ``` Replace `apt` to your world's file name(see the second step of the [overall guidence](https://hackmd.io/@dANNy1209/HylB7L-PoC)), and remember to remove the drone we created before, as it provides a drone by default when launching the gazebo world. Start the simulation as usual and you should now see your world and can fly around using the commands provided in last section.