示例#1
0
    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())
示例#2
0
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