2 # -*- coding: iso-8859-15 -*-
9 import dynamic_reconfigure.client
11 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
12 from actionlib_msgs.msg import GoalStatus
13 from geodesy import utm
18 rospy.init_node('gps_goto_coords')
19 rospy.on_shutdown(self.on_shutdown)
21 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
22 self.tfBuffer = tf2_ros.Buffer()
23 self.listener = tf2_ros.TransformListener(self.tfBuffer)
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})
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")
33 def next_pos(self, lat, lon):
34 rospy.loginfo("Moving to (%f, %f)" % (lat, lon))
36 point = utm.fromLatLong(lat, lon)
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)
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)
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()
64 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
65 rospy.loginfo("The base moved to (%f, %f)" % (lat, lon))
67 rospy.logerr("The base failed to (%f, %f). Error: %d" % (lat, lon, self.move_base.get_state()))
70 def on_shutdown(self):
71 rospy.loginfo("Canceling all goals")
72 self.move_base.cancel_all_goals()
74 def run(self, sCSVFile):
78 self.next_pos(float(pos[0]), float(pos[1]))
79 self.move_base.cancel_all_goals()
82 if __name__ == "__main__":