Automatic take off and land

Simulate, build and deploy your algorithms with Erle-Copter

Source code

If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.

The following tutorial will run through the basics of control through mavros as applied to an Erle-Copter quadcopter simulated in Gazebo. At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters, hovering for 10 seconds and land.



REMINDER: configure your environment before starting this tutorial.


Figure 1 - Diagram takeoff and land node

Download and compile the ROS node in your workspace

cd ~/simulation/ros_catkin_ws/src
git clone https://github.com/erlerobot/ros_erle_takeoff_land
cd ..
catkin_make --pkg ros_erle_takeoff_land

source ~/simulation/ros_catkin_ws/devel/setup.bash
rosrun ros_erle_takeoff_land ros_erle_takeoff_land

ros/ros.h is a convenience include that includes all the headers necessary to use the most common public pieces of the ROS system. This includes the mavros/*.h messages, which resides in the mavros package.

#include <cstdlib>

#include <ros/ros.h>
#include <mavros/CommandBool.h>
#include <mavros/CommandTOL.h>
#include <mavros/SetMode.h>

Initialize ROS. This allows ROS to do name remapping through the command line — not important for now. This is also where we specify the name of our node. Node names must be unique in a running system. The ros::init() function needs to see argc and argv so that it can perform any ROS arguments and name remapping that were provided at the command line.

ros::init(argc, argv, "mavros_takeoff");

Create a handle to this process’ node. The first NodeHandle created will actually do the initialization of the node, and the last one destructed will cleanup any resources the node was using. NodeHandle is the main access point to communications with the ROS system. The first NodeHandle constructed will fully initialize this node, and the last NodeHandle destructed will close down the node.

ros::NodeHandle n;

This creates a client for the mavros::SetMode service. The ros::ServiceClient object is used to call the service later on. Here we instantiate an autogenerated service class, and assign values into its request member. A service class contains two members, request and response. It also contains two class definitions, Request and Response. This actually calls the service. Since service calls are blocking, it will return once the call is done. If the service call succeeded, call() will return true and the value in srv.response will be valid. If the call did not succeed, call() will return false and the value in srv.response will be invalid.

    ////////////////////////////////////////////
    /////////////////GUIDED/////////////////////
    ////////////////////////////////////////////
    ros::ServiceClient cl = n.serviceClient<mavros::SetMode>("/mavros/set_mode");
    mavros::SetMode srv_setMode;
    srv_setMode.request.base_mode = 0;
    srv_setMode.request.custom_mode = "GUIDED";
    if(cl.call(srv_setMode)){
        ROS_ERROR("setmode send ok %d value:", srv_setMode.response.success);
    }else{
        ROS_ERROR("Failed SetMode");
        return -1;
    }

This creates a client for the arming service. If srv.request.value is True the vehicle try to arm.

    ////////////////////////////////////////////
    ///////////////////ARM//////////////////////
    ////////////////////////////////////////////
    ros::ServiceClient arming_cl = n.serviceClient<mavros::CommandBool>("/mavros/cmd/arming");
    mavros::CommandBool srv;
    srv.request.value = true;
    if(arming_cl.call(srv)){
        ROS_ERROR("ARM send ok %d", srv.response.success);
    }else{
        ROS_ERROR("Failed arming or disarming");
    }

This creates a client for the takeoff service. With srv_takeoff.request.altitude is possible to configure the desire altitude. Change the latitude or the longitude with the service message.

    ////////////////////////////////////////////
    /////////////////TAKEOFF////////////////////
    ////////////////////////////////////////////
    ros::ServiceClient takeoff_cl = n.serviceClient<mavros::CommandTOL>("/mavros/cmd/takeoff");
    mavros::CommandTOL srv_takeoff;
    srv_takeoff.request.altitude = 10;
    srv_takeoff.request.latitude = 0;
    srv_takeoff.request.longitude = 0;
    srv_takeoff.request.min_pitch = 0;
    srv_takeoff.request.yaw = 0;
    if(takeoff_cl.call(srv_takeoff)){
        ROS_ERROR("srv_takeoff send ok %d", srv_takeoff.response.success);
    }else{
        ROS_ERROR("Failed Takeoff");
    }

This part of the code allows you to create your own behaviour. In this case Erle-Copter waits 10 second holding the altitude and the position.


    ////////////////////////////////////////////
    /////////////////DO STUFF///////////////////
    ////////////////////////////////////////////
    sleep(10);

This creates a client for the Land service.

    ////////////////////////////////////////////
    ///////////////////LAND/////////////////////
    ////////////////////////////////////////////
    ros::ServiceClient land_cl = n.serviceClient<mavros::CommandTOL>("/mavros/cmd/land");
    mavros::CommandTOL srv_land;
    srv_land.request.altitude = 10;
    srv_land.request.latitude = 0;
    srv_land.request.longitude = 0;
    srv_land.request.min_pitch = 0;
    srv_land.request.yaw = 0;
    if(land_cl.call(srv_land)){
        ROS_INFO("srv_land send ok %d", srv_land.response.success);
    }else{
        ROS_ERROR("Failed Land");
    }

Erle-Copter is available at our webstore