Exemplo n.º 1
0
    def __init__(self, robot_pose, robot_info, options=None):
        """Create controllers and the state transitions"""
        QuickBotSupervisor.__init__(self, robot_pose, robot_info)

        self.extgoal = False

        if options is not None:
            try:
                self.parameters.goal.x = options.x
                self.parameters.goal.y = options.y
                self.extgoal = True
            except Exception:
                pass

        # Fill in some parameters
        self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
        self.parameters.ir_max = robot_info.ir_sensors.rmax
        self.parameters.direction = 'left'
        self.parameters.distance = 0.2
        self.robot = robot_info
        # generate global path
        self.parameters.ga_path = ga.ga_execute(
            (0, 0), (self.parameters.goal.x, self.parameters.goal.y))
        self.parameters.ga_path.append(
            (self.parameters.goal.x, self.parameters.goal.y))
        global global_cnt
        global_cnt = len(self.parameters.ga_path)
        point_cnt = self.parameters.point_cnt
        #print self.parameters.ga_path, "ga_path"

        #Add controllers
        self.avoidobstacles = self.create_controller('AvoidObstacles',
                                                     self.parameters)
        self.gtg = self.create_controller('GoToGoal', self.parameters)
        self.hold = self.create_controller('Hold', None)
        self.wall = self.create_controller('FollowWall', self.parameters)
        self.path = self.create_controller('FollowPath', self.parameters)
        self.blending = self.create_controller("Blending", self.parameters)

        # Define transitions
        self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))
        self.add_controller(self.gtg, (self.at_goal, self.hold),
                            (self.at_obstacle, self.avoidobstacles))
        self.add_controller(self.avoidobstacles, (self.at_goal, self.hold),
                            (self.free, self.path))
        # self.add_controller(self.blending,
        #                     (self.at_goal, self.hold),
        #                     (self.free, self.path),
        #                     (self.at_obstacle, self.avoidobstacles),
        #                     )
        self.add_controller(
            self.path,
            (lambda: self.next_point(), self.path),
            (self.at_goal, self.hold),
            #(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
            (self.at_obstacle, self.avoidobstacles))

        self.current = self.path
Exemplo n.º 2
0
    def __init__(self, robot_pose, robot_info, options = None):
        """Create controllers and the state transitions"""
        QuickBotSupervisor.__init__(self, robot_pose, robot_info)

        self.extgoal = False

        if options is not None:
            try:
                self.parameters.goal.x = options.x
                self.parameters.goal.y = options.y
                self.extgoal = True
            except Exception:
                pass

        # Fill in some parameters
        self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
        self.parameters.ir_max = robot_info.ir_sensors.rmax
        self.parameters.direction = 'left'
        self.parameters.distance = 0.2
        self.robot = robot_info
        # generate global path
        self.parameters.ga_path = ga.ga_execute((0,0), (self.parameters.goal.x, self.parameters.goal.y))
        self.parameters.ga_path.append((self.parameters.goal.x, self.parameters.goal.y))
        global global_cnt
        global_cnt = len(self.parameters.ga_path)
        point_cnt = self.parameters.point_cnt
        #print self.parameters.ga_path, "ga_path"
        
        #Add controllers
        self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters)
        self.gtg = self.create_controller('GoToGoal', self.parameters)
        self.hold = self.create_controller('Hold', None)
        self.wall = self.create_controller('FollowWall', self.parameters)
        self.path = self.create_controller('FollowPath', self.parameters)
        self.blending = self.create_controller("Blending", self.parameters)
                
        # Define transitions
        self.add_controller(self.hold,
                            (lambda: not self.at_goal(), self.gtg))
        self.add_controller(self.gtg,
                            (self.at_goal, self.hold),
                            (self.at_obstacle, self.avoidobstacles))
        self.add_controller(self.avoidobstacles,
                            (self.at_goal, self.hold),
                            (self.free, self.path)
                            )
        # self.add_controller(self.blending,
        #                     (self.at_goal, self.hold),
        #                     (self.free, self.path),
        #                     (self.at_obstacle, self.avoidobstacles),
        #                     )
        self.add_controller(self.path,
                            (lambda: self.next_point(), self.path),
                            (self.at_goal, self.hold),
                            #(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
                            (self.at_obstacle, self.avoidobstacles))

        self.current = self.path
