PX4 Offboard Control Using MAVROS on ROS

This post is written by Jaeyoung Lim

Overview

Recently opensource autopilots have become reliable by various failsafe functions integrated by the opensource community. Developing different softwares for each application can be time consuming and hard to verify the reliablility of the software. An alternative to modifying the autopilot software, is to use an onboard computer to command the autopilot using high level commands. This way, the developer can use a reliable autopilot software which is still reliable and capable of staying in the air and use a software developed for the developer’s own purpose.
This article is to introduce how easy and practical it is to connect a PC, using ROS to control the autopilot running onbaord an onboard PC. A nice tutorial is up on the PX4 website but is a little unfriendly and I wanted to share some of the troubleshooting experiences while I was implementing the system.

로열모_5분스피치_임재영

what is ROS?

ROS is a framework for incorporating various modules of robotics by configuring modules as a TCP network. The ROS community is currently growing in a very fast pace both in the community and industry. The ROS wiki has great tutorials in understanding the framework. Please get used to using ROS following tutorials 1.~9. in the tutorials page.

Introduction to MAVROS

MAVROS is a ROS package which enables ROS to interact with autopilot software using the MAVLink protocol. MAVlink consists of 17 bytes and includes the message ID, target ID and data. The message ID shows what the data is. Message IDs can be seen in the messageID command set.
b15e6d130c9d606305c31ffedb3c1118.media.400x98

This enables MAVLink to be able to get information from multiple UAVs if messages are transferred in the same channel. Messages can either be transmitted through wireless signals.

mavros_raw

Installing MAVROS

MAVROS can be installed using the source in the mavros repository. The default dialect of MAVROS is apm. As we are installing px4 on the pixhawk flight controller, MAVROS should be built from source, by configuring mavros by the command below.

catkin config --cmake-args -DMAVLINK_DIALECT=common

Configuring PX4 for ROS

For the companion computer to communicate with the flight controller, a USB2TTL converter is needed to convert the voltage of the communication levels. A brief overview of configuring the companion computer with the pixhawk is shown in this link.

IMG_20151221_122909

It is recommended that the offboard control is done through the TELEM2 port. Telem2 port can be activated usign the SYS_COMPANION parameter. This can be done by modifying the parameter on qgroundcontrol.

SYS_COMPANION 921600

Configuring MAVROS for PX4

To launch MAVROS, the easiest way is to use the launch file

roslaunch mavros px4.launch

Launching MAVROS, it may not work on the first time. Do not panic!

Some parameters should be modified to be able to talk with the flight controller. The device name and baudrate should be configured to be able to talk to the autopilot.

<node name="mavros" pkg="mavros" type="mavros_node" output="screen">
	<param name="fcu_url" value="$(arg fcu_url)" />
	<param name="gcs_url" value="$(arg gcs_url)" />
	<param name="target_system_id" value="$(arg tgt_system)" />
	<param name="target_component_id" value="$(arg tgt_component)" />
	<!-- enable heartbeat send and reduce timeout -->
	<param name="conn_heartbeat" value="5.0" />
	<param name="conn_timeout" value="5.0" />
	<!-- automatically start mavlink on USB -->
	<param name="startup_px4_usb_quirk" value="true" />

</node>

Setting Setpoints

Setpoints should be sent to the flight controller with a 0.5s time out. This is a safety factor so that the control should converge to a specific objective. Otherwise the flight controller will not go into offboard mode. Setpoints that can be used are as below.

  • Position Setpoints
  • Velocity Setpoints
  • Attitude Setpoints
  • Acceleration Setpoints
  • Actuator Control

An example node written in cpp is shown below. The node publishes setpoint_poistion_local message.

Integrating Companion Computer to quadrotor

The quadrotor is based on a F330 frame and the current integration follows as below. 330 frame feeled a little small for an onboard computer. For future integration, 450 frame should be more appropriate.

IMG_20151221_202606

Position Control Example

For position control, external position estimation using the Vicon motion capture system was used for more precise position measurement. The block diagram visualized using rtq_graph is shown as below.

