install(PROGRAMS

    scripts/initial_pos.py

    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

install(DIRECTORY launch/

    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 

<launch>

<node pkg="bp_initialpos" name="initial_pos" type="initial_pos.py" output="screen">

</node>

</launch>

#!/usr/bin/env python

import rospy

import tf

import time

from tf.transformations import *

from std_msgs.msg import String

from geometry_msgs.msg import Pose

from geometry_msgs.msg import Quaternion

filenm="/opt/bp/tmp"

tms=

tp_x=0.0

tp_y=0.0

tp_a=0.0

i=tms

def get_pos(data):

    global tp_x

    global tp_y

    global tp_a

    global i

    global tms

    (roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])

    if(i==):

        rospy.loginfo("current position(x:%f,y:%f),theta:%f", data.position.x, data.position.y, yaw)

        if(tp_x==round(data.position.x,)):

            if(tp_y==round(data.position.y,)):

                if(tp_a==round(yaw,)):

                    rospy.loginfo("still!")

                else:

                    with open(filenm, 'w+') as f:

                        f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))

                    rospy.loginfo("write!")

            else:

                with open(filenm, 'w+') as f:

                    f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))

                rospy.loginfo("write!")

        else:

            with open(filenm, 'w+') as f:

                f.write(str(data.position.x)+","+str(data.position.y)+","+str(yaw))

            rospy.loginfo("write!")

        i=tms

    tp_x=round(data.position.x,)

    tp_y=round(data.position.y,)

    tp_a=round(yaw,)

    i=i-

    #rospy.loginfo("current position(x:%f,y:%f,z:%f)", data.position.x, data.position.y, data.position.z)

def poslistener():

    # In ROS, nodes are uniquely named. If two nodes with the same

    # name are launched, the previous one is kicked off. The

    # anonymous=True flag means that rospy will choose a unique

    # name for our 'listener' node so that multiple listeners can

    # run simultaneously.

    rospy.init_node('poslistener', anonymous=True)

    rospy.Subscriber("robot_pose", Pose, get_pos)

    # spin() simply keeps python from exiting until this node is stopped

    rospy.spin()

if __name__ == '__main__':

    poslistener()