From 46045fa28568b7ff40fa23104b0ed70af2b277b6 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sat, 8 Jul 2017 18:20:48 +0200 Subject: [PATCH] added gps_follow_waypoints script --- scripts/gps_follow_waypoints.py | 63 +++++++++++++++++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100755 scripts/gps_follow_waypoints.py 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]) -- 2.39.2