Offboard PX4 with T265 and ROS / Does not Take off

Hello Everyone. Dear Community
I know this is not the place to ask but somehow it is related to ROS because I am using MAVROS.
So I will appreciate any suggestion.
I want to share with you guys what I am doing now and what are the issues I am facing.
First of all, this is my setup:

  • Pixhawk4 with PX4 v1.11.3 FMU V5
  • Jeton TX2 with ROS Melodic
  • Mavros, Gazebo, and VIO_bridge from Auterion.

I have followed these instructions from the PX4 developer guide to use the offboard mode with a companion computer running ROS.

These are the instructions:

cd ~/catkin_ws/src
git clone
//build the package
cd ~/catkin_ws/src
catkin build px4_realsense_bridge
//Configure the camera orientation
<node pkg="tf" type="static_transform_publisher" name="tf_baseLink_cameraPose"
    args="0 0 0 0 0 0 base_link camera_pose_frame 1000"/>
//PX4 Tuning in QGroundControl
EKF2_AID_MASK	344    Set vision position fusion, vision velocity fusion, vision yaw fusion and external vision rotation accoring to your desired fusion model.
EKF2_HGT_MODE	Vision Set to Vision to use the vision a primary source for altitude estimation.
EKF2_EV_DELAY	175    Set to the difference between the timestamp of the measurement and the "actual" capture time. For more information see below.
EKF2_EV_POS_X   0.0    Set the position of the vision sensor with respect to the vehicles body frame.
EKF2_EV_POS_Y   0.0    Set the position of the vision sensor with respect to the vehicles body frame.
EKF2_EV_POS_Z   0    Set the position of the vision sensor with respect to the vehicles body frame.

//Running VIO bridge
cd ~/catkin_ws/src
roslaunch px4_realsense_bridge bridge.launch
//running mavros
roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS2:921600" gcs_url:="udp://:14401@"

Until here everything works well.
Then, I read the mavlink inspector and I got the odometry messages including the quaternion and XYZ position. When I move the drone I can see those values changing. So that means everything is ok until here.

The problem comes when I run the script to control the drone. The drone only is arming and is not taking-off. But when I run this code using the gazebo simulator it works fine and the iris drone fly. IN the real world is not working. Here is my code:

#!/usr/bin/env python
This code is to control the position of the drone in the Z axis
Author: @Diego Herrera

import rospy
import mavros
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode
from geometry_msgs.msg import Point

# Callback method for state subscriber
current_state = State()  # Reading the current state from mavros msgs

def state_callback(state):
    global current_state
    current_state = state

local_position_publisher = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped,
                                           queue_size=10)  #
state_subscriber = rospy.Subscriber(mavros.get_topic('state'), State, state_callback)

arming_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), CommandBool)
set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)

pose = PoseStamped()

def coordinates_xyz(data):
    print("Callback function")

    X = data.x
    Y = data.y
    Z = data.z
    print("X value: ", X)
    print("Y value: ", Y)
    print("Z value: ", Z)
    # pose = PoseStamped()
    pose.pose.position.x = X
    pose.pose.position.y = Y
    pose.pose.position.z = Z
    # pose.header.stamp =
    # local_position_publisher.publish(pose)

def position_control():
    print("Position control def")
    rospy.init_node('Offboard_node', anonymous=True)
    prev_state = current_state
    rate = rospy.Rate(20.0)

    # Sending a few points before start
    for i in range(100):

    # We need to wait for FCU connection
    while not current_state.connected:

    last_request = rospy.get_rostime()

    while not rospy.is_shutdown():
        now = rospy.get_rostime()
        if current_state.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5.)):
            set_mode_client(base_mode=0, custom_mode="OFFBOARD")
            last_request = now

            if not current_state.armed and (now - last_request > rospy.Duration(5.)):
                last_request = now

        if current_state.armed:
            print("Drone ready to fly")
        if prev_state.armed != current_state.armed:
            rospy.loginfo("Vehicle armed: %r" % current_state.armed)
        if prev_state.mode != current_state.mode:
            rospy.loginfo("Current mode: %s" % current_state.mode)

        prev_state = current_state
        rospy.Subscriber("SOKA_DRONE", Point, coordinates_xyz)
        pose.header.stamp =

if __name__ == '__main__':
    except rospy.ROSInterruptException:

As I said, this code works fine in gazebo-simulator but running in the real-world is only arming and not takeoff.
I really need help with this. Any suggestions I will appreciate.

From the top of my head, do you have GPS lock? Do all pre-flight tests pass?

You will need to post that on the PX4 development forum. This is the ardupilot development forum. We also speak mavlink, we also use mavros, but we run on a different firmware, and on 74 different hardware boards :slight_smile: including all pixhawk

I would suggest 1) verify the command message is being receive on the real flight controller, 2) verify the taking off function works with the real vehicle.

I wouldn’t assume a command that works for simulation would also work with real vehicle as is. Any command might require additional delay between commands / longer time to be executed.

1 Like

Thank you. I do not have a GPS lock. In QGC I can see the mavlink messages that are being sent by the companion computer(TX2 with T265 real sense) and when I move the drone I can see the X, Y, Z coordinates changing… Also, The drone is arming by the companion computer but is not taking off… Today I will try something different and I will report it…

I will verify if can takeoff from QGrondControl. Because I am receiving all data from the companion computer to the Pixhawk because I can read the values in Mavlink inspector.

@DiegoHerrera1890 if you share the flight log ( I can give you an idea what’s happening. We should probably relocate the discussion to PX4 discuss.

hii I post it on PX4 forum ( but not one is answering…
BTW, this my log
It seems is not changing to OFFBOARD mode

For anyone else that stumbles across this, it’s been fixed in MAVROS.

1 Like