Multi UAV Simulation with Gazebo not arming

Jaroan · January 1, 2020, 5:30am

I am using the Multi-Vehicle Simulation with Gazebo launch file to control two iris quads using mavros as mentioned in this link: https://dev.px4.io/v1.9.0/en/simulation/multi-vehicle-simulation.html
This is the launch file for UAV0 and UAV1 (https://github.com/PX4/Firmware/blob/master/launch/multi_uav_mavros_sitl.launch). The two quads successfully spawn as shown:

But as soon as my code for arming and to takeoff to a fixed altitude is run, one quad disarms and RTL is activated.

INFO [commander] Takeoff detected
[ INFO] [1577854094.335299051, 36.744000000]: FCU: Takeoff detected
[ WARN] [1577854094.351666413, 36.752000000]: CMD: Unexpected command 176, result 1
[ WARN] [1577854094.359550906, 36.756000000]: CMD: Unexpected command 176, result 0
WARN [commander] Failsafe enabled: no RC and no offboard
INFO [navigator] RTL HOME activated
INFO [navigator] RTL: landing at home position.

Or both quads disarm:

INFO] [1577855245.322935498, 16.592000000]: WP: mission received
[ INFO] [1577855245.485226891, 16.688000000]: WP: mission received
INFO [mavlink] partner IP: 127.0.0.1
INFO [mavlink] partner IP: 127.0.0.1
[ WARN] [1577855415.729473543, 120.628000000]: CMD: Unexpected command 519, result 4
[ WARN] [1577855415.732836438, 120.632000000]: CMD: Unexpected command 519, result 0
[ INFO] [1577855415.945690088, 120.720000000]: FCU: ARMED by Arm/Disarm component command
WARN [mc_pos_control] Offboard activation failed with error: Activation Failed
WARN [mc_pos_control] Position-Ctrl activation failed with error: Activation Failed
WARN [mc_pos_control] Altitude-Ctrl activation failed with error: Activation Failed
WARN [commander] Failsafe enabled: no RC and no offboard
INFO [navigator] RTL HOME activated
INFO [navigator] RTL: landing at home position.
[ERROR] [1577855432.274693169, 127.960000000]: FCU: Failsafe enabled: no RC and no offboard
[ INFO] [1577855432.381533663, 128.012000000]: FCU: Failsafe mode activated
[ INFO] [1577855432.458289029, 128.060000000]: FCU: RTL HOME activated
[ INFO] [1577855432.555041961, 128.112000000]: FCU: RTL: landing at home position.
[ INFO] [1577855438.520489221, 130.724000000]: FCU: DISARMED by Auto disarm initiated
INFO [logger] closed logfile, bytes written: 7441066
INFO [logger] closed logfile, bytes written: 7615973

Here is my code snippet that I run:

h1=raw_input("Enter takeoff Height for quad 1:-")
h0=raw_input("Enter takeoff Height for quad 0:-")

