Offboard PX4 with T265 and ROS / Does not Take off

Hello Everyone. Dear Community
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 https://github.com/Auterion/VIO.git
//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@127.0.0.1:14550"

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
email: alberto18_90@outlook.com
'''

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


mavros.set_namespace()
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)
    print("")
    # pose = PoseStamped()
    pose.pose.position.x = X
    pose.pose.position.y = Y
    pose.pose.position.z = Z
    # pose.header.stamp = rospy.Time.now()
    # 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):
        local_position_publisher.publish(pose)
        rate.sleep()

    # We need to wait for FCU connection
    while not current_state.connected:
        rate.sleep()

    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

        else:
            if not current_state.armed and (now - last_request > rospy.Duration(5.)):
                arming_client(True)
                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 = rospy.Time.now()
        local_position_publisher.publish(pose)
        rate.sleep()


if __name__ == '__main__':
    try:
        position_control()
    except rospy.ROSInterruptException:
        pass

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.

It doesn’t seem to like the frame (MAV_FRAME_GLOBAL=0) set in your odometry data.

Are you able to set the frame to MAV_FRAME_BODY_FRD?

In PX4 1.11 the odometry message handler is checking the estimator_type field of the ODOMETRY msg. The current stable MAVROS doesn’t fill that field, sending the 0 you see in the “Estimator source 0…”.

There are multiple options to get arround this:

  1. Downgrade to PX4 1.10
  2. Compile the master version of MAVROS instead of using the latest stable (this problem was fixed a few days ago here: https://github.com/mavlink/mavros/pull/1525) so that it sends the correct value for the estimator_type field
  3. Modify the PX4 mavlink_receiver/handle_message_odometry function so that it doesn’t check the estimator_source

Chose the one that fits you better

Thanks @antonio-sc66, I wasn’t aware of the MAVROS situation, here’s a fix for PX4 that will be included in the imminent v1.12 release. https://github.com/PX4/PX4-Autopilot/pull/16774

1 Like