Ask Your Question
0

Localizing turtlebot programmatically via initialpose topic

asked 2016-02-19 11:58:28 -0500

ebaumert gravatar image

updated 2016-02-19 11:58:56 -0500

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]
edit retag flag offensive close merge delete

Comments

start this node after amcl node, it will work.

bvbdort gravatar imagebvbdort ( 2016-02-19 12:27:09 -0500 )edit

I noticed a slight mistake as start_pos.pose.covariance[8:34] should be start_pos.pose.covariance[8:35]. But the message published does not change and even with AMCL running...nothing happens. Does running RVIZ maybe prevent the node from working?

ebaumert gravatar imageebaumert ( 2016-02-22 11:43:45 -0500 )edit

have a look at this code , it will help you test set pose

bvbdort gravatar imagebvbdort ( 2016-02-22 12:13:06 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-02-23 07:52:44 -0500

ebaumert gravatar image

What eventually made it work was suggested by michaelyuan1 in the following thread. I have the node sleep for two seconds and republish the position to the initialpose topic. Also I removed the following line start_pos.header.stamp = rospy.Time.now()

Hope it will continue working.

edit flag offensive delete link more

Comments

I wonder if it's important to pay attention to seq? Like, get the current pose, and increment the seq?

rahvee gravatar imagerahvee ( 2018-06-08 08:49:50 -0500 )edit

Even when I increment seq I have the same experience as you: the system doesn't accept the new pose until you wait a little while and publish it a second time. I did as Dan suggested: subscribe to amcl_pose and publish to initialpose in a loop until it works.

rahvee gravatar imagerahvee ( 2018-06-08 15:12:28 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-02-19 11:58:28 -0500

Seen: 1,168 times

Last updated: Feb 23 '16