def newQuarticBezier(connectnode, targetnode, T, reverse=False):
if reverse:
end_acc = connectnode.next_traj.acc(0)
p4 = connectnode.pos
p3 = -T/4 * connectnode.vel + p4
p2 = T**2/12 * end_acc + 2*p3 - p4
p0 = targetnode.pos
p1 = T/4 * targetnode.vel + p0
# Ad-hoc fix?
p2 = Vec3(p2.x, p2.y, (p0.z + p4.z) / 2)
else:
start_acc = connectnode.prev_traj.acc(connectnode.prev_traj.T)
p0 = connectnode.pos
p1 = T/4 * connectnode.vel + p0
p2 = T**2/12 * start_acc + 2*p1 - p0
p4 = targetnode.pos
p3 = -T/4 * targetnode.vel + p4
# Ad-hoc fix?
p2 = Vec3(p2.x, p2.y, (p0.z + p4.z) / 2)
return Bezier([p0, p1, p2, p3, p4], T)
def create_waypoints(self, gates):
# Get start pose from tf tree
start_pose = newPoseStamped(0, 0, 0, 0, 0, 0, self.base_frame)
start_pose = self.tf_buff.transform(start_pose, 'map',
rospy.Duration(0.2))
roll, pitch, yaw = euler_from_quaternion(
(start_pose.pose.orientation.x, start_pose.pose.orientation.y,
start_pose.pose.orientation.z, start_pose.pose.orientation.w))
# Start creating waypoints
start_wp = Waypoint('start', start_pose.pose.position.x,
start_pose.pose.position.y,
start_pose.pose.position.z, 0.0, 0.0, 0.0, yaw)
# Waypoints in front of and after each gate
waypoints = [start_wp]
for gate in self.map.gates:
before_wp, after_wp = self.gate_to_waypoints(gate)
waypoints.extend([before_wp, after_wp])
# Last waypoint 0.5 m after last gate
yaw = self.map.gates[-1]['heading'] * math.pi / 180
normal = Vec3(math.cos(yaw), math.sin(yaw), 0.0)
pos = self.map.gates[-1]['position'] + 0.5 * normal
vel = Vec3(0.0, 0.0, 0.0)
last_wp = Waypoint('final', pos.x, pos.y, pos.z, vel.x, vel.y, vel.z,
0.0)
waypoints.append(last_wp)
return waypoints
def create_map(map_data, type='grid'):
"""Create map object dictionary map_dict."""
# Access map parameters through ros
coll_rad = rospy.get_param(rospy.get_namespace() +
'crazyflie/collision_radius')
resolution = rospy.get_param(rospy.get_namespace() + 'map/resolution')
off_ground = rospy.get_param(rospy.get_namespace() + 'map/gate_off_ground')
side_margin = rospy.get_param(rospy.get_namespace() +
'map/gate_side_margin')
z_min = 0.2
z_max = 1.0
# airspace_min = Vec3(*map_data['airspace']['min'])
airspace_min = Vec3(map_data['airspace']['min'][0],
map_data['airspace']['min'][1], z_min)
# airspace_max = Vec3(*map_data['airspace']['max'])
airspace_max = Vec3(map_data['airspace']['max'][0],
map_data['airspace']['max'][1], z_max)
# Convert obstacle to point cloud with finer resolution than map representation
points = set()
for wall in map_data['walls']:
points = points.union(
plane_to_points(wall['plane'], resolution, coll_rad))
for gate in map_data['gates']:
points = points.union(
gate_to_points(gate, resolution, coll_rad, map_data['gate_size'],
off_ground, side_margin, airspace_max.z))
points = {
point
for point in points if (airspace_min <= point <= airspace_max)
}
# Adjust gate position to middle of gate for later convenience
for gate in map_data['gates']:
gate['width'] = map_data['gate_size'][0]
gate['height'] = map_data['gate_size'][1]
gate['position'] = Vec3(gate['position'][0], gate['position'][1],
off_ground + gate['height'] / 2)
# Create map of specified by type
if type == 'kd':
map = Kdmap(points, map_data['gates'], map_data['markers'],
airspace_min, airspace_max, resolution)
elif type == 'octo':
map = Octomap(points, map_data['gates'], map_data['markers'],
airspace_min, airspace_max, resolution)
else:
map = Gridmap(points, map_data['gates'], map_data['markers'],
airspace_min, airspace_max, resolution)
# Publish map shapes to rviz
if rospy.get_param(rospy.get_name() + '/use_rviz'):
publish_map_to_rviz(map, points)
return map
def set_yaw_gates(self, traj_msg, start_wp, traj):
# Add yaw trajectory based on gates
prev_yaw = (start_wp.yaw * 180 / math.pi) % 360
i = 0
# for gate in self.map.gates[]:
for gate in self.map.gates[:-1]:
wp1, wp2 = self.gate_to_waypoints(gate)
# Loop until pre-gate waypoint
pre_gate_pieces = []
duration = 0
while True:
bezier = traj[i]
piece = traj_msg.pieces[i]
pre_gate_pieces.append(piece)
duration += piece.duration.to_sec()
i += 1
wp_pos = Vec3(wp1.x, wp1.y, wp1.z)
bz_pos = bezier.pos(-1)
if wp_pos == bz_pos:
break
# Calculate yawrate
gate_yaw = gate['heading']
if abs(gate_yaw - prev_yaw) <= 180:
yawrate = (gate_yaw - prev_yaw) / duration
else:
yawrate = (360 + gate_yaw - prev_yaw) / duration
# Turn yaw and yawrate into coefficients
t = 0
for piece in pre_gate_pieces:
piece.poly_yaw[0] = prev_yaw + t * yawrate
piece.poly_yaw[1] = yawrate
t += piece.duration.to_sec()
# Loop until post-gate waypoint
t = 0
while True:
bezier = traj[i]
piece = traj_msg.pieces[i]
piece.poly_yaw[0] = gate_yaw
t += piece.duration.to_sec()
i += 1
wp_pos = Vec3(wp2.x, wp2.y, wp2.z)
bz_pos = bezier.pos(-1)
if wp_pos == bz_pos:
break
prev_yaw = gate_yaw
# Set yaw values for after the last gate using escaped loop variables
for piece in traj_msg.pieces[i - 1:]:
piece.poly_yaw[0] = gate_yaw
def get_gate_poses(self, gate):
"""Gets one pose before and one pose after gate."""
theta = gate['heading'] * 2 * math.pi / 360
normal = Vec3(math.cos(theta), math.sin(theta), 0.0)
point1 = gate['position'] - self.wp_gate_dist * normal
point2 = gate['position'] + self.wp_gate_dist * normal
pose1 = PoseStamped()
pose1.header.frame_id = 'map'
(pose1.pose.position.x, pose1.pose.position.y,
pose1.pose.position.z) = point1.x, point1.y, point1.z
(pose1.pose.orientation.x, pose1.pose.orientation.y,
pose1.pose.orientation.z,
pose1.pose.orientation.w) = quaternion_from_euler(0.0, 0.0, theta)
pose2 = PoseStamped()
pose2.header.frame_id = 'map'
(pose2.pose.position.x, pose2.pose.position.y,
pose2.pose.position.z) = point2.x, point2.y, point2.z
(pose2.pose.orientation.x, pose2.pose.orientation.y,
pose2.pose.orientation.z,
pose2.pose.orientation.w) = quaternion_from_euler(0.0, 0.0, theta)
return [pose1, pose2]
def goalnode_from_wp(wp):
pos = Vec3(wp.x, wp.y, wp.z)
vel = Vec3(wp.vx, wp.vy, wp.vz)
acc = Vec3(0, 0, 0)
T = 1
p0 = pos
p1 = T/2 * vel + p0
p2 = T**2/2 * acc + 2*p1 - p0
bezier = Bezier([p0, p1, p2], T)
goalnode = RRTNode(pos, vel, wp.yaw)
goalnode.set_next_link(None, bezier, update_cost=False)
goalnode.cost_to_goal = 0
return goalnode
def startnode_from_wp(wp):
pos = Vec3(wp.x, wp.y, wp.z)
vel = Vec3(wp.vx, wp.vy, wp.vz)
acc = Vec3(0, 0, 0)
T = 1
p2 = pos
p1 = -T/2 * vel + p2
p0 = T**2/2 * acc + 2*p1 - p2
bezier = Bezier([p0, p1, p2], T)
startnode = RRTNode(pos, vel, wp.yaw)
startnode.set_prev_link(None, bezier, update_cost=False)
startnode.cost = 0
return startnode
def __init__(self, min, max, points, resolution):
self.min = min
self.max = max
self.isleaf = False
self.isfree = False
self.isoccupied = False
self.low_tree = None
self.high_tree = None
diag = self.max - self.min
# No points, then we are done
if not points:
self.isleaf = True
self.isfree = True
return
# Check if all dimensions are smaller than resolution
elif diag <= Vec3(resolution, resolution, resolution):
self.isleaf = True
self.isoccupied = True
return
# Not a leaf so we have to actually do something
(self.split_dim, self.split_value, low_points,
high_points) = self.find_split(points, resolution)
low_min = Vec3(*self.min)
low_max_values = [self.max.x, self.max.y, self.max.z]
low_max_values[self.split_dim] = self.split_value
low_max = Vec3(*low_max_values)
self.low_tree = Kdtree(low_min, low_max, low_points, resolution)
high_min_values = [self.min.x, self.min.y, self.min.z]
high_min_values[self.split_dim] = self.split_value
high_min = Vec3(*high_min_values)
high_max = Vec3(*self.max)
self.high_tree = Kdtree(high_min, high_max, high_points, resolution)
# Remove subtrees if both turn out to be occupied
if self.low_tree.isoccupied and self.high_tree.isoccupied:
self.low_tree = None
self.high_tree = None
self.isleaf = True
self.isoccupied = True
return
def find_subtree_centers(self, center, size):
directions = [
Vec3(1, 1, 1),
Vec3(-1, 1, 1),
Vec3(-1, -1, 1),
Vec3(1, -1, 1),
Vec3(1, 1, -1),
Vec3(-1, 1, -1),
Vec3(-1, -1, -1),
Vec3(1, -1, -1)
]
return [center + direct * size / 4 for direct in directions]
def steer(self, connectnode, targetnode, reverse=False):
# Change distance of target
diff = targetnode.pos - connectnode.pos
dist = abs(diff)
if dist > self.max_step_len:
direction = diff / dist
pos = connectnode.pos + self.max_step_len * direction
if self.map.query([pos]):
return
targetnode = RRTNode(pos, targetnode.vel, targetnode.yaw)
for T in np.linspace(self.min_step_time, self.max_step_time, num=11):
if reverse:
bezier = Bezier.newPenticBezier(
targetnode.pos,
targetnode.vel,
Vec3(0, 0, 0),
connectnode.pos,
connectnode.vel,
connectnode.acc,
T
)
if self.check_bezier(bezier):
targetnode.set_next_link(connectnode, bezier)
return targetnode
else:
bezier = Bezier.newPenticBezier(
connectnode.pos,
connectnode.vel,
connectnode.acc,
targetnode.pos,
targetnode.vel,
Vec3(0, 0, 0),
T
)
if self.check_bezier(bezier):
targetnode.set_prev_link(connectnode, bezier)
return targetnode
return
def gate_to_points(gate, resolution, coll_rad, gate_size, off_ground,
side_margin, z_max):
gate_width = gate_size[0]
gate_height = gate_size[1]
mid = Vec3(*gate['position'])
theta = gate['heading'] * 2 * math.pi / 360
normal = Vec3(math.cos(theta), math.sin(theta), 0.0)
parallel = normal.cross(Vec3(0.0, 0.0, 1.0))
# Create four planes to represent gate w.r.t. collision
bottom_start = mid - parallel * (side_margin + gate_width / 2)
bottom_stop = mid + parallel * (side_margin + gate_width / 2) + Vec3(
0.0, 0.0, off_ground)
bottom_plane = {'start': bottom_start, 'stop': bottom_stop}
bottom_points = plane_to_points(bottom_plane, resolution, coll_rad)
top_start = bottom_start + Vec3(0.0, 0.0, off_ground + gate_height)
top_stop = bottom_stop + Vec3(0.0, 0.0, z_max - off_ground)
top_plane = {'start': top_start, 'stop': top_stop}
top_points = plane_to_points(top_plane, resolution, coll_rad)
side1_start = mid - parallel * gate_width / 2 + Vec3(0.0, 0.0, off_ground)
side1_stop = mid - parallel * (side_margin + gate_width / 2) + Vec3(
0.0, 0.0, off_ground + gate_height)
side1_plane = {'start': side1_start, 'stop': side1_stop}
side1_points = plane_to_points(side1_plane, resolution, coll_rad)
side2_start = mid + parallel * gate_width / 2 + Vec3(0.0, 0.0, off_ground)
side2_stop = mid + parallel * (side_margin + gate_width / 2) + Vec3(
0.0, 0.0, off_ground + gate_height)
side2_plane = {'start': side2_start, 'stop': side2_stop}
side2_points = plane_to_points(side2_plane, resolution, coll_rad)
points = set()
points = points.union(bottom_points)
points = points.union(top_points)
points = points.union(side1_points)
points = points.union(side2_points)
return points
def plane_to_points(plane, resolution, coll_rad):
start = Vec3(*plane['start'])
stop = Vec3(*plane['stop'])
xyvec = Vec3(stop.x - start.x, stop.y - start.y,
0.0) # Plane vector in (xy)-plane
zvec = Vec3(0.0, 0.0, stop.z - start.z) # Plane vector along (z)-axis
cvec = zvec.cross(
xyvec) # Plane normal vector, used for (c)ollision map inflation
xylen = abs(xyvec)
zlen = abs(zvec)
xyvec = xyvec.unit()
zvec = zvec.unit()
cvec = cvec.unit()
nr_zsteps = round((zlen + 2 * coll_rad) / (resolution / 2) + 2)
zlinspace = np.linspace(-coll_rad, zlen + coll_rad, nr_zsteps)
nr_xysteps = round((xylen + 2 * coll_rad) / (resolution / 2) + 2)
xylinspace = np.linspace(-coll_rad, xylen + coll_rad, nr_xysteps)
nr_csteps = round(2 * coll_rad / (resolution / 2) + 2)
clinspace = np.linspace(-coll_rad, coll_rad, nr_csteps)
points = set()
for z in zlinspace:
zpart = z * zvec
start_zpart = start + zpart
for xy in xylinspace:
xypart = xy * xyvec
start_zxypart = start_zpart + xypart
for c in clinspace:
cpart = c * cvec
point = start_zxypart + cpart
points.add(point)
return points
def sample_configuration(self):
# Sample position
rand = np.random.rand(3)
min = np.array(self.map.min)
max = np.array(self.map.max)
pos = Vec3(*(rand*max + (1-rand)*min))
while self.map.query([pos]):
rand = np.random.rand(3)
min = np.array(self.map.min)
max = np.array(self.map.max)
pos = Vec3(*(rand*max + (1-rand)*min))
# Sample velocity
vel = Vec3(*(np.random.rand(3)))
while abs(vel) > 1:
vel = Vec3(*(np.random.rand(3)))
vel *= self.vel_max
# "Sample" yaw
#yaw = np.random.rand() * 2*math.pi
yaw = 0.0
return RRTNode(pos, vel, yaw)
def gate_to_waypoints(self, gate):
yaw = gate['heading'] * math.pi / 180
normal = Vec3(math.cos(yaw), math.sin(yaw), 0.0)
pre_pos = gate['position'] - self.wp_pre_gate_dist * normal
post_pos = gate['position'] + self.wp_post_gate_dist * normal
pre_vel = self.wp_gate_vel * normal
post_vel = self.wp_gate_vel * normal
pre_id = 'pre_gate_{}'.format(gate['id'])
post_id = 'post_gate_{}'.format(gate['id'])
pre_wp = Waypoint(pre_id, pre_pos.x, pre_pos.y, pre_pos.z, pre_vel.x,
pre_vel.y, pre_vel.z, 0.0)
post_wp = Waypoint(post_id, post_pos.x, post_pos.y, post_pos.z,
post_vel.x, post_vel.y, post_vel.z, 0.0)
return pre_wp, post_wp
def gridmap_to_msgs(map, id_gen):
marker = Marker()
marker.header.frame_id = 'map'
marker.ns = 'collision_cubes'
marker.id = next(id_gen)
marker.type = Marker.CUBE_LIST
marker.action = Marker.ADD
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = map.resolution
marker.scale.y = map.resolution
marker.scale.z = map.resolution
marker.points = []
for xi in range(map.shape[0]):
for yi in range(map.shape[1]):
for zi in range(map.shape[2]):
if map.grid[xi, yi, zi]:
p = Vec3(xi + 0.5, yi + 0.5,
zi + 0.5) * map.resolution + map.origin
marker.points.append(Point(x=p.x, y=p.y, z=p.z))
return [marker]
def __init__(self,
center,
size,
points,
max_depth,
bound_min=None,
bound_max=None):
self.center = center
self.size = size
self.isleaf = False
self.isfree = False
self.isoccupied = False
self.subtrees = []
if bound_min is None:
bound_min = -Vec3(float('Inf'), float('Inf'), float('Inf'))
if bound_max is None:
bound_max = Vec3(float('Inf'), float('Inf'), float('Inf'))
tree_max = center + Vec3(size / 2, size / 2, size / 2)
tree_min = center - Vec3(size / 2, size / 2, size / 2)
if (not points) and (tree_min >= bound_min) and (tree_max <=
bound_max):
self.isleaf = True
self.isfree = True
elif max_depth == 0:
self.isleaf = True
self.isoccupied = True
elif tree_max.x <= bound_min.x or tree_max.y <= bound_min.y or tree_max.z <= bound_min.z:
self.isleaf = True
self.isoccupied = True
elif tree_min.x >= bound_max.x or tree_min.y >= bound_max.y or tree_min.z >= bound_max.z:
self.isleaf = True
self.isoccupied = True
else:
centers = self.find_subtree_centers(self.center, self.size)
points_per_octant = self.split_by_octant(points, self.center)
diagonal = Vec3(size / 4, size / 4, size / 4)
# Generate subtrees
for subcenter, subpoints in zip(centers, points_per_octant):
sub_bound_min = bound_min
sub_bound_max = bound_max
if subcenter - diagonal >= bound_min:
sub_bound_min = None
if subcenter + diagonal <= bound_max:
sub_bound_max = None
self.subtrees.append(
Octree(subcenter, size / 2, subpoints, max_depth - 1,
sub_bound_min, sub_bound_max))
# Remove subtrees if all turn out to be occupied
for subtree in self.subtrees:
if not subtree.isoccupied:
break
else:
self.isleaf = True
self.isoccupied = True
self.subtrees = []
assert not (self.isfree and self.isoccupied)
return
