In the last tutorial we have set up the ROS environment to use MAVROS to talk to PX4. In this tutorial we will run a small example program to autonomously hover a simulated vehicle.

Simulation First!

We highly recommend you to try this program in simulation first, for safety reasons 🙂

Start a PX4 simulation, you can do either:

  • HIL using jMAVsim
  • HIL using Gazebo
  • SIL using jMAVsim
  • SIL using Gazebo

They vary slightly in set up and the simulated data is also slightly different, the important thing is to make sure you can talk to PX4 in your simulation.

Create a Off-board node

In the last tutorial, we created the package px4_mavros in the catkin workspace. Now we will need to put some source code to start the off-board node.

First, copy this block of code in a file “offb_node.cpp” (example from https://dev.px4.io/en/ros/mavros_offboard.html) and put it in “~/catkin_ws/src/px4_mavros/src”.

/**
 * @file offb_node.cpp
 * @brief offboard example node, written with mavros version 0.14.2, px4 flight
 * stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.success){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

Now you need to edit the CMakeLists.txt file in “~/catkin_ws/src/px4_mavros”. At the end of the file, add
add_executable(offb_node src/offb_node.cpp)

target_link_libraries(offb_node ${catkin_LIBRARIES})

Go to “~/catkin_ws”, and do

catkin_make

This will build the example we have just added to the package.

 

ROSRUN the Example

Now start your simulation, and in a terminal, launch mavros

roslaunch mavros px4.launch

In a new terminal, run the example

rosrun px4_mavros offb_node

If everything is set up correctly, you will get a message “offboard enable”, followed by the simulated vehicle taking off and hover at 2m above the ground.

 

Plot Vehicle Status using rqt_plot

Rqt_plot provides a nice and clean interface to plot the PX4 vehicle status, you can in fact plot all the available ROS topics. Let us now plot the vehicle local position z and the local position set point z together.

In a new terminal, run

rqt_plot

This will bring up a rqt_plot window. In the top left corner, type “/mavros/local_position/pose/pose/position/z”, then hit the green “+” next to it. Now in the same field, clear and type “/mavros/setpoint_raw/target_local/pose/position/x”, then hit the green “+”. This will plot both set point and current z position on the same axes.

 

In the next post I will introduce the integration of external position estimation, such as information coming from Optitrack.