Jan, > I am attempting to use robot_pose_ekf to combine the measurements from > my IMU with the odometry of the robot. For this purpose I have > configured robot_pose_ekf to not use visual_odometry input. Running the > launch file, every topic is correctly connected. However, the ekf filter > repeatedly prints: > >   Odom sensor activated >   odom sensor not active any more This message is shown when the robot pose ekf did not receive any odometry messages for 1 second (you can configure this timeout using the 'sensor_timeout' parameters). When a sensor stops publishing data for more than 1 second, the robot pose ekf will automatically stop expecting data from that source, until the sensor starts publishing again. You can also get this message when the main thread of the robot pose ekf gets locked up trying to transform tf data. Judging from the debug message below, this might have been the case. >   [DEBUG] 1275662646.245471000: Could not transform imu message from > /xsens to base_footprint. Imu will not be activated yet. The robot pose ekf tries to transform all incoming data into the base_footprint frame. Since you were missing this frame in your tf tree, the robot pose ekf gets stuck here for 0.5 seconds trying to query tf. This explains the problem above. > As my tf-tree is set up according to the navigation-stack tutorial my > robots coordinate frame is called "base_link", instead of > "base_footprint". In order to set up a connection to my robots frame, I > have added this static transform: > >   args="0 0 0 0 0 0 /base_link /base_footprint 20" machine="mach"/> > > I have also noticed two additional frame_ids that are not connected to > the rest of the tf-tree, namely "odom_combined" and "odomwheel". Tf only supports tree structures, meaning that each link in tf should have exactly one parent. When you publish the static transform between base_link and base_footprint, you specify that base_link is the parent of base_footprint. But the robot pose ekf publishes the transform between odom_combined and base_footprint, with odom_combined the parent of base_footprint. So in the end base_footprint has two parents. You can solve this by switching base_link and base_footprint around in your static transform publisher. I've never seen the odomwheel link in the tf tree. Who is publishing this link? tf_monitor should be able to tell you. > From this I gather that what I have done is not the proper way to solve the > problem. But what is the proper way to solve it? Is there a way to remap > frame_ids similar to remapping topics? Or is this not the cause of my > problem at all? So the only problem you have is the static transform between base_link and base_footprint, everything else looks fine. > With this the robot_pose_ekf now publishes fused pose estimates. Now > this warning pops up from time to time: > >    [ WARN] 1275665674.294791000: Robot pose ekf diagnostics discovered > a potential problem. The robot pose ekf is configured to check for large inconsistencies between incoming sensor data. The thresholds for these checks are configured for the pr2 robot, so you can safely ignore these warnings on other platforms. As you've probably noticed, the robot pose ekf was initially designed for the pr2, and then we tried to make it more generic. But it really needs a rewrite to make it work smoothly on different robots. I'm planning a complete rewrite in the next months. Wim -- -- Wim Meeussen Willow Garage Inc.