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
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
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