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 = []