Robotics StackExchange | Archived questions

Editing /odom message for robot_localization

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, 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]
---
korys@korys-ThinkPad:~$ rostopic echo /odom -n 1
header: 
  seq: 2407
  stamp: 
    secs: 240
    nsecs: 800000000
  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, 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]
---

Asked by LukasK on 2017-06-22 20:25:51 UTC

Comments

Including a message sample from "rostopic echo" for both might help.

Asked by Humpelstilzchen on 2017-06-23 00:38:31 UTC

I added the messages. Maybe it will be better for me to make a video...

Asked by LukasK on 2017-06-23 15:25:33 UTC

Answers

I think you will have to change the odom topic in the launch file to :

  <param name="odom0" value="/fake_odom"/>

Else the robot_localization node will still be subscribing to the topic /odom which is not what you want.

Also since you are not fusing the output with a gps , the world_frame parameter should be odom

<param name="world_frame" value="map"/>

Asked by geewhiz on 2017-07-06 12:53:52 UTC

Comments