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()
