Editing /odom message for robot_localization

edit

Hello guys, I have following problem:

I set robot_localization to accept /odom messages from Stage simulator and visualize location in Rviz. As far as I adjust the param <param name="odom0" value="/odom"/> everything goes well (robot moves expectedly, stops when it hits an obstacle - even though the velocity remains constant and I can manipulate robot's position through Stage). /odom is the direct output of Stage.

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true" >

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <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="map"/>

      <param name="transform_time_offset" value="0.0"/>
      <param name="transform_timeout" value="0.0"/>

      <param name="odom0" value="/odom"/>
      <rosparam param="odom0_config">[true,  true,  false,
                                      false,  false,  true,
                                      true,  true,  false,
                                      false,  false,  true,
                                      false,  false,  false]</rosparam>

      <param name="odom0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>

      <param name="print_diagnostics" value="true"/>

      <param name="odom0_queue_size" value="10"/>

      <param name="debug"           value="true"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

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

      <rosparam param="initial_estimate_covariance">[1e-9, 0, ...  

    </node>

Immediately after I substitute /odom for /fake_odom from my node, which subscribes /odom and directly publishes it, I get a strange behavior. The robot barely moves and doesn't stop after hitting the obstacle. Even moving it through Stage is impossible (reaction is visible, but tends to stay at the same place). Sometimes it navigates even in opposite direction. What am I missing, please? I am doing this in purpose to manipulate inputs and test the covariance matrices for robot_localization.

import rospy
from nav_msgs.msg import Odometry

def listener():
    rospy.init_node('fake_odom', anonymous=True)
    rospy.Subscriber("/odom", Odometry, talker_fake_odom)
    rospy.spin()

def talker_fake_odom(data):
    pub = rospy.Publisher('/fake_odom', Odometry, queue_size=10)                       
    pub.publish(data)
    rospy.sleep(0.1)

if __name__ == '__main__':
    try:
        listener()

    except rospy.ROSInterruptException:
        pass

The messages from /odom and /fake_odom are indeed identical. Here's the sample:

korys@korys-ThinkPad:~$ rostopic echo /fake_odom1 -n 1
header: 
  seq: 2225
  stamp: 
    secs: 222
    nsecs: 600000000
  frame_id: odom
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 2.0
      y: 2.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.382683432365
      w: 0.923879532511
  covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [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 ...