From 20be80d9d63d4fd8ccdda8b1096d660085f555a4 Mon Sep 17 00:00:00 2001 From: snktshrma Date: Sun, 23 Jun 2024 18:19:26 +0530 Subject: [PATCH] Dev: Added waypoint goal interface wiki --- dev/source/docs/ros.rst | 1 + .../docs/ros2-waypoint-goal-interface.rst | 126 ++++++++++++++++++ 2 files changed, 127 insertions(+) create mode 100644 dev/source/docs/ros2-waypoint-goal-interface.rst diff --git a/dev/source/docs/ros.rst b/dev/source/docs/ros.rst index 737c4e8915..b39879fc81 100644 --- a/dev/source/docs/ros.rst +++ b/dev/source/docs/ros.rst @@ -65,6 +65,7 @@ ROS 2 ROS 2 with SITL ROS 2 with SITL in Gazebo ROS 2 over Ethernet + ROS 2 waypoint goal interface Cartographer SLAM with ROS 2 in SITL diff --git a/dev/source/docs/ros2-waypoint-goal-interface.rst b/dev/source/docs/ros2-waypoint-goal-interface.rst new file mode 100644 index 0000000000..e4f3ad5d45 --- /dev/null +++ b/dev/source/docs/ros2-waypoint-goal-interface.rst @@ -0,0 +1,126 @@ +.. _ros2-waypoint-goal-interface: + +==================================== +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: :ref:`ROS 2 `, :ref:`ROS 2 with SITL `, and :ref:`ROS 2 with gazebo `. + +Once that's done, simply run: + +.. code-block:: bash + + 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 + +.. code-block:: bash + + 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 +================== + +1. **Arming the Plane**: + - Run the following command to arm the motors: + + .. code-block:: bash + + ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: true}" + +2. **Switching to Takeoff Mode**: + - Change the flight mode to takeoff: + + .. code-block:: bash + + ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 13}" + +3. **Reaching Takeoff Altitude**: + - Wait until the plane reaches the takeoff altitude. You can monitor the altitude using: + + .. code-block:: bash + + ros2 topic echo /ap/geopose/filtered + + - Ensure the plane's altitude reaches the desired level (e.g., 630 meters AGL). + +4. **Switching to Guided Mode**: + - Once the takeoff altitude is reached, switch to guided mode: + + .. code-block:: bash + + ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 15}" + +5. **Sending Waypoint Command**: + - Publish the goal position to guide the plane to the desired waypoint: + + .. code-block:: bash + + 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 +=============== + +.. code-block:: bash + + 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 `_: + +1. **Initialization**: + - The node is initialized with `PlaneWaypointFollower(Node)`, which sets up the necessary parameters and clients for communication with ArduPilot services. + +2. **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. + +3. **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. + +4. **Callback Function**: + - `geopose_cb(self, msg: GeoPoseStamped)` processes incoming GeoPose messages and updates the current position. + +5. **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. + +6. **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: + +.. youtube:: SHHw190RaHc + :width: 100% + +.. youtube:: FnChCmwBbHA + :width: 100% + +For more information, refer to this `PR `__.