Service call failed: service [/add_crazyflie] responded with an error: timeout

[ZimingR]

Hello,
When I tried to fly four crazyflies, I ran into the problem of not being able to connect to crazyflies.

Exception thrown while processing service call: timeout
Service call failed: service [/add_crazyflie] responded with an error: timeout

Sorry I'm new to crazyflie and ROS...When I try to run this script on collegue's system, it succeeded. In fact, cfclient shows that I can connect to four crazyflies separately, and search for them with the 'scan' command.

However, when I run roslaunch crazyflie_demo a_multi_position1119.launch, I get the following output:

rzm@rzm:~/crazyflie_ws$ roslaunch crazyflie_demo a_multi_position1119.launch
... logging to /home/rzm/.ros/log/e0fea836-fc07-11eb-a617-b4d5bdf90af1/roslaunch-rzm-8947.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://rzm:34705/

SUMMARY
========

PARAMETERS
 * /crazyflie1/crazyflie_add/enable_logging_battery: False
 * /crazyflie1/crazyflie_add/enable_logging_imu: False
 * /crazyflie1/crazyflie_add/enable_logging_magnetic_field: False
 * /crazyflie1/crazyflie_add/enable_logging_packets: False
 * /crazyflie1/crazyflie_add/enable_logging_pressure: False
 * /crazyflie1/crazyflie_add/enable_logging_temperature: False
 * /crazyflie1/crazyflie_add/genericLogTopicFrequencies: [100, 100, 100]
 * /crazyflie1/crazyflie_add/genericLogTopic_log1acc_Variables: ['acc.x', 'acc.y'...
 * /crazyflie1/crazyflie_add/genericLogTopic_log1euler_Variables: ['stabilizer.roll...
 * /crazyflie1/crazyflie_add/genericLogTopic_log1xyz_Variables: ['stateEstimate.x...
 * /crazyflie1/crazyflie_add/genericLogTopics: ['log1xyz', 'log1...
 * /crazyflie1/crazyflie_add/tf_prefix: crazyflie1
 * /crazyflie1/crazyflie_add/uri: radio://0/80/2M
 * /crazyflie2/crazyflie_add/enable_logging_battery: False
 * /crazyflie2/crazyflie_add/enable_logging_imu: False
 * /crazyflie2/crazyflie_add/enable_logging_magnetic_field: False
 * /crazyflie2/crazyflie_add/enable_logging_packets: False
 * /crazyflie2/crazyflie_add/enable_logging_pressure: False
 * /crazyflie2/crazyflie_add/enable_logging_temperature: False
 * /crazyflie2/crazyflie_add/genericLogTopicFrequencies: [100, 100, 100]
 * /crazyflie2/crazyflie_add/genericLogTopic_log2acc_Variables: ['acc.x', 'acc.y'...
 * /crazyflie2/crazyflie_add/genericLogTopic_log2euler_Variables: ['stabilizer.roll...
 * /crazyflie2/crazyflie_add/genericLogTopic_log2xyz_Variables: ['stateEstimate.x...
 * /crazyflie2/crazyflie_add/genericLogTopics: ['log2xyz', 'log2...
 * /crazyflie2/crazyflie_add/tf_prefix: crazyflie2
 * /crazyflie2/crazyflie_add/uri: radio://0/110/2M
 * /crazyflie3/crazyflie_add/enable_logging_battery: False
 * /crazyflie3/crazyflie_add/enable_logging_imu: False
 * /crazyflie3/crazyflie_add/enable_logging_magnetic_field: False
 * /crazyflie3/crazyflie_add/enable_logging_packets: False
 * /crazyflie3/crazyflie_add/enable_logging_pressure: False
 * /crazyflie3/crazyflie_add/enable_logging_temperature: False
 * /crazyflie3/crazyflie_add/genericLogTopicFrequencies: [100, 100, 100]
 * /crazyflie3/crazyflie_add/genericLogTopic_log3acc_Variables: ['acc.x', 'acc.y'...
 * /crazyflie3/crazyflie_add/genericLogTopic_log3euler_Variables: ['stabilizer.roll...
 * /crazyflie3/crazyflie_add/genericLogTopic_log3xyz_Variables: ['stateEstimate.x...
 * /crazyflie3/crazyflie_add/genericLogTopics: ['log3xyz', 'log3...
 * /crazyflie3/crazyflie_add/tf_prefix: crazyflie3
 * /crazyflie3/crazyflie_add/uri: radio://0/95/2M
 * /crazyflie4/crazyflie_add/enable_logging_battery: False
 * /crazyflie4/crazyflie_add/enable_logging_imu: False
 * /crazyflie4/crazyflie_add/enable_logging_magnetic_field: False
 * /crazyflie4/crazyflie_add/enable_logging_packets: False
 * /crazyflie4/crazyflie_add/enable_logging_pressure: False
 * /crazyflie4/crazyflie_add/enable_logging_temperature: False
 * /crazyflie4/crazyflie_add/genericLogTopicFrequencies: [100, 100, 100]
 * /crazyflie4/crazyflie_add/genericLogTopic_log3acc_Variables: ['acc.x', 'acc.y'...
 * /crazyflie4/crazyflie_add/genericLogTopic_log3euler_Variables: ['stabilizer.roll...
 * /crazyflie4/crazyflie_add/genericLogTopic_log3xyz_Variables: ['stateEstimate.x...
 * /crazyflie4/crazyflie_add/genericLogTopics: ['log4xyz', 'log4...
 * /crazyflie4/crazyflie_add/tf_prefix: crazyflie4
 * /crazyflie4/crazyflie_add/uri: radio://0/120/1M
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    crazyflie_server (crazyflie_driver/crazyflie_server)
    log (rosbag/record)
  /crazyflie1/
    crazyflie_add (crazyflie_driver/crazyflie_add)
    position1 (crazyflie_demo/a_position1.py)
  /crazyflie2/
    crazyflie_add (crazyflie_driver/crazyflie_add)
    position2 (crazyflie_demo/a_position2.py)
  /crazyflie3/
    crazyflie_add (crazyflie_driver/crazyflie_add)
    position3 (crazyflie_demo/a_position3.py)
  /crazyflie4/
    crazyflie_add (crazyflie_driver/crazyflie_add)
    position4 (crazyflie_demo/a_position4.py)

ROS_MASTER_URI=http://localhost:11311

process[crazyflie_server-1]: started with pid [8962]
process[crazyflie1/crazyflie_add-2]: started with pid [8963]
process[crazyflie1/position1-3]: started with pid [8969]
process[crazyflie2/crazyflie_add-4]: started with pid [8975]
[ INFO] [1628839820.480687903]: wait_for_service /add_crazyflie
[ WARN] [1628839820.481489461]: Shutdown request received.
[ WARN] [1628839820.481523876]: Reason given for shutdown: [[/crazyflie_server] Reason: new node registered with same name]
[ INFO] [1628839820.482775042]: found /add_crazyflie
process[crazyflie2/position2-5]: started with pid [8976]
[ INFO] [1628839820.486494123]: Adding radio://0/80/2M as crazyflie1 with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
process[crazyflie3/crazyflie_add-6]: started with pid [8978]
process[crazyflie3/position3-7]: started with pid [8979]
process[crazyflie4/crazyflie_add-8]: started with pid [8989]
[ INFO] [1628839820.503309383]: wait_for_service /add_crazyflie
[ INFO] [1628839820.506601458]: found /add_crazyflie
process[crazyflie4/position4-9]: started with pid [8991]
[ INFO] [1628839820.511153990]: wait_for_service /add_crazyflie
[ INFO] [1628839820.513702399]: found /add_crazyflie
process[log-10]: started with pid [8993]
[ INFO] [1628839820.521334768]: wait_for_service /add_crazyflie
[ INFO] [1628839820.522642542]: found /add_crazyflie
[ INFO] [1628839820.553410082]: Subscribing to /crazyflie1/log1acc
[ INFO] [1628839820.555396960]: Subscribing to /crazyflie1/log1euler
[ INFO] [1628839820.556531274]: Subscribing to /crazyflie1/log1xyz
[ INFO] [1628839820.557899638]: Subscribing to /crazyflie2/log2acc
[ INFO] [1628839820.559061407]: Subscribing to /crazyflie2/log2euler
[ INFO] [1628839820.560240067]: Subscribing to /crazyflie2/log2xyz
[ INFO] [1628839820.561390765]: Subscribing to /crazyflie3/log3acc
[ INFO] [1628839820.562562483]: Subscribing to /crazyflie3/log3euler
[ INFO] [1628839820.563758532]: Subscribing to /crazyflie3/log3xyz
[ INFO] [1628839820.564990871]: Subscribing to /crazyflie4/log3acc
[ INFO] [1628839820.566194657]: Subscribing to /crazyflie4/log3euler
[ INFO] [1628839820.567323694]: Subscribing to /crazyflie4/log3xyz
[ INFO] [1628839820.568715927]: Recording to '2021-08-13-15-30-20.bag'.
[ WARN] [1628839820.572355954]: Shutdown request received.
[ WARN] [1628839820.572406530]: Reason given for shutdown: [[/log] Reason: new node registered with same name]
shutdown request: [/crazyflie2/position2] Reason: new node registered with same name
shutdown request: [/crazyflie3/position3] Reason: new node registered with same name
shutdown request: [/crazyflie1/position1] Reason: new node registered with same name
shutdown request: [/crazyflie4/position4] Reason: new node registered with same name
Traceback (most recent call last):
  File "/home/rzm/crazyflie_ws/src/crazyflie_ros/crazyflie_demo/scripts/a_position1.py", line 42, in <module>
    rospy.wait_for_service('update_params')
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 166, in wait_for_service
    raise ROSInterruptException("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
Traceback (most recent call last):
  File "/home/rzm/crazyflie_ws/src/crazyflie_ros/crazyflie_demo/scripts/a_position3.py", line 44, in <module>
    rospy.wait_for_service('update_params')
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 166, in wait_for_service
    raise ROSInterruptException("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
Traceback (most recent call last):
  File "/home/rzm/crazyflie_ws/src/crazyflie_ros/crazyflie_demo/scripts/a_position2.py", line 42, in <module>
    rospy.wait_for_service('update_params')
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 166, in wait_for_service
    raise ROSInterruptException("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
Traceback (most recent call last):
  File "/home/rzm/crazyflie_ws/src/crazyflie_ros/crazyflie_demo/scripts/a_position4.py", line 39, in <module>
    rospy.wait_for_service('update_params')
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 166, in wait_for_service
    raise ROSInterruptException("rospy shutdown")
rospy.exceptions.ROSInterruptException: rospy shutdown
[ERROR] [1628839821.492665218]: Exception thrown while processing service call: timeout
[ERROR] [1628839821.492837624]: Service call failed: service [/add_crazyflie] responded with an error: timeout
[ INFO] [1628839821.493858043]: Adding radio://0/110/2M as crazyflie2 with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
[crazyflie1/crazyflie_add-2] process has finished cleanly
log file: /home/rzm/.ros/log/e0fea836-fc07-11eb-a617-b4d5bdf90af1/crazyflie1-crazyflie_add-2*.log
[ERROR] [1628839822.494577838]: Exception thrown while processing service call: timeout
[ INFO] [1628839822.494783198]: Adding radio://0/95/2M as crazyflie3 with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
[ERROR] [1628839822.494816687]: Service call failed: service [/add_crazyflie] responded with an error: timeout
[crazyflie2/crazyflie_add-4] process has finished cleanly
log file: /home/rzm/.ros/log/e0fea836-fc07-11eb-a617-b4d5bdf90af1/crazyflie2-crazyflie_add-4*.log
[ERROR] [1628839823.495800261]: Exception thrown while processing service call: timeout
[ INFO] [1628839823.495976730]: Adding radio://0/120/1M as crazyflie4 with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
[ERROR] [1628839823.496001259]: Service call failed: service [/add_crazyflie] responded with an error: timeout
[crazyflie3/crazyflie_add-6] process has finished cleanly
log file: /home/rzm/.ros/log/e0fea836-fc07-11eb-a617-b4d5bdf90af1/crazyflie3-crazyflie_add-6*.log
[ERROR] [1628839824.496974584]: Exception thrown while processing service call: timeout
[ERROR] [1628839824.497117239]: Service call failed: service [/add_crazyflie] responded with an error: timeout
[crazyflie4/crazyflie_add-8] process has finished cleanly
log file: /home/rzm/.ros/log/e0fea836-fc07-11eb-a617-b4d5bdf90af1/crazyflie4-crazyflie_add-8*.log

I was so confused that I tried to be as comprehensive as possible. Below is my .launch file and one of the python scripts：

a_muti_position1119.launch

<?xml version="1.0"?>

<launch>
  <arg name="uri1" default="radio://0/80/2M" />
  <arg name="uri2" default="radio://0/110/2M" />
  <arg name="uri3" default="radio://0/95/2M" />
  <arg name="uri4" default="radio://0/120/1M" />


  <include file="$(find crazyflie_driver)/launch/crazyflie_server.launch"/>

  <group ns="crazyflie1">
    <node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
      <param name="uri" value="$(arg uri1)" />
      <param name="tf_prefix" value="crazyflie1" />

      <param name="enable_logging_imu" value="False" />
      <param name="enable_logging_temperature" value="False" />
      <param name="enable_logging_magnetic_field" value="False" />
      <param name="enable_logging_pressure" value="False" />
      <param name="enable_logging_battery" value="False" />
      <param name="enable_logging_packets" value="False" />

      <rosparam>
        genericLogTopics: ["log1xyz","log1euler","log1acc"]
        genericLogTopicFrequencies: [100,100,100]
        genericLogTopic_log1xyz_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
        genericLogTopic_log1euler_Variables: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
        genericLogTopic_log1acc_Variables: ["acc.x", "acc.y",  "acc.z"]
      </rosparam>
    </node>
    <node name="position1" pkg="crazyflie_demo" type="a_position1.py" output="screen">
    </node>
  </group>
  
  
  <group ns="crazyflie2">
    <node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
      <param name="uri" value="$(arg uri2)" />
      <param name="tf_prefix" value="crazyflie2" />

      <param name="enable_logging_imu" value="False" />
      <param name="enable_logging_temperature" value="False" />
      <param name="enable_logging_magnetic_field" value="False" />
      <param name="enable_logging_pressure" value="False" />
      <param name="enable_logging_battery" value="False" />
      <param name="enable_logging_packets" value="False" />
      <rosparam>
        genericLogTopics: ["log2xyz","log2euler",'log2acc']
        genericLogTopicFrequencies: [100,100,100]
        genericLogTopic_log2xyz_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
        genericLogTopic_log2euler_Variables: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
        genericLogTopic_log2acc_Variables: ["acc.x", "acc.y",  "acc.z"]    
      </rosparam>
      
    </node>
  
    <node name="position2" pkg="crazyflie_demo" type="a_position2.py" output="screen">
    </node>
  </group>
  

  <group ns="crazyflie3">
    <node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
      <param name="uri" value="$(arg uri3)" />
      <param name="tf_prefix" value="crazyflie3" />
      <param name="enable_logging_imu" value="False" />
      <param name="enable_logging_temperature" value="False" />
      <param name="enable_logging_magnetic_field" value="False" />
      <param name="enable_logging_pressure" value="False" />
      <param name="enable_logging_battery" value="False" />
      <param name="enable_logging_packets" value="False" />
      <rosparam>
        genericLogTopics: ["log3xyz","log3euler","log3acc"]
        genericLogTopicFrequencies: [100,100,100]
        genericLogTopic_log3xyz_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
        genericLogTopic_log3euler_Variables: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
        genericLogTopic_log3acc_Variables: ["acc.x", "acc.y",  "acc.z"]
      </rosparam>
      
    </node>
 
    <node name="position3" pkg="crazyflie_demo" type="a_position3.py" output="screen">
    </node>
  </group>


  <group ns="crazyflie4">
    <node pkg="crazyflie_driver" type="crazyflie_add" name="crazyflie_add" output="screen">
      <param name="uri" value="$(arg uri4)" />
      <param name="tf_prefix" value="crazyflie4" />
      <param name="enable_logging_imu" value="False" />
      <param name="enable_logging_temperature" value="False" />
      <param name="enable_logging_magnetic_field" value="False" />
      <param name="enable_logging_pressure" value="False" />
      <param name="enable_logging_battery" value="False" />
      <param name="enable_logging_packets" value="False" />
      <rosparam>
        genericLogTopics: ["log4xyz","log4euler","log4acc"]
        genericLogTopicFrequencies: [100,100,100]
        genericLogTopic_log3xyz_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
        genericLogTopic_log3euler_Variables: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
        genericLogTopic_log3acc_Variables: ["acc.x", "acc.y",  "acc.z"]
      </rosparam>
      
    </node>
 
    <node name="position4" pkg="crazyflie_demo" type="a_position4.py" output="screen">
    </node>
  </group>

  <node pkg="rosbag" type="record" name="log" output="screen" args="/crazyflie1/log1xyz /crazyflie2/log2xyz /crazyflie3/log3xyz /crazyflie4/log3xyz /crazyflie1/log1euler /crazyflie2/log2euler /crazyflie3/log3euler /crazyflie4/log3euler /crazyflie1/log1acc /crazyflie2/log2acc /crazyflie3/log3acc /crazyflie4/log3acc"/>

</launch>

As well as my python scripts (a_position1.py):

#!/usr/bin/env python

import rospy
from crazyflie_driver.msg import Position
from std_msgs.msg import Empty
from crazyflie_driver.srv import UpdateParams
from crazyflie_driver.msg import Hover
import csv
import time
if __name__ == '__main__':
    
    rospy.init_node('position1', anonymous=True)
    rate = rospy.Rate(100)
    worldFrame = rospy.get_param("~worldFrame", "/world")
    
    name = "/crazyflie1/cmd_position"

    msg = Position()
    msg.header.seq = 0
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = worldFrame
    # msg.x = -0.346
    # msg.y = 0.6 + 0.2
    # msg.z = 0.0
    # msg.yaw = 0.0

    pub = rospy.Publisher(name, Position, queue_size=1)

    stop_pub = rospy.Publisher("/crazyflie1/cmd_stop", Empty, queue_size=1)
    stop_msg = Empty()

   # stop_pub2 = rospy.Publisher("cmd_second_stop", Empty, queue_size=1)

    pub2 = rospy.Publisher("/crazyflie1/cmd_hover", Hover, queue_size=1)
    msg2 = Hover()
    msg2.header.seq = 0
    msg2.header.stamp = rospy.Time.now()
    msg2.header.frame_id = worldFrame
    msg2.yawrate = 0


    rospy.wait_for_service('update_params')
    rospy.loginfo("found update_params service")
    update_params = rospy.ServiceProxy('update_params', UpdateParams)

    rospy.set_param("kalman/resetEstimation", 1)
    update_params(["kalman/resetEstimation"])
    time.sleep(0.1)
    rospy.set_param("kalman/resetEstimation", 0)
    update_params(["kalman/resetEstimation"])
    time.sleep(0.5)
   
    # take off
    zDistance = 0.3
    time_range = 1 + int(10*zDistance/0.4)
    while not rospy.is_shutdown():
        for y in range(time_range):
            msg2.vx = 0.0
            msg2.vy = 0.0
            msg2.yawrate = 0.0
            msg2.zDistance = y / 25.0
            msg2.header.seq += 1
            msg2.header.stamp = rospy.Time.now()
            pub2.publish(msg2)
            time.sleep(0.005)
        for y in range(200):
            msg2.vx = 0.0
            msg2.vy = 0.0
            msg2.yawrate = 0.0
            msg2.zDistance = zDistance
            msg2.header.seq += 1
            msg2.header.stamp = rospy.Time.now()
            pub2.publish(msg2)
            time.sleep(0.005)
        break

    rec1 = csv.reader(open('/home/rzm/crazyflie_ws/src/crazyflie_ros/crazyflie_demo/scripts/rectan1.csv', 'r'))
    while not rospy.is_shutdown():
        for step in range(300):
            msg.x,msg.y,msg.z,msg.yaw = -0.5*0.75, 0.866*0.75, 0.3, 0.0
            now = rospy.get_time()
            msg.header.seq += 1
            msg.header.stamp = rospy.Time.now()
            rospy.loginfo("sending...")
            rospy.loginfo(msg.x)
            rospy.loginfo(msg.y)
            rospy.loginfo(msg.z)
            rospy.loginfo(msg.yaw)
            rospy.loginfo(now)
            pub.publish(msg)
            time.sleep(0.005)
        for (xp,yp,zp) in rec1:
            # msg.x = float(xp) - 0.346*1
            # msg.y = float(yp) + 0.2*1
            msg.x = float(xp) 
            msg.y = float(yp) 
            msg.z = float(zp)            
            msg.yaw = 0.0
            
            msg.header.seq += 1
            msg.header.stamp = rospy.Time.now()
            rospy.loginfo("sending...")
            rospy.loginfo(msg.x)
            rospy.loginfo(msg.y)
            rospy.loginfo(msg.z)
            rospy.loginfo(msg.yaw)
          
            pub.publish(msg)
            time.sleep(0.005)
        break
    stop_pub.publish(stop_msg)

Any suggestions to solve this issue would be greatly appreciated. Thank you very much!

[whoenig]

You are currently using crazyflie_ros and not the crazyswarm. Unfortunately, the development of crazyflie_ros has ended in order to focus on the Crazyswarm (see https://github.com/whoenig/crazyflie_ros/). Perhaps you could port your code to use the Crazyswarm instead and open discussions when you run into issues?

[ZimingR]

Thanks for your suggestion and I'll try to use crazyswarm.