def on_load_free_space_points_button_click(self,
checked=False,
filename=None):
if filename is None:
filename = QFileDialog.getOpenFileName(
self,
'Import waypoints', #path,
"Waypoint YAML files (*.yaml)")
if filename and len(filename) > 0:
filename = filename[0]
else:
print("Invalid file path")
return
try:
self.free_space_points = utils.load_waypoints(filename)
self.free_spheres_worker.publish_spheres(
self.free_space_points['x'], self.free_space_points['y'],
self.free_space_points['z'],
[self.free_sphere_radius] * len(self.free_space_points['x']))
except KeyError:
print("Invalid file format")
return
except Exception as e:
print("Unknown error loading waypoints from {}".format(filename))
print(e)
return
def test_input_restrict_freespace(self):
# With single input l_max and A_max
order=dict(x=9, y=9, z=5, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
l_max = 1.0
A_max = 10.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order,
restrict_freespace=True,l_max=l_max,A_max=A_max)
def test_input_restrict_freespace_vector_l_A(self):
order=dict(x=9, y=9, z=9, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
l_max = np.ones(waypoints['x'].shape[1]-1)*0.5
l_max[0] = 3.0
l_max[5] = 2.0
l_max[-1] = 1.0
A_max = np.ones(waypoints['x'].shape[1]-1)*20.0
A_max[0] = 10.0
A_max[-1] = 10.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order,
restrict_freespace=True,l_max=l_max,A_max=A_max)
def on_load_waypoints_button_click(self):
filename = QFileDialog.getOpenFileName(self, 'Import waypoints', '',
"Waypoint YAML files (*.yaml)")
if filename and len(filename) > 0:
filename = filename[0]
else:
print("Invalid file path")
return
try:
self.in_waypoints = utils.load_waypoints(filename)
except KeyError:
print("Invalid file format")
return
except Exception as e:
print("Unknown error loading waypoints from {}".format(filename))
print(e)
return
def on_load_waypoints_button_click(self, checked=False, filename=None):
if filename is None:
filename = QFileDialog.getOpenFileName(
self,
'Import waypoints', #path,
"Waypoint YAML files (*.yaml)")
if filename and len(filename) > 0:
filename = filename[0]
else:
print("Invalid file path")
return
try:
self.in_waypoints = utils.load_waypoints(filename)
self.global_dict['full_waypoints'] = self.in_waypoints
print("Loaded waypoints from {}".format(filename))
except KeyError:
print("Invalid file format")
return
except Exception as e:
print("Unknown error loading waypoints from {}".format(filename))
print(e)
return
def test_basic_input_order(self):
order=dict(x=9, y=9, z=5, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order)