def picture(self):
'''In this scenario the drone captures a picture of a requested object
'''
print highlight_magenta(
' PICTURE STARTED ')
print magenta("flying to start point")
target = 0
while target == 0:
view_list = self.ids
if [21] in view_list and [22] in view_list and len(view_list) == 3:
view_list = view_list[view_list != [21]]
view_list = view_list[view_list != [22]]
view_list = view_list[0]
target = view_list - 35
print magenta("start point found")
print magenta(" >>> fly to target " + str(target) +
" <<< ")
rospy.sleep(0.5)
if target == 1:
tag_left = [23]
tag_right = [24]
elif target == 2:
tag_left = [25]
tag_right = [26]
elif target == 3:
tag_left = [27]
tag_right = [28]
elif target == 4:
tag_left = [29]
tag_right = [30]
while target != 0:
if tag_left in self.ids and tag_right in self.ids: # voor een goeie foto
print magenta("target " + str(target) + " found")
target = 0
cv2.imshow("preview", self.frame)
cv2.waitKey()
rospy.sleep(0.5)
self.task_str = ""
print highlight_magenta(
' PICTURE COMPLETED ')
def guide(self):
''' In this scenario a person is guided to a certain object
'''
print highlight_magenta(' GUIDE STARTED ')
print magenta("flying to start point")
target = 0
while target == 0:
view_list = self.ids
if [21] in view_list and [22] in view_list and len(view_list) == 3:
view_list = view_list[view_list != [21]]
view_list = view_list[view_list != [22]]
view_list = view_list[0]
target = view_list - 35
print magenta("start point found")
print magenta(" >>> fly to target " + str(target) +
" <<< ")
rospy.sleep(0.5)
if target == 1:
tag_left = [23]
tag_right = [24]
elif target == 2:
tag_left = [25]
tag_right = [26]
elif target == 3:
tag_left = [27]
tag_right = [28]
elif target == 4:
tag_left = [29]
tag_right = [30]
while target != 0:
if tag_left in self.ids and tag_right in self.ids:
print magenta("target " + str(target) + " found")
target = 0
rospy.sleep(0.5)
self.task_str = ""
print highlight_magenta(' GUIDE COMPLETED ')
def events(self):
'''This function enables the actions on obstacles and or stoplight
'''
if [15] in self.ids and not self.obstacle_1:
print magenta("Watch out! Unknown obstacle ahead!")
self.obstacle_1 = True
elif self.obstacle_1 and [15] not in self.ids:
self.obstacle_1 = False
if [18] in self.ids:
print magenta(
"Watch out! Dynamic obstacle ahead! Please land and wait for 10 seconds"
)
rospy.sleep(10)
print magenta("You are allowed to start again")
while [14] in self.ids and [13] not in self.ids:
print highlight_red('The light is red! You can not pass.')
rospy.sleep(2)
while [13] in self.ids:
print highlight_green('The light is green! you can proceed.')
rospy.sleep(2)
def count(self):
''' This scenario lets the drone count all the objects on a predefined trajectory
'''
print highlight_magenta(
' COUNTING STARTED ')
print magenta('fly to start point!')
counting_list = [[50], [51], [52], [53], [54], [55], [56], [57], [58],
[59], [60], [61], [62], [63], [64], [65], [66], [67],
[68], [69], [70], [71], [72], [73], [74], [75], [76],
[77], [78], [79]]
red_c = 0
blue_c = 0
green_c = 0
counted_list = []
while [11] not in self.ids:
rospy.sleep(0.5)
print magenta('starting to count')
while [12] not in self.ids:
for i in self.ids:
if i in counting_list:
counting_list.remove(i)
counted_list.append(i)
print magenta(' >>> found one <<< ')
rospy.sleep(0.5)
for i in counted_list:
if 50 <= i[0] <= 59:
red_c += 1
elif 60 <= i[0] <= 69:
blue_c += 1
elif 70 <= i[0] <= 79:
green_c += 1
print highlight_magenta(
' REPORT ')
print red(' red:' + str(red_c))
print blue(' blue:' + str(blue_c))
print green(' green:' + str(green_c))
print highlight_magenta(
' COUNTING COMPLETED ')
self.task_str = ""
def update(self, cmd):
"""Updates the motionplanner and the deployer solving the Point2point
problem. Publishes trajectories resulting from calculations.
Args:
cmd : contains data sent over Trigger topic.
"""
# In case goal has changed: set new goal.
if cmd.goal_pos != self._goal:
self._goal = cmd.goal_pos
self._vehicle.set_initial_conditions(
[
cmd.pos_state.position.x, cmd.pos_state.position.y,
cmd.pos_state.position.z
],
# [cmd.vel_state.x, cmd.vel_state.y, cmd.vel_state.z]
)
self._vehicle.set_terminal_conditions([
self._goal.position.x, self._goal.position.y,
self._goal.position.z
])
self._deployer.reset()
print magenta('---- Motionplanner received a new goal -'
' deployer resetted ----')
state0 = [
cmd.pos_state.position.x, cmd.pos_state.position.y,
cmd.pos_state.position.z
]
input0 = [cmd.vel_state.x, cmd.vel_state.y, cmd.vel_state.z]
for k in range(self.n_dyn_obst):
pos = cmd.dyn_obstacles[k].pose
vel = cmd.dyn_obstacles[k].velocity
# Dirty fix necessary to make dynamic obstacle work in OMG-tools.
# -> NIET OKE, gedverdekke Ruben.
pos = np.round(pos, 1)
vel = np.round(vel, 1)
obst_i = k + self.n_stat_obst
(self._deployer.problem.environment.obstacles[obst_i].set_state({
'position':
pos,
'velocity':
vel
}))
trajectories = self._deployer.update(cmd.current_time,
state0) # input0)
calc_succeeded = True
return_status = self._deployer.problem.problem.stats()['return_status']
if (return_status != 'Solve_Succeeded'):
print highlight_red(return_status, ' -- brake! ')
calc_succeeded = False
self._result = Trajectories(u_traj=trajectories['input'][0, :],
v_traj=trajectories['input'][1, :],
w_traj=trajectories['input'][2, :],
x_traj=trajectories['state'][0, :],
y_traj=trajectories['state'][1, :],
z_traj=trajectories['state'][2, :],
success=calc_succeeded)
print(str(max(trajectories['input'][0, :])))
print(str(max(trajectories['input'][1, :])))
print(str(max(trajectories['input'][2, :])))
self._mp_result_topic.publish(self._result)
def magenta(self):
return ansi(c.magenta(self.txt))
def __init__(self, name):
self.name = name
self.printable_name = bold(magenta(self.name))
self.score = 0
module=col.cyan(('{r.module}')),
levelname=col.cyan('{r.levelname}'),
msg=col.bold(col.yellow('{r.msg}')),
),
'INFO': '{asctime} {module} {levelname} {msg}'.format(
asctime=col.highlight_blue('{r.asctime}'),
module=col.cyan(('{r.module}')),
levelname=col.green('{r.levelname}'),
msg=col.bold(col.yellow('{r.msg}')),
),
'WARNING': '{asctime} {module} {levelname} {msg}'.format(
asctime=col.highlight_blue('{r.asctime}'),
module=col.cyan(('{r.module}')),
levelname=col.bold(col.magenta('{r.levelname}')),
msg=col.bold(col.yellow('{r.msg}')),
),
'ERROR': '{asctime} {module} {levelname} {msg}'.format(
asctime=col.highlight_blue('{r.asctime}'),
module=col.cyan(('{r.module}')),
levelname=col.red('{r.levelname}'),
msg=col.red('{r.msg}'),
),
'CRITICAL': '{asctime} {module} {levelname} {msg}'.format(
asctime=col.highlight_blue('{r.asctime}'),
module=col.cyan(('{r.module}')),
levelname=col.bold(col.red('{r.levelname}')),
msg=col.red('{r.msg}'),
def inventory(self):
''' In this scenario an inventory is made of several objects
'''
inventory_list = [1, 2, 3, 4, 5]
print highlight_magenta(
' INVENTORY STARTED ')
print magenta('making inventory of items:' + str(inventory_list))
report = ["", "", "", "", "", "", ""]
inventory_list.append('home')
while len(inventory_list) > 1:
if [21] in self.ids and [22] in self.ids and 1 in inventory_list:
print magenta(" >>> target 1 <<< ")
report[1] = " " + str(
np.count_nonzero(self.ids == [31])) + " of element 1"
inventory_list.remove(1)
print magenta(' fly to machine ' + str(inventory_list[0]))
if [23] in self.ids and [24] in self.ids and 2 in inventory_list:
print magenta(" >>> target 2 <<< ")
report[2] = " " + str(
np.count_nonzero(self.ids == [32])) + " of element 2"
inventory_list.remove(2)
print magenta(' fly to machine ' + str(inventory_list[0]))
if [25] in self.ids and [26] in self.ids and 3 in inventory_list:
print magenta(" >>> target 3 <<< ")
report[3] = " " + str(
np.count_nonzero(self.ids == [33])) + " of element 3"
inventory_list.remove(3)
print magenta(' fly to machine ' + str(inventory_list[0]))
if [27] in self.ids and [28] in self.ids and 4 in inventory_list:
print magenta(" >>> target 4 <<< ")
report[4] = " " + str(
np.count_nonzero(self.ids == [34])) + " of element 4"
inventory_list.remove(4)
print magenta(' fly to machine ' + str(inventory_list[0]))
if [29] in self.ids and [30] in self.ids and 5 in inventory_list:
print magenta(" >>> target 5 <<< ")
report[5] = " " + str(
np.count_nonzero(self.ids == [35])) + " of element 5"
inventory_list.remove(5)
print magenta(' fly to machine ' + str(inventory_list[0]))
rospy.sleep(0.5)
print highlight_magenta(
' REPORT ')
for i in range(len(report)):
if report[i] != "":
print magenta(report[i])
self.task_str = ""
print highlight_magenta(
' INVENTORY COMPLETED ')
def home_inspection_smart(self):
''' this scenario lets the drone inspect several objects if failing or functional
However, each item influences the objects that will have to be inspected later on.
'''
inspection_list = {1}
dependency_dict = {
"M1": {2, 3},
"M2": {5},
"M3": {},
"M4": {},
"M5": {},
"M6": {},
"M7": {},
"M8": {},
"M9": {},
}
print highlight_magenta(
' INSPECTION STARTED ')
print magenta('inspecting machines' + str(tuple(inspection_list)))
report = [0, 0, 0, 0, 0, 0]
print magenta(' fly to machine ' +
str(random.choice(tuple(inspection_list))))
while len(inspection_list) > 0:
self.events()
if [21] in self.ids and [22] in self.ids and 1 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 1 ok <<< ")
inspection_list.discard(1)
report[1] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 1 not ok <<< ")
inspection_list.discard(1)
inspection_list = inspection_list.union(
dependency_dict["M1"])
report[1] = 2
else:
print magenta(
" >>> Machine 1 inconclusive <<< ")
inspection_list.discard(1)
inspection_list = inspection_list.union(
dependency_dict["M1"])
report[1] = 3
if len(inspection_list) > 0:
print magenta(' machines to inspect' +
str(tuple(inspection_list)))
print magenta(
' fly to machine ' +
str(random.choice(tuple(inspection_list))))
else:
print magenta(' fly home')
else:
print magenta("other marker or not enclosed")
if [23] in self.ids and [24] in self.ids and 2 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 2 ok <<< ")
inspection_list.discard(2)
report[2] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 2 not ok <<< ")
inspection_list.discard(2)
inspection_list = inspection_list.union(
dependency_dict["M2"])
report[2] = 2
else:
print magenta(
" >>> Machine 2 inconclusive <<< ")
inspection_list.discard(2)
inspection_list = inspection_list.union(
dependency_dict["M2"])
report[2] = 3
if len(inspection_list) > 0:
print magenta(' machines to inspect' +
str(tuple(inspection_list)))
print magenta(
' fly to machine ' +
str(random.choice(tuple(inspection_list))))
else:
print magenta(' fly home')
if [25] in self.ids and [26] in self.ids and 3 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 3 ok <<< ")
inspection_list.discard(3)
report[3] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 3 not ok <<< ")
inspection_list.discard(3)
inspection_list = inspection_list.union(
dependency_dict["M3"])
report[3] = 2
else:
print magenta(
" >>> Machine 3 inconclusive <<< ")
inspection_list.discard(3)
inspection_list = inspection_list.union(
dependency_dict["M3"])
report[3] = 3
if len(inspection_list) > 0:
print magenta(' machines to inspect' +
str(tuple(inspection_list)))
print magenta(
' fly to machine ' +
str(random.choice(tuple(inspection_list))))
else:
print magenta(' fly home')
if [27] in self.ids and [28] in self.ids and 4 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 4 ok <<< ")
inspection_list.discard(4)
report[4] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 4 not ok <<< ")
inspection_list.discard(4)
inspection_list = inspection_list.union(
dependency_dict["M4"])
report[4] = 2
else:
print magenta(
" >>> Machine 4 inconclusive <<< ")
inspection_list.discard(4)
inspection_list = inspection_list.union(
dependency_dict["M4"])
report[4] = 3
if len(inspection_list) > 0:
print magenta(' machines to inspect' +
str(tuple(inspection_list)))
print magenta(
' fly to machine ' +
str(random.choice(tuple(inspection_list))))
else:
print magenta(' fly home')
if [29] in self.ids and [30] in self.ids and 5 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 5 ok <<< ")
inspection_list.discard(5)
report[5] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 5 not ok <<< ")
inspection_list.discard(5)
inspection_list = inspection_list.union(
dependency_dict["M5"])
report[5] = 2
else:
print magenta(
" >>> Machine 5 inconclusive <<< ")
inspection_list.discard(5)
inspection_list = inspection_list.union(
dependency_dict["M5"])
report[5] = 3
if len(inspection_list) > 0:
print magenta(' machines to inspect' +
str(tuple(inspection_list)))
print magenta(
' fly to machine ' +
str(random.choice(tuple(inspection_list))))
else:
print magenta(' fly home')
rospy.sleep(0.5)
print highlight_magenta(
' REPORT ')
for i in range(len(report)):
if report[i] == 1:
print green(' MACHINE ' + str(i) + ' OK')
elif report[i] == 2:
print red(' MACHINE ' + str(i) + ' NOT OK')
elif report[i] == 3:
print red(' MACHINE ' + str(i) + ' INCONCLUSIVE')
print highlight_magenta(
' INSPECTION COMPLETED ')
self.task_str = ""
def home_inspection(self):
''' this scenario lets the drone inspect several objects if failing or functional
'''
inspection_list = [1, 3, 5]
print highlight_magenta(
' INSPECTION STARTED ')
print magenta('inspecting machines' + str(inspection_list))
report = [0, 0, 0, 0, 0, 0]
inspection_list.append('home')
print magenta(' fly to machine ' + str(inspection_list[0]))
while len(inspection_list) > 1:
self.events()
if [21] in self.ids and [22] in self.ids and 1 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 1 ok <<< ")
inspection_list.remove(1)
report[1] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 1 not ok <<< ")
inspection_list.remove(1)
report[1] = 2
else:
print magenta(
" >>> Machine 1 inconclusive <<< ")
inspection_list.remove(1)
report[1] = 3
print magenta(' fly to machine ' + str(inspection_list[0]))
else:
print magenta("other marker or not enclosed")
if [23] in self.ids and [24] in self.ids and 2 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 2 ok <<< ")
inspection_list.remove(2)
report[2] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 2 not ok <<< ")
inspection_list.remove(2)
report[2] = 2
else:
print magenta(
" >>> Machine 2 inconclusive <<< ")
inspection_list.remove(2)
report[2] = 3
print magenta(' fly to machine ' + str(inspection_list[0]))
else:
print magenta("other marker or not enclosed")
if [25] in self.ids and [26] in self.ids and 3 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 3 ok <<< ")
inspection_list.remove(3)
report[3] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 3 not ok <<< ")
inspection_list.remove(3)
report[3] = 2
else:
print magenta(
" >>> Machine 3 inconclusive <<< ")
inspection_list.remove(3)
report[3] = 3
print magenta(' fly to machine ' + str(inspection_list[0]))
else:
print magenta("other marker or not enclosed")
if [27] in self.ids and [28] in self.ids and 4 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 4 ok <<< ")
inspection_list.remove(4)
report[4] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 4 not ok <<< ")
inspection_list.remove(4)
report[4] = 2
else:
print magenta(
" >>> Machine 4 inconclusive <<< ")
inspection_list.remove(4)
report[4] = 3
print magenta(' fly to machine ' + str(inspection_list[0]))
else:
print magenta("other marker or not enclosed")
if [29] in self.ids and [30] in self.ids and 5 in inspection_list:
if len(self.ids) == 3:
if [1] in self.ids:
print magenta(" >>> Machine 5 ok <<< ")
inspection_list.remove(5)
report[5] = 1
elif [2] in self.ids:
print magenta(" >>> Machine 5 not ok <<< ")
inspection_list.remove(5)
report[5] = 2
else:
print magenta(
" >>> Machine 5 inconclusive <<< ")
inspection_list.remove(5)
report[5] = 3
print magenta(' fly to machine ' + str(inspection_list[0]))
else:
print magenta("other marker or not enclosed")
rospy.sleep(0.5)
print highlight_magenta(
' REPORT ')
for i in range(len(report)):
if report[i] == 1:
print green(' MACHINE ' + str(i) + ' OK')
elif report[i] == 2:
print red(' MACHINE ' + str(i) + ' NOT OK')
elif report[i] == 3:
print red(' MACHINE ' + str(i) + ' INCONCLUSIVE')
print highlight_magenta(
' INSPECTION COMPLETED ')
self.task_str = ""