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://:email@example.com: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: firstname.lastname@example.org ''' 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.