def go_to_point(coordinates='absolute', point=[]):
if len(point) == 0:
raise Exception('Empty point!')
print('-> go to point ' + str(point) + ' mode ' + coordinates)
if coordinates == 'absolute':
activated, uid = api.activateBehavior(behavior_names['go_to_point'],
coordinates=point)
else:
activated, uid = api.activateBehavior(behavior_names['go_to_point'],
relative_coordinates=point)
return uid
def run_mission(paths=[]):
print('Starting mission...')
activated, uid = api.activateBehavior(behavior_names['slam'])
if not activated:
raise Exception('Unable to continue without SLAM')
rospy.sleep(0.2)
result = api.executeBehavior(behavior_names['take_off'])
rospy.sleep(1)
max_tries = 5
times = []
full_test_elapsed = 0
fails = 0
for path in paths:
str_path = None
tries = 0
print('-> follow path to point {}'.format(str(path)))
while str_path is None and tries < max_tries:
print('-> get a valid path tries {}'.format(str(tries)))
result = api.executeBehavior(behavior_names['generate'],
coordinates=path)
str_path = get_path()
print('-> got path back')
tries += 1
start = time.time()
result = api.executeBehavior(behavior_names['follow'], path=str_path)
end = time.time()
elapsed = end - start
times.append(elapsed)
full_test_elapsed += elapsed
print('-> result {}'.format(result))
if result != 'GOAL_ACHIEVED':
fails += 1
print('Finish mission...')
return full_test_elapsed, times, fails
def run_mission(coordinates='absolute', points=[]):
print('Starting mission...')
activated, uid = api.activateBehavior(behavior_names['slam'])
if not activated:
raise Exception('Unable to continue without SLAM')
rospy.sleep(0.2)
print('-> take off')
result = api.executeBehavior(behavior_names['take_off'])
print('-> result {}'.format(result))
times = []
fails = 0
for point in points:
print('-> go to point {}'.format(str(point)))
start = time.time()
result = api.executeBehavior(behavior_names['go_to_point'], coordinates=point)
end = time.time()
elapsed = end - start
times.append(elapsed)
print('-> result {}'.format(result))
if result != 'GOAL_ACHIEVED':
fails += 1
print('-> land')
result = api.executeBehavior(behavior_names['land'])
print('-> result {}'.format(result))
print('Finish mission...')
return times, fails
def reset():
# rospy.init_node('reset')
print('Reset sequence started...')
uid = api.executeBehavior('LAND')
print('-> Landed')
gazebo_pub = rospy.ServiceProxy('/gazebo/reset_world', Empty)
gazebo_pub.call()
print('-> Gazebo worl reset')
planner_pub = rospy.Publisher('/drone11/move_base/syscommand', String, queue_size=1)
planner_pub.publish(String('reset'))
print('-> Planner reset')
activated, uid = api.activateBehavior('SELF_LOCALIZE_AND_MAP_BY_LIDAR')
print('-> SLAM')
rospy.sleep(0.5)
print('... done')
def run_mission(coordinates='absolute', points=[]):
print('run_mission', coordinates, points)
print('Starting mission...')
print('-> activate SLAM')
print(api.activateBehavior(behavior_names['slam']))
print('-> execute TAKE_OFF')
print(api.executeBehavior(behavior_names['take_off']))
char = get_input()
index = 0
while char != 'q' and index < len(points):
point = points[index]
go_to_point(coordinates=coordinates, point=point)
index += 1
char = get_input()
print(api.executeBehavior(behavior_names['land']))
print('Finish mission...')
def run_mission(paths=[]):
print('Starting mission...')
activated, uid = api.activateBehavior(behavior_names['slam'])
if not activated:
raise Exception('Unable to continue without SLAM')
rospy.sleep(0.2)
result = api.executeBehavior(behavior_names['take_off'])
times = []
fails = 0
for path in paths:
print('-> follow path with lenght {}'.format(str(len(path))))
start = time.time()
result = api.executeBehavior(behavior_names['follow'], path=path)
end = time.time()
elapsed = end - start
times.append(elapsed)
print('-> result {}'.format(result))
if result != 'GOAL_ACHIEVED':
fails += 1
print('Finish mission...')
return full_test_elapsed, times, fails