rosgraph2

This is an example to show how to control the quadrotor using the setpoint_position topic. External position estimation was used using the vicon motion capture system.


ctrl_position.launch

<launch>

<!--arg name="fcu_url" default="serial:///dev/ttyUSB0:921600" -->
<arg name="fcu_url" default="udp://:14540@192.168.1.36:14557"/>
<arg name="gcs_url" default="udp://:14556@192.168.150.2:14550" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />


<node name="mavros" pkg="mavros" type="mavros_node" output="screen">
	<param name="fcu_url" value="$(arg fcu_url)" />
	<param name="gcs_url" value="$(arg gcs_url)" />
	<param name="target_system_id" value="$(arg tgt_system)" />
	<param name="target_component_id" value="$(arg tgt_component)" />

	<!--rosparam command="load" file="$(find mavros)/launch/px4_blacklist.yaml"-->

	<!-- enable heartbeat send and reduce timeout -->
	<param name="conn_heartbeat" value="5.0" />
	<param name="conn_timeout" value="5.0" />
	<!-- automatically start mavlink on USB -->
	<param name="startup_px4_usb_quirk" value="true" />
	<param name="mocap/use_tf" value="true"/>
	<param name="mocap/use_pose" value="false"/>
</node>

<node name="setpoint_pub" pkg="inrol_ros" type="pub_setpoints_pos" output="screen">

</node>

<node pkg="vicon_bridge" type="vicon_bridge" name="vicon" output="screen">
	<param name="stream_mode" value="ServerPush" type="str" />
	<!--param name="datastream_hostport" value="147.47.175.54:801" type="str" /-->
	<param name="datastream_hostport" value="147.46.175.54:801" type="str" />
	<param name="tf_ref_frame_id" value="/world" type="str" />
	<remap from="/vicon/quad/quad" to="/mavros/mocap/tf"/>	       
</node>


</launch>

pub_setpoints.cpp

#include <ros/ros.h>
#include <std_msgs/String.h> 
#include <stdio.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"

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

   ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local",100);
   ros::Rate loop_rate(100);
   ros::spinOnce();

   geometry_msgs::PoseStamped msg;
   int count = 1;
	
		//PositionReciever qp;:
		//Body some_object;
		//qp.connect_to_server();

	
   while(ros::ok()){
	   //some_object = qp.getStatus();
		// some_object.print();
		//printf("%f\n",some_object.position_x);
       msg.header.stamp = ros::Time::now();
       msg.header.seq=count;
       msg.header.frame_id = 1;
       msg.pose.position.x = 0.0;//0.001*some_object.position_x;
       msg.pose.position.y = 0.0;//0.001*some_object.position_y;
       msg.pose.position.z = 1.0;//0.001*some_object.position_z;
       msg.pose.orientation.x = 0;
       msg.pose.orientation.y = 0;
       msg.pose.orientation.z = 0;
       msg.pose.orientation.w = 1;

       chatter_pub.publish(msg);
       ros::spinOnce();
       count++;
       loop_rate.sleep();
   }
   
      
   return 0;
}

Attitude Control Example

pub_att.cpp

#include <ros/ros.h>
#include <std_msgs/String.h> 
#include <stdio.h>
#include <math.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include "std_msgs/Float64.h"

