def set_up_fleming(map_):
"""Set up a map as the generic Fleming space configuration.
Parameters
----------
map_ : Map
The map to add elements.
"""
# Make vicon field space object
# <>TODO: Remove field object?
field_w = 7.5 # [m] field width
field = MapArea('Field', [field_w, field_w], has_relations=False)
# Make wall objects
l = 1.15 # [m] wall length
w = 0.1524 # [m] wall width
wall_shape = [l, w]
poses = np.array([[-7, -1.55, 1],
[-7, -1.55 - l, 1],
[-7 + l/2 + w/2, -1, 0],
[-7 + 3*l/2 + w/2, -1, 0],
[-7 + 5*l/2 + w/2, -1, 0],
[-2, -1.55, 1],
[-2 + 1*l/2 + w/2, -1, 0],
[-2 + 3*l/2 + w/2, -1, 0],
[-2 + 5*l/2 +w/2, -1, 0],
[-7.45 + 1*l/2 + w/2, 1.4, 0],
[-7.45 + 3*l/2 + w/2, 1.4, 0],
[-7.45 + 5*l/2 + w/2, 1.4, 0],
[-7.45 + 7*l/2 + w/2, 1.4, 0],
[-7.45 + 9*l/2 + w/2, 1.4, 0],
[l/2 + w/2, 1.4, 0],
[3*l/2 + w/2, 1.4, 0],
[0, 1.4 + l/2, 1],
[0, 1.4 + 3*l/2, 1],
])
poses = poses * np.array([1, 1, 90])
walls = []
for i in range(poses.shape[0]):
name = 'Wall ' + str(i)
pose = poses[i, :]
wall = MapObject(name, wall_shape, pose=pose, color_str='sienna',
has_relations=False, map_bounds=map_.bounds)
walls.append(wall)
# Make rectangular objects (desk, bookcase, etc)
labels = ['Bookcase', 'Desk', 'Chair', 'Filing Cabinet',
'Dining Table', 'Mars Poster', 'Cassini Poster',
'Fridge', 'Checkers Table', 'Fern']
colors = ['sandybrown', 'sandybrown', 'brown', 'black',
'brown', 'bisque', 'black',
'black', 'sandybrown', 'sage']
poses = np.array([[0, -1.2, 270], # Bookcase
[-5.5, -2, 0], # Desk
[3, -2, 270], # Chair
[-4, -1.32, 270], # Filing Cabinet
[-8.24, -2.15, 270], # Dining Table
[-4.38, 3.67, 270], # Mars Poster
[1.38, 3.67, 270], # Cassini Poster
[-9.1, 3.3, 315], # Fridge
[2.04, 2.66, 270], # Checkers Table
[-2.475, 1.06, 270], # Fern
])
sizes = np.array([[0.18, 0.38], # Bookcase
[0.61, 0.99], # Desk
[0.46, 0.41], # Chair
[0.5, 0.37], # Filing Cabinet
[1.17, 0.69], # Dining Table
[0.05, 0.84], # Mars Poster
[0.05, 0.56], # Cassini Poster
[0.46, 0.46], # Fridge
[0.5, 0.5], # Checkers Table
[0.5, 0.5], # Fern
])
landmarks = []
for i, pose in enumerate(poses):
landmark = MapObject(labels[i], sizes[i], pose=pose,
color_str=colors[i], map_bounds=map_.bounds)
landmarks.append(landmark)
# Add walls to map
for wall in walls:
map_.add_obj(wall)
# Add landmarks to map
for landmark in landmarks:
map_.add_obj(landmark)
# Create areas
labels = ['Study', 'Library', 'Kitchen', 'Billiard Room', 'Hallway',
'Dining Room']
colors = ['aquamarine', 'lightcoral', 'goldenrod', 'sage',
'cornflowerblue', 'orchid']
# points = np.array([[[-7.0, -3.33], [-7.0, -1], [-2, -1], [-2, -3.33]],
# [[-2, -3.33], [-2, -1], [4.0, -1], [4.0, -3.33]],
# [[-9.5, 1.4], [-9.5, 3.68], [0, 3.68], [0, 1.4]],
# [[0, 1.4], [0, 3.68], [4, 3.68], [4, 1.4]],
# [[-9.5, -1], [-9.5, 1.4], [4, 1.4], [4, -1]],
# [[-9.5, -3.33], [-9.5, -1], [-7, -1], [-7, -3.33]],
# ])
s = 0.0 # coarse scale factor
points = np.array([[[-7.0 - s, -3.33], [-7.0 - s, -1 + s], [-2 + s, -1 + s], [-2 + s, -3.33]],
[[-2 - s, -3.33], [-2 - s, -1 + s], [4.0, -1 + s], [4.0, -3.33]],
[[-9.5, 1.4 - s], [-9.5, 3.68], [0 + s, 3.68], [0 + s, 1.4 - s]],
[[0 - s, 1.4 - s], [0 - s, 3.68], [4, 3.68], [4, 1.4 - s]],
[[-9.5, -1 - s], [-9.5, 1.4 + s], [4, 1.4 + s], [4, -1 - s]],
[[-9.5, -3.33], [-9.5, -1 + s], [-7 + s, -1 + s], [-7 + s, -3.33]],
])
for i, pts in enumerate(points):
centroid = [pts[0, 0] + np.abs(pts[2, 0] - pts[0, 0]) / 2,
pts[0, 1] + np.abs(pts[1, 1] - pts[0, 1]) / 2, 0]
area = MapArea(name=labels[i], shape_pts=pts, pose=centroid,
color_str=colors[i], map_bounds=map_.bounds)
map_.add_area(area)
# Relate landmarks and areas
for landmark in landmarks:
if area.shape.intersects(Point(landmark.pose)):
area.contained_objects[landmark.name] = landmark
landmark.container_area = area
landmark.define_relations(map_.bounds)
area.define_relations(map_.bounds)
# <>TODO: Include area demarcations
map_.feasible_layer.define_feasible_regions(map_.static_elements)
def __init__(self,
name,
pose=None,
pose_source='python',
color_str='darkorange',
map_cfg={},
create_mission_planner=True,
goal_planner_cfg={},
path_planner_cfg={},
**kwargs):
# Object attributes
self.name = name
self.pose_source = pose_source
# Setup map
self.map = Map(**map_cfg)
# If pose is not given, randomly place in feasible layer.
feasible_robot_generated = False
if pose is None:
while not feasible_robot_generated:
x = random.uniform(self.map.bounds[0], self.map.bounds[2])
y = random.uniform(self.map.bounds[1], self.map.bounds[3])
if self.map.feasible_layer.pose_region.contains(Point([x, y])):
feasible_robot_generated = True
theta = random.uniform(0, 359)
pose = [x, y, theta]
self.pose2D = Pose(self, pose, pose_source)
self.pose_history = np.array(([0, 0, 0], self.pose2D.pose))
if pose_source == 'python':
self.publish_to_ROS = False
else:
self.publish_to_ROS = True
# Setup planners
if create_mission_planner:
self.mission_planner = MissionPlanner(self)
self.goal_planner = GoalPlanner(self, **goal_planner_cfg)
# If pose_source is python, this robot is just in simulation
if not self.publish_to_ROS:
self.path_planner = PathPlanner(self, **path_planner_cfg)
self.controller = Controller(self)
# Define MapObject
# <>TODO: fix this horrible hack
create_diameter = 0.34
if self.name == 'Deckard':
pose = [0, 0, -np.pi / 4]
r = create_diameter / 2
n_sides = 4
pose = [0, 0, -np.pi / 4]
x = [
r * np.cos(2 * np.pi * n / n_sides + pose[2]) + pose[0]
for n in range(n_sides)
]
y = [
r * np.sin(2 * np.pi * n / n_sides + pose[2]) + pose[1]
for n in range(n_sides)
]
shape_pts = Polygon(zip(x, y)).exterior.coords
else:
shape_pts = Point([0, 0]).buffer(create_diameter / 2)\
.exterior.coords
self.map_obj = MapObject(self.name,
shape_pts[:],
has_relations=False,
blocks_camera=False,
color_str=color_str)
self.update_shape()
def __init__(self,
name,
pose=None,
pose_source='python',
color_str='darkorange',
map_cfg={},
create_mission_planner=True,
goal_planner_cfg={},
path_planner_cfg={},
**kwargs):
# Object attributes
self.name = name
self.pose_source = pose_source
# Setup map
self.map = Map(**map_cfg)
# If pose is not given, randomly place in feasible layer.
feasible_robot_generated = False
if pose is None:
while not feasible_robot_generated:
x = random.uniform(self.map.bounds[0], self.map.bounds[2])
y = random.uniform(self.map.bounds[1], self.map.bounds[3])
if self.map.feasible_layer.pose_region.contains(Point([x, y])):
feasible_robot_generated = True
theta = random.uniform(0, 359)
pose = [x, y, theta]
self.pose2D = Pose(self, pose, pose_source)
self.pose_history = np.array(([0, 0, 0], self.pose2D.pose))
if pose_source == 'python':
self.publish_to_ROS = False
else:
self.publish_to_ROS = True
# Setup planners
if create_mission_planner:
self.mission_planner = MissionPlanner(self)
self.goal_planner = GoalPlanner(self,
**goal_planner_cfg)
# If pose_source is python, this robot is just in simulation
if not self.publish_to_ROS:
self.path_planner = PathPlanner(self, **path_planner_cfg)
self.controller = Controller(self)
# Define MapObject
# <>TODO: fix this horrible hack
create_diameter = 0.34
if self.name == 'Deckard':
pose = [0, 0, -np.pi / 4]
r = create_diameter / 2
n_sides = 4
pose = [0, 0, -np.pi / 4]
x = [r * np.cos(2 * np.pi * n / n_sides + pose[2]) + pose[0]
for n in range(n_sides)]
y = [r * np.sin(2 * np.pi * n / n_sides + pose[2]) + pose[1]
for n in range(n_sides)]
shape_pts = Polygon(zip(x, y)).exterior.coords
else:
shape_pts = Point([0, 0]).buffer(create_diameter / 2)\
.exterior.coords
self.map_obj = MapObject(self.name, shape_pts[:], has_relations=False,
blocks_camera=False, color_str=color_str)
self.update_shape()
class Robot(object):
__metaclass__ = ABCMeta
"""Class definition for the generic robot object.
.. image:: img/classes_Robot.png
Parameters
----------
name : str
The robot's name.
pose : array_like, optional
The robot's initial [x, y, theta] in [m,m,degrees] (defaults to
[0, 0.5, 0]).
pose_source : str
The robots pose source. Either a rostopic name, like 'odom' or
'tf', or 'python'
color_str : str
The color of the robots map object
**kwargs
Arguments passed to the ``MapObject`` attribute.
"""
def __init__(self,
name,
pose=None,
pose_source='python',
color_str='darkorange',
map_cfg={},
create_mission_planner=True,
goal_planner_cfg={},
path_planner_cfg={},
**kwargs):
# Object attributes
self.name = name
self.pose_source = pose_source
# Setup map
self.map = Map(**map_cfg)
# If pose is not given, randomly place in feasible layer.
feasible_robot_generated = False
if pose is None:
while not feasible_robot_generated:
x = random.uniform(self.map.bounds[0], self.map.bounds[2])
y = random.uniform(self.map.bounds[1], self.map.bounds[3])
if self.map.feasible_layer.pose_region.contains(Point([x, y])):
feasible_robot_generated = True
theta = random.uniform(0, 359)
pose = [x, y, theta]
self.pose2D = Pose(self, pose, pose_source)
self.pose_history = np.array(([0, 0, 0], self.pose2D.pose))
if pose_source == 'python':
self.publish_to_ROS = False
else:
self.publish_to_ROS = True
# Setup planners
if create_mission_planner:
self.mission_planner = MissionPlanner(self)
self.goal_planner = GoalPlanner(self, **goal_planner_cfg)
# If pose_source is python, this robot is just in simulation
if not self.publish_to_ROS:
self.path_planner = PathPlanner(self, **path_planner_cfg)
self.controller = Controller(self)
# Define MapObject
# <>TODO: fix this horrible hack
create_diameter = 0.34
if self.name == 'Deckard':
pose = [0, 0, -np.pi / 4]
r = create_diameter / 2
n_sides = 4
pose = [0, 0, -np.pi / 4]
x = [
r * np.cos(2 * np.pi * n / n_sides + pose[2]) + pose[0]
for n in range(n_sides)
]
y = [
r * np.sin(2 * np.pi * n / n_sides + pose[2]) + pose[1]
for n in range(n_sides)
]
shape_pts = Polygon(zip(x, y)).exterior.coords
else:
shape_pts = Point([0, 0]).buffer(create_diameter / 2)\
.exterior.coords
self.map_obj = MapObject(self.name,
shape_pts[:],
has_relations=False,
blocks_camera=False,
color_str=color_str)
self.update_shape()
def update_shape(self):
"""Update the robot's map_obj.
"""
self.map_obj.move_absolute(self.pose2D.pose)
@abstractmethod
def update(self, i=0):
"""Update all primary functionality of the robot.
This includes planning and movement for both cops and robbers,
as well as sensing and map animations for cops.
Parameters
----------
i : int, optional
The current animation frame. Default is 0 for non-animated robots.
"""
# <>TODO: @Matt Figure out how to move this back to pose class.
if self.pose_source == 'tf':
self.pose2D.tf_update()
if self.mission_planner.mission_status is not 'stopped':
# Update statuses and planners
self.mission_planner.update()
self.goal_planner.update()
if self.publish_to_ROS is False:
self.path_planner.update()
self.controller.update()
# Add to the pose history, update the map
self.pose_history = np.vstack(
(self.pose_history, self.pose2D.pose[:]))
self.update_shape()
class Robot(object):
__metaclass__ = ABCMeta
"""Class definition for the generic robot object.
.. image:: img/classes_Robot.png
Parameters
----------
name : str
The robot's name.
pose : array_like, optional
The robot's initial [x, y, theta] in [m,m,degrees] (defaults to
[0, 0.5, 0]).
pose_source : str
The robots pose source. Either a rostopic name, like 'odom' or
'tf', or 'python'
color_str : str
The color of the robots map object
**kwargs
Arguments passed to the ``MapObject`` attribute.
"""
def __init__(self,
name,
pose=None,
pose_source='python',
color_str='darkorange',
map_cfg={},
create_mission_planner=True,
goal_planner_cfg={},
path_planner_cfg={},
**kwargs):
# Object attributes
self.name = name
self.pose_source = pose_source
# Setup map
self.map = Map(**map_cfg)
# If pose is not given, randomly place in feasible layer.
feasible_robot_generated = False
if pose is None:
while not feasible_robot_generated:
x = random.uniform(self.map.bounds[0], self.map.bounds[2])
y = random.uniform(self.map.bounds[1], self.map.bounds[3])
if self.map.feasible_layer.pose_region.contains(Point([x, y])):
feasible_robot_generated = True
theta = random.uniform(0, 359)
pose = [x, y, theta]
self.pose2D = Pose(self, pose, pose_source)
self.pose_history = np.array(([0, 0, 0], self.pose2D.pose))
if pose_source == 'python':
self.publish_to_ROS = False
else:
self.publish_to_ROS = True
# Setup planners
if create_mission_planner:
self.mission_planner = MissionPlanner(self)
self.goal_planner = GoalPlanner(self,
**goal_planner_cfg)
# If pose_source is python, this robot is just in simulation
if not self.publish_to_ROS:
self.path_planner = PathPlanner(self, **path_planner_cfg)
self.controller = Controller(self)
# Define MapObject
# <>TODO: fix this horrible hack
create_diameter = 0.34
if self.name == 'Deckard':
pose = [0, 0, -np.pi / 4]
r = create_diameter / 2
n_sides = 4
pose = [0, 0, -np.pi / 4]
x = [r * np.cos(2 * np.pi * n / n_sides + pose[2]) + pose[0]
for n in range(n_sides)]
y = [r * np.sin(2 * np.pi * n / n_sides + pose[2]) + pose[1]
for n in range(n_sides)]
shape_pts = Polygon(zip(x, y)).exterior.coords
else:
shape_pts = Point([0, 0]).buffer(create_diameter / 2)\
.exterior.coords
self.map_obj = MapObject(self.name, shape_pts[:], has_relations=False,
blocks_camera=False, color_str=color_str)
self.update_shape()
def update_shape(self):
"""Update the robot's map_obj.
"""
self.map_obj.move_absolute(self.pose2D.pose)
@abstractmethod
def update(self, i=0):
"""Update all primary functionality of the robot.
This includes planning and movement for both cops and robbers,
as well as sensing and map animations for cops.
Parameters
----------
i : int, optional
The current animation frame. Default is 0 for non-animated robots.
"""
# <>TODO: @Matt Figure out how to move this back to pose class.
if self.pose_source == 'tf':
self.pose2D.tf_update()
if self.mission_planner.mission_status is not 'stopped':
# Update statuses and planners
self.mission_planner.update()
self.goal_planner.update()
if self.publish_to_ROS is False:
self.path_planner.update()
self.controller.update()
# Add to the pose history, update the map
self.pose_history = np.vstack((self.pose_history,
self.pose2D.pose[:]))
self.update_shape()