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, y
])
error = np.linalg.norm(current_position - current_waypoint)
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 = ExtendedCommand()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
command_msg.z = next_waypoint[3]
command_msg.mode = ExtendedCommand.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
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,
y])
error = np.linalg.norm(current_position - current_waypoint)
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 = ExtendedCommand()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
command_msg.z = next_waypoint[3]
command_msg.mode = ExtendedCommand.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
def __init__(self):
# get parameters
# 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.waypoint_filename = rospy.get_param('~waypoint_filename', "/home/jarvis/roscopter/src/ros_copter/params/waypoints.csv")
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('waypoint', ExtendedCommand, queue_size=5, latch=True)
self.waypoint_list = []
with open(self.waypoint_filename, 'rb') as csvfile:
dialect = csv.Sniffer().sniff(csvfile.read(10))
csvfile.seek(0)
reader = csv.reader(csvfile, dialect)
for row in reader:
data = map(float, row)
# Wrap yaw from -PI to PI
while data[3] <= -3.141593:
data[3] += 2*3.141593
while data[3] > 3.141503:
data[3] -= 2*3.141593
print(data)
self.waypoint_list.append(data)
self.current_waypoint_index = 0
command_msg = ExtendedCommand()
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]
command_msg.z = current_waypoint[3]
command_msg.mode = ExtendedCommand.MODE_XPOS_YPOS_YAW_ALTITUDE
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 __init__(self):
# get parameters
# 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.waypoint_filename = rospy.get_param('~waypoint_filename',
"waypoints.csv")
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)
print("1")
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('shredder/ground_truth/odometry',
Odometry,
self.odometryCallback,
queue_size=5)
self.waypoint_pub_ = rospy.Publisher('waypoint',
ExtendedCommand,
queue_size=5,
latch=True)
# Start Up Waypoint List
self.waypoint_list = []
print(self.waypoint_filename)
if self.waypoint_filename:
file = csv.reader(open(self.waypoint_filename, 'r'))
print("Waypoints file")
for row in file:
data = map(float, row)
# Wrap yaw from -PI to PI
while data[3] <= -3.141593:
data[3] += 2 * 3.141593
while data[3] > 3.141503:
data[3] -= 2 * 3.141593
print(data)
self.waypoint_list.append(data)
self.current_waypoint_index = 0
command_msg = ExtendedCommand()
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]
command_msg.z = current_waypoint[3]
command_msg.mode = ExtendedCommand.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
print("spinning")
rospy.spin()
