X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fgps_follow_waypoints.py;h=78eaea5a6715cbc093f172332f4d0ec56c54561f;hp=3ac75a0e523474e5db59b538fb87f0ca051655c7;hb=2f194b3fe82009a77b79021f0c57e6ca55c1706f;hpb=46045fa28568b7ff40fa23104b0ed70af2b277b6

diff --git a/scripts/gps_follow_waypoints.py b/scripts/gps_follow_waypoints.py
index 3ac75a0..78eaea5 100755
--- a/scripts/gps_follow_waypoints.py
+++ b/scripts/gps_follow_waypoints.py
@@ -6,6 +6,8 @@ import rospy
 import tf
 import actionlib
 import tf2_ros
+import dynamic_reconfigure.client
+import numpy as np
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from actionlib_msgs.msg import GoalStatus
 from geodesy import utm
@@ -14,10 +16,16 @@ from math import *
 class GPSGotoCoords:
 	def __init__(self):
 		rospy.init_node('gps_goto_coords')
+		rospy.on_shutdown(self.on_shutdown)
+
 		self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
 		self.tfBuffer = tf2_ros.Buffer()
 		self.listener = tf2_ros.TransformListener(self.tfBuffer)
 
+		rospy.loginfo("Setting paramters")
+		self.dynreconf = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
+		self.dynreconf.update_configuration({'max_vel_x': 1.0, 'max_vel_theta': 1.2, 'min_in_place_vel_theta': 1.0})
+
 		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")
@@ -44,19 +52,32 @@ class GPSGotoCoords:
 		goal.target_pose.pose.orientation.w = odom_quat[3]
 		self.move_base.send_goal(goal)
 
-		self.move_base.wait_for_result()
+		while not self.move_base.wait_for_result(rospy.Duration(1.0)) and not rospy.is_shutdown():
+			# Get the current position
+			pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
+			# Cancel if we are close enough to goal
+			if np.linalg.norm([point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x]) < 1:
+				rospy.loginfo("Goal within 1m, canceling")
+				self.move_base.cancel_goal()
+				return
 
 		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))
+			rospy.logerr("The base failed to (%f, %f). Error: %d" % (lat, lon, self.move_base.get_state()))
 			exit(1)
 
+	def on_shutdown(self):
+		rospy.loginfo("Canceling all goals")
+		self.move_base.cancel_all_goals()
+
 	def run(self, sCSVFile):
 		f = open(sCSVFile)
 		for line in f:
 			pos = line.split(",")
 			self.next_pos(float(pos[0]), float(pos[1]))
+			self.move_base.cancel_all_goals()
+
 
 if __name__ == "__main__":
 	p = GPSGotoCoords()