def main():
from cops_and_robots.map_tools.map_elements import MapArea, MapObject
fig = plt.figure()
ax = fig.add_subplot(111)
area1 = MapArea('Area1', [1, 1], pose=[1, 1, 0], visible=True, color_str='blue')
area2 = MapArea('Area2', [1, 1], pose=[-1, 1, 0], visible=True, color_str='lightblue')
object1 = MapObject('Object1', [1, 1], pose=[0, 1, 0], visible=True, color_str='red')
object2 = MapObject('Object2', [1, 1], pose=[0, -1, 0], visible=True, color_str='magenta')
static_elements = [area1, object1]
dynamic_elements = [area2, object2]
information_elements = []
elements = {'static': static_elements, 'dynamic': dynamic_elements, 'information':information_elements}
invisible_elements = [area2, object1]
bounds = [-5, -5, 5, 5]
sl = ShapeLayer(elements, bounds=bounds)
ax.set_xlim(bounds[0], bounds[2])
ax.set_ylim(bounds[1], bounds[3])
# sl = ShapeLayer(elements, invisible_elements=invisible_elements, bounds=[-5, -5, 5, 5])
ani = animation.FuncAnimation(sl.fig, sl.update,
frames=xrange(100),
interval=100,
repeat=True,
blit=False)
plt.show()
def __init__(self,
robot_pose=(0, 0, 0),
visible=True,
default_color=cnames['yellow'],
element_dict={},
min_view_dist=0.3,
max_view_dist=1.0):
self.element_dict = element_dict
# Define nominal viewcone
self.min_view_dist = min_view_dist # [m]
self.max_view_dist = max_view_dist # [m]
self.view_angle = math.pi / 2 # [rad]
viewcone_pts = [
(0, 0),
(self.max_view_dist * math.cos(self.view_angle / 2),
self.max_view_dist * math.sin(self.view_angle / 2)),
(self.max_view_dist * math.cos(-self.view_angle / 2),
self.max_view_dist * math.sin(-self.view_angle / 2)),
(0, 0),
]
# Create SoftMax model for camera
self.detection_model = camera_model_2D(self.min_view_dist,
self.max_view_dist)
# Instantiate Sensor superclass object
update_rate = 1 # [hz]
has_physical_dimensions = True
super(Camera, self).__init__(update_rate, has_physical_dimensions)
# Set the ideal and actual viewcones
self.ideal_viewcone = MapObject(
'Ideal viewcone',
viewcone_pts,
visible=False,
blocks_camera=False,
color_str='pink',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.viewcone = MapObject(
'Viewcone',
viewcone_pts,
alpha=0.2,
visible=True,
blocks_camera=False,
color_str='lightyellow',
pose=robot_pose,
has_relations=False,
centroid_at_origin=False,
)
self.view_pose = (0, 0, 0)
# <>TODO: Add in and test an offset of (-0.1,-0.1)
self.offset = (0, 0, 0) # [m] offset (x,y,theta) from center of robot
self._move_viewcone(robot_pose)
# Set up the VB fusion parameters
self.vb = VariationalBayes()
def set_up_fleming(map_):
"""Set up a map as the generic Fleming space configuration.
"""
# Make vicon field space 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])
n_walls = poses.shape[0]
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]],
])
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.contains(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)
goal_points = [
(0, 0, 0),
(2, 0, 0),
(2, 0.5, 90),
(2, 1.5, 90),
(2, 1.7, 90),
]
# Define Map and its objects
bounds = (-5, -5, 5, 5)
l = 1.2192 # [m] wall length
w = 0.1524 # [m] wall width
pose = (2.4, 0, 90)
wall1 = MapObject('wall1', (l, w), pose=pose)
pose = (2, 2.2, 0)
wall2 = MapObject('wall2', (l, w), pose=pose)
shape_layer = ShapeLayer(bounds=bounds)
shape_layer.add_obj(wall1)
shape_layer.add_obj(wall2)
shape_layer.plot()
# Define Particle Filter
# target_pose = (10,10,0)
# particle_filter = ParticleFilter(bounds=bounds,"Roy")
# particle_filter.update_viewcone(kinect,target_pose)
# Move camera and update the camera
for point in goal_points:
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
if self.name == 'Deckard':
pose = [0, 0, -np.pi/4]
r = iRobotCreate.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(iRobotCreate.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()
