def __init__(self, name, goal_planner_type='stationary'):
# Stop attribute
# rospy.Subscriber('/'+name.lower()+'/stop', Bool, self.stop_callback)
# Object attributes
self.name = name.lower()
self.Pose = Pose(self.name)
#Late hack for saving positions
self.allPoses = []
self.fileName = name + '_' + 'goal_planner_type_' + str(
time.time())[0:-4] + '.npy'
# Select and instantiate the goal planner
if goal_planner_type == 'stationary':
from stationary_planner import StationaryGoalPlanner
self.goal_planner = StationaryGoalPlanner(self.name,
self.Pose.pose)
elif goal_planner_type == 'simple':
from simple_planner import SimpleGoalPlanner
self.goal_planner = SimpleGoalPlanner(self.name, self.Pose.pose)
elif goal_planner_type == 'pomdp':
from pomdp_planner import PomdpGoalPlanner
self.goal_planner = PomdpGoalPlanner(self.init_belief,
self.init_map_bounds,
self.init_delta, self.name,
self.Pose.pose)
# These variables are now in the goal planner, let's not store the references in two places for debugging purposes
del self.init_belief
del self.init_map_bounds
del self.init_delta
elif goal_planner_type == 'rob_int':
from robber_evasion_planner import robberEvasionGoalPlanner
self.goal_planner = robberEvasionGoalPlanner(
self.name, self.Pose.pose)
elif goal_planner_type == 'audio': # Jeremy's Audio Planner. Intergration with this goal planner class has not been set up yet
from audio_planner import AudioGoalPlanner
self.goal_planner = AudioGoalPlanner()
# Other planners not used. No integration thus far done
elif goal_planner_type == 'trajectory':
from trajectory_planner import TrajectoryGoalPlanner
self.goal_planner = TrajectoryGoalPlanner()
elif goal_planner_type == 'particle':
from particle_planner import ParticleGoalPlanner
self.goal_planner = ParticleGoalPlanner()
elif goal_planner_type == 'MAP':
from probability_planner import PorbabilityGoalPlanner
self.goal_planner = ProbabilityGoalPlanner()
else:
print("No goal planner selected. Check instantation of robot.py")
raise
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()