save_base_map_poses_to_file

#!/usr/bin/env python
import signal
import sys
import argparse
import rospy
import tf


def main(node_name, robot_frame, map_frame, file_name):
    rospy.init_node(node_name)
    tf_listener = tf.TransformListener()
    tf_received = False

    while(not rospy.is_shutdown()):
         # get pose name from user
         pose_name = input("\nPlease enter the pose name: ")

         # get transformation between map and base_link
         while(not tf_received):
             try:
                 tf_listener.waitForTransform(map_frame, robot_frame, rospy.Time.now(), rospy.Duration(1))
                 (trans, rot) = tf_listener.lookupTransform(map_frame, robot_frame, rospy.Time(0))
                 (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rot)

                 pose_description = "%s: [%lf, %lf, %lf]\n" % (pose_name, trans[0], trans[1], yaw)
                 print(pose_description)
                 tf_received = True
             except Exception as e:
                 rospy.logerr("error transforming from '{}' to '{}': {}", robot_frame, map_frame, e)
                 tf_received = False
                                                                                              
         # write position into a file
         if tf_received:
             with open(file_name, 'a') as pose_file:
                pose_file.write(pose_description)
         tf_received = False


if __name__ == "__main__":
    def signal_handler(sig, frame):
        print("Terminating script...")
        sys.exit(0)

    signal.signal(signal.SIGINT, signal_handler)

    parser = argparse.ArgumentParser(description="script to record navigation_goals.yaml")
    parser.add_argument('-n', '--node_name', default='save_base_map_pose_to_file',
                        help='name of node to intiate (default: save_base_map_pose_to_file')
    parser.add_argument('-f', '--file_name', default='navigation_goals.yaml',
                        help='name of file to append goals to (default: navigation_goals.yaml)')
    parser.add_argument('-r', '--robot_frame', default='/base_link',
                        help='frame of robot base (default: /base_link)')
    parser.add_argument('-m', '--map_frame', default='/map',
                        help='static frame, e.g. of map (default: /map)')
    args = parser.parse_args()

    main(args.node_name, args.robot_frame, args.map_frame, args.file_name)