Exemplo n.º 3
0
    def __init__(self, robot_pose, robot_info, options=None):
        """Create controllers and the state transitions"""
        K3Supervisor.__init__(self, robot_pose, robot_info)
        self.extgoal = False

        if options is not None:
            try:
                self.parameters.goal.x = options.x
                self.parameters.goal.y = options.y
                self.extgoal = True
            except Exception:
                pass

        # The maximal distance to an obstacle (inexact)
        self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2

        # Fill in some parameters
        self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
        self.parameters.ir_max = robot_info.ir_sensors.rmax
        self.parameters.direction = 'left'
        self.parameters.distance = self.distmax * 0.85
        self.parameters.ga_path = ga.ga_execute(
            (0, 0), (self.parameters.goal.x, self.parameters.goal.y))
        self.parameters.ga_path.append(
            (self.parameters.goal.x, self.parameters.goal.y))
        global global_cnt
        global_cnt = len(self.parameters.ga_path)
        point_cnt = self.parameters.point_cnt

        self.process_state_info(robot_info)

        #Add controllers
        self.gtg = self.create_controller('GoToGoal', self.parameters)
        self.avoidobstacles = self.create_controller('K3_AvoidObstacles',
                                                     self.parameters)
        self.wall = self.create_controller('FollowWall', self.parameters)
        self.hold = self.create_controller('Hold', None)
        self.path = self.create_controller('FollowPath', self.parameters)

        # Define transitions
        # self.add_controller(self.hold,
        #                     (lambda: not self.at_goal(), self.gtg))
        # self.add_controller(self.gtg,
        #                     (self.at_goal, self.hold),
        #                     (self.at_wall, self.wall))
        self.add_controller(self.wall, (self.at_goal, self.hold),
                            (self.unsafe, self.avoidobstacles),
                            (self.wall_cleared, self.gtg))
        # self.add_controller(self.avoidobstacles,
        #                     (self.at_goal, self.hold),
        #                     (self.safe, self.wall))
        self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))

        self.add_controller(self.avoidobstacles, (self.at_goal, self.hold),
                            (self.free, self.path))

        self.add_controller(
            self.path,
            (lambda: self.next_point(), self.path),
            (self.at_goal, self.hold),
            #(lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
            (self.at_obstacle, self.avoidobstacles))

        self.current = self.path
Exemplo n.º 4
0
    def __init__(self, robot_pose, robot_info, options=None):
        """Create controllers and the state transitions"""
        K3Supervisor.__init__(self, robot_pose, robot_info)
        self.extgoal = False

        if options is not None:
            try:
                self.parameters.goal.x = options.x
                self.parameters.goal.y = options.y
                self.extgoal = True
            except Exception:
                pass

        # The maximal distance to an obstacle (inexact)
        self.distmax = robot_info.ir_sensors.rmax + robot_info.wheels.base_length / 2

        # Fill in some parameters
        self.parameters.sensor_poses = robot_info.ir_sensors.poses[:]
        self.parameters.ir_max = robot_info.ir_sensors.rmax
        self.parameters.direction = "left"
        self.parameters.distance = self.distmax * 0.85
        self.parameters.ga_path = ga.ga_execute((0, 0), (self.parameters.goal.x, self.parameters.goal.y))
        self.parameters.ga_path.append((self.parameters.goal.x, self.parameters.goal.y))
        global global_cnt
        global_cnt = len(self.parameters.ga_path)
        point_cnt = self.parameters.point_cnt

        self.process_state_info(robot_info)

        # Add controllers
        self.gtg = self.create_controller("GoToGoal", self.parameters)
        self.avoidobstacles = self.create_controller("K3_AvoidObstacles", self.parameters)
        self.wall = self.create_controller("FollowWall", self.parameters)
        self.hold = self.create_controller("Hold", None)
        self.path = self.create_controller("FollowPath", self.parameters)

        # Define transitions
        # self.add_controller(self.hold,
        #                     (lambda: not self.at_goal(), self.gtg))
        # self.add_controller(self.gtg,
        #                     (self.at_goal, self.hold),
        #                     (self.at_wall, self.wall))
        self.add_controller(
            self.wall, (self.at_goal, self.hold), (self.unsafe, self.avoidobstacles), (self.wall_cleared, self.gtg)
        )
        # self.add_controller(self.avoidobstacles,
        #                     (self.at_goal, self.hold),
        #                     (self.safe, self.wall))
        self.add_controller(self.hold, (lambda: not self.at_goal(), self.gtg))

        self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.path))

        self.add_controller(
            self.path,
            (lambda: self.next_point(), self.path),
            (self.at_goal, self.hold),
            # (lambda: self.parameters.point_cnt == len(self.parameters.ga_path) - 1 and not self.next_point(), self.gtg),
            (self.at_obstacle, self.avoidobstacles),
        )

        self.current = self.path