From c02e5a3106222781847ab94441c9d3deaeead385 Mon Sep 17 00:00:00 2001 From: Ryan Date: Mon, 19 Feb 2024 01:29:21 -0700 Subject: [PATCH] Add README for latest navigation behavior in ROS 2 (#40) Signed-off-by: Ryan Friedman --- README.md | 133 +++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 106 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 86e6cf3..cea8db5 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ docker build --file docker/Dockerfile --tag terrain-navigation-ros2 . ``` You can see the image exists: -```Bash +```bash docker images >>> REPOSITORY TAG IMAGE ID CREATED SIZE >>> terrain-navigation-ros2 latest 5565f845ab4f 2 weeks ago 774MB @@ -38,7 +38,7 @@ docker run --network=host -it -v $(pwd):/root/ros2_ws/src/ethz-asl/terrain-navig ### Running the Build For dependencies that do not have binaries available, pull them into your ROS workspace using vcs. -```Bash +```bash cd ~/ros2_ws/src wget https://raw.githubusercontent.com/ethz-asl/terrain-navigation/ros2/terrain-navigation.repos vcs import --recursive < terrain-navigation.repos @@ -46,7 +46,7 @@ vcs import --recursive < terrain-navigation.repos For dependencies available through binaries, use rosdep. This package depends on [GDAL](https://gdal.org/index.html) to read georeferenced images and GeoTIFF files. -```Bash +```bash sudo apt update rosdep update source /opt/ros/humble/setup.bash @@ -54,46 +54,125 @@ rosdep install --from-paths src --ignore-src ``` Build the package -```Bash +```bash colcon build --packages-up-to terrain_navigation_ros ``` -## Running with PX4 SITL(Software-In-The-Loop) +## Running with PX4 SITL(Software-In-The-Loop), Gazebo and QGroundControl (QGC) + +Pre-requisites: +* Ubuntu 22.04 with ROS 2 Humble +* Gazebo harmonic -To setup PX4, clone the [ETHZ ASL PX4 autopilot project](https://github.com/ethz-asl/ethzasl_fw_px4/tree/pr-hinwil-testing) +### PX4 SITL Session with Gazebo + +To setup PX4, clone the a custom version of PX4. The setup instructions can be found in the [documentation](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html) for the latest dependencies. -For Ubuntu 20.04+ROS noetic with Gazebo classic: +This branch contains a small fix to the height rate setpoint forward ported to main, and a DEM for Gazebo of the region used in the terrain planner. -```Bash -git clone https://github.com/ethz-asl/ethzasl_fw_px4.git --branch pr-hinwil-testing --recursive -cd .. -cd ethzasl_fw_px4 -bash ./Tools/setup/ubuntu.sh +First set up the PX4 development environment: +```bash +git clone https://github.com/srmainwaring/PX4-AutoPilot --branch prs/pr-hinwil-testing-rebased --recursive +cd PX4-AutoPilot +./Tools/setup/ubuntu.sh +``` -make px4_sitl gazebo +Now, configure the shell environment to run `gz_standard_vtol`. +```bash +# Source environment variables mentioned in PX4-Autopilot/Tools/simulation/gz/worlds/davosdorf.sdf +export PX4_HOME_LAT=46.8132056 +export PX4_HOME_LON=9.8479538 +export PX4_HOME_ALT=1562.0 +export PX4_GZ_VERBOSE=4 +export PX4_GZ_MODEL_POSE="0,0,1562,0,0,0" +export PX4_GZ_WORLD=davosdorf +# TODO remove these variables and make it use a proper aligned world. + +make px4_sitl gz_standard_vtol ``` -Do source the relevant environment variables before launching terrain navigation. For instance, for Ubuntu 20.04 + ROS Noetic + Gazebo classic: +Due to the camera spawn location bug in [PX4-Autpilot#22659](https://github.com/PX4/PX4-Autopilot/issues/22659), +in the Gazebo Entity Tree view, click "standard_vtol_0" -> "Follow" to center the camera on the vehicle. +The QuadPlane will be visible sitting on terrain in Davos, Switzerland. -```Bash -px4_dir=~/ethzasl_fw_px4 -source $px4_dir/Tools/simulation/gazebo/setup_gazebo.bash $px4_dir $px4_dir/build/px4_sitl_default -export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$px4_dir -export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$px4_dir/Tools/simulation/gazebo/sitl_gazebo -export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins +### QGC + + +QGroundControl (QGC) will be used for loading a mission and sending a takeoff command. +QGC can also monitor the vehicle's flight mode and status through MAVLink. + +[Install QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) to configure and fly the vehicle. The instructions below may be specific to version 4.3.0. + +Let's get the vehicle flying, and plan a mission to fly! + +1. Disconnect any flight controllers; these would interfere with SITL. +1. Start QGC. +1. [Enable virtual joystick](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/settings_view/virtual_joystick.html#enable-the-thumbsticks). + Without this, the QGC status bar will be yellow and report an arming check report of "No manual control input". +1. Upload a mission. Go to the "Plan" screen, click "File" -> Storage "Open..." and select the `davosdorf` mission: + * [davosdorf_mission.plan](./terrain_navigation_ros/config/davosdorf_mission.plan) +1. Then, click the light blue flashing "Upload Required" button to upload the mission to the vehicle. +1. Go back to the "Fly" screen. +1. Use the slider to start the mission. The vehicle will launch vertically, transition to fixed wing mode, and enter a loiter next to Lake Davos. + +Once the vehicle is flying in a loiter, plan a mission, engage the planner and fly through the rviz UI (Video: https://youtu.be/EJWyGSqaKb4) + +## ROS 2 Terrain Planner, MavROS and RVIZ + +Launch the terrain planner, mavros, and rviz nodes in separate terminals. + +These can be combined but it helps with debugging to run them separately: + +Terminal 1 - Terrain Planner: +```bash +ros2 launch terrain_navigation_ros terrain_planner.launch.py rviz:=false +``` + +Terminal 2 - MAVRos: +```bash +ros2 launch terrain_navigation_ros mavros.launch.py flight_stack:=px4 ``` -*Note*: The path for gazebo may vary for a different version of Gazebo, update `GAZEBO_PLUGIN_PATH` as well as `ROS_PACKAGE_PATH` -accordingly. +If you get warnings from mavros like this, ignore them: +``` +[mavros_node-1] [WARN] [1705686179.019743422] [mavros.guided_target]: PositionTargetGlobal failed because no origin +``` +TODO: Fix QOS incompatibility on the following topic: +```bash +ros2 topic info /mavros/global_position/gp_origin -v +``` -The default launch file can be run as the following command. -```Bash -ros2 launch terrain_navigation_ros test_terrain_planner.launch.py + +Terminal 3 - RVIZ: +```bash +ros2 launch terrain_navigation_ros rviz.launch.py +``` + + + +Set the start and goal positions. If the interactive marker server is not updating correctly this may done using service calls: + +Terminal 4 - Services +```bash +ros2 service call /terrain_planner/set_start planner_msgs/srv/SetVector3 "{vector: {x: 1570, y: -330, z: -1}}" +ros2 service call /terrain_planner/set_goal planner_msgs/srv/SetVector3 "{vector: {x: -100, y: -200, z: -1}}" ``` -You can use [QGroundcontrol](http://qgroundcontrol.com/) to configure and fly the vehicle. Get the vehicle flying, and plan a mission to fly! +Trigger the planner and set the planner to navigate. The equivalent ROS 2 service calls are: + +Terminal 4 - Services +```bash +ros2 service call /terrain_planner/trigger_planning planner_msgs/srv/SetVector3 "{vector: {z: 10.0}}" +ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 2}" +``` +Figure: rviz after planning +px4_terrain_rviz_plan -Once the vehicle is flying in a loiter, plan a mission, engage the planner and fly through the rviz UI (Video: https://youtu.be/EJWyGSqaKb4) +Finally engage the planner by switching the plane to OFFBOARD mode. Using mavros directly this is: +ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: OFFBOARD}" +Figure: Gazebo and QGC at the goal +px4_terrain_gz_goal +px4_terrain_qgc_goal