Revision history [back]

I think I have solved it at last. The final launch file, ignoring the navigation stack for now, looks like this:

<launch>
    <param name="/use_sim_time" value="true"/>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mypkg)/launch/default.rviz"/>
    <node name="stage" pkg="stage_ros" type="stageros" args="--clock $(find mypkg)/data/map.world"/>

    <node name="slam" pkg="gmapping" type="slam_gmapping" args="scan:=base_scan">
        <param name="transform_publish_period" value="1"/>
    </node>
</launch>

Notice I increased the transform_publish_period to a whole second, which curiously doesn't raise any errors. Also, below is the code I mentioned earlier, which now successfully publishes the estimated pose. Some bits are unnecessary and rough (e.g. do I really need the tf_callback?) but it's a start:

#!/usr/bin/python3
import rospy
from geometry_msgs.msg import *
from nav_msgs.msg import MapMetaData, OccupancyGrid
import tf
from tf.msg import tfMessage

class Explorer(object):
    def __init__(self):
        self.tf = tf.TransformListener()
        self.tf_sub = rospy.Subscriber('/tf', tfMessage, self.tf_callback)
        self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        self.pose_pub = rospy.Publisher('/estimatedpose', PoseStamped, queue_size=1)

    def tf_callback(self, msg):
        pass

    def map_callback(self, msg):
        head = msg.header
        info = msg.info
        data = msg.data

        if self.tf.canTransform('/base_link', '/map', rospy.Time()):
            p,q = self.tf.lookupTransform('/base_link','/map',rospy.Time())
            pose = Pose(Point(*p), Quaternion(*q))
            estp = PoseStamped()
            estp.pose = pose
            estp.header.frame_id = 'map'
            print(estp)
            self.pose_pub.publish(estp)

if __name__ == '__main__':
    rospy.init_node('explore')
    node = Explorer()
    rospy.spin()