def draw_environment(path_to_follow, original_path, robot, costmap):
"""
Draw obstacles, path and a robot
:param path_to_follow: numpy array of (x, y, angle) of a path left to follow
:param original_path: numpy array of (x, y, angle) of the original path that perhaps has been
followed up to some point.
:param robot IRobot: the robot that will supply the draw function
:param costmap Costmap: the costmap that will provide the obstacle data
:return: numpy BGR rendered image
"""
img = prepare_canvas(costmap.get_data().shape)
draw_wide_path(img,
original_path,
robot_width=2 * inscribed_radius(robot.get_footprint()),
origin=costmap.get_origin(),
resolution=costmap.get_resolution(),
color=(240, 240, 240))
if len(path_to_follow):
draw_wide_path(img,
path_to_follow,
robot_width=2 * inscribed_radius(robot.get_footprint()),
origin=costmap.get_origin(),
resolution=costmap.get_resolution(),
color=(220, 220, 220))
# Draw goal:
goal_x, goal_y, goal_theta = original_path[-1]
draw_robot(robot,
img,
goal_x,
goal_y,
goal_theta,
color=(240, 240, 240),
costmap=costmap,
draw_steering_details=False)
x, y, angle = robot.get_pose()
draw_robot(robot, img, x, y, angle, color=(0, 100, 0), costmap=costmap)
draw_world_map(img, costmap.get_data())
# for pose in path_to_follow:
# x, y, angle = pose
# draw_robot(robot, img, x, y, angle, color=(100, 0, 0), costmap=costmap, alpha=0.1, draw_steering_details=False)
return img
def test_inscribed_radius():
# square clockwise
footprint = np.array([[-1, -1], [-1, 1], [1, 1], [1, -1]])
assert abs(inscribed_radius(footprint) - 1.) < 1e-6
# square anticlockwise
footprint = np.array([[1, -1], [1, 1], [-1, 1], [-1, -1]])
assert abs(inscribed_radius(footprint) - 1.) < 1e-6
# square rotated
footprint = np.array([[1, 0], [0, 1], [-1, 0], [0, -1]])
assert abs(inscribed_radius(footprint) - np.sqrt(2) / 2) < 1e-6
# triangle
footprint = np.array([[1, -1], [-1, 1], [-1, -1]])
assert abs(inscribed_radius(footprint) - 0.0) < 1e-6
footprint = np.array([[2, -1], [-1, 1], [-1, -1]])
assert abs(inscribed_radius(footprint) - 0.27735) < 1e-6
# square with an appendix
# The appendix has line segments 0.1 meters close if the distance is define by normals only
footprint = np.array([[1, -1], [1, 1], [0.1, 1], [0.1, 1.1], [-0.1, 1.1],
[-0.1, 1.], [-1, 1], [-1, -1]])
assert abs(inscribed_radius(footprint) - 1.) < 1e-6
realistic_mule = np.array([
# left side
[1.13, 0.33],
[0.85, 0.33],
[0.85, 0.4675],
[0.23, 0.4675],
[0.23, 0.33],
[-0.23, 0.33],
# right side
[-0.23, -0.33],
[0.23, -0.33],
[0.23, -0.4675],
[0.85, -0.4675],
[0.85, -0.33],
[1.13, -0.33],
])
assert abs(inscribed_radius(realistic_mule) - 0.23) < 1e-6
beta_radius = inscribed_radius(_tricycle_footprint())
assert np.abs(beta_radius - 0.3697) < 1e-3
def compute_cost_possibly_circumscribed_thresh(footprint, resolution,
cost_scaling_factor):
'''
Compute costs of possibly circumscribed radius needed for some planners (e.g. sbpl)
:param footprint: n x 2 numpy array with ROS-style footprint (x, y coordinates)
:param resolution: costmap resolution
:param cost_scaling_factor: 1 over the distance (in world units) at which costs
beyond the inscribed radius decay by a factor of e.
:return float: cost of possibly circumscribed radius
'''
distance = circumscribed_radius(footprint)
inscribed_rad = inscribed_radius(footprint)
distance_to_cost = _pixel_distance_to_cost(distance / resolution + 1,
resolution, inscribed_rad,
cost_scaling_factor)
return max(0, distance_to_cost - 1)
def inflate_costmap(costmap, cost_scaling_factor, footprint):
'''
Inflate data with costs
Costmap inflation is explained here:
http://wiki.ros.org/costmap_2d#Inflation
:param costmap CostMap2D: obstacle map
:param cost_scaling_factor Float: how to scale the inflated costs
:param footprint np.ndarray(N, 2)[Float]: footprint of the robot
:return CostMap2D: inflated costs
'''
image_for_dist_transform = CostMap2D.LETHAL_OBSTACLE - costmap.get_data()
distance = distance_transform(image_for_dist_transform)
inscribed_rad = inscribed_radius(footprint)
inflated_data = _pixel_distance_to_cost(distance, costmap.get_resolution(),
inscribed_rad, cost_scaling_factor)
return CostMap2D(data=inflated_data,
origin=costmap.get_origin(),
resolution=costmap.get_resolution())