//#include </home/mahesh/catkin_ws/src/beginner_tutorials/src/Qualisys.h>

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

   ros::Publisher pub_att = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_attitude/attitude",100);
   ros::Publisher pub_thr = n.advertise<std_msgs::Float64>("/mavros/setpoint_attitude/att_throttle", 100);
   ros::Rate loop_rate(100);
   ros::spinOnce();
   geometry_msgs::PoseStamped cmd_att;
   std_msgs::Float64 cmd_thr;
   int count = 1;
   double v[3]={1.0, 0.0, 0.0};
   double lambda = 0.3;


   double v_norm=sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
   double theta=0.0;
   

		//PositionReciever qp;
		//Body some_object;
		//qp.connect_to_server();

	
   while(ros::ok()){
	   //some_object = qp.getStatus();
		// some_object.print();
		//printf("%f\n",some_object.position_x);
		//Create attitude command message
       cmd_att.header.stamp = ros::Time::now();
       cmd_att.header.seq=count;
       cmd_att.header.frame_id = 1;
       cmd_att.pose.position.x = 0.0;//0.001*some_object.position_x;
       cmd_att.pose.position.y = 0.0;//0.001*some_object.position_y;
       cmd_att.pose.position.z = 0.0;//0.001*some_object.position_z;

       cmd_att.pose.orientation.x = sin(theta/2.0)*v[0]/v_norm;
       cmd_att.pose.orientation.y = sin(theta/2.0)*v[1]/v_norm;
       cmd_att.pose.orientation.z = sin(theta/2.0)*v[2]/v_norm;
       cmd_att.pose.orientation.w = cos(theta/2.0);
      /*
	   double q_norm = sqrt(sin(theta/2.0)*sin(theta/2.0)+cos(theta/2.0)*cos(theta/2.0));
		printf("%f\t",v_norm);
		printf("%f\n", q_norm);
	*/
	   //Create throttle command message
	   cmd_thr.data = lambda;
      
	   pub_att.publish(cmd_att);
       pub_thr.publish(cmd_thr);
	   ros::spinOnce();
       count++;
	   theta=0.3*sin(count/300.0);
       loop_rate.sleep();
/*
	if(count>1000){
		count =0;
	}
	*/
   }
   
      
   return 0;
}

The video below shows some bias in yaw when controlling the attitude of the quadrotor. I believe this is due to the induced magnetic field from the motor currents which will offset the magnetic field in high attitudes.

Advertisements

