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