mavlink
Mavros traffic freezes when I use the SetMode Service Proxy
Issue details
I'm using the mavros/set_mode ROS service to set the mode of the drone. This is how I'm using it:
In init
service_timeout = 30
rospy.wait_for_service('mavros/set_mode', service_timeout)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
In set_mode function:
def set_mode(self, mode, timeout):
"""mode: PX4 mode string, timeout(int): seconds"""
rospy.loginfo("setting FCU mode: {0}".format(mode))
old_mode = self.state.mode
loop_freq = 2 # Hz
rate = rospy.Rate(loop_freq)
mode_set = False
for i in xrange(timeout * loop_freq):
if self.state.mode == mode:
mode_set = True
rospy.loginfo("set mode success | seconds: {0} of {1}".format(
i / loop_freq, timeout))
break
else:
try:
rospy.loginfo("[set_mode] : waiting for set_mode service.")
rospy.wait_for_service(service='mavros/set_mode', timeout=5.0)
rospy.loginfo("[set_mode] : set_mode service is available.")
res = self.set_mode_srv(base_mode=0, custom_mode=mode)
if not res.mode_sent:
rospy.logerr("failed to send mode command")
except rospy.ServiceException as e:
rospy.logerr(e)
except rospy.ROSException as e:
rospy.logerr("[LOGIC][set_mode][ROSException] : {}".format(e))
rate.sleep()
if(not mode_set):
rospy.logwarn("[set_mode] : failed to set mode. new mode: {0}, old mode: {1}, timeout(sec): {2}".format(mode, old_mode, timeout))
return mode_set
This is what is printed in the terminal. After this mavros freezes up, for example, nothing is published over mavros/local_position/pose topic.
[INFO] [1637270303.405313, 125.740000]: [set_mode] : setting FCU mode: OFFBOARD [INFO] [1637270303.409133, 125.744000]: [set_mode] : waiting for set_mode service. [INFO] [1637270303.414774, 125.748000]: [set_mode] : set_mode service is available. [ WARN] [1637270303.435382839, 125.772000000]: CMD: Unexpected command 176, result 0
If we compare with the print statements in the function, the next line of code after the last is:
res = self.set_mode_srv(base_mode=0, custom_mode=mode)
For some reason not known to me, mavros SetMode service proxy is causing mavros to freeze up. All of the data traffic freezes, the drone doesn't switch to OFFBOARD, it doesn't ARM, it stops responding to all commands. However, the weird part is it only happens occasionally, i.e., if I restart the PX4-SITL simulation, and reattempt to set mode, the drone sometimes switches to the OFFBOARD mode, arms, and takes off successfully. Before you ask, Yes, I'm publishing setpoints to the drone at ~20 Hz. For the sake of completeness, when everything works, this is what I see in the terminal output.
[INFO] [1637338061.027372, 1194.308000]: [set_mode] : setting FCU mode: OFFBOARD
[INFO] [1637338061.029514, 1194.308000]: [set_mode] : waiting for set_mode service.
[INFO] [1637338061.036751, 1194.316000]: [set_mode] : set_mode service is available.
[ WARN] [1637338061.058234206, 1194.336000000]: CMD: Unexpected command 176, result 0
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
[INFO] [1637338061.627722, 1194.808000]: [set_mode] : loop: 0 seconds: 0 of 5
[INFO] [1637338061.631194, 1194.812000]: [set_mode] : waiting for set_mode service.
[INFO] [1637338061.642668, 1194.820000]: [set_mode] : set_mode service is available.
[INFO] [1637338061.836344, 1195.012000]: mode changed from MANUAL to OFFBOARD
[INFO] [1637338062.127522, 1195.308000]: [set_mode] : loop: 1 seconds: 0 of 5
[INFO] [1637338062.130300, 1195.308000]: [set_mode] : set mode success. seconds: 1 of 5
MAVROS version and platform
Mavros: HEAD detached at 1.5.2 - I have some modifications in mavros/launch/px4_config.yaml, if this is relevant, let me know and I'll upload it. ROS: Melodic Ubuntu: 18.04.6 LTS
Autopilot type and version
[ ] ArduPilot [x ] PX4
Version: HEAD detached at v1.11.3
Node logs
copy output of mavros_node. Usually console where you run roslaunch
Diagnostics
$rostopic echo -n1 /diagnostics
header:
seq: 671
stamp:
secs: 495
nsecs: 96000000
frame_id: ''
status:
-
level: 0
name: "pix1/mavros: FCU connection"
message: "connected"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Received packets:"
value: "51543"
-
key: "Dropped packets:"
value: "0"
-
key: "Buffer overruns:"
value: "0"
-
key: "Parse errors:"
value: "0"
-
key: "Rx sequence number:"
value: "86"
-
key: "Tx sequence number:"
value: "0"
-
key: "Rx total bytes:"
value: "7197943"
-
key: "Tx total bytes:"
value: "3015377"
-
key: "Rx speed:"
value: "inf"
-
key: "Tx speed:"
value: "inf"
-
level: 0
name: "pix1/mavros: GPS"
message: "3D fix"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Satellites visible"
value: "10"
-
key: "Fix type"
value: "3"
-
key: "EPH (m)"
value: "0.00"
-
key: "EPV (m)"
value: "0.00"
-
level: 0
name: "pix1/mavros: Heartbeat"
message: "Normal"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Heartbeats since startup"
value: "333"
-
key: "Frequency (Hz)"
value: "1.000000"
-
key: "Vehicle type"
value: "Quadrotor"
-
key: "Autopilot type"
value: "PX4 Autopilot"
-
key: "Mode"
value: "MANUAL"
-
key: "System status"
value: "Standby"
-
level: 2
name: "pix1/mavros: System"
message: "Sensor health"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Sensor present"
value: "0x3020002F"
-
key: "Sensor enabled"
value: "0x1021002F"
-
key: "Sensor health"
value: "0x1020000F"
-
key: "3D gyro"
value: "Ok"
-
key: "3D accelerometer"
value: "Ok"
-
key: "3D magnetometer"
value: "Ok"
-
key: "absolute pressure"
value: "Ok"
-
key: "GPS"
value: "Fail"
-
key: "rc receiver"
value: "Fail"
-
key: "AHRS subsystem health"
value: "Ok"
-
key: "CPU Load (%)"
value: "0.0"
-
key: "Drop rate (%)"
value: "0.0"
-
key: "Errors comm"
value: "0"
-
key: "Errors count #1"
value: "0"
-
key: "Errors count #2"
value: "0"
-
key: "Errors count #3"
value: "0"
-
key: "Errors count #4"
value: "0"
-
level: 0
name: "pix1/mavros: Battery"
message: "Normal"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Voltage"
value: "16.20"
-
key: "Current"
value: "-1.0"
-
key: "Remaining"
value: "100.0"
-
level: 0
name: "pix1/mavros: Time Sync"
message: "Normal"
hardware_id: "udp://0.0.0.0:[email protected]:14580"
values:
-
key: "Timesyncs since startup"
value: "3325"
-
key: "Frequency (Hz)"
value: "10.000000"
-
key: "Last RTT (ms)"
value: "0.000000"
-
key: "Mean RTT (ms)"
value: "0.113082"
-
key: "Last remote time (s)"
value: "359.188000000"
-
key: "Estimated time offset (s)"
value: "135.867985369"
---
Check ID
$ rosrun mavros checkid
WARNING: mavros/target_system_id not set. Used default value: 1
WARNING: mavros/target_component_id not set. Used default value: 1
NOTE: Target parameters may be unset, but that may be result of incorrect --mavros-ns option.Double check results!
ERROR. I got 0 addresses, but not your target 1:1
---
Received 0 messages, from 0 addresses
sys:comp list of messages
@emileakbarzadeh No, I still experience the same problem. Every once in a while (about 30 % of the time) mavros topics stop publishing when I try to take off using the SetMode service proxy.