From: Erik Andresen <erik@vontaene.de>
Date: Sat, 8 Jul 2017 16:20:48 +0000 (+0200)
Subject: added gps_follow_waypoints script
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=46045fa28568b7ff40fa23104b0ed70af2b277b6;p=ros_wild_thumper.git

added gps_follow_waypoints script
---

diff --git a/scripts/gps_follow_waypoints.py b/scripts/gps_follow_waypoints.py
new file mode 100755
index 0000000..3ac75a0
--- /dev/null
+++ b/scripts/gps_follow_waypoints.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import sys
+import rospy
+import tf
+import actionlib
+import tf2_ros
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from actionlib_msgs.msg import GoalStatus
+from geodesy import utm
+from math import *
+
+class GPSGotoCoords:
+	def __init__(self):
+		rospy.init_node('gps_goto_coords')
+		self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+		self.tfBuffer = tf2_ros.Buffer()
+		self.listener = tf2_ros.TransformListener(self.tfBuffer)
+
+		rospy.loginfo("Waiting for the move_base action server to come up")
+		self.move_base.wait_for_server()
+		rospy.loginfo("Got move_base action server")
+
+	def next_pos(self, lat, lon):
+		rospy.loginfo("Moving to (%f, %f)" % (lat, lon))
+
+		point = utm.fromLatLong(lat, lon)
+
+		# Get the current position
+		pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
+		# calculate angle between current position and goal
+		angle_to_goal = atan2(point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x)
+		odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle_to_goal)
+
+		goal = MoveBaseGoal()
+		goal.target_pose.header.frame_id = "utm"
+		goal.target_pose.header.stamp = rospy.Time.now()
+		goal.target_pose.pose.position.x = point.easting
+		goal.target_pose.pose.position.y = point.northing
+		goal.target_pose.pose.orientation.x = odom_quat[0]
+		goal.target_pose.pose.orientation.y = odom_quat[1]
+		goal.target_pose.pose.orientation.z = odom_quat[2]
+		goal.target_pose.pose.orientation.w = odom_quat[3]
+		self.move_base.send_goal(goal)
+
+		self.move_base.wait_for_result()
+
+		if self.move_base.get_state() == GoalStatus.SUCCEEDED:
+			rospy.loginfo("The base moved to (%f, %f)" % (lat, lon))
+		else:
+			rospy.logerr("The base failed to (%f, %f)" % (lat, lon))
+			exit(1)
+
+	def run(self, sCSVFile):
+		f = open(sCSVFile)
+		for line in f:
+			pos = line.split(",")
+			self.next_pos(float(pos[0]), float(pos[1]))
+
+if __name__ == "__main__":
+	p = GPSGotoCoords()
+	p.run(sys.argv[1])