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()