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
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.
including all pixhawk