ekf_localization AR-Drone

asked 2015-09-11 05:12:52 -0600

César gravatar image

updated 2015-09-14 10:29:37 -0600

Hello,

I'm trying to use the ekf_localization node with the AR-Drone and I'm facing some problems. Before using it with the real drone, I'm using a simulator ( https://github.com/dougvk/tum_simulator ), which publishes an /ardrone/imu topic. In order to use odometry data, I've created a node which publishes a Odometry message (nav_msgs) with the frame_id (odom), the child_frame_id (base_link) , the planar velocities vx and vy (obtained from optical flow onboard) and the altitude z. My launch file is:

<launch>

<param name="/use_sim_time" value="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>
  <param name="odom0" value="/ardrone/Odometry"/>
  <param name="imu0" value="/ardrone/imu"/>
  <rosparam param="odom0_config">[false, false,  true,
                                  false, false, false,
                                  true,  true,  false,
                                  false, false, false,
                                  false, false, false]</rosparam>
  <rosparam param="imu0_config">[false, false, false,
                                 true,  true,  true,
                                 false, false, false,
                                 true,  true,  true,
                                 false, false, false]</rosparam>
  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>
  <rosparam param="process_noise_covariance">[...]</rosparam>
  <param name="print_diagnosis" value="true"/>
</node>

</launch>

The filter estimates well the altitude, but the x and y position have huge errors. The drone isn't takeoff and the estimated planar position are growing a lot.

Any help or hint would be appreciated.

Thanks

EDIT: I've recorded a bag file: https://github.com/CesarAsturias/catk...

The drone only takeoff in this file. The topic /ardrone/Odometry is published by the following simple node:

    self.msg = Odometry()
    self.nav_subs = rospy.Subscriber("/ardrone/navdata", Navdata, self.Navdata_callback)
    self.msg_pub = rospy.Publisher('/ardrone/Odometry', Odometry, queue_size=10)
    self.rate = rate
    self.r = rospy.Rate(self.rate)

    while self.msg.header.stamp.secs == "":
        rospy.sleep(0.1)
    rospy.loginfo("Start publishing")

    while not rospy.is_shutdown():
        self.msg_pub.publish(self.msg)
        self.r.sleep()

def Navdata_callback(self, navdata):
    self.msg.header.stamp.secs = navdata.header.stamp.secs
    self.msg.twist.twist.linear.x = navdata.vx / 1000
    self.msg.twist.twist.linear.y = navdata.vy / 1000
    self.msg.pose.pose.position.z = navdata.altd 
    self.msg.header.frame_id = 'odom'
    self.msg.child_frame_id = 'base_link'

I've created this node because the simulator, unlike the ardrone_autonomy package, doesn't publish the odometry message.

I don't know if this is what you want, but here are the messages that the filter takes and the result, in two points of time. Before the quadrotor takeoff:

/ardrone/Odometry:

  • header
    • seq: 19969
    • stamp
    • frame_id: odom
  • child_frame_id: base_link
  • pose
    • pose
      • position
        • x: 0.000000
        • y: 0.000000
        • z: 40.000000
      • orientation
        • x: 0.000000
        • y: 0.000000
        • z: 0.000000
        • w: 0.000000
    • covariance
  • twist
    • twist
      • linear
        • x: -0.001639
        • y: 0.000001
        • z: 0.000000
      • angular
    • covariance

/ardrone/ imu_Raw

  • header
    • seq: 42180
    • stamp
      • secs: 1081
      • nsecs: 546000000
    • frame_id: ardrone_base_link
  • orientation
    • x: -0.000293
    • y: 0.001232
    • z: -0.000024
    • w: 0.999690
  • orientation_covariance
    • [0]: 0.001276 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you please post sample messages?

Tom Moore gravatar imageTom Moore ( 2015-09-11 06:21:36 -0600 )edit

I think there may be something wrong with the bag file:

rosbag info subset.bag 
ERROR reading subset.bag: empty file

Also, it would be helpful if you could post a sample message from every input source.

Tom Moore gravatar imageTom Moore ( 2015-09-13 08:22:52 -0600 )edit

I'm also facing this issue.Did you solve this?

Pramod gravatar imagePramod ( 2019-05-08 00:47:08 -0600 )edit