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 convergence_test_hyperbolic():
# Circular Coordinate System
# Rotation mode: CORDIC algorithm compute when |angle| < 64°
# Vectoring mode: CORDIC algorithm when |result| < 64°
z = np.arange(-70, 70, 0.01)
cosh, arctanh = [], []
cordic = Cordic()
for i in z:
cosh_cordic, _ = cordic.cosh_sinh(i)
cosh.append(cosh_cordic)
fig, axes = plt.subplots(1, 2)
axes[0].plot(z, cosh, 'b-')
axes[0].set_title('Rotation Mode')
axes[0].set_ylabel('Magnitude')
axes[0].set_xlabel('Angle')
for i in z:
x = np.cos(deg_to_rad(i))
y = np.sin(deg_to_rad(i))
arctanh_cordic = cordic.arctanh(x, y)
arctanh.append(arctanh_cordic)
axes[1].plot(z, arctanh, 'b-')
axes[1].set_title('Vectoring Mode')
axes[1].set_ylabel('Angle')
axes[1].set_xlabel('Angle')
plt.show()
def __init__(self, obj, angle):
self.obj = obj
radians = deg_to_rad(angle)
self.sin_theta = sin(radians)
self.cos_theta = cos(radians)
self.bbox = AABB()
self.hasbox = obj.bounding_box(0, 1, self.bbox)
_min = Point3( float('inf'), float('inf'), float('inf'))
_max = Point3(float('-inf'), float('-inf'), float('-inf'))
for i in range(2):
for j in range(2):
for k in range(2):
x = i * self.bbox._max.x + (1 - i) * self.bbox._min.x
y = j * self.bbox._max.y + (1 - j) * self.bbox._min.y
z = k * self.bbox._max.z + (1 - k) * self.bbox._min.z
newx = self.cos_theta * x + self.sin_theta * z
newz = -self.sin_theta * x + self.cos_theta * z
tester = Vec3(newx, y, newz)
for c in range(3):
_min[c] = min(_min[c], tester[c])
_max[c] = max(_max[c], tester[c])
self.bbox = AABB(_min, _max)
def main():
for runway, info in RUNWAYS.iteritems():
angles = (
tuple(utils.deg_to_rad(coord) for coord in pt)
for pt in info if isinstance(pt, tuple)
)
print '{}: {}'.format(runway, round(get_heading(info[0], *angles), 3))
def __init__(self,
lookfrom,
lookat,
vup,
vfov,
aspect_ratio,
aperture,
focus_dist,
t0=0,
t1=0):
self.lookfrom = lookfrom
self.lookat = lookat
self.vup = vup
self.vfov = vfov
self.aspect_ratio = aspect_ratio
self.aperture = aperture
self.focus_dist = focus_dist
self.theta = deg_to_rad(self.vfov)
self.h = tan(self.theta / 2)
self.viewport_height = 2.0 * self.h
self.viewport_width = self.aspect_ratio * self.viewport_height
self.w = (self.lookfrom - self.lookat).unit_vector()
self.u = (self.vup.cross(self.w)).unit_vector()
self.v = self.w.cross(self.u)
self.origin = self.lookfrom
self.horizontal = self.focus_dist * self.viewport_width * self.u
self.vertical = self.focus_dist * self.viewport_height * self.v
self.lower_left_corner = self.origin - self.horizontal / 2 - self.vertical / 2 - self.focus_dist * self.w
self.lens_radius = aperture / 2
self.time0 = t0
self.time1 = t1
def cosh_sinh(self, angle_deg):
angle_rad = deg_to_rad(angle_deg)
x_current = self.__const_hyperbolic
y_current, z_current = 0.0, angle_rad
x, y, _ = self.__iterations_compute(x_current,
y_current,
z_current,
mode='rotation',
coord='hyperbolic')
return x, y
def cos_sin(self, angle_deg):
angle_rad = deg_to_rad(angle_deg)
x_current = self.__const_circular
y_current, z_current = 0.0, angle_rad
x, y, _ = self.__iterations_compute(x_current,
y_current,
z_current,
mode='rotation',
coord='circular')
return x, y
def set_jaw_direct(self, jaw, unit='rad'):
"""
:param jaw: jaw angle
:param unit: 'rad' or 'deg'
"""
if unit == 'deg':
jaw = U.deg_to_rad(jaw)
msg = JointState()
msg.position = [jaw]
self.__set_position_jaw_pub.publish(msg)
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 position_control(self, des_distance = 0):
roll, pitch, yaw, gaz = [0 for i in range(4)]
self.controller_pos_z.setpoint = self.target_z
if self.target_yaw is not None:
yaw,dr = rotate(self.target_yaw, self.drone_info.alpha, K_ANG_Z)
#convert target wrt drone reference frame and change sign
if des_distance == 0:
alpha_rad = utils.deg_to_rad(self.drone_info.alpha)
x_drone_ref = -math.cos(alpha_rad)*(self.target_x - self.drone_info.x) - math.sin(alpha_rad)*(self.target_y - self.drone_info.y)
y_drone_ref = math.sin(alpha_rad)*(self.target_x - self.drone_info.x) - math.cos(alpha_rad)*(self.target_y - self.drone_info.y)
target_reached = False
if self.target_yaw is None:
if abs(x_drone_ref) < TOL_DIST and abs(y_drone_ref) < TOL_DIST and abs(self.drone_info.z - self.target_z) < TOL_DIST:
target_reached = True
else:
if abs(x_drone_ref) < TOL_DIST and abs(y_drone_ref) < TOL_DIST and abs(self.drone_info.z - self.target_z) < TOL_DIST and abs(dr) < 7:
target_reached = True
if target_reached:
if self.drone_info.time_keep_pos is None:
self.drone_info.time_keep_pos = rospy.Time.now()
elif (rospy.Time.now() - self.drone_info.time_keep_pos).to_sec() > TIME:
self.drone_info.move_accomplished = True
#hover
rospy.loginfo("Move accomplished!")
self.drone_info.time_keep_pos = None
roll,pitch,yaw,gaz = [0,0,0,0]
else:
self.drone_info.time_keep_pos = None
roll = self.controller_pos_y.update(y_drone_ref, self.navdata.vy)
pitch = self.controller_pos_x.update(x_drone_ref, self.navdata.vx)
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
else:
if abs(dr) < 30: #start moving only if the target is not behind!
distance = -(math.sqrt((self.target_x - self.drone_info.x)**2 + (self.target_y - self.drone_info.y)**2) - des_distance)
if abs(distance) < TOL_DIST and abs(self.drone_info.z - self.target_z) < TOL_DIST and abs(dr) < 7:
self.drone_info.move_accomplished = True
#hover
rospy.loginfo("Move accomplished!")
roll,pitch,yaw,gaz = [0,0,0,0]
else:
pitch = self.controller_pos_x.update(distance, self.navdata.vx)
roll = self.controller_speed_y.update(self.navdata.vy, 0.0)
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
return utils.adjust_command(roll, pitch, yaw, gaz)
def set_pose_direct(self, pos, rot, unit='rad'):
"""
:param pos_des: position array [x,y,z]
:param rot_des: rotation array [Z,Y,X euler angle]
:param unit: 'rad' or 'deg'
"""
if unit == 'deg':
rot = U.deg_to_rad(rot)
# set in position cartesian mode
frame = self.NumpyArraytoPyKDLFrame(pos, rot)
msg = posemath.toMsg(frame)
# go to that position by goal
self.__set_position_cartesian_pub.publish(msg)
def set_joint(self, joint, unit='rad', wait_callback=True):
"""
:param joint: joint array [j1, ..., j6]
:param unit: 'rad', or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
joint = U.deg_to_rad(joint)
msg = JointState()
msg.position = joint
if wait_callback:
return self.__set_position_goal_joint_publish_and_wait(msg)
else:
self.__set_position_goal_joint_pub.publish(msg)
return True
def set_jaw(self, jaw, unit='rad', wait_callback=True):
"""
:param jaw: jaw angle
:param unit: 'rad' or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
jaw = U.deg_to_rad(jaw)
msg = JointState()
msg.position = [jaw]
if wait_callback:
return self.__set_position_goal_jaw_publish_and_wait(msg)
else:
self.__set_position_goal_jaw_pub.publish(msg)
return True
def set_pose(self, pos, rot, unit='rad', wait_callback=True):
"""
:param pos_des: position array [x,y,z]
:param rot_des: rotation array [Z,Y,X euler angle]
:param unit: 'rad' or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
rot = U.deg_to_rad(rot)
# set in position cartesian mode
frame = self.NumpyArraytoPyKDLFrame(pos, rot)
msg = posemath.toMsg(frame)
# go to that position by goal
if wait_callback:
return self.__set_position_goal_cartesian_publish_and_wait(msg)
else:
self.__set_position_goal_cartesian_pub.publish(msg)
return True
def iterations_test():
iterations = np.arange(1, 20, 1)
angles = np.arange(1, 90, 1)
error = []
for iteration in iterations:
error_in_angle = []
for angle in angles:
cordic = Cordic(iterations_param=iteration)
cos, _ = cordic.cos_sin(angle)
cos_numpy = np.cos(deg_to_rad(angle))
err = absolute_error(cos_numpy, cos)
error_in_angle.append(err)
mean_error = mean(error_in_angle)
error.append(mean_error)
plt.plot(iterations, error, 'b*-')
plt.title('Absolute Error')
plt.ylabel('Error (%)')
plt.xlabel('Iterations')
plt.xticks([3, 6, 9, 12, 15, 18])
plt.show()
def resolution_bits_test():
bits = np.arange(1, 20, 1)
angles = np.arange(1, 90, 1)
error = []
for bit in bits:
error_in_angle = []
for angle in angles:
cordic = Cordic(resolution_param=bit)
cos, _ = cordic.cos_sin(angle)
cos_numpy = np.cos(deg_to_rad(angle))
err = absolute_error(cos_numpy, cos)
error_in_angle.append(err)
mean_error = mean(error_in_angle)
error.append(mean_error)
plt.plot(bits, error, 'b*-')
plt.title('Absolute Error')
plt.ylabel('Error (%)')
plt.xlabel('Resolution in Bits')
plt.xticks([3, 6, 9, 12, 15, 18])
plt.show()
def receive_position(self, position):
self.x = position.x
self.y = position.y
self.alpha = position.alpha
if self.tag_located is False and position.tag_located is True: #from now, the position should be correct!
self.tag_located = True
self.listx = []
self.listy = []
if len(self.listx) == 1000: self.listx.pop(0)
if len(self.listy) == 1000: self.listy.pop(0)
self.listx.append(position.x)
self.listy.append(position.y)
if not self.marker_callback_started:
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)
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 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 controller(self,event):
if self.low.navdata.status == DroneStatus.Flying or self.low.navdata.status == DroneStatus.GotoHover or self.low.navdata.status == DroneStatus.Hovering:
#if flying, mantain hovering if not specified anything else
self.command.linear.x, self.command.linear.y, self.command.linear.z, self.command.angular.z = [0,0,0,0]
if self.low.drone_info.command == DroneCommands.NoCommand:
pass
#keyboard controls
elif self.low.drone_info.command == DroneCommands.YawLeft:
self.command.angular.z = 0.7
elif self.low.drone_info.command == DroneCommands.YawRight:
self.command.angular.z = -0.7
elif self.low.drone_info.command == DroneCommands.PitchForward:
self.command.linear.x = 1
elif self.low.drone_info.command == DroneCommands.PitchBackward:
self.command.linear.x = -1
elif self.low.drone_info.command == DroneCommands.RollLeft:
self.command.linear.y = 1
elif self.low.drone_info.command == DroneCommands.RollRight:
self.command.linear.y = -1
elif self.low.drone_info.command == DroneCommands.IncreaseAlt:
self.command.linear.z = 1
elif self.low.drone_info.command == DroneCommands.DecreaseAlt:
self.command.linear.z = -1
#########################################################################################
#tag detection
elif self.low.drone_info.command == DroneCommands.StartFollow:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.follow_tag(self.low.drone_info.angle_to_mantain, self.low.drone_info.tag_to_follow)
#########################################################################################
#face following
elif self.low.drone_info.command == DroneCommands.FollowFace:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.follow_face()
#follow belt
elif self.low.drone_info.command == DroneCommands.FollowBelt:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.follow_belt() #there is only one belt
#########################################################################################
#position control
elif self.low.drone_info.command == DroneCommands.GoAtPoint:
if not self.low.drone_info.move_accomplished:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.position_control(0.0)
else:
rospy.loginfo("Move accomplished!")
if self.low.after_move is not None:
if self.low.after_move == 1:
self.low.send_land()
elif self.low.after_move == 2:
self.low.go_at_point(self.low.drone_info.x, self.low.drone_info.y,
self.low.drone_info.z, self.low.drone_info.alpha + 135)
elif self.low.after_move == 3:
self.low.go_at_point(self.low.drone_info.x, self.low.drone_info.y,
self.low.drone_info.z, self.low.drone_info.alpha - 135)
elif self.low.after_move == 4:
self.low.go_at_point(self.low.drone_info.x + 2.5*math.cos(utils.deg_to_rad(self.low.drone_info.alpha - 90)),
self.low.drone_info.y + 2.5*math.sin(utils.deg_to_rad(self.low.drone_info.alpha - 90)),
self.low.drone_info.z, self.low.drone_info.alpha - 90)
self.low.after_move = None
#########################################################################################
#orbit
elif self.low.drone_info.command == DroneCommands.Orbit:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.orbit(TagIds.MasterTag)
#########################################################################################
#orbit sync
elif self.low.drone_info.command == DroneCommands.OrbitSync:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.orbit(TagIds.MasterTag)
#########################################################################################
#explore by continously spinning on current place
elif self.low.drone_info.command == DroneCommands.Rotate:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.rot_exploration()
#########################################################################################
#this behaviour supposes the perfect alignment rescurer-box
elif self.low.drone_info.command == DroneCommands.FollowMe:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = self.low.follow_me()
#finally the command is published with eventual modifications
if self.low.drone_info.avoid_walls:
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = avoid_wall(self.command, self.low.drone_info, self.low.navdata)
#it may overwrite some commands of avoid_walls, but tbelt's to be safer
if self.low.drone_info.avoid_drones: #only with 2 drones (for now)
self.command.linear.y, self.command.linear.x, self.command.angular.z, self.command.linear.z = avoid_drones(self.command, self.low.drone_info, self.low.drone_2_info, self.low.navdata)
'''if not self.low.drone_info.task == DroneTask.FlyBAF or self.low.drone_info.task == DroneTask.FlyH:
if self.low.drone_info.track_room_marker:
if self.command.angular.z == 0 and self.low.drone_info.room_located: #None is considered 0.0 after being sent!
#self.command.angular.z = - K_ANG_Z*self.low.drone_info.marker_displacement
self.command.angular.z = track_room_marker(self.low.drone_info)
#this can be done in the task control function
if self.command.linear.z == 0:
self.low.controller_pos_z.setpoint = 1.2
self.command.linear.z = self.low.controller_pos_z.update(self.low.drone_info.z, self.low.navdata.vz)
else:
self.command.angular.z = track_generic_marker(self.low.drone_info, self.low.drone_info.flyght_tag)'''
if self.low.drone_info.time_go_up is not None and (rospy.Time.now() - self.low.drone_info.time_go_up).to_sec() < 0.25: #for gesture demo
self.command.linear.z = 1 #overwrite
elif (self.low.drone_info.time_go_up is not None and self.low.drone_info.task == DroneTask.DemoGesture and
(rospy.Time.now() - self.low.drone_info.time_go_up).to_sec() > 0.3):
self.low.drone_info.time_go_up = None
self.low.drone_info.time_go_down = rospy.Time.now() #move up and down after a word
if self.low.drone_info.time_go_down is not None and (rospy.Time.now() - self.low.drone_info.time_go_down).to_sec() < 0.2: #for gesture demo
self.command.linear.z = -1 #overwrite
'''if self.low.drone_info.task == DroneTask.DemoGesture:
if self.low.drone_info.time_go_back is not None and (rospy.Time.now() - self.low.drone_info.time_go_back).to_sec() < 7: #for gesture demo
self.low.controller_pos_x.setpoint = 2.2 #overwrite
if (rospy.Time.now() - self.low.drone_info.time_go_back).to_sec() < 1:
self.command.linear.x = -0.5
else:
self.low.controller_pos_x.setpoint = 1.7'''
self.pubCommand.publish(self.command)
def _update_rot_angle_angle(self, limage: LsystemImage) -> None:
rad_angle = deg_to_rad(self.spinBox_angle.value())
limage.set_rot_angle(rad_angle)
def get_roll_follow_belt(drone_info, other_drones, controller_speed_y, controller_pos_y, navdata):
distances = []
dist_front = []
dist_front_nat = []
drone_2_info = None
drone_3_info = None
drone_4_info = None
for drone in other_drones:
if drone_2_info is None: drone_2_info = drone
elif drone_3_info is None: drone_3_info = drone
elif drone_4_info is None: drone_4_info = drone
if drone_2_info is not None:
dist = drone_2_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_2_info.alpha_belt))
dist_front_nat.append(drone_2_info.alpha_belt)
if drone_3_info is not None:
dist = drone_3_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_3_info.alpha_belt))
dist_front_nat.append(drone_3_info.alpha_belt)
if drone_4_info is not None:
dist = drone_4_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_4_info.alpha_belt))
dist_front_nat.append(drone_4_info.alpha_belt)
if distances == [] or abs(drone_info.alpha_belt) < min(dist_front): # I am alone or the "master"
drone_info.role = 0
if abs(drone_info.alpha_belt) < 35:
roll = controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(drone_info.alpha_belt)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = Y_SP
else: controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
drone_info.role = 1
if len(distances) == 1:
if drone_info.alpha_belt < -145 or drone_info.alpha_belt > 145:
if drone_info.alpha_belt > 0:
angle_ref = drone_info.alpha_belt - 180
else: angle_ref = drone_info.alpha_belt + 180
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
elif len(distances) > 1:
front = dist_front.index(min(dist_front)) + 2 #this is the drone I don't have to consider
dist_front_nat.pop(front - 2)
go_90 = False
go_minus_90 = False
#choose pal angle
if len(distances) == 2:
if front == 2:
pal_angle = drone_3_info.alpha_belt
else: #should be 3
pal_angle = drone_2_info.alpha_belt
if drone_info.alpha_belt > 0 and pal_angle > 0:
if drone_info.alpha_belt > pal_angle: #I have to go to the other side
go_minus_90 = True
else:
go_90 = True
elif drone_info.alpha_belt < 0 and pal_angle < 0:
if drone_info.alpha_belt < pal_angle:
go_90 = True
else:
go_minus_90 = True
else:
if drone_info.alpha_belt > 0:
go_90 = True
else: go_minus_90 = True
elif len(distances) == 3:
all_same_side = False
go_behind = False
dist_pos = filter(lambda x: x > 0, dist_front_nat)
dist_neg = filter(lambda x: x < 0, dist_front_nat)
if ((dist_neg == [] and drone_info.alpha_belt > 0) or
(dist_pos == [] and drone_info.alpha_belt < 0)):
all_same_side = True
if ((all_same_side and drone_info.alpha_belt > 0 and drone_info.alpha_belt > min(dist_pos)
and drone_info.alpha_belt < max(dist_pos)) or
(all_same_side and drone_info.alpha_belt < 0 and
drone_info.alpha_belt < max(dist_neg) and drone_info.alpha_belt > min(dist_neg))):
go_behind = True
else: #not all on the same side or on the same side but I don't have to go behind
if all_same_side:
if drone_info.alpha_belt > 0:
if drone_info.alpha_belt < min(dist_pos):
go_90 = True
elif drone_info.alpha_belt > max(dist_pos):
go_minus_90 = True
else:
if drone_info.alpha_belt > max(dist_neg):
go_minus_90 = True
elif drone_info.alpha_belt < min(dist_neg):
go_90 = True
else:
if drone_info.alpha_belt > 0:
if not dist_pos == []:
pal_pos = dist_pos[0]
if drone_info.alpha_belt < pal_pos:
go_90 = True
else: go_behind = True
else: go_90 = True
else:
if not dist_neg == []:
pal_neg = dist_neg[0]
if drone_info.alpha_belt > pal_neg:
go_minus_90 = True
else: go_behind = True
else: go_minus_90 = True
if go_behind:
if drone_info.alpha_belt < -145 or drone_info.alpha_belt > 145:
if drone_info.alpha_belt > 0:
angle_ref = drone_info.alpha_belt - 180
else: angle_ref = drone_info.alpha_belt + 180
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
if go_90:
if drone_info.alpha_belt > 0:
if drone_info.alpha_belt > 55 or drone_info.alpha_belt < 125:
angle_ref = drone_info.alpha_belt - 90
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt < 90: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
elif go_minus_90:
if drone_info.alpha_belt < 0:
if drone_info.alpha_belt < -55 or drone_info.alpha_belt > -125:
angle_ref = drone_info.alpha_belt + 90
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt < -90: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
'''else:
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)
controller_speed_y.setpoint = -(dd - ds)/float(150)
if controller_speed_y.setpoint > Y_SP: controller_speed_y.setpoint = Y_SP
elif controller_speed_y.setpoint < -Y_SP: controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)'''
return roll
def _calc_radians(self):
"""
Calculates the radians in range 0 to 2π from given decimal degrees
"""
radian = deg_to_rad(self.decimal_degree)
return radian
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 receive_navdata(self, navdata):
if navdata.state == DroneStatus.TakingOff:
self.start_time = rospy.Time.now()
if self.last_altd is None or self.last_rotZ is None:
self.update_time()
self.last_altd = navdata.altd/1e3
self.last_rotZ = navdata.rotZ
else:
if self.using_odometry:
measures_corrupted = False
alpha_variation = navdata.rotZ - self.last_rotZ
while alpha_variation > 180: alpha_variation -= 360
while alpha_variation < -180: alpha_variation += 360
z_variation = navdata.altd/1e3 - self.last_altd
if (rospy.Time.now() - self.start_time).to_sec() < 5:
z_tol = 0.7
else: z_tol = 0.12
if abs(alpha_variation) > 15 or abs(z_variation) > z_tol:
measures_corrupted = True
rospy.loginfo("Measurements corrupted!")
if not measures_corrupted:
self.alpha = self.alpha + alpha_variation
while self.alpha > 180: self.alpha -= 360
while self.alpha < -180: self.alpha += 360
self.z = self.z + z_variation
now = rospy.Time.now()
delta_t = (now - self.prev_time).to_sec()
self.prev_time = now
alpha_rad = utils.deg_to_rad(self.alpha)
vx = navdata.vx/1e3
vy = navdata.vy/1e3
self.x = self.x + delta_t*(vx*math.cos(alpha_rad) - vy*math.sin(alpha_rad))
self.y = self.y + delta_t*(vx*math.sin(alpha_rad) + vy*math.cos(alpha_rad))
self.pubPosition.publish(Position(self.alpha, self.x, self.y, self.z, self.tag_located, None, -1))
self.last_rotZ = navdata.rotZ
self.last_altd = navdata.altd/1e3
else:
self.update_time()
self.last_rotZ = navdata.rotZ
self.last_altd = navdata.altd/1e3
#event if I'm not using odometry, keep up to date useful datas
else:
self.update_time()
self.last_rotZ = navdata.rotZ
self.last_altd = navdata.altd/1e3
def __init__(self, data):
self.floor_height = data["floor_height"]
self.ceiling_height = data["ceiling_height"]
# Floor texture.
floor_texture_path = data.get("floor_texture", "textures/default.png")
floor_texture_image = pyglet.image.load(floor_texture_path)
self.floor_texture = floor_texture_image.get_mipmapped_texture()
# Ceiling texture
ceiling_texture_path = data.get("ceiling_texture",
"textures/default.png")
ceiling_texture_image = pyglet.image.load(ceiling_texture_path)
self.ceiling_texture = ceiling_texture_image.get_mipmapped_texture()
# Wall texture.
wall_texture_path = data.get("wall_texture", "textures/default.png")
wall_texture_image = pyglet.image.load(wall_texture_path)
self.wall_texture = wall_texture_image.get_mipmapped_texture()
self.wall_texture_fit = data.get("wall_texture_fit",
WALL_TEXTURE_FIT_PER_WALL)
# Texture scales (1.0 means the texture is applied to 1m squares)
self.floor_texture_scale = data.get("floor_texture_scale", 1.0)
self.ceiling_texture_scale = data.get("ceiling_texture_scale", 1.0)
self.wall_texture_scale = data.get("wall_texture_scale", 1.0)
# Texture rotation, degrees
self.floor_texture_angle = data.get("floor_texture_angle", 0.0)
self.ceiling_texture_angle = data.get("ceiling_texture_angle", 0.0)
self.floor_texture_angle = utils.deg_to_rad(self.floor_texture_angle)
self.ceiling_texture_angle = utils.deg_to_rad(
self.ceiling_texture_angle)
# Light emission
self.emit = data.get("emit", 0.0)
# Wall vertex data, ordered clockwise
self.vertices = []
for vertex in data["vertices"]:
self.vertices.append(tuple(vertex))
# Correct incorrect winding
try:
self.check_winding()
except InvalidRoomError:
self.vertices.reverse()
# Check for other errors
self.check_walls()
# Walls shared with other rooms; key = wall index, value = other room
self.shared_walls = {}
# Work out bounding box
min_x = max_x = self.vertices[0][0]
min_y = max_y = self.vertices[0][1]
for x, y in self.vertices:
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
self.bounding_box = (min_x, max_x, min_y, max_y)
# Texture coordinates for walls (uses same indexes as self.vertices)
self.wall_texture_vertices = None
self.wall_lightmap_vertices = None
self.wall_texture_floor_height = None
self.wall_texture_ceiling_height = None
self.wall_lightmap_floor_height = None
self.wall_lightmap_ceiling_height = None
# Triangulated data (generated later)
self.floor_data_vbo = GLuint()
self.ceiling_data_vbo = GLuint()
self.wall_data_vbo = GLuint()
self.floor_data_count = 0
self.ceiling_data_count = 0
self.wall_data_count = 0
glGenBuffers(1, self.floor_data_vbo)
glGenBuffers(1, self.ceiling_data_vbo)
glGenBuffers(1, self.wall_data_vbo)
# Update texture and lightmap coords
self.generate_wall_tex_coords()
# Meshes
self.meshes = []
for mesh_data in data.get("meshes", []):
self.meshes.append(Mesh(mesh_data, self))
# self.triangles = []
self.wall_triangles = []
