def to_AzAlt(self, latitude, *args, **kwargs):
"""
Converts this HADec into an AzAlt object provided that latitude is given
@param Latitude: A latitude in decimal degrees or as a SmartLat instance
@return: AzAlt object for that observer
For Converting HA,Dec > Az,Alt:
x = Declination in radians (-pi to pi)
y = Latitude in radians (-pi to pi)
z = HourAngle in radians (0 to 2pi), (which requires longitude if converting from RA)
> xt = Altitude in radians
> yt = Azimuth in radians
@param latitude: Observer's latitude in RADIANS
@return HA in Radians, Dec in Radians
"""
if not isinstance(latitude, SmartLat):
latitude = SmartLat(latitude)
lat_rad = latitude.radians
ha_rad = self.X.radians
dec_rad = self.Y.radians
alt_rad, az_rad = coord_rotate_rad(dec_rad, lat_rad, ha_rad) #Convert to az + alt (coord rotate)
#Cast back to Deg for conversion to an AzAlt object
az = rad_to_deg(az_rad)
alt = rad_to_deg(alt_rad)
return AzAlt(az,alt)
def compute_statistics(err,
verbose=False,
variable='Translational',
use_deg=True):
"""
Computes the mean, RMSE, standard deviation, median, min and max from a vector of errors
@param err: a MxN np array.
M is the number of components by sample (M=3 if SO(3), M=6 if SE(3)). N is the number of samples.
"""
stats = {}
abs_err = np.fabs(err)
# RMSE
stats['rmse'] = np.sqrt(np.dot(abs_err, abs_err) / len(abs_err))
# Mean
stats['mean'] = np.mean(abs_err) # computed by column
# Standard Deviation
stats['std'] = np.std(abs_err) # computed by column
# Median
stats['median'] = np.median(abs_err) # computed by column
# Min
stats['min'] = np.min(np.fabs(abs_err)) # computed by column
# Max
stats['max'] = np.max(abs_err) # computed by column
if verbose:
for key in stats:
if variable == 'Rotational':
if use_deg:
print(' %s %s: %f deg' %
(variable, key, utils.rad_to_deg(stats[key])))
else:
print(' %s %s: %f rad' % (variable, key, stats[key]))
else:
print(' %s %s: %f m' % (variable, key, stats[key]))
else:
if variable == 'Rotational':
if use_deg:
print(' %s rmse: %f deg' %
(variable, utils.rad_to_deg(stats['rmse'])))
else:
print(' %s rmse: %f rad' % (variable, stats['rmse']))
else:
print(' %s rmse: %f m' % (variable, stats['rmse']))
return stats
def receive_marker(self,data):
tag_seen = False
for marker in data.markers:
if marker.id == TagIds.RoomLeftTag or marker.id == TagIds.RoomFrontTag:
tag_seen = True
q = utils.msg_to_quaternion(marker.pose.pose.orientation)
roll,pitch,yaw = euler_from_quaternion(q)
self.tags_buffer_room.append(TagAR(marker.id,marker.pose.pose.position.x,
marker.pose.pose.position.y,
marker.pose.pose.position.z,
roll,pitch,yaw))
if not tag_seen:
self.tags_buffer_room.append(None)
if self.tags_buffer_room.is_empty():
self.using_odometry = True
else:
self.using_odometry = False
if not self.tag_located:
self.tag_located = True
#get the tag tbelt will be used for estimating the position + drone orientation
self.alpha,ref_tag = get_alpha_by_vision_info(self.tags_buffer_room, 0.0)
#get informations relative to tbelt tag
x_tag,y_tag,dist_tag,pitch_tag,count_tag = get_average_tag_info(self.tags_buffer_room, ref_tag)
#compute the horizontal displacement
displacement_x = utils.rad_to_deg(math.atan2(x_tag, dist_tag))
x_ref,y_ref,z_ref = get_ref_coor(ref_tag)
self.x = x_ref - math.cos(utils.deg_to_rad(self.alpha - displacement_x))*(dist_tag + 0.15)
self.y = y_ref - math.sin(utils.deg_to_rad(self.alpha - displacement_x))*(dist_tag + 0.15)
self.z = z_ref + y_tag
self.pubPosition.publish(Position(self.alpha, self.x, self.y, self.z, self.tag_located, displacement_x, ref_tag))
def to_HADec(self, latitude, *args, **kwargs):
"""
Returns the equivalent (Hour Angle, Declination) point for your AzAlt at your latitude
@param latitude: The observer's latitude in decimal degrees or as a SmartLat object
@return: A HADec coordinate pair object
"""
#Ensure latitude is in correct format
if not isinstance(latitude, SmartLat): #Cast strings / decimals for longitude into a SmartLon Longitude object
latitude = SmartLat(Decimal(latitude))
#Convert to radians
lat_rad = latitude.radians
ha_rad, dec_rad = self._rotate_to_HADec(lat_rad)
ha = rad_to_deg(ha_rad)
dec = rad_to_deg(dec_rad)
#Return new HADec object
return HADec(ha, dec)
def arctanh(self, x, y):
x_current = x
y_current, z_current = y, 0.0
_, _, z = self.__iterations_compute(x_current,
y_current,
z_current,
mode='vectoring',
coord='hyperbolic')
return rad_to_deg(z)
def track_room_marker(drone_info):
yaw = 0
if not drone_info.search_marker:
if not drone_info.room_marker_tracked == -1:
yaw = - K_ANG_Z*drone_info.marker_displacement
#search the closest tag wrt the current position
else:
distances = []
#append left
d_left = math.sqrt((RoomTagCoor.LeftX - drone_info.x)**2 + (RoomTagCoor.LeftY - drone_info.y)**2)
distances.append(d_left)
d_front = math.sqrt((RoomTagCoor.FrontX - drone_info.x)**2 + (RoomTagCoor.FrontY - drone_info.y)**2)
distances.append(d_front)
#...others#
min_dist = min(distances)
min_index = distances.index(min_dist)
#this is verbose but preserves the usage of arbitrary tag ids
if min_index == 0:
new_ref_tag = TagIds.RoomLeftTag
y = RoomTagCoor.LeftY
x = RoomTagCoor.LeftX
elif min_index == 1:
new_ref_tag = TagIds.RoomFrontTag
y = RoomTagCoor.FrontY
x = RoomTagCoor.FrontX
#...others...#
if drone_info.room_marker_tracked == TagIds.RoomLeftTag:
cur_index = 0
elif drone_info.room_marker_tracked == TagIds.RoomFrontTag:
cur_index = 1
#...others...#
#if it is the ref tag, it's ok the current displacement'
if drone_info.room_marker_tracked == new_ref_tag: #the last condition should not be necessary
yaw = - K_ANG_Z*drone_info.marker_displacement
#otherwise, rotate to track the closest room marker
else:
target_yaw = utils.rad_to_deg(math.atan((y - drone_info.y)/(x - drone_info.x)))
if ((y > drone_info.y and x < drone_info.x) or (y < drone_info.y and x < drone_info.x)):
target_yaw += 180
target_yaw = utils.normalize_angle(target_yaw)
rospy.loginfo("target yaw: %f" %target_yaw)
yaw, dr = rotate(target_yaw, drone_info.alpha, K_ANG_Z_EXPL)
return yaw
def get_current_jaw(self, unit='rad'):
"""
:param unit: 'rad' or 'deg'
:return: Numpy.float64
"""
jaw = np.float64(self.__position_jaw_current)
if unit == "deg":
jaw = U.rad_to_deg(self.__position_jaw_current)
return jaw
def get_current_joint(self, unit='rad'):
"""
:param unit: 'rad' or 'deg'
:return: List
"""
joint = self.__position_joint_current
if unit == 'deg':
joint = U.rad_to_deg(self.__position_joint_current)
joint[2] = self.__position_joint_current[2]
return joint
def show_results(resolution=14):
"""
Files are read in alphabetical order
1. Coordinate System
2. Enable
3. Mode
4. X Python Values - Compute with numpy
5. X VHDL Values
6. Y Python Values - Compute with numpy
7. Y VHDL Values
8. Z Python Values - Compute with numpy
9. Z VHDL Values
"""
real_values = create_files_to_simulate()
sin_python, cos_python, arctan_python, sinh_python, cosh_python, arctanh_python, axes_circular_python, axes_hyperbolic_python, axes_arctanh_python = real_values
data_output = read_files()
coord, enable, mode = data_output[0], data_output[1], data_output[2]
x, y, z = data_output[4], data_output[6], data_output[8]
sin, cos, arctan = [], [], []
sinh, cosh, arctanh = [], [], []
for index in range(len(enable)):
if enable[index] == 1: # If the module is enabled
if coord[
index] == 0: # If the module is configurate in circular coordinate system
if mode[index] == 0: # If the module is operating in rotation mode
cos.append(decoding(x[index], resolution))
sin.append(decoding(y[index], resolution))
else: # If the module is operating in vectoring mode
arctan.append(rad_to_deg(decoding(z[index], resolution)))
else: # If the module is configure in hyperbolic coordinate system
if mode[index] == 0: # If the module is operating in rotation mode
cosh.append(decoding(x[index], resolution))
sinh.append(decoding(y[index], resolution))
else: # If the module is operating in vectoring mode
arctanh.append(rad_to_deg(decoding(z[index], resolution)))
plot_results(cos, cos_python, axes_circular_python, 'Cos')
plot_results(sin, sin_python, axes_circular_python, 'Sin')
plot_results(arctan, arctan_python, axes_circular_python, 'Arctan')
plot_results(sinh, sinh_python, axes_hyperbolic_python, 'Sinh')
plot_results(cosh, cosh_python, axes_hyperbolic_python, 'Cosh')
plot_results(arctanh, arctanh_python, axes_arctanh_python, 'Arctanh')
def get_current_pose(self,
unit='rad'): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
pos, rot = self.PyKDLFrame_to_NumpyArray(
self.__position_cartesian_current)
if unit == 'deg':
rot = U.rad_to_deg(rot)
return pos, rot
def avoid_drones(command, drone_info, drone_2_info, navdata):
roll, pitch, yaw, gaz = [command.linear.y, command.linear.x, command.angular.z, command.linear.z]
#the b & f flight gives the precedence
if not drone_info.task == DroneTask.FlyH:
drones = [drone_2_info]
forbid_pitch_front = False
forbid_pitch_back = False
forbid_roll_left = False
forbid_roll_right = False
for drone in drones:
distance = ((drone.x - drone_info.x)**2 + (drone.y - drone_info.y)**2)
if distance < TOL_DRONE:
angle_between_drones = utils.rad_to_deg(math.atan((drone.y - drone_info.y)/(drone.x - drone_info.x)))
if ((drone.y > drone_info.y and drone.x < drone_info.x) or
(drone.y < drone_info.y and drone.x < drone_info.x)):
angle_between_drones += 180
while angle_between_drones > 180: angle_between_drones -= 360
while angle_between_drones < -180: angle_between_drones += 360
control_angle = drone_info.alpha - angle_between_drones
control_angle = utils.normalize_angle(control_angle)
if control_angle > -45 and control_angle < 45: forbid_pitch_front = True
elif control_angle > 45 and control_angle < 135: forbid_roll_right = True
elif control_angle > -135 and control_angle < -45: forbid_roll_left = True
else: forbid_pitch_back = True
if forbid_pitch_front:
if navdata.vx > 0.2 and pitch > 0: pitch = -1 #if speed is too high, hovering is not sufficient to stop the drone!
else: pitch = 0
elif forbid_pitch_back:
if navdata.vx < -0.2 and pitch < 0: pitch = 1
else: pitch = 0
if forbid_roll_left:
if navdata.vy > 0.2 and roll > 0: roll = -1
else: pitch = 0
elif forbid_roll_right:
if navdata.vy < -0.2 and roll < 0: roll = 1
else: pitch = 0
rospy.loginfo("forbid pitch front: %d" %forbid_pitch_front)
rospy.loginfo("forbid pitch back: %d" %forbid_pitch_back)
rospy.loginfo("forbid pitch left: %d" %forbid_roll_left)
rospy.loginfo("forbid pitch right: %d" %forbid_roll_right)
return [roll, pitch, yaw, gaz]
def receive_marker_room(self,data):
msg = ''
for marker in data.markers:
if marker.id == TagIds.RoomFrontTag or marker.id == TagIds.RoomBackTag or marker.id == TagIds.RoomLeftTag or marker.id == TagIds.RoomRightTag:
q = utils.msg_to_quaternion(marker.pose.pose.orientation)
roll,pitch,yaw = euler_from_quaternion(q)
msg += 'Marker ID: %d\n' %marker.id
msg += 'X: %f\n' %marker.pose.pose.position.x
msg += 'Y: %f\n' %marker.pose.pose.position.y
msg += 'Z: %f\n' %marker.pose.pose.position.z
msg += 'Yaw: %f\n' %utils.rad_to_deg(yaw)
msg += '*****************\n'
self.marker_room_message = msg
def get_ellipse(x, y): a = np.column_stack((x, y)) a = a - np.repeat(a.mean(axis=0), a.shape[0]).reshape(a.shape[1], a.shape[0]).T # mean center (w, v) = np.linalg.eig(np.cov(a.T)) i = np.nonzero((-w).argsort()==0)[0][0] # indices for the top 2 eigenvalues j = np.nonzero((-w).argsort()==1)[0][0] xy = (np.mean(x), np.mean(y)) width = np.sqrt(w[i]) * 2 height = np.sqrt(w[j]) * 2 unit = np.array([1, 0]) vi = v[:,i] #vi = vi / norm(vi) # already normalized theta = utils.get_theta(vi, unit) angle = utils.rad_to_deg(theta) return (xy, width, height, angle)
def receive_marker_individual(self,data):
msg = 'Individual markers: \n'
for marker in data.markers:
if (marker.id in TagIds.AllFollowTags or marker.id == TagIds.FrontTag or marker.id == TagIds.BackTag or marker.id == TagIds.LeftTag or marker.id == TagIds.RightTag or
marker.id == TagIds.BeltBackTag or marker.id == TagIds.BeltFrontTag or marker.id == TagIds.BeltLeftTag or marker.id == TagIds.BeltRightTag):
q = utils.msg_to_quaternion(marker.pose.pose.orientation)
roll,pitch,yaw = euler_from_quaternion(q)
msg += 'Marker ID: %d\n' %marker.id
msg += 'X: %f\n' %marker.pose.pose.position.x
msg += 'Y: %f\n' %marker.pose.pose.position.y
msg += 'Z: %f\n' %marker.pose.pose.position.z
msg += 'Yaw: %f\n' %utils.rad_to_deg(yaw)
msg += '*****************\n'
self.marker_message = self.status_message + msg + 'Room Markers: \n' + self.marker_room_message
def follow_me(self):
roll, pitch, yaw, gaz = [0,0,0,0]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_belt, TagIds.BeltFakeTag)
if count is not 0:
if not self.controller_pos_z.setpoint == 0.1:
self.controller_pos_z.setpoint = 0.1
if not self.controller_speed_x.setpoint == VEL_BACK:
self.controller_speed_x.setpoint = VEL_BACK
if not self.drone_info.belt_located:
self.drone_info.belt_located = True
if dist_med > 2.0:
if self.drone_info.time_anim is None:
utils.flight_animation(4)
self.drone_info.time_anim = rospy.Time.now()
elif (rospy.Time.now() - self.drone_info.time_anim).to_sec()> 6:
self.drone_info.time_anim = None
else:
if self.drone_info.time_anim is not None:
self.drone_info.time_anim = None
displacement_x = utils.rad_to_deg(math.atan2(x_med, dist_med))
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
yaw = -K_ANG_Z*displacement_x
roll = self.controller_speed_y.update(self.navdata.vy, 0.0)
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
utils.led_animation(1)
else: #count is 0
if self.drone_info.z < ALT or self.drone_info.belt_located:
#should not happen unless it's the beginning
self.setup_pid(None, None, ALT + 0.1, 0.0, None)
roll = self.controller_speed_y.update(self.navdata.vy,0.0)
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
utils.led_animation(5)
pass
return utils.adjust_command(roll, pitch, yaw, gaz)
def get_ellipse(x, y):
a = np.column_stack((x, y))
a = a - np.repeat(a.mean(axis=0), a.shape[0]).reshape(
a.shape[1], a.shape[0]).T # mean center
(w, v) = np.linalg.eig(np.cov(a.T))
i = np.nonzero(
(-w).argsort() == 0)[0][0] # indices for the top 2 eigenvalues
j = np.nonzero((-w).argsort() == 1)[0][0]
xy = (np.mean(x), np.mean(y))
width = np.sqrt(w[i]) * 2
height = np.sqrt(w[j]) * 2
unit = np.array([1, 0])
vi = v[:, i]
#vi = vi / norm(vi) # already normalized
theta = utils.get_theta(vi, unit)
angle = utils.rad_to_deg(theta)
return (xy, width, height, angle)
def get_current_jaw_and_wait(self,
unit='rad'
): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
self.__get_jaw_event.clear()
# the position is originally not received
self.__get_jaw = False
# recursively call this function until the position is received
self.__get_jaw_event.wait(20) # 1 minute at most
if self.__get_jaw:
jaw = np.float64(self.__position_jaw_current)
if unit == "deg":
jaw = U.rad_to_deg(self.__position_jaw_current)
return jaw
else:
return []
def receive_marker_and_print_path(self, data):
#exploit the callback function of the marker to update the plot of the path
#the room node must be launched before the main node!
if not self.marker_callback_started: self.marker_callback_started = True
if self.count == 0:
self.count += 1
self.axes.clear()
self.axes.set_xlim([-2,7])
self.axes.set_ylim([-2,7])
self.axes.grid(True)
self.axes.set_autoscaley_on(False)
self.axes.set_autoscalex_on(False)
for marker in data.markers:
if marker.id == TagIds.RoomFrontTag or marker.id == TagIds.RoomBackTag or marker.id == TagIds.RoomLeftTag or marker.id == TagIds.RoomRightTag:
displacement_x = utils.rad_to_deg(math.atan2(marker.pose.pose.position.x,marker.pose.pose.position.z))
orientation = self.alpha - displacement_x
while orientation > 180: orientation -= 360
while orientation < -180: orientation += 360
new_tagX = math.cos(utils.deg_to_rad(orientation))*marker.pose.pose.position.z + self.x
new_tagY = math.sin(utils.deg_to_rad(orientation))*marker.pose.pose.position.z + self.y
self.axes.plot(new_tagX, new_tagY, 'ro')
self.axes.plot(self.listx[0:500], self.listy[0:500],'co', markersize=3)
self.axes.plot(self.listx[501:999], self.listy[501:999],'mo', markersize=3)
self.axes.plot(self.x, self.y, 'yo', markersize=5)
#plot the orientation
self.axes.plot([self.x, self.x + math.cos(utils.deg_to_rad(self.alpha))],[self.y, self.y + math.sin(utils.deg_to_rad(self.alpha))], 'y')
if self.box_position_received:
self.axes.plot(self.x_box, self.y_box, 'go', markersize=5)
self.draw_room()
self.canvas.draw()
elif self.count < 60:
self.count += 1
else: self.count = 0
def get_current_pose_and_wait(self,
unit='rad'
): # Unit: pos in (m) rot in (rad) or (deg)
"""
:param unit: 'rad' or 'deg'
:return: Numpy.array
"""
self.__get_position_event.clear()
# the position is originally not received
# self.__get_position = False
# recursively call this function until the position is received
# self.__get_position_event.wait(20) # 1 minute at most
if self.__get_position_event.wait(20): # 1 minute at most
pos, rot = self.PyKDLFrame_to_NumpyArray(
self.__position_cartesian_current)
if unit == 'deg':
rot = U.rad_to_deg(rot)
return pos, rot
else:
return [], []
def follow_belt(self):
roll, pitch, yaw, gaz = [0 for i in range(4)]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_belt, TagIds.BeltFakeTag)
if count is not 0:
if not self.controller_pos_x.setpoint == 2.0:
self.controller_pos_x.setpoint = 2.0
if not self.controller_pos_z.setpoint == 0.1:
self.controller_pos_z.setpoint = 0.1
if not self.drone_info.belt_located:
self.drone_info.belt_located = True
displacement_x = utils.rad_to_deg(math.atan2(x_med, dist_med))
yaw = -K_ANG_Z*displacement_x
other_drones = filter(lambda drone: drone.command == DroneCommands.FollowBelt and drone.belt_in_sight and
drone.status is DroneStatus.Flying, [self.drone_2_info, self.drone_3_info, self.drone_4_info])
roll = get_roll_follow_belt(self.drone_info, other_drones,
self.controller_pos_y, self.controller_speed_y, self.navdata)
#keep the distance
pitch = - self.controller_pos_x.update(dist_med, -self.navdata.vx)
if pitch < 0: pitch = pitch*2
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
#for later in case
if displacement_x < 0:
#for knowing from where to start rotating
self.drone_info.last_yaw = POWER_YAW
else:
self.drone_info.last_yaw = -POWER_YAW
self.drone_info.last_z = self.drone_info.z
self.drone_info.alpha_start_expl = self.drone_info.alpha
else:
if self.drone_info.z < ALT:
#should not happen unless it's the beginning
self.setup_pid(None, None, ALT + 0.1, None, 0.0)
roll = self.controller_speed_y.update(self.navdata.vy,0.0)
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
elif self.drone_info.belt_located:
self.setup_pid(None, None, self.drone_info.last_z, 0.0, 0.0)
roll = self.controller_speed_y.update(self.navdata.vy,0.0)
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
diff = self.drone_info.alpha - self.drone_info.alpha_start_expl
diff = utils.normalize_angle(diff)
if diff < -20:
yaw = POWER_YAW
self.drone_info.last_yaw = yaw
elif diff > 20:
yaw = -POWER_YAW
self.drone_info.last_yaw = yaw
else: yaw = self.drone_info.last_yaw
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
utils.led_animation(5)
return utils.adjust_command(roll, pitch, yaw, gaz)
def orbit(self, box_tag):
roll, pitch, yaw, gaz = [0 for i in range(4)]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_box, box_tag)
if count is not 0:
#if values were changed after lost of visual tracking
if not self.controller_pos_x.setpoint == 1.6:
self.controller_pos_x.setpoint = 1.6
if not self.controller_pos_z.setpoint == 0.1:
self.controller_pos_z.setpoint = 0.1
#the alpha angle computed with side markers is surely related to the correct bundle!
displacement_x = utils.rad_to_deg(math.atan2(x_med, dist_med))
if not self.drone_info.box_located:
self.drone_info.box_located = True
#update box position (if only odometry is used, it will move!)
self.drone_info.x_box = self.drone_info.x + math.cos(utils.deg_to_rad(self.drone_info.alpha - displacement_x))*dist_med
self.drone_info.y_box = self.drone_info.y + math.sin(utils.deg_to_rad(self.drone_info.alpha - displacement_x))*dist_med
self.pubBoxPosition.publish(Point(self.drone_info.x_box, self.drone_info.y_box, 0.0))
yaw = -K_ANG_Z*displacement_x
if self.drone_info.command is DroneCommands.OrbitSync:
distances = []
if (self.drone_2_info.box_in_sight and self.drone_2_info.command is DroneCommands.OrbitSync and
self.drone_2_info.status is DroneStatus.Flying):
dist = self.drone_2_info.alpha_box - self.drone_info.alpha_box
dist = utils.normalize_angle(dist)
distances.append(dist)
if (self.drone_3_info.box_in_sight and self.drone_3_info.command is DroneCommands.OrbitSync and
self.drone_3_info.status is DroneStatus.Flying):
dist = self.drone_3_info.alpha_box - self.drone_info.alpha_box
dist = utils.normalize_angle(dist)
distances.append(dist)
#...fill with other distances#
if not distances == []:
distances_pos = filter(lambda x: x >= 0, distances)
distances_neg = filter(lambda x: x < 0, distances)
if not distances_pos == []:
dd = min(distances_pos)
else: dd = 360 + min(distances_neg)
if not distances_neg == []:
ds = -max(distances_neg)
else: ds = 360 - max(distances_pos)
self.controller_speed_y.setpoint = -(VEL_TAN + (dd - ds)/float(1000))
else: self.controller_speed_y.setpoint = -VEL_TAN
rospy.loginfo("setpoint 1: %f" %self.controller_speed_y.setpoint)
#keep the distance
pitch = - self.controller_pos_x.update(dist_med, -self.navdata.vx)/float(2) #receive the dist_med
roll = self.controller_speed_y.update(self.navdata.vy, 0.0)
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
self.drone_info.last_z = self.drone_info.z
else:
#if visual contact is lost, try to restore it using odometry informations
if self.drone_info.box_located:
self.target_x = self.drone_info.x_box
self.target_y = self.drone_info.y_box
self.target_z = self.drone_info.last_z
self.target_yaw = utils.rad_to_deg(math.atan((self.target_y - self.drone_info.y)/(self.target_x - self.drone_info.x)))
if ((self.target_y > self.drone_info.y and self.target_x < self.drone_info.x) or
(self.target_y < self.drone_info.y and self.target_x < self.drone_info.x)):
self.target_yaw += 180
self.target_yaw = utils.normalize_angle(self.target_yaw)
self.setup_pid(0.0, None, None, None, 0.0)
roll, pitch, yaw, gaz = self.position_control(1.5)
pitch = pitch / float(2)
else:
#should not happen unless at the beginning
self.setup_pid(None, None, ALT, None, 0.0)
roll, pitch, yaw, gaz = self.rot_exploration()
utils.led_animation(5)
return utils.adjust_command(roll, pitch, yaw, gaz)
def follow_tag(self, angle_to_mantain, tag_to_follow):
roll, pitch, yaw, gaz = [0 for i in range(4)]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_follow, tag_to_follow)
if tag_to_follow is not None and count is not 0:
if self.controller_pos_z.prev_setpoint is not None:
self.controller_pos_z.setpoint = self.controller_pos_z.prev_setpoint
elif not self.controller_pos_z.setpoint == 0.0:
self.controller_pos_z.setpoint = 0.0
if pitch_med > 0: pitch_pos = True
else: pitch_pos = False
displacement_x = utils.rad_to_deg(math.asin(x_med/dist_med))
if displacement_x > 0: tag_on_left = False
else: tag_on_left = True
yaw = - K_ANG_Z*displacement_x
if displacement_x < 0:
self.drone_info.last_seen = 1
else: self.drone_info.last_seen = -1
#keep the distance
pitch = - self.controller_pos_x.update(dist_med, -self.navdata.vx) #receive the dist_med
if pitch < 0: pitch *= 1.5
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
#follow the orientation
if tag_on_left and not(pitch_pos):
[beta,omega,alpha,gamma] = get_angles_from_vision(displacement_x, pitch_med, VisionData.TAG_ON_LEFT, VisionData.PITCH_NEG)
displacement_y = dist_med*math.sin(alpha + angle_to_mantain)/math.sin(gamma)
elif not(tag_on_left) and pitch_pos:
[beta,omega,alpha,gamma] = get_angles_from_vision(displacement_x, pitch_med, VisionData.TAG_ON_RIGHT, VisionData.PITCH_POS)
displacement_y = -dist_med*math.sin(alpha - angle_to_mantain)/math.sin(gamma)
elif tag_on_left and pitch_pos:
[beta,omega,alpha,gamma] = get_angles_from_vision(displacement_x, pitch_med, VisionData.TAG_ON_LEFT, VisionData.PITCH_POS)
displacement_y = -dist_med*math.sin(alpha - angle_to_mantain)/math.sin(gamma)
else: #tag_on_right and not pitch pos
[beta,omega,alpha,gamma] = get_angles_from_vision(displacement_x, pitch_med, VisionData.TAG_ON_RIGHT, VisionData.PITCH_NEG)
displacement_y = dist_med*math.sin(alpha + angle_to_mantain)/math.sin(gamma)
roll = self.controller_pos_y.update(displacement_y, self.navdata.vy)
if self.drone_info.angle_to_mantain is not 0.0: roll = roll / 2
self.drone_info.last_z = self.drone_info.z
else: #search starting rotating from where the tag was last seen
if self.drone_info.last_seen == 1:
yaw = 0#POWER_YAW
elif self.drone_info.last_seen == -1:
yaw = 0#-POWER_YAW
#and try not to go around too much
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
roll = self.controller_speed_y.update(self.navdata.vy, 0.0)
#only the belt following changes the setpoint
self.controller_pos_z.setpoint = ALT
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
#moreover, signal tbelt no tag was detected with a led animation
utils.led_animation(5)
return utils.adjust_command(roll, pitch, yaw, gaz)
def render_image(self, orig_img):
"return properly rotated surface for the given image"
angle = -1 * utils.rad_to_deg(self.angle)
img = pygame.transform.rotate(orig_img, angle)
draw_rec = img.get_rect(center=self.pos)
return img, draw_rec
def create_files_to_simulate(resolution=14):
angles = np.arange(-89, 89, 1)
path_input = os.path.abspath(__file__)
path_dir, _ = os.path.split(path_input)
path_input_dir = os.path.join(path_dir, 'input')
inputs_files = [
'input_x.txt', 'input_y.txt', 'input_z.txt', 'input_mode.txt',
'input_coor.txt', 'input_enable.txt'
]
path_inputs = []
for file in inputs_files:
path_inputs.append(os.path.join(path_input_dir, file))
axes_circular, axes_hyperbolic, axes_arctanh = [], [], []
x, y, z = [], [], [] # Values to simulate the module
enable, mode, coord = [], [], [] # Values to simulate the module
sin, cos, arctan = [], [], [] # Values to compare
sinh, cosh, arctanh = [], [], [] # Values to compare
for angle in angles:
angle_rad = deg_to_rad(angle)
angle_fixed = coding(angle_rad, resolution)
axes_circular.append(angle)
# Enable
enable.append(1)
# --- Circular coordinate system
# Adding data for vectoring mode
x.append(coding(np.cos(angle_rad), resolution))
y.append(coding(np.sin(angle_rad), resolution))
z.append(0)
mode.append(1)
coord.append(0)
# Adding data for rotation mode
x.append(0)
y.append(0)
z.append(angle_fixed)
mode.append(0)
coord.append(0)
# Save real data to compare with the simulation
arctan.append(
rad_to_deg(np.arctan2(np.sin(angle_rad), np.cos(angle_rad))))
sin.append(np.sin(angle_rad))
cos.append(np.cos(angle_rad))
# --- Hyoperbolic coordinate system
if (abs(angle)) < 61:
if (abs(
coding(np.sin(angle_rad), resolution) /
coding(np.cos(angle_rad), resolution))) < 0.6:
# Adding data for vectoring mode
x.append(coding(np.cos(angle_rad), resolution))
y.append(coding(np.sin(angle_rad), resolution))
z.append(0)
mode.append(1)
coord.append(1)
# Save real data to compare with the simulation
arctanh.append(
rad_to_deg(
np.arctanh(np.sin(angle_rad) / np.cos(angle_rad))))
axes_arctanh.append(np.sin(angle_rad) / np.cos(angle_rad))
# Adding data for rotation mode
x.append(0)
y.append(0)
z.append(angle_fixed)
mode.append(0)
coord.append(1)
# Save real data to compare with the simulation
sinh.append(np.sinh(angle_rad))
cosh.append(np.cosh(angle_rad))
axes_hyperbolic.append(angle_rad)
write_file(path_inputs[0], x)
write_file(path_inputs[1], y)
write_file(path_inputs[2], z)
write_file(path_inputs[3], mode)
write_file(path_inputs[4], coord)
write_file(path_inputs[5], enable)
return sin, cos, arctan, sinh, cosh, arctanh, axes_circular, axes_hyperbolic, axes_arctanh
def sample(self, position, heading, pitch):
"""Return the RGB value of the incident light at the given position.
Renders the scene to a cubemap and gets the average of the pixels.
"""
# Bind the main, full-size FBO
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, self.sample_fbo)
for buffer_id in GL_COLOR_BUFFER_BIT, GL_DEPTH_BUFFER_BIT:
glClear(buffer_id)
# Draw each face of the cube map
for setup in self.view_setups:
# Setup matrix
glViewport(*setup["viewport"])
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(90.0, 1.0, 0.001, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glEnable(GL_DEPTH_TEST)
glRotatef(90.0, 0.0, 1.0, 0.0)
glRotatef(-90.0, 1.0, 0.0, 0.0)
glRotatef(utils.rad_to_deg(pitch) + setup["pitch"],
0.0, 1.0, 0.0)
glRotatef(utils.rad_to_deg(heading) + setup["heading"],
0.0, 0.0, -1.0)
glTranslatef(-position[0], -position[1], -position[2])
# Draw the scene
self.render_func()
# Draw multiplier map on top. First, set the matrix
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glViewport(0, 0, self.sample_size, self.sample_size)
glOrtho(0.0, 1.0, 0.0, 1.0, -1.0, 1.0)
# Setup the state
glDisable(GL_DEPTH_TEST)
glColor4f(1.0, 1.0, 1.0, 1.0)
glEnable(GL_BLEND)
glBlendFunc(GL_ZERO, GL_SRC_COLOR)
glEnable(GL_TEXTURE_2D)
glBindTexture(GL_TEXTURE_2D, self.multiplier_map.id)
# Draw the map
utils.draw_rect()
# Reset the state
glDisable(GL_BLEND)
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0)
# Get the average value of all the pixels in the sample
if self.average_method == HARDWARE:
sample_average = self.average_hardware()
elif self.average_method == SOFTWARE:
sample_average = self.average_software()
else:
raise ValueError("Unknown sample method %s" % self.average_method)
# Divide by a constant otherwise the compensation map won't give you
# the full range. TODO: generate the constant
incident_light = [val / 0.40751633986928104 for val in sample_average]
return incident_light
def compute_statistics(err,
verbose=False,
variable='Translational',
use_deg=True,
title='',
save=False):
"""
Computes the mean, RMSE, standard deviation, median, min and max from a vector of errors
@param err: a MxN np array.
M is the number of components by sample (M=3 if SO(3), M=6 if SE(3)). N is the number of samples.
"""
stats = {}
abs_err = np.fabs(err)
#print(abs_err.shape)
# RMSE
stats['rmse'] = np.sqrt(np.dot(abs_err, abs_err) / len(abs_err))
#print(len(abs_err))
# Mean
stats['mean'] = np.mean(abs_err) # computed by column
# Standard Deviation
stats['std'] = np.std(abs_err) # computed by column
# Median
stats['median'] = np.median(abs_err) # computed by column
# Min
stats['min'] = np.min(np.fabs(abs_err)) # computed by column
# Max
stats['max'] = np.max(abs_err) # computed by column
if verbose:
for key in stats:
if variable == 'Rotational':
if use_deg:
print('%s %s %s [deg]: %f' %
(title, variable, key, utils.rad_to_deg(stats[key])))
else:
print('%s %s %s [rad]: %f' %
(title, variable, key, stats[key]))
else:
print('%s %s %s [m]: %f' % (title, variable, key, stats[key]))
else:
if variable == 'Rotational':
if use_deg:
print('%s %s rmse [deg]: %f' %
(title, variable, utils.rad_to_deg(stats['rmse'])))
else:
print('%s %s rmse [rad]: %f' %
(title, variable, stats['rmse']))
else:
print('%s %s rmse [m]: %f' % (title, variable, stats['rmse']))
if save:
filename = 'statistics-%s-%s.csv' % (title.replace(
' ', ''), variable.replace(' ', ''))
print('saving statistics file: %s' % filename)
with open(filename, 'w') as f:
w = csv.writer(f)
w.writerow(stats.keys())
w.writerow(stats.values())
return stats
def arctanh(self, x, y):
x_current = coding(x, self.__resolution)
y_current, z_current = coding(y, self.__resolution), 0.0
_, _, z = self.__iterations_compute(x_current, y_current, z_current, mode='vectoring', coord='hyperbolic')
return rad_to_deg(z)