Migrate ROS environment and setup demo MAVROS node (#39)

.devcontainer/bashrc_setup.sh

@@ -4,10 +4,10 @@
 echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc
 echo 'source /root/ros2_ws/install/setup.bash' >> /root/.bashrc
 
-echo "export PATH=/root/ros2_ws/src/ardupilot/Tools/autotest:$PATH" >> /root/.bashrc
-echo "export PATH=/usr/lib/ccache:$PATH" >> /root/.bashrc
+echo 'export PATH=/root/ros2_ws/src/ardupilot/Tools/autotest:$PATH' >> /root/.bashrc
+echo 'export PATH=/usr/lib/ccache:$PATH' >> /root/.bashrc
 
-echo "export PATH=\"$PATH:$HOME/.local/bin\"" >> /root/.bashrc
+echo 'export PATH=$PATH:$HOME/.local/bin' >> /root/.bashrc
 
 # So that the container doesn't close
-/bin/bash
+/bin/bash -i

.gitignore

@@ -8,3 +8,6 @@ mav.*
 terrain
 eeprom.bin
 logs
+build
+install
+log

CMakeLists.txt

@@ -22,8 +22,7 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
-find_package(px4_msgs REQUIRED)
-find_package(uavf_ros2_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 
@@ -37,14 +36,28 @@ include_directories(include)
 #  DESTINATION lib/${PROJECT_NAME}
 #)
 
+set(msg_files
+  "msg/GpsAltitudePosition.msg"
+)
+set(srv_files
+)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
+  ${srv_files}
+)
+
 # Install Python executables
-ament_python_install_package(${PROJECT_NAME})
 install(PROGRAMS
   scripts/commander_node.py
+  scripts/demo_commander_node.py
   scripts/trajectory_planner_node.py
   scripts/waypoint_tracker_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+ament_export_dependencies(rosidl_default_runtime)
 
 
 if(BUILD_TESTING)

Dockerfile

@@ -173,6 +173,9 @@ RUN cp src/ardupilot/Tools/vagrant/mavinit.scr /root/.mavinit.scr
 #since apt installs the specific version we want we uninstall the pip version
 RUN python -m pip uninstall matplotlib -y 
 
+RUN sudo apt-get install -y ros-humble-mavros ros-humble-mavros-extras
+RUN bash -ic "source /opt/ros/humble/setup.bash && ros2 run mavros install_geographiclib_datasets.sh"
+
 # Sourcing script at runtime
 COPY .devcontainer/bashrc_setup.sh /usr/local/bin/bashrc_setup.sh
 RUN chmod 777 /usr/local/bin/bashrc_setup.sh

README.md

@@ -3,7 +3,7 @@
 ## Dev Container Setup (one time setup)
 1. Ensure you have Docker and X-11 forwarding installed on your device (google this)
 2. Clone the repo and edit devcontainer.json accordingly for your X-11 setup
-3. In VSCode, first run `python3 setup_image.py`
+3. In VSCode, first run `python3 setup_image.py` (alternatively run `echo "FROM t0mmyn/uavf_2024" > .devcontainer/Dockerfile` to download from a mirrored x86 image)
 4. Then open the command palette (cmd/ctrl+shift+p) and run `rebuild and reopen in dev container`
 5. To verify your setup, run `run_tests.sh`
 
@@ -38,7 +38,7 @@
 6. To use git inside the docker container, you may have to manually log in to GitHub again if the built-in credential forwarding isn't working. I recommend using the [GitHub CLI](https://cli.github.com/) to do this.
 7. If you want to use the simulator:
 	1. Follow instructions in `sim_instructions.md`.
-	2. If you want it to run it in a GUI, one way is using the remote desktop environment in the dev container. Open `localhost:6080` in a web browser, then enter password `vscode`, then use the menu in the bottom left to open a terminal, `cd /home/ws/PX4-Autopilot`, then run `make px4_sitl gazebo-classic`.
+	2. If you want it to run it in a GUI, one way is using the remote desktop environment in the dev container. Open `localhost:6080` in a web browser, then enter password `vscode`, then use the menu in the bottom left to open a terminal.
 	3. The X sockets should also be mounted and should work if you run `xhost +` on your machine.
 
 

msg/GpsAltitudePosition.msg

@@ -0,0 +1,6 @@
+uint64 timestamp	                # time since system start (microseconds)
+
+float64 lat			                # Latitude, (degrees)
+float64 lon			                # Longitude, (degrees)
+float32 alt			                # Altitude AMSL, (meters)
+float32 alt_ellipsoid		        # Altitude above ellipsoid, (meters)

package.xml

@@ -13,14 +13,12 @@
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclpy</build_depend>
-  <build_depend>px4_msgs</build_depend>
-  <build_depend>uavf_ros2_msgs</build_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclpy</exec_depend>
-  <exec_depend>px4_msgs</exec_depend>
-  <exec_depend>uavf_ros2_msgs</exec_depend>
 
   <test_depend>ament_cmake_nose</test_depend>
   <test_depend>ament_lint_auto</test_depend>

scripts/demo_commander_node.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+
+import std_msgs.msg
+import mavros_msgs.msg
+import mavros_msgs.srv
+import rclpy
+import rclpy.node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+import sensor_msgs.msg
+from threading import Thread
+from uavf_2024.msg import GpsAltitudePosition
+import sys
+
+class CommanderNode(rclpy.node.Node):
+    def __init__(self):
+        super().__init__('uavf_commander_node')
+
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+            depth = 1
+        )
+
+        # set up some clients - not all of these are used right now.
+
+        self.global_pos_sub = self.create_subscription(
+            sensor_msgs.msg.NavSatFix,
+            'ap/geopose/filtered',
+            self.global_pos_cb,
+            qos_profile)
+        self.got_pos = False
+
+        self.arm_client = self.create_client(mavros_msgs.srv.CommandBool, 'mavros/cmd/arming')
+
+        self.mode_client = self.create_client(mavros_msgs.srv.SetMode, 'mavros/set_mode')
+
+        self.takeoff_client = self.create_client(mavros_msgs.srv.CommandTOL, 'mavros/cmd/takeoff')
+
+        self.waypoints_client = self.create_client(mavros_msgs.srv.WaypointPush, 'mavros/mission/push')
+
+    def global_pos_cb(self, global_pos):
+        self.got_pos = True
+        self.last_pos = global_pos
+
+if __name__ == '__main__':
+    rclpy.init()
+    node = CommanderNode()
+
+    spinner = Thread(target = rclpy.spin, args = (node,))
+
+
+    with open(sys.argv[1]) as f:
+        waypoints = [tuple(map(float, line.split(','))) for line in f]
+
+
+    spinner.start()
+
+
+    print('Pushing waypoints')
+
+    node.waypoints_client.call(mavros_msgs.srv.WaypointPush.Request(start_index = 0,
+        waypoints = 
+            [
+                mavros_msgs.msg.Waypoint(
+                    frame = mavros_msgs.msg.Waypoint.FRAME_GLOBAL_REL_ALT,
+                    command = mavros_msgs.msg.CommandCode.NAV_WAYPOINT,
+                    is_current = True,
+                    autocontinue = True,
+
+                    param1 = 0.0,
+                    param2 = 5.0,
+                    param3 = 0.0,
+                    param4 = float('NaN'),
+
+                    x_lat = wp[0],
+                    y_long = wp[1],
+                    z_alt = 20.0
+                )
+
+                for wp in waypoints
+            ]
+    ))
+
+    print("Sent waypoints, setting node")
+
+
+    node.mode_client.call(mavros_msgs.srv.SetMode.Request( \
+        base_mode = 0,
+        custom_mode = 'AUTO'
+    ))
+
+    print('Set mode.')
+
+
+    node.destroy_node()
+    rclpy.shutdown()

sim_instructions.md

@@ -1,4 +1,6 @@
-# Instructions for running the Gazebo sim
+# Instructions for testing using SITL
+
+(We recommend using `tmux` in the dev-container - you'll be running a lot of programs at once!)
 
 ## Set up params
 ```
@@ -11,17 +13,19 @@ Once it's started, type the following command and then control-c:
 param set DDS_ENABLE 1
 ```
 
+You should only need to do this once.
+
 ## Launch SITL
 
 ## Simple SITL
 
 Run the following to start a simple SITL
 
 ```
-ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501
+ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501 home:=38.31633,-76.55578,142,0
 ```
 
-After starting the SITL you can connect to it with MAVProxy.
+After starting the SITL one options is connecting to it and sending commands directly with MAVProxy.
 
 ```
 mavproxy.py --console --map --aircraft test --master=:14550
@@ -37,3 +41,31 @@ ros2 launch ardupilot_gz_bringup iris_runway.launch.py
 Gazebo is another option, but I found it very slow through the layer of virtualization on my machine, and harder to set up. 
 
 
+## Start offboard control.
+
+Launch MAVROS. (It converts ROS messages sent to it into commands sent to the flight control software.)
+
+```
+ros2 launch mavros apm.launch fcu_url:=udp://:14551@
+```
+
+Build uavf_2024.
+
+```
+cd /home/ws && colcon build --merge-install && source install/setup.bash
+```
+
+In the GCS (the window where you have `mavproxy` open), manually make the drone takeoff:
+```
+mode guided
+arm throttle # might need to wait a bit for the sim to "heat up"
+takeoff 20
+```
+
+Launch the demo commander node. For now it just pushes a set of waypoints and switches the drone to AUTO to start it.
+```
+ros2 run uavf_2024 demo_commander_node.py /home/ws/uavf_2024/uavf_2024/gnc/data/TEST_MISSION
+```
+
+
+

uavf_2024/gnc/data/FLIGHT_BOUNDARY

@@ -0,0 +1,14 @@
+38.31729702009844, -76.55617670782419
+38.31594832826572, -76.55657341657302
+38.31546739500083, -76.55376201277696
+38.31470980862425, -76.54936361414539
+38.31424154692598, -76.54662761646904
+38.31369801280048, -76.54342380058223
+38.31331079191371, -76.54109648475954
+38.31529941346197, -76.54052104837133
+38.31587643291039, -76.54361305817427
+38.31861642463319, -76.54538594175376
+38.31862683616554, -76.55206138505936
+38.31703471119464, -76.55244787859773
+38.31674255749409, -76.55294546866578
+38.31729702009844, -76.55617670782419

uavf_2024/gnc/data/TEST_MISSION

@@ -0,0 +1,4 @@
+38.31619507, -76.55484077
+38.31559883, -76.55582833
+38.31591432, -76.55919177
+38.31684638, -76.55708783