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