Simulating robot_localization on pioneer3at with xsens 300 AHRS

edit

I am testing robot_localization using the receive_xsens stack listed here: link text, and the ros-pioneer3at stack listed here: link text.

I set up the frame id's to match those in the pioneer3at simulation stack as follows:

map_frame = Pioneer3AT/map
odom_frame = Pioneer3AT/odom
base_link_frame = Pioneer3AT/base_link
world_frame = Pioneer3AT/odom

I configured xsens imu to produce orientation, angular velocity, and acceleration. The imu0 configuration looks like this:

<param name="imu0" value="/imu/data" />

I will incorporate p3at hardware later with wheel encoders. For now trying to fuse simulated odometry from the ros-pioneer3at stack. Specifically, I have configured the odom0 as follows:

<param name="odom0" value="Pioneer3AT/pose" />

After configuring, I launch the ros-pioneer3at simulation, then launch the receive_xsens hardware, then launch the ekf_localization to fuse the sensor and simulation data.

There is no data being published to odometry/filtered. I am not sure where to start the debugging.

-----edit 1-----

Here is the node graph:

rosgraph.png

-----edit 2-----

Thanks for the insight!

Here is the script for /Pioneer3AT/pose:

import rospy
import tf
from nav_msgs.msg  import Odometry
def subCB(msg):
  P = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z )
  Q = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
  br = tf.TransformBroadcaster()
  br.sendTransform(P, Q, rospy.Time.now(), "/Pioneer3AT/base_link", "world")
if __name__ == '__main__':
  rospy.init_node('tf_from_pose')
  rospy.Subscriber("/Pioneer3AT/pose", Odometry, subCB)
  rospy.spin()

Also, here is the ekf_localization_node launch file I used to try and fuse the xsens imu hardware with the ros-pioneer3at simulation:

<launch>
<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="two_d_mode" value="false"/>

<param name="map_frame" value="/Pioneer3AT/map"/>
<param name="odom_frame" value="/Pioneer3AT/odom"/>
<param name="base_link_frame" value="/Pioneer3AT/base_link"/>
<param name="world_frame" value="/Pioneer3AT/odom"/>

<param name="odom0" value="/Pioneer3AT/pose"/>
<param name="imu0" value="/imu/data"/>

<rosparam param="odom0_config">[true, true, true, 
                                true, true, true, 
                                false,  false, false, 
                                false, false, false,
                                false, false, false]</rosparam>

<rosparam param="imu0_config">[false, false, false, 
                               true,  true,  true, 
                               false, false, false, 
                               true,  true,  true,
                               true,  true,  true]</rosparam>

  <param name="odom0_differential" value="false" />
  <param name="imu0_differential" value="false" />
  <param name="imu0_remove_gravitational_acceleration" value="true"/>


  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="imu0_pose_rejection_threshold" value="0.3"/>
  <param name="imu0_angular_velocity_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/> -->

  <rosparam param="process_noise_covariance">
[0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0 ...