Gazebo and PR2 simulation setup publishing nan values on base_odometry/odom
Some background: Essentially I have set up pr2 in simulation in an empty world on gazebo.
I would like to set the initial pose of the robot, so I have done the following:
model_state = gazebo.msg.ModelState()
model_state.model_name = 'pr2'
model_state.pose.position.x = map2lab[0]
model_state.pose.position.y = map2lab[1]
rospy.wait_for_service('/gazebo/set_model_state')
try:
sms = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
print 'Model State: ' + str(model_state)
sms(model_state)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
At this point, I'm not sure if it has broken yet, but I then would also like to move the robot around, so I set up the following:
self.pub = rospy.Publisher('/base_controller/command', Twist)
rospy.init_node('locomotionCommand')
Which later I use to publish something like the following (in which v and w are always real numbers):
twist = Twist()
twist.linear.x = v
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = w
self.pub.publish(twist)
So sometime in here, the pr2 jumps and disappears and I get the following error repeated at some high frequency:
[ERROR] [1343312019.823059305, 40.582000000]: TF_NAN_INPUT: Ignoring transform
for child_frame_id "/wheelodom" from authority "default_authority" because of a
nan value in the transform (nan nan nan) (-nan -nan -nan -nan)
I'm fairly certain it is only on /base_odometry/odom, here's a sample:
header:
seq: 5320
stamp:
secs: 58
nsecs: 971000000
frame_id: odom
child_frame_id: ''
pose:
pose:
position:
x: nan
y: nan
z: 0.0
orientation:
x: nan
y: nan
z: nan
w: nan
covariance: [4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00028900000000000003]
twist:
twist:
linear:
x: 2.2798345089
y: 2.02649641037
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.20633020997
covariance: [4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00028900000000000003]
I'm at a loss to figure out what is going on here.
Other important info I forgot to mention: Ubuntu 12.04, Ros Fuerte, all up to date.
I'm also getting this error: Could not transform imu message from torso_lift_link to base_footprint But I'm not sure that is as relevant to my problem? or maybe it is?