Mavros traffic freezes when I use the SetMode Service Proxy

[canberkgurel]

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:14540@127.0.0.1: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:14540@127.0.0.1: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:14540@127.0.0.1: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:14540@127.0.0.1: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:14540@127.0.0.1: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:14540@127.0.0.1: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

[canberkgurel]

If you need me to run any experiments, let me know!

[emileakbarzadeh]

Hi, has there been any update?

[canberkgurel]

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