added gps_follow_waypoints script
[ros_wild_thumper.git] / scripts / gps_follow_waypoints.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import sys
5 import rospy
6 import tf
7 import actionlib
8 import tf2_ros
9 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
10 from actionlib_msgs.msg import GoalStatus
11 from geodesy import utm
12 from math import *
13
14 class GPSGotoCoords:
15         def __init__(self):
16                 rospy.init_node('gps_goto_coords')
17                 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
18                 self.tfBuffer = tf2_ros.Buffer()
19                 self.listener = tf2_ros.TransformListener(self.tfBuffer)
20
21                 rospy.loginfo("Waiting for the move_base action server to come up")
22                 self.move_base.wait_for_server()
23                 rospy.loginfo("Got move_base action server")
24
25         def next_pos(self, lat, lon):
26                 rospy.loginfo("Moving to (%f, %f)" % (lat, lon))
27
28                 point = utm.fromLatLong(lat, lon)
29
30                 # Get the current position
31                 pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
32                 # calculate angle between current position and goal
33                 angle_to_goal = atan2(point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x)
34                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle_to_goal)
35
36                 goal = MoveBaseGoal()
37                 goal.target_pose.header.frame_id = "utm"
38                 goal.target_pose.header.stamp = rospy.Time.now()
39                 goal.target_pose.pose.position.x = point.easting
40                 goal.target_pose.pose.position.y = point.northing
41                 goal.target_pose.pose.orientation.x = odom_quat[0]
42                 goal.target_pose.pose.orientation.y = odom_quat[1]
43                 goal.target_pose.pose.orientation.z = odom_quat[2]
44                 goal.target_pose.pose.orientation.w = odom_quat[3]
45                 self.move_base.send_goal(goal)
46
47                 self.move_base.wait_for_result()
48
49                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
50                         rospy.loginfo("The base moved to (%f, %f)" % (lat, lon))
51                 else:
52                         rospy.logerr("The base failed to (%f, %f)" % (lat, lon))
53                         exit(1)
54
55         def run(self, sCSVFile):
56                 f = open(sCSVFile)
57                 for line in f:
58                         pos = line.split(",")
59                         self.next_pos(float(pos[0]), float(pos[1]))
60
61 if __name__ == "__main__":
62         p = GPSGotoCoords()
63         p.run(sys.argv[1])