def __init__(self, env, slam, control, sensor, pose, robot_id, v=0.0, color="yellow"):
self._env = env
self._slam = slam
self._control = control
self._sensor = sensor
self._pose = pose
self._id = robot_id
self._v = v
self._color = color
# Radius is 6% of env height
self._r = int(self._env.h() * 0.06)
# Create a control, if none was given
if not self._control:
noise = (0.3, 0.3, 0.05) # (x, y, theta) noise
self._control = Control(env, v, noise)
# Create a sensor, if none was given
if not self._sensor:
sensor_range = self._r * 4.0
noise = ((sensor_range * 0.05, 0.01), np.matrix([[0.70, 0.0], [0.0, 0.70]]))
self._sensor = Sensor(self._env, sensor_range=sensor_range, noise=noise, width=2.0*self._r)
# Create a SLAM algorithm object, if none was given
if not self._slam:
M = 20 # number of particles
self._slam = FastSLAM(self._control, self._sensor, pose, M, self._env.get_landmark_count())
class Robot(object):
# Constructor
# @param env The Robot's Environment
# @param slam SLAM algorithm
# @param control A Control object
# @param sensor A Sensor object
# @param pose Initial pose
# @param v Translational velocity
# @param color Color of the robot (black by default)
def __init__(self, env, slam, control, sensor, pose, robot_id, v=0.0, color="yellow"):
self._env = env
self._slam = slam
self._control = control
self._sensor = sensor
self._pose = pose
self._id = robot_id
self._v = v
self._color = color
# Radius is 6% of env height
self._r = int(self._env.h() * 0.06)
# Create a control, if none was given
if not self._control:
noise = (0.3, 0.3, 0.05) # (x, y, theta) noise
self._control = Control(env, v, noise)
# Create a sensor, if none was given
if not self._sensor:
sensor_range = self._r * 4.0
noise = ((sensor_range * 0.05, 0.01), np.matrix([[0.70, 0.0], [0.0, 0.70]]))
self._sensor = Sensor(self._env, sensor_range=sensor_range, noise=noise, width=2.0*self._r)
# Create a SLAM algorithm object, if none was given
if not self._slam:
M = 20 # number of particles
self._slam = FastSLAM(self._control, self._sensor, pose, M, self._env.get_landmark_count())
def slam(self): return self._slam
def pose(self): return self._pose
# Draw the robot at its current pose
# @param canvas The canvas onto which to draw the robot
def draw(self, canvas):
(x, y, theta) = self._pose
# Lest we forgot to render the sensor "beam"...
self._sensor.draw(canvas, self._pose)
# Draw the circle, which is the body of the robot
canvas.create_oval(
x - self._r,
y + self._r,
x + self._r,
y - self._r,
outline="black",
fill=self._color,
width=1,
tags=('robot', 'body'))
# Next, wheel axis
canvas.create_line(
x - int(self._r * sin(theta)),
y + int(self._r * cos(theta)),
x + int(self._r * sin(theta)),
y - int(self._r * cos(theta)),
fill="black",
tags=('robot', 'wheelaxis'))
# Now comes the wheels
# TODO (not strictly necessary)
# Finally, draw the arrow indicating robot direction perpendicular
# to the wheel axis
canvas.create_line(
x,
y,
x + self._r * cos(theta),
y + self._r * sin(theta),
arrow=LAST,
arrowshape=(3,5,3),
fill="black",
tags=('robot', 'direction'))
# Update the Robot's pose
# @param dt Time delta
def update(self, dt):
(x, y, theta) = self._pose
# Make the move (might be undone in case of collision)
(new_x, new_y, _) = self._control.move(self._pose, dt)
# Take a sensor reading, see if there are any impending collisions
measurements = self._sensor.sense((new_x, new_y, theta))
sensed_collision = False
if len(measurements) > 0:
# Taking this move will lead to a collision, so stay where we are
new_x, new_y = x, y
sensed_collision = True
# Query the control for the new heading
self._pose = (new_x, new_y, self._control.action(sensed_collision, theta))
# Finally, run the SLAM algorithm
self._slam._x = self._pose
self._slam.run(measurements, dt)
return self