66 thoughts on “PX4 Offboard Control Using MAVROS on ROS

  1. Ok What I want to do is implement an Attitude+Position controller that I implemented in Matlab during my Robotics: Aerial Robotics course in Coursera Specialization but I am having trouble doing so because I dont understand well how to get the current state, I did what you told me above and I think it works I just had to fix a roscpp problem that came with my ROS installation and havent had the time to try it out. what confuses me is that you posted an attitude controller and I dont understand where or how you get the state if youre not subscribed to any topics just published

    Liked by 1 person

    • As you can see in the code, I am publishing “/mavros/setpoint_attitude/attitude” messages

      When the message is received by the quadrotor, an attitutde controler INSIDE the quadrotor will take care of controlling the attitude.

      Maybe what you are trying to do is to run the controller outside pixhawk and command values to the quadrotor which is also possible

      Like

      • Hi Jaeyoung Lim. Thank you for your instruction. But can you guide me how to code an attitude controller to control the quadrotor after publishing set point attitude?

        Like

      • The attitude controller is run by PX4 which is running inside the flight controller(Pixhawk).
        You do not need to control the attitude in addition to publishing the setpoint attitude.

        You still need to send throttle commands to make the quadrotor fly while sending attitude setpoint commands

        Like

  2. Hi ! Thank you a ot for this tutorial, it’s very interesting but I have some problem before begin it.
    I don’t download mavros and mavros extras and I compil them but in catkin_make_isolated. I have an error when i compil catkin_make. (Ubutun 14.02 , ros indigo)
    I had dowload vicon_bridge too but i don’t know where it go this package ?
    Can you help me ?
    Thanl you a lot

    Like

  3. HI Jaeyoung Lim, Thank you a lot for this tutorial. I have some questions about vicon package. I had download it but i don’t kno how to build it and to add it in my project. Can you help me ?
    Thank you a lot
    Regards

    Like

  4. Hello there! Nice tutorial, how do you sent vicon values from the mocap computer to the companion? WiFi ? I was using a telemetry radio but it is very unreilable and, oddly enough, sometimes it lose connection even if we are at 5 meter distance. I checked that because px4 is often logging “vision timeout”.

    Like

    • In the flight test video, the quadrotor is connected to the laptop via a usb cable. The TELEM2 port is connected to the computer via USB2TTL

      Usually a companion computer integrated ‘on’ the quadrotor was used to fetch the vicon data from the server. The companion computer was connected to the network via wifi.

      I guess telemetry modules(433MHz AFSK modules, right?) are not appropriate for OFFBOARD control as they are too slow and unreliable. Especially if you are forwarding mocap data. Baudrate for wired TELEM2 port is 921600 compared to the badrate of the 433MHz telemetry module’s baudrate which is usually 57600

      In my opinion the fastest way to check for you is simply connecting the quadrotor with a long usb cable possible with a repearter attached to it.

      Like

      • Thank you very much! I will try wifi first. Yes I was using the 433MHz AFSK modules and they are nightmares, but for mocap data forwarding they were working just fine until some days ago. I would like to set a small onboard companion computer such as a raspberry. Do you have any suggestion for that? I need low power comsumption and small volume, the only feature I need is wifi. Thank you so much, awesome blog by the way 😉

        Like

      • I guess the cheapest option will be a raspberry pi, as it incorporates some GPIOs in a small package. However, Pis are not powerful enough for image processing applications.

        For image processing, I guess you will need something like an odroid Xu4 or u3

        Like

    • If you use the telemetry (I guess you are using 433MHz AFSK modules) you are constrained with slow baudrates(~57600) and packet loss.

      You should have a companion computer onboard the quadrotor which is connected to the pixhawk by wire. This way you can use baudrates upto 921600. The companion computer should be tied up in the network via wifi to access the vicon data.

      Like

      • Hello JaeYoung! I have an Odroid XU4 connected to my pixhawk 2.0 (working with a 3dr solo) through usb wire and a lot of the data is not published. The baud rate is set at 921600. Topics like /mavros/global_position/global are not being published but topics like /mavros/imu/data_raw are published. When the 3dr solo is connected to mavros via wifi, all of the data is available and published. Any ideas on why some data is not accessible when connected through usb?

        Thanks for your help!

        Like

  5. Hey, sorry for possible multiple posts but i had a connection problem. Anyway, awesome and clear tutorial! How do you send mocap values from the vision computer to the companion? WiFi? I am using two telemetry radio modules but they are pretty slow. Moreover, from yesterday I see in px4 log files “vision timeout” very often. It seems that the are losing some mavlink package and the receiver is not reading, i really need a more reliable solution for that.

    Like

    • You can use various methods to make it move actually:

      so if you want to specify a position, you can use the setpoint_position topic

      if you want to move it by controlling it by velocity you can use the setpoint_velocity topic

      you can also averride the RC inputs using mavros. Details can be found here : http://wiki.ros.org/mavros

      Like

  6. HI Jaeyoung Lim,

    Thanks a lot for the useful tutorial. I am trying to get Offboard Control working using PX4 and Intel NUC onboard PC.

    I have used the files you provided in the Position Control Example above – ctrl_position.launch and pub_setpoints.cpp. However, I am running into the REJECT OFFBOARD problem. I am new to this, and hence would appreciate your help.

    Here is the sequence of steps I am doing:
    – Turn on Onboard PC
    – Power on PX4 using Lithium Polymer Battery
    – roscore & roslaunch on the Onboard PC (using the ctrl_position.launch file provided above)
    – Press safety switch
    – ARM the quad (the motors then start spinning – I have not put in the rotors yet, since I am only testing at this point)
    – Turn on RC (which is in Manual Control mode)
    – Increase Throttle using the RC (the FCU shows “TAKEOFF DETECTED” message)
    – Switch the Offboard mode using the RC

    This is when I receive the FCU: REJECT OFFBOARD message.

    I am not sure what I am doing wrong. Do you think this is a Firmware issue?

    Like

      • how can I do this Im having that same problem these are the outputs most of the time when I can connect
        FCU: Rejecting mode switch cmd
        FCU: REJECT OFFBOARD
        Also I am using a usb cable connected directly to the pixhawks USB port does this matter?

        Like

      • I think the setpoints you are publishing are not properly subscribed by the mavros node or some how the pixhawk board is not receiving the setpoint topics. Would you check if the setpoints are properly being sent to the flight controller?

        You should NOT use the usb port. You have to send the setpoints topic to the TELEM2 port on pixhawk.

        Like

  7. Hi Jaeyoung Lim, I came here from your link in youtube. Could you share an example where setpoint_velocity/cmd_vel topic is used. I am working on autonomous takeoff where Optical Flow sensor is used in a GPS denied evironment.

    Like

  8. Hi,
    I am using ROS indigo with XU4. I am doing SLAM with quadrotor using moveit.
    I have a problem that i don’t know how to control the quadrotor after i got the plan.In the other word,quadrotor can’t execute.I have already connected to mavros.Do you know how should i do to let quadrotor fly?
    I am new in ROS. Hope you can help me with that.

    Like

    • I am not sure if I have fully understood your problem. Is it that the px4 is not entering offboard mode? are you sending setpoints within the timeout? Setpoints have a 0.5 second timeout

      Like

      • sorry,i redescribe my problem again.
        I have an action server(action controller) and controller manager for moveit now.
        But i don’t know how to use action server to control quadrotor after i got the trajectory planned by moveit.
        I set px4 to stabilized mode when i want it execute the trajectory.
        I didn’t have setpoint.cpp. Should i write setpoint.cpp? or maybe action server can control quadrotor via mavros?

        this is my rqt

        thank you for your reply.

        Like

      • Hi,
        I know write a node which subscribes the trajectory from moveit and sends to mavros is one method.
        But i am trying to understand how to implement the trajectory with action server.
        Maybe i should subscribe the command from my_controller_node?
        (i attatch the new graph below)

        Do you know the other method that subscribe from controller?

        Thanks for help.

        Like

  9. Hi,

    1. Recently, I am finding the transfer function of drone. Its input is roll pitch and yaw rate from topic mavros/imu_raw_data of mavros and its output is PWM signals of roll pitch yaw and throttle from mavros/actuator control that are coded on ROS, mavros, Odroid xu4 and px4 native stack firmware. The control signal of drone is from radio controller and sometimes this signal will be combined with chirping signals on roll pitch and yaw so that it will make my drone move around x y z axis and collect the necessary input and output data.
    2. To achieve this, I used topic mavros/rc/in which reads the roll pitch yaw and throttle signal from radio controller and scaled them to the range of -1 to 1. After that, I produce directly to the output of actuator using topic mavros/ actuator control (roll pitch yaw throttle channel of group 0). This code is fine! I can change my Pixhawk into Offboard Mode, then arm motor and control drone through radio signal, add chirping noise using switch on radio controller ( topic mavros/rc/in) as well as save the input and output data in text file.
    3. However, when running this code in my experiment, my drone oscillates a lot in take-off process and flip itself occasionally. I guess that the major reason is that the control signal from radio alone to control drone directly is not good.
    4. After that, I tried to turn it into Manual mode and used QRoundControl software to adjust PID parameters for roll pitch and yaw rate. Fortunately, my drone operates well with less fluctuation. However, with this flight mode, I cannot run my code as in section1 and 2 proposed because it is clear that my code only runs with Offboard Mode.
    Therefore, can anybody here that have experience with controlling drone with mavros and px4 native stack give me some ideas to solve my problem. I will run manual mode first and then switch it to offboard mode when my drone is stable? but to do that, what i have to add in my code or a process? or should I convert my code to program PX4 native stack firmware?
    Thank you too much for any help and discussion.

    Like

  10. Hi,
    I have successfully used position control. I am trying to use velocity control. I am using setpoint_velocity/cmd_vel (geometry_msgs/TwistStamped) . It works for linear x,y,z speeds. However I have problem with yaw. I am setting goalSpeed.twist.angular.z=X . If X is between 0.3 – 0.5 (for anti clockwise) or -0.3 – -0.5 (for clockwise) I can see the UAV is turning however not very fast and not so precise as position controller. If you give 1 to X, UAV starts to turn in one direction then suddenly it starts turn in the other direction. By the way I am using Gazebo to test it. Do you have any idea for yaw velocity control?
    THX

    Liked by 1 person

  11. Hello,

    This is perfect ! thanks for this. I have been trying this for sometime but everytime i try to get vicon_bridge to compile on the odroid u3, it keeps failing. This is the error i keep getting.

    ““
    [ 11%] Built target _vicon_bridge_generate_messages_check_deps_Markers
    [ 11%] Built target _vicon_bridge_generate_messages_check_deps_viconGrabPose
    [ 12%] Built target msvc_bridge
    Linking CXX executable /home/odroid/indigo_ws/devel/lib/vicon_bridge/testclient
    /usr/bin/ld.bfd.real: skipping incompatible /home/odroid/indigo_ws/src/vicon_bridge/vicon_sdk/Vicon_SDK_1.3_Linux/32-bit/libViconDataStreamSDK_CPP.so when searching for -lViconDataStreamSDK_CPP
    /usr/bin/ld.bfd.real: cannot find -lViconDataStreamSDK_CPP
    collect2: error: ld returned 1 exit status
    make[2]: *** [/home/odroid/indigo_ws/devel/lib/vicon_bridge/testclient] Error 1
    make[1]: *** [vicon_bridge/CMakeFiles/testclient.dir/all] Error 2
    make: *** [all] Error 2
    make: INTERNAL: Exiting with 5 jobserver tokens available; should be 4!

    ““

    Could you help me please. im using Lubuntu 14.04 and ROS Indigo.

    Like

  12. Hello,

    This has been a great tutorial and taught me many things, thank you for that. From what I understand, the local_position topic gives us the local position which is measured by the Pixhawk’s on-board sensors (and maybe GPS?). Right now I am working on inputting other data to local_position topic and would like to as you whether if it is possible? Thank you for your help.

    Like

  13. Hello Jaeyoung,

    Good job what you did here. I wanted to know how you could do your position estimator collaborate with Vicon readings. In my case, when I echo /mavros/local_position/pose and /mavros/mocap/tf topics, they have very different values. If you could guide me to solve this, I would appreciate it. Thanks in advance.

    Tarik

    Like

    • I think it is normal to have different values
      a. The local position is the position estimated by PX4 running on the pixhawk board: The origin is where you armed
      b. The mocap/tf topic is coming from the vicon mocap system: The origin is where you have selected it to be origin

      There is an onboard position estimator running in PX4. You can set the weights using the parameters
      CBRK_NO_VISION to 0
      `ATT_EXT_HDG_M to 1 or 2

      Like

  14. Hello, Lim
    Thank you for your nice work.

    May I ask you some questions?
    I want to use visual SLAM to provide local position. And I use the LPE estimator. Right now, I publish the camera pose to “/mavros/vision_pose/pose”, but I have no idea, how to set the Coordinate System of camera pose?

    Thank you in advance!

    Like

  15. Thanks for nice tutorial for using external pose estimation data for controlling UAVs.
    I have a question,
    How can you hookup motion capture system with ROS?
    As I know, Motion capture is running on Windows so I wonder how can hookup that data with ROS
    I found a article for this in ROS wiki, but actually I can’t find how to install and setup that system by following that.

    Regards.

    Like

    • Thank you,

      Motion capture systems usually communicate their motion data through the network. Usually ROS nodes that works for motion capture runs on Ubuntu and accesses the motion capture through the network. Which the operation system doesn’t matter.

      Which motion capture system are you using? If you are using Vicon you can use a package called vicon_bridge

      Like

  16. Hi Jaeyoung Lim , in case i have two drones , so i have to run two mavros nodes at the same time, but two nodes with the same name is not acceptable while we declare mavros node in launch file. Therefore, what i have to edit in mavros package so that I can run mavros node at 2 processors. Thanks.

    Like

  17. Hi Jaeyoung Lim, I could not find any files and topics like this

    Instead of that, I saw px4_config.yaml and “heartbeat” and “time out” inside that file. Is that due to the difference of mavros version?

    Like

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s