def test_turning_radius_scaling(self):
a = dubins.shortest_path((0, 0, 0), (10, 10, math.pi / 4.0),
1.0).path_length()
b = dubins.shortest_path((0, 0, 0), (10, 10, math.pi / 4.0),
2.0).path_length()
self.assert_(b > a)
def test_colocated_configurations(self):
# Regression: previous version of the code did not work correctly
# for goal and initial configurations were too close together
qi = (0, 0, 0)
qg = (0, 0, 0)
turning_radius = 1.0
# not raising an exception is sufficient
dubins.shortest_path(qi, qg, turning_radius)
def pursuitToWaypoint(waypoint, wp_cnt):
q0 = (rear_axle_center.position.x, rear_axle_center.position.y,
rear_axle_theta)
q1 = (waypoint[0], waypoint[1], waypoint[2])
turning_radius = 2.0
step_size = 1
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(step_size)
curve_pts = mark_straight(configurations)
for i, c in enumerate(configurations):
velocity = 2
MAX_VEL = 1.5
MIN_VEL = 0.2
if wp_cnt == 1:
MAX_VEL = 1.5
elif wp_cnt == 2:
MAX_VEL = 1
elif wp_cnt == 3:
MAX_VEL = 1
elif wp_cnt == 4:
MAX_VEL = 0.3
MIN_VEL = 0.05
velocity = max(MIN_VEL, min(MAX_VEL, velocity))
_pursuitToWaypoint([c[0], c[1]], velocity, wp_cnt)
def handle_dubins(course_data,
exit_row_index,
entry_row_index,
angle_tolerance=0.523599):
# exit_row_index = int(sys.argv[2]) # row the rover is exiting
# entry_row_index = int(sys.argv[3]) # row it's about to go down
exit_row, entry_row = [], []
print("incoming course data: {}".format(course_data))
print("exit_row_index: {}".format(exit_row_index))
print("exit_row_index type: {}".format(type(exit_row_index)))
# gets start and end rows arrays:
for row_obj in course_data['rows']:
if int(row_obj['index']) == int(exit_row_index):
exit_row = row_obj['row']
elif int(row_obj['index']) == int(entry_row_index):
entry_row = row_obj['row']
print("exit and entry rows: {}; {}".format(exit_row, entry_row))
# pick end point for start row, first point end row:
exit_row_angle = get_row_angle(exit_row)
entry_row_angle = get_row_angle(entry_row)
print("exit row angle: {}".format(math.degrees(exit_row_angle)))
print("entry row angle: {}".format(math.degrees(entry_row_angle)))
# calculates row spacing at exit/entry points:
distance_apart = math.sqrt((exit_row[-1][0] - entry_row[0][0])**2 +
(exit_row[-1][1] - entry_row[0][1])**2)
print("Straight distance between exit and entry rows: {} meters".format(
distance_apart))
# determines which end of row to use based on row angles:
angle_diff = abs(exit_row_angle - entry_row_angle)
# angle_diff_abs = math.degrees(abs(angle_diff))
print("angle diff b/w rows {}".format(math.degrees(angle_diff)))
turning_radius = 1.5
step_size = 0.5
q0, q1 = determine_dubins_AB_points(exit_row, entry_row, exit_row_angle,
entry_row_angle, angle_diff,
angle_tolerance)
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(step_size)
plot_handler(configurations, exit_row, entry_row, q0, q1)
dubins_course = np.array(configurations)
return dubins_course
def get_heuristic(self, state, goal):
assert len(state) == 3, "state must be of form (x,y,heading)"
assert len(goal) == 3, "goal must be of form (x,y,heading)"
s = (state[0], state[1], angles.normalize_angle_positive(state[2]))
g = (goal[0], goal[1], angles.normalize_angle_positive(goal[2]))
path = dubins.shortest_path(s, g, self.turning_radius - 0.01)
return path.path_length()
def buffered_paths(self):
sampling_path = {}
true_path = {}
for i, goal in enumerate(self.goals):
path = dubins.shortest_path(self.cp, goal, self.tr)
fconfig, _ = path.sample_many(self.ss / 10)
ftemp = []
for c in fconfig:
if c[0] > self.extent[0] and c[0] < self.extent[1] and c[
1] > self.extent[2] and c[1] < self.extent[
3] and not self.obstacle_world.in_obstacle(
(c[0], c[1]), buff=0.0):
ftemp.append(c)
else:
break
try:
ttemp = ftemp[0::10]
for m, c in enumerate(ttemp):
if c[0] <= self.extent[0] + 3 * self.tr or c[0] >= self.extent[1] - 3 * self.tr or c[1] <= \
self.extent[2] + 3 * self.tr or c[1] >= self.extent[
3] - 3 * self.tr or self.obstacle_world.in_obstacle((c[0], c[1]), buff=3 * self.tr):
ttemp = ttemp[0:m - 1]
if len(ttemp) < 2:
pass
else:
sampling_path[i] = ttemp
true_path[i] = ftemp[0:ftemp.index(ttemp[-1]) + 1]
except:
pass
return sampling_path, true_path
def nonholonomic_obstacle_free_(self, cur, target):
"""Since euclidean distance is not a good heuristic for non-holonomic vehicles, we use dubin's path to estimate path length. While these
values can be pre-computed in a heuristic lookup table (HLUT),
there are many positions to precompute so the time saved by this
may not outweigh the high overhead of precomputing all state heuristics.
Also, dubin's path is a fast computation and this library is a Cython wrapper and thus still performs quickly. Since dubin's path will estimate a large path length to perfectly reach a target, but we only care about approximately reaching, we return euclidean distance instead of the target is within some radius r.
Args:
cur ([type]): [description]
target ([type]): [description]
Returns:
[type]: [description]
"""
start_time = time.time()
decimal_places = 3
cur_pose = np.round(cur[:3], decimal_places) # x0, y0, theta0
target_pose = np.round(target[:3], decimal_places) # x1, y1, theta1
# due to floating precision errors, make angles discrete
cur_pose[2] = round(cur_pose[2], 2)
# NOTE: we perform search from q0=target_pos to q1=cur_pos
# since we are doing backward search, this is actually what's
# going on, and this avoids issues of transforming frames
path = dubins.shortest_path(target_pose, cur_pose, self.min_turn_rad)
dist = self.euc_dist(cur, target)
end_time = time.time()
self.nonholonomic_obstacle_free_time += (end_time - start_time)
if dist < self.goal_thresh:
return dist
else:
return path.path_length()
def make_sample_paths(self):
'''Connect the current_pose to the goal places'''
coords = {}
true_coords = {}
for i,goal in enumerate(self.goals):
g = (goal[0],goal[1],self.cp[2])
path = dubins.shortest_path(self.cp, goal, self.tr)
configurations, _ = path.sample_many(self.ss)
true_coords[i], _ = path.sample_many(self.ss/5)
coords[i] = [config for config in configurations if config[0] > self.extent[0] and config[0] < self.extent[1] and config[1] > self.extent[2] and config[1] < self.extent[3] and not self.obstacle_world.in_obstacle((config[0], config[1]), buff=self.tr)]
# find the "shortest" path in sample space
current_min = 1000
for key,path in coords.items():
if len(path) < current_min and len(path) > 1:
current_min = len(path)
# limit all paths to the shortest path in sample space
# NOTE! for edge cases nar borders, this limits the paths significantly
for key,path in coords.items():
if len(path) > current_min:
path = path[0:current_min]
coords[key]=path
for key,path in true_coords.items():
ftemp = []
for c in path:
if c[0] == coords[key][-1][0] and c[1] == coords[key][-1][1]:
ftemp.append(c)
break
else:
ftemp.append(c)
true_path[key] = ftemp
return coords, true_coords
def fitSpline(self,waypoints):
# spline configurations
turning_radius = 0.67
step_size = 0.5
waypoints = np.insert(waypoints, 0, [0,0,0], axis=0)# prepend zero state to waypoints # TODO: check yaw
# find heading-fitting spline
path_list = np.empty((0,3))
for i in range(waypoints.shape[0] - 1):
q0 = (waypoints[i,0], waypoints[i,1], waypoints[i,2])
q1 = (waypoints[i+1,0], waypoints[i+1,1], waypoints[i+1,2])
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(step_size)
configurations = np.array(configurations)
path_list = np.vstack((path_list, configurations))
# Publish as nav_msgs/Path message
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = '/map'
for i in range(path_list.shape[0]):
pose = PoseStamped()
pose.pose.position.x = path_list[i,0]
pose.pose.position.y = path_list[i,1]
path_msg.poses.append(pose)
self.spline_path_pub.publish(path_msg)
self.spline_points = path_list
self.spline_distance = np.sum(np.sqrt(np.sum(np.diff(path_list[:,:2], axis=0)**2, axis=1)))
self.spline_cum_dist = np.cumsum(np.sqrt(np.sum(np.diff(path_list[:,:2], axis=0)**2, axis=1)))
print("Published Spline Path. Distance (m): ", self.spline_distance)
def interpolate(self, s1, s2):
"""Returns a sequence of states between two states
given a motion that connects the two states.
We use a shooting method type approach to interpolate between the two states
@param: s1 - parent state
@param: s2 - child state
@return edge - sequence of states (including end points)
"""
step_size = self.path_resolution
s1_n = (s1[0], s1[1], angles.normalize_angle_positive(s1[2]))
s2_n = (s2[0], s2[1], angles.normalize_angle_positive(s2[2]))
path = dubins.shortest_path(s1, s2, self.radius - 0.01)
edge, _ = path.sample_many(step_size)
#Dubins returns edge in [0, 2pi], we got to normalize it to (-pi, pi] for our code
normalized_edge = []
for config in edge:
new_config = (config[0], config[1],
angles.normalize_angle(config[2]))
normalized_edge.append(new_config)
normalized_edge.append(tuple(s2))
return normalized_edge
def goalCallback(msg):
start_pose = (msg.start.x, msg.start.y, msg.start.theta)
goal_pose = (msg.goal.x, msg.goal.y, msg.goal.theta)
dubins_path = dubins.shortest_path(start_pose, goal_pose, 1.0)
num_samples = msg.vel * msg.dt
path_points = dubins_path.sample_many(num_samples)[0]
dpath = Path()
dpath.header.stamp = rospy.Time()
dpath.header.frame_id = "map"
dpath.path = []
for i in range(len(path_points)):
point = Pose2D()
point.x = path_points[i][0]
point.y = path_points[i][1]
point.theta = path_points[i][2]
dpath.path.append(point)
path_pub.publish(dpath)
def construct_dubins_traj(traj_point_0, traj_point_1):
""" Construc a trajectory in the X-Y space and in the time-X,Y,Theta space.
Arguments:
traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad).
traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad).
Returns:
traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad).
traj_distance (float): The length ofthe trajectory (m).
"""
q0 = (traj_point_0[1], traj_point_0[2], traj_point_0[3])
q1 = (traj_point_1[1], traj_point_1[2], traj_point_1[3])
turning_radius = 0.5
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, distances = path.sample_many(DISTANCE_STEP_SIZE)
traj_distance = distances[-1]
traj_time = traj_point_1[0] - traj_point_0[0]
time_step_size = traj_time / len(distances)
traj = []
traj_point_time = traj_point_0[0]
for c in configurations:
traj_point = [traj_point_time, c[0], c[1], c[2]]
traj.append(traj_point)
traj_point_time += time_step_size
return traj, traj_distance
def construct_dubins_traj(traj_point_0, traj_point_1):
""" Construc a trajectory in the X-Y space and in the time-X,Y,Theta space.
Arguments:
traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad).
traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad).
Returns:
traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad).
traj_distance (float): The length ofthe trajectory (m).
"""
# traj = []
# traj_point = [0,0,0,0]
# traj.append(traj_point)
turning_rad = 0.25
step_size = DISTANCE_STEP_SIZE
path = dubins.shortest_path(traj_point_0[1:], traj_point_1[1:],
turning_rad)
configs, _ = path.sample_many(step_size)
dt = (traj_point_1[0] - traj_point_0[0]) / len(configs)
timeStamp = traj_point_0[0]
for i in range(len(configs)):
configs[i] = list(configs[i])
configs[i].insert(0, timeStamp)
timeStamp += dt
configs.append(traj_point_1)
traj = configs
traj_distance = np.arange(len(configs)) * step_size #TODO: verify this
return traj, traj_distance
def construct_dubins_traj(traj_point_0, traj_point_1):
""" Construc a trajectory in the X-Y space and in the time-X,Y,Theta space.
Arguments:
traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad).
traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad).
Returns:
traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad).
traj_distance (float): The length ofthe trajectory (m).
"""
traj = []
q0 = traj_point_0[1:]
q1 = traj_point_1[1:]
turning_radius = 1
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(DISTANCE_STEP_SIZE)
t_0 = traj_point_0[0]
time_step = (traj_point_1[0] - traj_point_0[0]) / len(configurations)
traj_distance = len(configurations) * DISTANCE_STEP_SIZE
for i in range(0, len(configurations)):
step = [
t_0 + time_step * i, configurations[i][0], configurations[i][1],
configurations[i][2]
]
traj.append(step)
return traj, traj_distance
def make_sample_paths(self):
'''Connect the current_pose to the goal places'''
coords = {}
for i, goal in enumerate(self.goals):
g = (goal[0], goal[1], self.cp[2])
path = dubins.shortest_path(self.cp, goal, self.tr)
configurations, _ = path.sample_many(self.ss)
coords[i] = [
config for config in configurations
if config[0] > self.extent[0] and config[0] < self.extent[1]
and config[1] > self.extent[2] and config[1] < self.extent[3]
]
# find the "shortest" path in sample space
current_min = 1000
for key, path in coords.items():
if len(path) < current_min and len(path) > 1:
current_min = len(path)
# limit all paths to the shortest path in sample space
for key, path in coords.items():
if len(path) > current_min:
path = path[0:current_min]
coords[key] = path
self.samples = coords
return coords
def test_half_loop(self):
r = 1.0
dx = r * math.sqrt(2) / 2
dy = r * math.sqrt(2) / 2
d = dubins.shortest_path((0, 0, 0), (0, 2 * r, -math.pi), r)
length = d.path_length()
self.assertAlmostEqual(length, math.pi * r)
def take_step(self, loc, goal):
'''
Create an intermediary goal towards the point of interest such that the robot only translates the step size specified
Input: Goal
Output: Navigable points to the intermediary goal
'''
coords = {}
dist = np.sqrt((loc[0] - goal[0])**2 + (loc[1] - goal[1])**2)
angle_to_goal = np.arctan2([goal[1] - loc[1]], [goal[0] - loc[0]])[0]
if dist > self.step_size:
new_goal = (loc[0] +
self.step_size * np.sin(np.pi / 2 - angle_to_goal),
loc[1] + self.step_size * np.sin(angle_to_goal),
angle_to_goal)
else:
new_goal = (goal[0], goal[1], angle_to_goal)
path = dubins.shortest_path(loc, new_goal, self.turning_radius)
configurations, _ = path.sample_many(self.sample_step)
configurations.append(new_goal)
temp = []
for i, config in enumerate(configurations):
if config[0] > self.ranges[0] and config[0] < self.ranges[
1] and config[1] > self.ranges[2] and config[
1] < self.ranges[3]:
temp.append(config)
else:
pass
return temp
def generatePath(self):
dubins_start_list = list(self.coord_s)
dubins_start_list.append(self.start_angle)
dubins_end_list = list(self.coord_e)
dubins_end_list.append(self.end_angle)
start_params = tuple(dubins_start_list)
end_params = tuple(dubins_end_list)
self.path = dubins.shortest_path(start_params, end_params, self.turning_radius)
def test_readme_demo(self):
q0 = (0, 0, math.pi / 2)
q1 = (1, 1, 0.0)
turning_radius = 1.0
step_size = 0.5
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(step_size)
def test_easy_lrl(self):
# Regression for https://github.com/AndrewWalker/Dubins-Curves/issues/2
r = 1.0
q0 = (0, 0, math.pi / 2.)
q1 = (1, 0, -math.pi / 2.)
d = dubins.shortest_path(q0, q1, r)
path_type = d.path_type()
self.assertAlmostEqual(path_type, dubins.LRL)
def compute_heuristic(self, config, goal):
"""
Use the Dubins path length from config to goal as the heuristic distance.
"""
# Implement here
path = dubins.shortest_path(config, goal, 1.0 / self.curvature)
return path.path_length()
def generate_path(x0, y0, x1, y1, theta0, theta1):
q0 = (x0, y0, theta0)
q1 = (x1, y1, theta1)
turning_radius = 2.0
step_size = 1.0
path = dubins.shortest_path(q0, q1, turning_radius)
configurations, _ = path.sample_many(step_size)
return configurations
def generate(self):
## Randomize starting and goal poses
x0 = np.random.randint(-40, 40)
y0 = np.random.randint(-40, 40)
psi_0 = np.radians(np.random.randint(0, 360))
x1 = np.random.randint(-40, 40)
y1 = np.random.randint(-40, 40)
psi_1 = np.radians(np.random.randint(0, 360))
self.q0 = [x0, y0, psi_0]
self.qg = [x1, y1, psi_1]
## Modify dubins to work for a straight offset from goal
self.q1 = self.qg.copy()
self.q1[0] -= 2 * self.turning_radius * np.cos(self.q1[2])
self.q1[1] -= 2 * self.turning_radius * np.sin(self.q1[2])
# Dubins
path = dubins.shortest_path(self.q0, self.q1, self.turning_radius)
qs, dist_dubins = path.sample_many(self.step_size)
qs = np.array(qs)
## Concatenate with reverse straight
s_s = self.distance((self.q1[0], self.q1[1]), (self.qg[0], self.qg[1]))
n_steps = int(s_s // self.step_size) + 1
straight = np.array([
np.linspace(self.q1[0], self.qg[0], n_steps),
np.linspace(self.q1[1], self.qg[1], n_steps),
self.qg[2] * np.ones(n_steps)
]).T
qs = np.vstack((qs, straight))
dist_straight = [dist_dubins[-1]]
for j in range(len(straight)):
dist_straight.append(dist_straight[j] + (s_s / n_steps))
self.dist = dist_dubins + dist_straight[1:] # ignore double counting
## x, y, curv, psi, dist
self.curv = []
for n in range(len(qs)):
if n == 0:
self.curv.append(
self.get_menger_curvature(qs[0], qs[n + 1], qs[n + 2]))
elif n == len(qs) - 1:
self.curv.append(
self.get_menger_curvature(qs[n - 2], qs[n - 1], qs[n]))
else:
self.curv.append(
self.get_menger_curvature(qs[n - 1], qs[n], qs[n + 1]))
self.x = qs[:, 0]
self.y = qs[:, 1]
self.psi = qs[:, 2]
return np.column_stack(
[self.x, self.y, self.curv, self.psi, self.dist])
def generate_path(self, config1, config2):
"""
Generate a dubins path from config1 to config2
The generated path is not guaranteed to be collision free
Use dubins_path_planning to get a path
return: (numpy array of [x, y, yaw], curve length)
"""
path = dubins.shortest_path(config1, config2, 1.0 / self.curvature)
configs, _ = path.sample_many(0.2)
return np.array(configs), path.path_length()
def _evaluateChromosome(i, tr=0.5):
# on distance travelled
dist = 0
assns = i.get_assignments()
for a1, a2 in zip(assns, assns[1:]):
a1x, a1y = a1.task.get_loc()
a2x, a2y = a2.task.get_loc()
dist += dubins.shortest_path((a1x, a1y, a1.heading),
(a2x, a2y, a2.heading), tr).path_length()
return dist,
def rewire(neighborhood, center):
for neighbor in neighborhood:
if validPath(trajectory(center.configuration, neighbor.configuration)):
if testDepth(neighbor, center) < neighbor.calcDepth():
newpath = dubins.shortest_path(center.configuration,
neighbor.configuration, 0.2)
neighborUpdate = Node(center, neighbor.configuration, newpath)
nodes.remove(neighbor)
nodes.append(neighborUpdate)
assert nodes[nodes.index(neighborUpdate)].parent == center
assert nodes[nodes.index(neighborUpdate)].path == newpath
def path(self, end, step_size):
path = dubins.shortest_path((self.pos.x, self.pos.y, self.theta),
(end.pos.x, end.pos.y, end.theta),
turning_radius)
configurations, lens = path.sample_many(step_size)
dubinsCars = []
for c in configurations:
dubinsCars.append(Car(Point(c[0], c[1]), c[2]))
return dubinsCars
def rrt():
global locations
while True:
_sample = sample()
_closestNode = nearest(_sample[0], _sample[1])
if validPath(trajectory(_closestNode.configuration, _sample)):
path = dubins.shortest_path(_closestNode.configuration, _sample,
0.2)
new = Node(_closestNode, _sample, path)
nodes.append(new)
locations = np.concatenate((locations, [new.location]), axis=0)
if new.x == goal[0] and new.y == goal[1]: return new
def take_step(self, loc):
''' Given a current location and a goal, determine the dubins curve sampling path'''
sampling_path = {}
true_path = {}
for i, goal in enumerate(self.goals):
dist = np.sqrt((loc[0] - goal[0])**2 + (loc[1] - goal[1])**2)
angle_to_goal = np.arctan2([goal[1] - loc[1]],
[goal[0] - loc[0]])[0]
if dist > self.step_size:
new_goal = (loc[0] +
self.step_size * np.sin(np.pi / 2 - angle_to_goal),
loc[1] + self.step_size * np.sin(angle_to_goal),
angle_to_goal)
else:
new_goal = (goal[0], goal[1], angle_to_goal)
path = dubins.shortest_path(loc, new_goal, self.turning_radius)
configurations, _ = path.sample_many(self.sample_step)
configurations.append(new_goal)
full_path = fconfig, _ = path.sample_many(self.sample_step / 5)
temp = []
for config in configurations:
if config[0] > self.ranges[0] and config[0] < self.ranges[
1] and config[1] > self.ranges[2] and config[
1] < self.ranges[
3] and not self.obstacle_world.in_obstacle(
(config[0], config[1]),
buff=self.turning_radius):
temp.append(config)
else:
break
if len(temp) < 2:
pass
else:
sampling_path[i] = temp
ftemp = []
for c in fconfig:
if len(temp) >= 2:
if c[0] == temp[-1][0] and c[1] == temp[-1]:
ftemp.append(c)
break
else:
ftemp.append(c)
else:
pass
true_path[i] = ftemp
return sampling_path, true_path
def generate_swath(ship: Ship, prim: Primitives, eps=1e-10) -> Swath:
"""
Will have key of (edge, start heading)
"""
swath_dict = {}
for origin, edge_set in prim.edge_set_dict.items():
start_pos = [prim.max_prim + ship.max_ship_length // 2] * 2 + [
origin[2]
]
for e in edge_set:
e = tuple(e)
array = np.zeros(
[(prim.max_prim + ship.max_ship_length // 2) * 2 + 1] * 2,
dtype=bool)
translated_e = np.asarray(e) + np.array(
[start_pos[0], start_pos[1], 0])
theta_0 = heading_to_world_frame(start_pos[2],
ship.initial_heading,
prim.num_headings)
theta_1 = heading_to_world_frame(translated_e[2],
ship.initial_heading,
prim.num_headings)
dubins_path = dubins.shortest_path(
(start_pos[0], start_pos[1], theta_0),
(translated_e[0], translated_e[1], theta_1),
ship.turning_radius - eps)
configurations, _ = dubins_path.sample_many(0.1)
for config in configurations:
x_cell = int(round(config[0]))
y_cell = int(round(config[1]))
theta = config[2] - ship.initial_heading
R = np.asarray([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
rot_vi = np.round(
np.array([[x_cell], [y_cell]]) +
R @ ship.vertices.T).astype(int)
rr, cc = draw.polygon(rot_vi[1, :], rot_vi[0, :])
array[rr, cc] = True
# for each starting heading rotate the swath 4 times
for i, h in enumerate(
range(0, prim.num_headings, prim.num_headings // 4)):
swath_dict[e, h + origin[2]] = np.rot90(array,
k=i,
axes=(1, 0))
return swath_dict
