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 import dynamic_reconfigure.client
  10 import numpy as np
  11 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
  12 from actionlib_msgs.msg import GoalStatus
  13 from geodesy import utm
  14 from math import *
  15
  16 class GPSGotoCoords:
  17         def __init__(self):
  18                 rospy.init_node('gps_goto_coords')
  19                 rospy.on_shutdown(self.on_shutdown)
  20
  21                 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  22                 self.tfBuffer = tf2_ros.Buffer()
  23                 self.listener = tf2_ros.TransformListener(self.tfBuffer)
  24
  25                 rospy.loginfo("Setting paramters")
  26                 self.dynreconf = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
  27                 self.dynreconf.update_configuration({'max_vel_x': 1.0, 'max_vel_theta': 1.2, 'min_in_place_vel_theta': 1.0})
  28
  29                 rospy.loginfo("Waiting for the move_base action server to come up")
  30                 self.move_base.wait_for_server()
  31                 rospy.loginfo("Got move_base action server")
  32
  33         def next_pos(self, lat, lon):
  34                 rospy.loginfo("Moving to (%f, %f)" % (lat, lon))
  35
  36                 point = utm.fromLatLong(lat, lon)
  37
  38                 # Get the current position
  39                 pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
  40                 # calculate angle between current position and goal
  41                 angle_to_goal = atan2(point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x)
  42                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle_to_goal)
  43
  44                 goal = MoveBaseGoal()
  45                 goal.target_pose.header.frame_id = "utm"
  46                 goal.target_pose.header.stamp = rospy.Time.now()
  47                 goal.target_pose.pose.position.x = point.easting
  48                 goal.target_pose.pose.position.y = point.northing
  49                 goal.target_pose.pose.orientation.x = odom_quat[0]
  50                 goal.target_pose.pose.orientation.y = odom_quat[1]
  51                 goal.target_pose.pose.orientation.z = odom_quat[2]
  52                 goal.target_pose.pose.orientation.w = odom_quat[3]
  53                 self.move_base.send_goal(goal)
  54
  55                 while not self.move_base.wait_for_result(rospy.Duration(1.0)) and not rospy.is_shutdown():
  56                         # Get the current position
  57                         pos = self.tfBuffer.lookup_transform("utm", 'base_link', rospy.Time(0), rospy.Duration(2.0))
  58                         # Cancel if we are close enough to goal
  59                         if np.linalg.norm([point.northing - pos.transform.translation.y, point.easting - pos.transform.translation.x]) < 1:
  60                                 rospy.loginfo("Goal within 1m, canceling")
  61                                 self.move_base.cancel_goal()
  62                                 return
  63
  64                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
  65                         rospy.loginfo("The base moved to (%f, %f)" % (lat, lon))
  66                 else:
  67                         rospy.logerr("The base failed to (%f, %f). Error: %d" % (lat, lon, self.move_base.get_state()))
  68                         exit(1)
  69
  70         def on_shutdown(self):
  71                 rospy.loginfo("Canceling all goals")
  72                 self.move_base.cancel_all_goals()
  73
  74         def run(self, sCSVFile):
  75                 f = open(sCSVFile)
  76                 for line in f:
  77                         pos = line.split(",")
  78                         self.next_pos(float(pos[0]), float(pos[1]))
  79                         self.move_base.cancel_all_goals()
  80
  81
  82 if __name__ == "__main__":
  83         p = GPSGotoCoords()
  84         p.run(sys.argv[1])