[SOLVED] ROS EKF Hover fails

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
toni
Beginner
Posts: 16
Joined: Fri Aug 31, 2018 11:12 am

[SOLVED] ROS EKF Hover fails

Post by toni »

Hello,

Following the instructions for running Hover with LPS in ROS (https://wiki.bitcraze.io/doc:lps:ros) I'm not able to fly my crazyflie. I get the same error as this thread (https://github.com/bitcraze/lps-ros/pull/5), but with the new firmware Release 2018 10, and latest commit of crazyflie_ros, and using ROS Kinetic in Ubuntu 16.04. Does anybody know where this error come from?

Ps The output I get after running roslaunch command is the following:

Code: Select all

toni@stephenson:~/catkin_ws/src/lps-ros/scripts$ roslaunch bitcraze_lps_estimator dwm_loc_ekf_hover.launch uri:=radio://0/100/2M x:=3.54 y:=0.87 z:=1.0
... logging to /home/toni/.ros/log/a152fdbe-e403-11e8-afa2-fcaa14e2dc3d/roslaunch-stephenson-11425.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order
xacro.py is deprecated; please use xacro instead
started roslaunch server http://stephenson:45058/

SUMMARY
========

PARAMETERS
 * /crazyflie/anchor0_pos: [1.72, 4.76, 1.51]
 * /crazyflie/anchor1_pos: [-0.9, 4.29, 1.07]
 * /crazyflie/anchor2_pos: [1.96, 0.88, 1.51]
 * /crazyflie/anchor3_pos: [-1.67, 3.28, 1.78]
 * /crazyflie/anchor4_pos: [2.17, 2.36, 1.7]
 * /crazyflie/anchor5_pos: [0.0, 1.8, 1.51]
 * /crazyflie/crazyflie_add/genericLogTopicFrequencies: [30, 30, 30]
 * /crazyflie/crazyflie_add/genericLogTopic_log_kfpos_Variables: ['kalman.stateX',...
 * /crazyflie/crazyflie_add/genericLogTopic_log_kfqt_Variables: ['kalman.q0', 'ka...
 * /crazyflie/crazyflie_add/genericLogTopic_log_ranges_Variables: ['ranging.distanc...
 * /crazyflie/crazyflie_add/genericLogTopics: ['log_kfpos', 'lo...
 * /crazyflie/crazyflie_add/tf_prefix: crazyflie
 * /crazyflie/crazyflie_add/uri: radio://0/100/2M
 * /crazyflie/joy/dev: /dev/input/js0
 * /crazyflie/joystick_controller/use_crazyflie_controller: True
 * /crazyflie/n_anchors: 6
 * /crazyflie/pose/name: goal
 * /crazyflie/pose/rate: 30
 * /crazyflie/pose/x: 3.54
 * /crazyflie/pose/y: 0.87
 * /crazyflie/pose/z: 1.0
 * /robot_description: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /crazyflie/
    controller_bridge (bitcraze_lps_estimator/crazyflie_controller_bridge.py)
    crazyflie_add (crazyflie_driver/crazyflie_add)
    joy (joy/joy_node)
    joystick_controller (crazyflie_demo/controller.py)
    log_range (bitcraze_lps_estimator/log_range.py)
    lps_efk_bridge (bitcraze_lps_estimator/lps_ekf_bridge.py)
    lps_viz (bitcraze_lps_estimator/lps_viz.py)
    pose (crazyflie_demo/publish_pose_teleop.py)
  /
    crazyflie_server (crazyflie_driver/crazyflie_server)
    link1_broadcaster (tf/static_transform_publisher)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[rviz-1]: started with pid [11445]
process[crazyflie/crazyflie_add-2]: started with pid [11446]
process[crazyflie/joy-3]: started with pid [11447]
process[crazyflie/joystick_controller-4]: started with pid [11454]
process[crazyflie/controller_bridge-5]: started with pid [11462]
process[crazyflie/pose-6]: started with pid [11475]
[ INFO] [1541759104.385996648]: wait_for_service /add_crazyflie
[ INFO] [1541759104.387699399]: waitForService: Service [/add_crazyflie] has not been advertised, waiting...
process[crazyflie/lps_efk_bridge-7]: started with pid [11486]
process[crazyflie/lps_viz-8]: started with pid [11496]
/home/toni/catkin_ws/src/crazyflie_ros/crazyflie_demo/scripts/publish_pose_teleop.py:46: SyntaxWarning: name 'lastData' is assigned to before global declaration
  global lastData
process[crazyflie/log_range-9]: started with pid [11497]
process[link1_broadcaster-10]: started with pid [11499]
process[crazyflie_server-11]: started with pid [11501]
[ INFO] [1541759104.455031672]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1541759104.456465611]: waitForService: Service [/add_crazyflie] is now available.
[ INFO] [1541759104.456540199]: found /add_crazyflie
[ INFO] [1541759104.463595846]: Adding radio://0/100/2M as crazyflie with trim(0.000000, 0.000000). Logging: 1, Parameters: 1, Use ROS time: 1
[ INFO] [1541759104.513113810]: CF Console: SYS: -------------------------
[ INFO] [1541759104.513916542]: CF Console: ---

[ INFO] [1541759104.515404886]: CF Console: SYS: Crazyflie 2.0 is up and r
[ INFO] [1541759104.516320129]: CF Console: unning!

[ INFO] [1541759104.517713701]: CF Console: SYS: Build 0:47ee696b2d3c (201
[ INFO] [1541759104.518786990]: CF Console: 8.10) MODIFIED

[ INFO] [1541759104.520208358]: CF Console: SYS: I am 0x333530303536471100
[ INFO] [1541759104.521620297]: CF Console: 2C0028 and I have 1024KB of fl
[ INFO] [1541759104.522439023]: CF Console: ash!

[ INFO] [1541759104.523856689]: CF Console: CFGBLK: v1, verification [OK]

[ INFO] [1541759104.525300755]: CF Console: DECK_DRIVERS: Found 16 drivers
[ INFO] [1541759104.526041190]: CF Console: 

[ INFO] [1541759104.527460113]: CF Console: DECK_INFO: Found 1 deck memory
[ INFO] [1541759104.528232605]: CF Console: .

[ INFO] [1541759104.529675414]: CF Console: DECK_CORE: 1 deck enumerated

[ INFO] [1541759104.531063189]: CF Console: MPU9250 I2C connection [OK].

[ INFO] [1541759104.532470589]: CF Console: AK8963 I2C connection [OK].

[ INFO] [1541759104.533860529]: CF Console: LPS25H I2C connection [OK].

[ INFO] [1541759104.535284551]: CF Console: ESTIMATOR: Estimator type forc
[ INFO] [1541759104.536073036]: CF Console: ed

[ INFO] [1541759104.537585893]: CF Console: ESTIMATOR: Using estimator 2

[ INFO] [1541759104.538975484]: CF Console: CONTROLLER: Using controller 1
[ INFO] [1541759104.539729328]: CF Console: 

[ INFO] [1541759104.541165642]: CF Console: PLATFORM: Low interference mod
[ INFO] [1541759104.542585962]: CF Console: e. NRF51 TX power offset by -1
[ INFO] [1541759104.543457695]: CF Console: 2db.

[ INFO] [1541759104.544875431]: CF Console: EEPROM: I2C connection [OK].

[ INFO] [1541759104.546228077]: CF Console: AK8963: Self test [OK].

[ INFO] [1541759104.547643160]: CF Console: DECK_CORE: Deck 0 test [OK].

[ INFO] [1541759104.549026326]: CF Console: SYS: Free heap: 11432 bytes

[ INFO] [1541759104.550488759]: CF Console: DWM: Automatic mode: detected 
[ INFO] [1541759104.551306018]: CF Console: TWR

[ INFO] [1541759104.557859940]: Requesting parameters...
[ INFO] [1541759104.565791440]: Found variables in cache.
[ INFO] [1541759104.827556388]: Requesting Logging variables...
[crazyflie/crazyflie_add-2] process has finished cleanly
log file: /home/toni/.ros/log/a152fdbe-e403-11e8-afa2-fcaa14e2dc3d/crazyflie-crazyflie_add-2*.log
[ WARN] [1541759104.838430564]: Don't know ack: Port: 5 Channel: 0 Len: 10
[ INFO] [1541759104.843382794]: Found variables in cache.
[ INFO] [1541759104.936349484]: Requesting memories...
[ INFO] [1541759104.944961847]: Memories: 6
[ INFO] [1541759104.959017898]: Ready...
[ INFO] [1541759104.959079427]: Elapsed: 0.449439 s
[INFO] [1541759104.999005]: found update_params service
[INFO] [1541759104.999453]: waiting for emergency service
[INFO] [1541759105.000837]: found emergency service
[INFO] [1541759105.001265]: waiting for land service
[INFO] [1541759105.002747]: found land service
[INFO] [1541759105.003067]: waiting for takeoff service
[INFO] [1541759105.004343]: found takeoff service
[INFO] [1541759105.116248]: Setting anchor position ...
[INFO] [1541759105.118058]: Anchor 0 at [1.72, 4.76, 1.51]
[ INFO] [1541759105.123963824]: Update parameters
[ERROR] [1541759105.124050215]: Could not find param anchorpos/anchor0x
[ERROR] [1541759105.124077173]: Could not find param anchorpos/anchor0y
[ERROR] [1541759105.124100220]: Could not find param anchorpos/anchor0z
[INFO] [1541759105.125154]: Anchor 1 at [-0.9, 4.29, 1.07]
[ INFO] [1541759105.132674521]: Update parameters
[ERROR] [1541759105.132725783]: Could not find param anchorpos/anchor1x
[ERROR] [1541759105.132752881]: Could not find param anchorpos/anchor1y
[ERROR] [1541759105.132788708]: Could not find param anchorpos/anchor1z
[INFO] [1541759105.133922]: Anchor 2 at [1.96, 0.88, 1.51]
[ INFO] [1541759105.139307431]: Update parameters
[ERROR] [1541759105.139361627]: Could not find param anchorpos/anchor2x
[ERROR] [1541759105.139386559]: Could not find param anchorpos/anchor2y
[ERROR] [1541759105.139409886]: Could not find param anchorpos/anchor2z
[INFO] [1541759105.140514]: Anchor 3 at [-1.67, 3.28, 1.78]
[ INFO] [1541759105.146245609]: Update parameters
[ERROR] [1541759105.146284090]: Could not find param anchorpos/anchor3x
[ERROR] [1541759105.146308604]: Could not find param anchorpos/anchor3y
[ERROR] [1541759105.146331022]: Could not find param anchorpos/anchor3z
[INFO] [1541759105.147529]: Anchor 4 at [2.17, 2.36, 1.7]
[ INFO] [1541759105.151772475]: Update parameters
[ERROR] [1541759105.151812982]: Could not find param anchorpos/anchor4x
[ERROR] [1541759105.151836658]: Could not find param anchorpos/anchor4y
[ERROR] [1541759105.151858308]: Could not find param anchorpos/anchor4z
[INFO] [1541759105.152887]: Anchor 5 at [0.0, 1.8, 1.51]
[ INFO] [1541759105.158669029]: Update parameters
[ERROR] [1541759105.158714424]: Could not find param anchorpos/anchor5x
[ERROR] [1541759105.158736773]: Could not find param anchorpos/anchor5y
[ERROR] [1541759105.158756677]: Could not find param anchorpos/anchor5z
Last edited by toni on Fri Dec 07, 2018 1:18 pm, edited 1 time in total.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: ROS EKF Hover fails

Post by arnaud »

The problem comes from the fact that this doc is very outdated and should not be followed. I am going to look at it to update or remove it.

Nowaday, you should setup the LPS system following the documentation on the bitcraze website, and then you can connect and control the Crazyflie using the standard crazyflie_ros driver without needing to do anything special about the LPS: the Crazyflie receives the anchor position from the anchors over radio, will estimates its position, and has onboard position control loops.

If you are looking at flying a swarm of Crazyflies in ROS, the LPS is supported by Crazyswarm by setting "motion_capture_type: none".
toni
Beginner
Posts: 16
Joined: Fri Aug 31, 2018 11:12 am

Re: ROS EKF Hover fails

Post by toni »

Thaks a lot for your response Arnaud, I have been trying the way you mention and it's working quite well (I was using 6 anchors in TWR, and one Crazyflie).
Right now I would like to do the same using 10 anchors in TWR, but the way the lps-ros package is built it doesn't accept to extend log to have more than 6 anchors (modifying launch log_range and log_range node). Is this caused because of the crazyflie_ros implementation or is this because of crazyflie log package limits in the hardware?
In case is caused by the second, would be possible to create two log blocks and feed them to log_range node from 2 separate topics? Is there another way to get the ranging values into ROS?
toni
Beginner
Posts: 16
Joined: Fri Aug 31, 2018 11:12 am

Re: ROS EKF Hover fails

Post by toni »

Just in case someone wants to do the same, it is the log limit that is causing this. You have to create 2 different logs in order to receive all data needed.
Supdan
Beginner
Posts: 7
Joined: Wed Sep 04, 2019 12:56 pm

Re: [SOLVED] ROS EKF Hover fails

Post by Supdan »

Hi toni,
I have the same problem as you,I was using 6 anchors in TWR, and one Crazyflie too,but when i run the launch dwm_loc_lms.launch and try to get the data in rosnode "/crazyfile/log_range",it shows all 0,just like this(by run "rostopic echo /crazyflie/log_ranges"):

Code: Select all

seq: 199
stamp:
secs: 1567605033
nsecs: 241867868
frame_id: "crazyflie/base_link"
values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
header:
seq: 200
stamp:
secs: 1567605033
nsecs: 336607384
frame_id: "crazyflie/base_link"
values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
header:
seq: 201
stamp:
secs: 1567605033
nsecs: 431375635
frame_id: "crazyflie/base_link"
values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
header:
seq: 202
stamp:
secs: 1567605033
nsecs: 532894534
frame_id: "crazyflie/base_link"
values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
....
do u have any ideas? thanks!
dan.
Supdan
Beginner
Posts: 7
Joined: Wed Sep 04, 2019 12:56 pm

Re: [SOLVED] ROS EKF Hover fails

Post by Supdan »

Hi toni,
I solved the above problem because my anchor uses the tdoa mode, so new problems arise. How do I get tdoa data in ros?

In the LPS documentation, twr mode does not apply to multiple crazyfiles, but unfortunately my system needs to use multiple drones. Do you have any idea to help me get tdoa data in rosnode ?Appreciate it

In addition, when I run dwm_loc_lms.launch, it will report the following error, although I can still get the log_range data.(Is it because I put the anchors too close, in order to test I just put them on my desk?)

Code: Select all

log file: /home/supdan/.ros/log/f650b618-cfb6-11e9-a68c-04d9f5821fc4/crazyflie-crazyflie_add-3*.log
Traceback (most recent call last):
  File "/home/supdan/catkin_ws/src/lps-ros/scripts/lps_viz.py", line 80, in <module>
    anchors_positions = get_anchors_pos()
  File "/home/supdan/catkin_ws/src/lps-ros/scripts/anchorpos.py", line 6, in get_anchors_pos
    n = rospy.get_param("n_anchors")
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/client.py", line 465, in get_param
    return _param_server[param_name] #MasterProxy does all the magic for us
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/msproxy.py", line 123, in __getitem__
    raise KeyError(key)
KeyError: 'n_anchors'
Thanks in advance again!
dan
Post Reply