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