def judge(img_name): """Image's label file existence judgement. Also calculate degree(C) :return: Position 0: `False` if label file not exist, `True` otherwise. Position 1: `None` if encountering some error during calculation, `degree` otherwise. """ lbl_name = img_name.split('_')[0] + LBL_SUFFIX lbl_path = LBL_DIR + lbl_name if os.path.exists(lbl_path): model = cahv(lbl_path) if not model: return [True, None] degree = angle(coordinate=model['A'], axis=2, module=True) if not degree: print("something wrong with image `%s`" % img_name) return [True, None] return [True, degree] else: return [False, None]
def o_void(self, msg): o1 = 0 o2 = 0 a = angle() ang = int(a.get_angle()) if msg.ranges[ang] < 25: print("OBSTACLE AHEAD") for i in range(ang, ang - 10, -1): if msg.ranges[i] > 20 or msg.ranges[i] == float("inf"): o1 = i - 1 break for i in range(ang, ang + 10): if msg.ranges[i] > 20 or msg.ranges[i] == float("inf"): o2 = i - 1 break if 90 - o1 < 90 - o2: self.turn = "right" else: self.turn = "left" else: self.turn = "straight" self.get = 1
def act(): """ Arguments: None Returns: degrees to be twisted by the bot """ snapshot() pin_code = str(barcode(imgPath)) degrees,amount = angle(degrees,pin_code) return pin_code
def __init__(self, c): verts= [(angle(c,v),v) for v in c.verts] verts= sorted( verts , key=lambda x: x[0] ) verts= [v for a,v in verts] verts.append(verts[0]) sf.VertexArray.__init__(self, sf.PrimitiveType.LINES_STRIP, len(verts) ) col= sf.Color(32,32,32) for i in range(len(verts)): self[i].color=col self[i].position= verts[i]
def repel(self, step): for i in range(1,len(self)): for j in range(i): if self[i] in self[j].neighbors: assert self[j] in self[i].neighbors a=angle(self[j],self[i]) dx,dy = self[i]-self[j] dist= sqrt(dx*dx+dy*dy) push= 1.0/dist a+=1.5707*push push= sin(a)*push*step,cos(a)*push*step self[i].inertia+= push self[j].inertia-= push
def repel(self, step): for i in range(1, len(self)): for j in range(i): if self[i] in self[j].neighbors: assert self[j] in self[i].neighbors a = angle(self[j], self[i]) dx, dy = self[i] - self[j] dist = sqrt(dx * dx + dy * dy) push = 1.0 / dist a += 1.5707 * push push = sin(a) * push * step, cos(a) * push * step self[i].inertia += push self[j].inertia -= push
def call(): """ Return the angle between hour hands and min hands and the time now. """ try: while True: t = datetime.now() minute = t.minute hour = t.hour print(t, angle.angle(hour, minute)) time.sleep(60) except KeyboardInterrupt: print('安全退出')
def __init__(self, c): verts= [(angle(c,v),v) for v in c.verts] verts= sorted( verts , key=lambda x: x[0] ) verts= [v for a,v in verts] verts.append(verts[0]) verts.insert(0,c) sf.VertexArray.__init__(self, sf.PrimitiveType.TRIANGLES_FAN, len(verts) ) col= sf.Color.BLACK for i in range(len(verts)): self[i].color=col self[i].position=verts[i] self[0].color=sf.Color.GREEN
def o_void(self, msg): o1 = 0 o2 = 0 a = angle() ang_d = int(a.get_angle()) self.drone_angle = ang_d ang = self.drone_lidar(ang_d) # if 171 <= ang <= 359: # ang = 540 - ang # elif 0 <= ang <= 180: # ang = 180 - ang # print("Drone: " +str(ang_d)) # print("LIDAR: " +str(ang)) # print("Dist.: " + str(msg.ranges[ang])) # print(" ") # print(" ") if msg.ranges[ang] <= 35: for i in range(ang, 0, -1): if i < 0: j = 360 - i else: j = i if msg.ranges[j] > 35 or msg.ranges[j] == float("inf"): o1 = j break for i in range(ang, 360): if i > 359: j = 360 + i else: j = i if msg.ranges[j] > 35 or msg.ranges[j] == float("inf"): o2 = j break if abs(90 - o1) <= abs(90 - o2): self.turn = (o1 - 10) self.word = "right" else: self.turn = (o2 + 10) self.word = "left" else: self.turn = ang_d self.word = "straight" self.get = 1
def test_45_degree(self): self.assertLessEqual(abs(angle.angle((1, 0), (1, 1)) - np.pi / 4), self.eps)
def test_90_degree(self): self.assertLessEqual(abs(angle.angle((1, 0), (0, 1)) - np.pi / 2), self.eps)
def test_0_degree(self): self.assertLessEqual(abs(angle.angle((1, 0), (1, 0)) - 0), self.eps)
location, location, location, location] rate = rospy.Rate(10) shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size= 10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 #print("I am here") dev = [100,1,10,0,0,0,0] testy = 0 currs = angle() turn = obstacle_avoidance() while self.mode=="HOVER" and not rospy.is_shutdown(): ang = currs.get_angle() print(turn.get_turn()) #print("Direction angle: " + str(ang)) if testy < 50: #print("basic: " + str(testy)) location = [5,0,10,0,0,0,0] testy = testy+1 time.sleep(1)
def nohit(self): print("I am in no hitting") while True: #destination point dev = [45, 45, 20, 0, 0, 0, 0] rate = rospy.Rate(10) self.des_pose = self.copy_pose(self.curr_pose) shape = len(self.loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) print self.mode t = 0 currs = angle() turn = obstacle_avoidance() while self.mode == "SURVEY" and not rospy.is_shutdown(): curr = currs.get_angle() if t < 10: print("basic") self.loc = [5, 5, 20, 0, 0, 0, 0] t += 1 elif turn.get_turn() == "straight": print(turn.get_turn()) self.loc = dev else: print(turn.get_turn()) vx = dev[0] - curr.x vy = dev[1] - curr.y vx = vx / math.sqrt(int(vx) ^ 2 + int(vy) ^ 2) vy = vy / math.sqrt(int(vx) ^ 2 + int(vy) ^ 2) ux_r = 1 ux_l = -1 uy_r = ux_r * vx / vy uy_l = ux_l * vx / vy mag_r = math.sqrt(ux_r ^ 2 + uy_r ^ 2) mag_l = math.sqrt(ux_l ^ 2 + uy_l ^ 2) angle_r = math.atan2(uy_r, ux_r) angle_l = math.atan2(uy_l, ux_l) if angle_r < 0: angle_r += angle_r + 360 if angle_l < 0: angle_l += angle_l + 360 if angle_r > angle_l: t = ux_r ux_r = ux_l ux_l = t t = uy_r uy_r = uy_l uy_l = t right = [ux_r / mag_r, uy_r / mag_r, 20, 0, 0, 0, 0] left = [ux_l / mag_l, uy_l / mag_l, 20, 0, 0, 0, 0] if turn.get_turn() == "right": self.loc += right if turn.get_turn() == "left": self.loc += left if self.waypointIndex == shape: self.waypointIndex = 1 # resetting the waypoint index if self.isReadyToFly: des_x = self.loc[0] des_y = self.loc[1] des_z = self.loc[2] self.des_pose.pose.position.x = des_x self.des_pose.pose.position.y = des_y self.des_pose.pose.position.z = des_z self.des_pose.pose.orientation.x = self.loc[3] self.des_pose.pose.orientation.y = self.loc[4] self.des_pose.pose.orientation.z = self.loc[5] self.des_pose.pose.orientation.w = self.loc[6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z pose_pub.publish(self.des_pose) rate.sleep()
def test_zero_length(self): self.assertTrue(np.isnan(angle.angle((1, 0), (0, 0)))) self.assertTrue(np.isnan(angle.angle((0, 0), (1, 0)))) self.assertTrue(np.isnan(angle.angle((0, 0), (0, 0))))
def test_angle(): assert angle(3) == 180 assert angle(4) == 360
def hover(self): location = self.hover_loc loc = [ location, location, location, location, location, location, location, location, location ] rate = rospy.Rate(10) shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 #print("I am here") dev = [100, 0, 10, 0, 0, 0, 0] testy = 0 currs = angle() turn = obstacle_avoidance() while self.mode == "HOVER" and not rospy.is_shutdown(): ang = currs.get_angle() print(turn.get_turn()) #print("Direction angle: " + str(ang)) if testy < 50: #print("basic: " + str(testy)) location = [5, 0, 10, 0, 0, 0, 0] testy = testy + 1 time.sleep(1) elif turn.get_turn() == "straight": mag = math.sqrt((dev[0] - location[0])**2 + (dev[1] - location[1])**2) loc = [(dev[0] - location[0]) / mag + location[0], (dev[1] - location[1]) / mag + location[1], 10, 0, 0, 0, 0] location = loc else: vx = dev[0] - location[0] vy = dev[1] - location[1] vx = vx / math.sqrt(int(vx)**2 + int(vy)**2) vy = vy / math.sqrt(int(vx)**2 + int(vy)**2) ux_r = 1 ux_l = -1 uy_r = ux_r * vx / vy uy_l = ux_l * vx / vy mag_r = math.sqrt(int(ux_r)**2 + int(uy_r)**2) mag_l = math.sqrt(ux_l**2 + uy_l**2) angle_r = math.atan2(uy_r, ux_r) angle_l = math.atan2(uy_l, ux_l) if angle_r < 0: angle_r += angle_r + 360 if angle_l < 0: angle_l += angle_l + 360 if angle_r > angle_l: t = ux_r ux_r = ux_l ux_l = t t = uy_r uy_r = uy_l uy_l = t right = [ ux_r / mag_r + location[0], uy_r / mag_r + location[1], 20, 0, 0, 0, 0 ] left = [ ux_l / mag_l + location[0], uy_l / mag_l + location[1], 20, 0, 0, 0, 0 ] if turn.get_turn() == "right": location = right if turn.get_turn() == "left": location = left #print("Location is: " + str(location)) if waypoint_index == shape: waypoint_index = 0 # changing the way point index to 0 sim_ctr = sim_ctr + 1 print "HOVER STOP COUNTER:" + str(sim_ctr) if self.isReadyToFly: des_x = location[0] des_y = location[1] des_z = location[2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = location[3] des_pose.pose.orientation.y = location[4] des_pose.pose.orientation.z = location[5] des_pose.pose.orientation.w = location[6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z if sim_ctr == 50: pass for t in range(10000): pose_pub.publish(des_pose) rate.sleep()