def init_uav(self, arm=False, takeoff=False, verbose=False):
if verbose:
print("Starting init_uav(). Atm we have to manualy accept every new command, but just remove the `input()` from the code to do this faster.")
res_setMode = self.setmode_srv(0,'GUIDED')
if verbose:
print("Res of setmode:", res_setMode)
res_set_ref_mode = self.set_ref_frame_srv(1) # FRAME_BODY_NED = 8 works for velocities
if verbose:
print("Res of set_ref_frame", res_set_ref_mode)
rospy.sleep(1)
print("Waiting for ekf initialisation")
origin = GeoPointStamped()
origin.header = Header()
origin.header.stamp = rospy.Time.now()
origin.position = GeoPoint()
origin.position.latitude = self.OPERATION_POSITION[0]
origin.position.longitude = self.OPERATION_POSITION[1]
origin.position.altitude = self.OPERATION_POSITION[2]
self.set_origin_pub.publish(origin)
if verbose:
print("Published ",origin)
while(True):
res = self.arm()
if res.success:
break
rospy.sleep(1)
self.disarm()
# input("Ekf initialized?")
#time.sleep(40) # For some reason, it does not work if we use rospy.sleep(...)
if arm or takeoff:
self.arm()
if takeoff:
self.takeoff()
print("Uav is now initialized. We are done with waiting.")
def run_test(self, filename):
lines = []
index = 0
obstacles = []
map_file = ""
start = []
self.stats["time_limit"] = self.default_time_limit
parameter_file_names = []
try:
with open(filename, "r") as testfile:
for line in testfile:
line = line.strip()
if line.startswith("line"):
lines.append([float(f) for f in line.split(" ")[1:]])
print("Read line ", lines[-1])
assert len(lines[-1]
) == 4 # should be startX startY endX endY
elif line.startswith("start"):
start = [float(f) for f in line.split(" ")[1:]]
start[2] = math.radians(start[2])
# assert len(start) == 4 # x y heading speed # assume speed is zero?
elif line.startswith("obstacle"):
obs = [float(f) for f in line.split(" ")[1:]]
if len(obs) == 4:
obs.append(5)
obs.append(20)
obstacles.append(obs)
# ignore test-defined time limit (deprecated)
# elif line.startswith("time_limit"):
# self.stats["time_limit"] = float(line[10:])
elif line.startswith("map_file"):
map_file = line[8:].strip()
elif line.startswith("parameter_file"):
parameter_file_names.append(line[15:])
except IOError as err:
print("Couldn't find file: " + filename)
return
try:
# Convert to lat long
lines = [self.convert_line(line) for line in lines]
except rospy.ServiceException as exc:
print("Map to LatLong service did not process request: " +
str(exc))
# # wait until clock is initialized
# while rospy.get_time() == 0:
# pass
if rospy.get_time() == 0:
print("Simulation does not appear to be running yet. Exiting.")
return
# load parameters and update dynamic reconfigure
self.load_parameters(parameter_file_names)
# load map file, if any
if map_file:
current_path = os.getcwd()
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": current_path + "/" + map_file})
else:
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": ""})
self.test_name = filename
# create results directory
now = datetime.now()
results_dir_path = "../results/" + now.strftime("%Y_%m_%d/%H:%M:%S_" +
self.test_name)
if not os.path.exists(results_dir_path):
os.makedirs(results_dir_path)
# initialize bag
self.bag_lock.acquire()
self.bag = rosbag.Bag(results_dir_path + "/logs.bag", 'w')
self.bag_lock.release()
self.piloting_mode_publisher.publish("autonomous")
# Set up set_pose request
if start:
gps = GeoPointStamped()
gps.position = self.convert_point(start[0], start[1])
h = NavEulerStamped()
h.orientation.heading = start[2]
self.set_pose(gps, h)
else:
print("Warning: no start state read; using default start state")
self.reset_publisher.publish(True)
rospy.sleep(0.2) # Let simulator reset
# finish setting up obstacles
for o in obstacles:
o.append(rospy.get_time())
o[2] = math.radians(o[2])
self.start_time = rospy.get_time()
self.end_time = self.start_time + self.stats["time_limit"]
# set up both path_planner goal and display for lines
goal = path_planner.msg.path_plannerGoal()
goal.path.header.stamp = rospy.Time.now()
display_item = GeoVizItem()
display_item.id = "current_path"
for line in lines:
# now sending all lines at once, so points are to be read in pairs
display_points = GeoVizPointList()
display_points.color.r = 1.0
display_points.color.a = 1.0
display_points.size = 5.0
for p in line:
pose = GeoPoseStamped()
pose.pose.position = p
goal.path.poses.append(pose)
display_points.points.append(p)
# TODO! -- goal.speed?
display_item.lines.append(display_points)
self.display_publisher.publish(display_item)
self.test_running = True
self.path_planner_client.wait_for_server()
self.path_planner_client.send_goal(goal, self.done_callback,
self.active_callback,
self.feedback_callback)
# Spin and publish obstacle updates every 0.5s
self.spin_until_done(obstacles)
print("Test %s complete in %s seconds." %
(self.test_name, (rospy.get_time() - self.start_time)))
# remove line from display
if display_item:
display_item.lines = []
self.display_publisher.publish(display_item)
self.piloting_mode_publisher.publish("standby")
# reporting
self.write_results(results_dir_path)
self.reset_stats()
self.bag_lock.acquire()
self.bag.close()
self.bag = None
self.bag_lock.release()