# initiate node for quad 0
rospy.init_node('set_accel_node_0', anonymous=True

# flight mode object
modes_1 = fcuModes_uav1()
modes_0 = fcuModes_uav0()

# controller object for quad 0
con0 = Controller()

# controller object for quad 1
con1 = Controller()

# ROS loop rate
rate = rospy.Rate(20.0)

x1,y1,z1 = 5,5,4 #waypoint1

x0,y0,z0 = 5,5,4 #waypoint2

# Subscribe to drone_0 state
rospy.Subscriber('uav0/mavros/state', State, con0.stateCb)

# Subscribe to drone_0's local position
rospy.Subscriber('uav0/mavros/local_position/pose', PoseStamped, con0.posCb)

# Subscribe to drone_0's velocity
rospy.Subscriber("uav0/mavros/local_position/velocity_local", TwistStamped , con0.velCb)

# Setpoint publisher for quad 0
sp_pub0 = rospy.Publisher('uav0/mavros/setpoint_raw/local', PositionTarget, queue_size=10)

# Velocity publisher for quad 0
sp_vel0 = rospy.Publisher('uav0/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)

# Subscribe to drone_1 state
rospy.Subscriber('uav1/mavros/state', State, con1.stateCb)

# Subscribe to drone_1's local position
rospy.Subscriber('uav1/mavros/local_position/pose', PoseStamped, con1.posCb)

# Subscribe to drone_1's velocity
rospy.Subscriber("uav1/mavros/local_position/velocity_local", TwistStamped , con1.velCb)

# Setpoint publisher for quad 1
sp_pub1 = rospy.Publisher('uav1/mavros/setpoint_raw/local', PositionTarget, queue_size=10)

# Velocity publisher for quad 1
sp_vel1 = rospy.Publisher('uav1/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)

# Make sure the drone 1 is armed
while not con1.state.armed:
    modes_1.setArm()
    rate.sleep()
print "Quad 1 Armed"

# Make sure the drone 0 is armed
while not con0.state.armed:
    modes_0.setArm()
    rate.sleep()
print "Quad 0 Armed"

# Quad 1 We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k=0
while k<10:
    sp_pub1.publish(con1.sp) 
    rate.sleep()
    k = k + 1
print "Few setpoint messages sent for quad 1"

# Quad 0 We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
j=0
while j<10:
    sp_pub0.publish(con0.sp)
    rate.sleep()
    j = j + 1
print "Few setpoint messages sent for quad 0"

# Quad 1 activate OFFBOARD mode
modes_1.setOffboardMode()
print "Offboard is set for Quad 1"

# Quad 0 activate OFFBOARD mode
modes_0.setOffboardMode()
print "Offboard is set for Quad 0"

# Takeoff for Quad 1
while not rospy.is_shutdown():
    con1.goto(0,0,int(h1) ,0)
    
    sp_pub1.publish(con1.sp)
    rate.sleep()

    if con1.dstFlag:
        print "TakeOff Successful for Quad 1"
        break
# Takeoff for Quad 0
while not rospy.is_shutdown():
    con0.goto(0,0,int(h0) ,0)
    
    sp_pub0.publish(con0.sp)
    rate.sleep()

    if con0.dstFlag:
        print "TakeOff Successful for Quad 0"
        break

Here is my Flight Mode Activator class code for UAV1:

class fcuModes_uav1:
    def __init__(self):
        pass

def setTakeoff(self):
	rospy.wait_for_service('uav1/mavros/cmd/takeoff')
	try:
		takeoffService = rospy.ServiceProxy('uav1/mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
		takeoffService(altitude = 3)
	except rospy.ServiceException, e:
		print "Service takeoff call failed: %s"%e

def setArm(self):
    rospy.wait_for_service('uav1/mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('uav1/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(True)
    except rospy.ServiceException, e:
        print "Service arming call failed: %s"%e

def setDisarm(self):
    rospy.wait_for_service('uav1/mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('uav1/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(False)
    except rospy.ServiceException, e:
        print "Service disarming call failed: %s"%e

def setStabilizedMode(self):
    rospy.wait_for_service('uav1/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='STABILIZED')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Stabilized Mode could not be set."%e

def setOffboardMode(self):
    rospy.wait_for_service('uav1/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='OFFBOARD')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Offboard Mode could not be set."%e

def setAltitudeMode(self):
    rospy.wait_for_service('uav1/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='ALTCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Altitude Mode could not be set."%e

def setPositionMode(self):
    rospy.wait_for_service('uav1/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='POSCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Position Mode could not be set."%e

def setAutoLandMode(self):
    rospy.wait_for_service('uav1/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='AUTO.LAND')
    except rospy.ServiceException, e:
           print "service set_mode call failed: %s. Autoland Mode could not be set."%e

When I execute the script, I get the following output:

jasmine@jasprism:~/src/Firmware$ ./myscript.sh
Enter takeoff Height for quad 1:-4
Enter takeoff Height for quad 0:-4
Quad 1 Armed
Quad 0 Armed
Few setpoint messages sent for quad 1
Few setpoint messages sent for quad 0
Offboard is set for Quad 1
Offboard is set for Quad 0

Jaeyoung-Lim · January 1, 2020, 6:10am

Does your offboard script work with a single vehicle? It is auto disarming most likely because it is not receiving any control inputs

Jaroan · January 1, 2020, 6:25am

It works with a single vehicle, the problem arises with both quad.

After re-running the code a few times, I can get both to take off. But again, one of the quad goes to failsafe mode or starts executing as per the script but at a higher altitude, midway between failsafe RTL.

Jaeyoung-Lim · January 1, 2020, 11:25am

It seems like you are not streaming the setpoints together with the node change

Jaroan · January 1, 2020, 1:49pm

Is there a workaround to this situation?

Jaeyoung-Lim · January 1, 2020, 2:15pm

You don’t need a workaround as far as I can see. You just need to make sure you are streaming setpoints at a constant rate.

Your code seems to stop and change node then stream which I think for multiple vehicles makes the setpoints time out and leave offvoard mode and rtl

Jaroan · January 2, 2020, 7:48am

Oh, okay. I’ll check that. Thank you!