location_memorizer.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('location_memorizer')
import rospy
import RFNServer
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from location_memorizer.srv import GetLocation, GetLocationResponse

pose = False # PoseStamped
locations = dict() # key: string ; element: PoseStamped
server = False # RFNServer


def cb_pose(msg):
    global pose
    # Make msg a PoseStamped
    pose = PoseStamped()
    pose.header.frame_id = msg.goal.target_pose.header.frame_id
    pose.pose = msg.goal.target_pose.pose.pose


def cb_get_location(srv_req):
    global locations, pose
    name = srv_req.name
    srv_resp = GetLocationResponse()
    if name in locations:
        loc = locations[name]
        srv_resp.success = True
        srv_resp.pose.header.frame_id = loc.header.frame_ide
        srv_resp.pose = loc.pose
    else:
        srv_resp.success = False
        # Do the least harm by returning a point close to where the user is.
        srv_resp.pose.header.frame_id = pose.header.frame_id
        srv_resp.pose = pose.pose
    return srv_resp


def cb_remember(filled_semantic_frame):
    global server, pose, locations
    if not pose:
        server.set_aborted()
        return
    name = RFNServer.frame_argument("location_name")
    if name in locations:
        rospy.logwarn("The location '" + name + "' already existed.  Overwriting.")
    locations[name] = pose
    rospy.loginfo("Storing pose for location '" + name + "':\n" + pose)
    server.set_succeeded()



def main():
    global server
    rospy.init_node('location_memorizer')
    rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, cb_pose)
    rospy.Service('get_location', GetLocation, cb_get_location)

    server = RFNServer("location_memorizer_rfn", cb_remember)
    server.register_with_frame("remembering_location")

    rospy.spin()


if __name__ == '__main__':
    main()