def __init__(self):
# get parameters
try:
self.waypoint_list = rospy.get_param('~waypoints')
except KeyError:
rospy.logfatal('waypoints not set')
rospy.signal_shutdown('Parameters not set')
# how close does the MAV need to get before going to the next waypoint?
self.threshold = rospy.get_param('~threshold', 5)
self.cyclical_path = rospy.get_param('~cycle', True)
self.prev_time = rospy.Time.now()
# set up Services
self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint,
self.addWaypointCallback)
self.remove_waypoint_service = rospy.Service('remove_waypoint',
RemoveWaypoint,
self.addWaypointCallback)
self.set_waypoint_from_file_service = rospy.Service(
'set_waypoints_from_file', SetWaypointsFromFile,
self.addWaypointCallback)
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state',
Odometry,
self.odometryCallback,
queue_size=5)
self.waypoint_pub_ = rospy.Publisher('high_level_command',
Command,
queue_size=5,
latch=True)
self.current_waypoint_index = 0
command_msg = Command()
current_waypoint = np.array(self.waypoint_list[0])
command_msg.x = current_waypoint[0]
command_msg.y = current_waypoint[1]
command_msg.F = current_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
next_point = self.waypoint_list[(self.current_waypoint_index + 1) %
len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = math.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
command_msg.header = Header()
command_msg.header.stamp = rospy.Time.now()
self.waypoint_pub_.publish(command_msg)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
def odometryCallback(self, msg):
# Get error between waypoint and current state
current_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
(r, p, y) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
current_position = np.array([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
])
error = np.linalg.norm(current_position - current_waypoint[0:3])
if error < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
if self.current_waypoint_index > len(self.waypoint_list):
self.current_waypoint_index -= 1
next_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
next_point = self.waypoint_list[(self.current_waypoint_index +
1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = math.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
command_msg.header = Header()
command_msg.header.stamp = rospy.Time.now()
self.waypoint_pub_.publish(command_msg)
