[SOLVED] ROS EKF Hover fails

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

[SOLVED] ROS EKF Hover fails

Post by toni » Tue Nov 13, 2018 4:10 pm

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: 1827
Joined: Tue Feb 06, 2007 12:36 pm

Re: ROS EKF Hover fails

Post by arnaud » Thu Nov 15, 2018 11:41 am

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: 13
Joined: Fri Aug 31, 2018 11:12 am

Re: ROS EKF Hover fails

Post by toni » Wed Nov 21, 2018 2:57 pm

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: 13
Joined: Fri Aug 31, 2018 11:12 am

Re: ROS EKF Hover fails

Post by toni » Mon Dec 03, 2018 3:43 pm

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.

Post Reply