Localizing turtlebot programmatically via initialpose topic

edit

Hi!

I am trying to build a node that localizes the turtlebot in my map so I not have to use the RVIZ GUI which is really laggy for me. Thus I did it once, took the values from the initialpose topic and now publish it as suggested in [this older thread].(http://answers.ros.org/question/9686/how-to-programatically-set-the-2d-pose-on-a-map/)

This is what I have - it successfully publishes an exact copy in the correct topic as expected - but the turtlebot does not seem to accept it. Also the location in RVIZ is not updated when running it in parallel to check whether something changed. Maybe someone has an idea why this is? Thank you very much in advance!

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def initial_pos_pub():
    publisher = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
    rospy.init_node('initial_pos_pub', anonymous=True)
    #Creating the message with the type PoseWithCovarianceStamped
    rospy.loginfo("This node sets the turtlebot's position to the red cross on the floor. It will shudown after publishing to the topic /initialpose")
    start_pos = PoseWithCovarianceStamped()
    #filling header with relevant information
    start_pos.header.frame_id = "map"
    start_pos.header.stamp = rospy.Time.now()
    #filling payload with relevant information gathered from subscribing
    # to initialpose topic published by RVIZ via rostopic echo initialpose
    start_pos.pose.pose.position.x = -0.846684932709
    start_pos.pose.pose.position.y = 0.333061099052
    start_pos.pose.pose.position.z = 0.0

    start_pos.pose.pose.orientation.x = 0.0
    start_pos.pose.pose.orientation.y = 0.0
    start_pos.pose.pose.orientation.z = -0.694837665627
    start_pos.pose.pose.orientation.w = 0.719166613815

    start_pos.pose.covariance[0] = 0.25
    start_pos.pose.covariance[7] = 0.25
    start_pos.pose.covariance[1:7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[8:34] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[35] = 0.06853891945200942

    rospy.loginfo(start_pos)
    publisher.publish(start_pos)

if __name__ == '__main__':
    try:
        initial_pos_pub()
    except rospy.ROSInterruptException:
        pass

And the published message reads as follows:

header: 
  seq: 0
  stamp: 
    secs: 1455903671
    nsecs: 170355081
  frame_id: map
pose: 
  pose: 
    position: 
      x: -0.846684932709
      y: 0.333061099052
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.694837665627
      w: 0.719166613815
  covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]