Goal Interface - Waypoints ROS2¶
This page shows how to set up ROS 2 DDS with ArduPilot SITL for global position control.
Note
these pages are a work-in-progress
Usage¶
This page assumes that you’ve successfully followed the ROS 2 basic setups from this wiki: ROS 2, ROS 2 with SITL, and ROS 2 with gazebo.
Once that’s done, simply run:
cd ~/ros2_ws/src
source ~/ros2_ws/install/setup.bash
cd ardupilot/libraries/AP_DDS
ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml
cd ~/ros2_ws/src/ardupilot
source ~/ros2_ws/install/setup.bash
./Tools/autotest/sim_vehicle.py -w -v ArduPlane --console --enable-dds --map -DG
Now that SITL is running, you have two options: run the ROS2 Node or use CLI commands.
Using CLI Commands¶
- Arming the Plane:
Run the following command to arm the motors:
ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: true}"
- Switching to Takeoff Mode:
Change the flight mode to takeoff:
ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 13}"
- Reaching Takeoff Altitude:
Wait until the plane reaches the takeoff altitude. You can monitor the altitude using:
ros2 topic echo /ap/geopose/filtered
Ensure the plane’s altitude reaches the desired level (e.g., 630 meters AGL).
- Switching to Guided Mode:
Once the takeoff altitude is reached, switch to guided mode:
ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 15}"
- Sending Waypoint Command:
Publish the goal position to guide the plane to the desired waypoint:
ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: -35.345996, longitude: 149.159017, altitude: 635, coordinate_frame: 5}"
Using ROS2 Node¶
cd ~/ros2_ws
source ./install/setup.bash
ros2 run ardupilot_dds_tests plane_waypoint_follower
Understanding the ROS 2 Node¶
Here’s a detailed explanation of how this ROS2 Node works:
- Initialization:
The node is initialized with PlaneWaypointFollower(Node), which sets up the necessary parameters and clients for communication with ArduPilot services.
- Service Clients:
self._client_arm and self._client_mode_switch are created to handle arming the motors and switching flight modes, respectively.
These clients wait for the respective services to become available before proceeding.
- Publishers and Subscribers:
A publisher self._global_pos_pub is created to send global position commands.
A subscriber self._subscription_geopose listens for GeoPose messages to update the current position of the plane.
- Callback Function:
geopose_cb(self, msg: GeoPoseStamped) processes incoming GeoPose messages and updates the current position.
- Arming and Mode Switching Functions:
arm(self) sends a request to arm the motors.
switch_mode(self, mode) sends a request to change the flight mode to either takeoff or guided.
arm_with_timeout(self, timeout: rclpy.duration.Duration) and switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration) try to arm and switch modes with a timeout.
- Mission Execution:
The node attempts to arm the plane and switch to takeoff mode.
Once the plane reaches the takeoff altitude, it switches to guided mode.
The node sends a goal position for the plane to reach using guided control.
The achieved_goal(goal_global_pos, cur_geopose) function checks if the plane has reached the goal.
Demo¶
Here’s a quick video showcasing the results expected from this tutorial:
For more information, refer to this PR.