Offboard PX4 with T265 and ROS / Does not Take off

edit

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 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 ...