def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.avoidobstacles = self.create_controller( 'avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal/obstacle self.add_controller(self.hold) 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.gtg), ) # Start in the 'go-to-goal' state self.current = self.gtg
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # 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.process_state_info(robot_info) #Add controllers self.gtg = self.create_controller('GoToGoal', self.parameters) self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.wall = self.create_controller('FollowWall', self.parameters) self.hold = self.create_controller('Hold', None) # 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)) # Start in the 'go-to-goal' state self.current = self.gtg
def draw(self, renderer): K3Supervisor.draw(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 away_angle = self.avoidobstacles.get_heading_angle(self.parameters) goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0,0, arrow_length*cos(away_angle), arrow_length*sin(away_angle)) # Week 3 renderer.set_pen(0) for v in self.avoidobstacles.vectors: x,y,z = v renderer.push_state() renderer.translate(x,y) renderer.rotate(atan2(y,x)) renderer.draw_line(0.01,0.01,-0.01,-0.01) renderer.draw_line(0.01,-0.01,-0.01,0.01) renderer.pop_state()
def draw(self, renderer): K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 away_angle = self.avoidobstacles.get_heading_angle(self.parameters) goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0, 0, arrow_length * cos(away_angle), arrow_length * sin(away_angle)) # Week 3 renderer.set_pen(0) for v in self.avoidobstacles.vectors: x, y, z = v renderer.push_state() renderer.translate(x, y) renderer.rotate(atan2(y, x)) renderer.draw_line(0.01, 0.01, -0.01, -0.01) renderer.draw_line(0.01, -0.01, -0.01, 0.01) renderer.pop_state()
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 if self.current == self.gtg: # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(self.gtg.heading_angle), arrow_length * sin(self.gtg.heading_angle)) elif self.current == self.avoidobstacles: # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow( 0, 0, arrow_length * cos(self.avoidobstacles.heading_angle), arrow_length * sin(self.avoidobstacles.heading_angle)) elif self.current == self.wall: # Draw vector to wall: renderer.set_pen(0x0000FF) renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) # Draw renderer.set_pen(0xFF00FF) renderer.push_state() renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0], self.wall.along_wall_vector[1]) renderer.pop_state() # Draw heading (who knows, it might not be along_wall) renderer.set_pen(0xFF00FF) renderer.draw_arrow(0, 0, arrow_length * cos(self.wall.heading_angle), arrow_length * sin(self.wall.heading_angle)) # Important sensors renderer.set_pen(0) for v in self.wall.vectors: x, y, z = v renderer.push_state() renderer.translate(x, y) renderer.rotate(atan2(y, x)) renderer.draw_line(0.01, 0.01, -0.01, -0.01) renderer.draw_line(0.01, -0.01, -0.01, 0.01) renderer.pop_state() elif self.current == self.path: goal_angle = self.path.get_heading_angle(self.parameters) renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # 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.path = [(1.0, 0.5), (0.5, 1.0)] self.process_state_info(robot_info) #Add controllers self.movingtopoint1 = self.create_controller('MovingToPoint1', self.parameters) self.movingtopoint2 = self.create_controller('MovingToPoint2', self.parameters) # Define transitions self.add_controller(self.movingtopoint1, (self.at_point1, self.movingtopoint2)) self.add_controller(self.movingtopoint2, (self.at_point2, self.movingtopoint1)) # Start in the 'go-to-goal' state self.current = self.movingtopoint1
def set_parameters(self, params): """Set parameters for itself and the controllers""" K3Supervisor.set_parameters(self, params) self.gtg.set_parameters(self.parameters) self.avoidobstacles.set_parameters(self.parameters) self.wall.set_parameters(self.parameters) self.path.set_parameters(self.parameters)
def process_state_info(self, state): """Update state parameters for the controllers and self""" K3Supervisor.process_state_info(self,state) # The pose for controllers self.parameters.pose = self.pose_est # Distance to the goal self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances() # Smallest reading translated into distance from center of robot vectors = \ numpy.array( [numpy.dot( p.get_transformation(), numpy.array([d,0,1]) ) for d, p in zip(self.parameters.sensor_distances, self.parameters.sensor_poses) \ if abs(p.theta) < 2.4] ) self.distmin = min((sqrt(a[0]**2 + a[1]**2) for a in vectors))
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.goal_angle), arrow_length*sin(self.blending.goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.away_angle), arrow_length*sin(self.blending.away_angle)) # Draw heading renderer.set_pen(0x0000FF) renderer.draw_arrow(0,0, arrow_length*cos(self.blending.heading_angle), arrow_length*sin(self.blending.heading_angle))
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # 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.path = [(1.0, 0.5), (0.5, 1.0)] self.process_state_info(robot_info) #Add controllers self.movingtopoint1 = self.create_controller('MovingToPoint1', self.parameters) self.movingtopoint2 = self.create_controller('MovingToPoint2', self.parameters) # Define transitions self.add_controller(self.movingtopoint1, (self.at_point1, self.movingtopoint2)) self.add_controller(self.movingtopoint2, (self.at_point2, self.movingtopoint1)) # Start in the 'go-to-goal' state self.current = self.movingtopoint1
def process_state_info(self, state): """Update state parameters for the controllers and self""" K3Supervisor.process_state_info(self, state) # The pose for controllers self.parameters.pose = self.pose_est # Distance to the goal self.distance_from_goal = sqrt( (self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances() # Smallest reading translated into distance from center of robot vectors = \ numpy.array( [numpy.dot( p.get_transformation(), numpy.array([d,0,1]) ) for d, p in zip(self.parameters.sensor_distances, self.parameters.sensor_poses) \ if abs(p.theta) < 2.4] ) self.distmin = min((sqrt(a[0]**2 + a[1]**2) for a in vectors))
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 if self.current == self.gtg: # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow( 0, 0, arrow_length * cos(self.gtg.heading_angle), arrow_length * sin(self.gtg.heading_angle) ) elif self.current == self.avoidobstacles: # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow( 0, 0, arrow_length * cos(self.avoidobstacles.heading_angle), arrow_length * sin(self.avoidobstacles.heading_angle), ) elif self.current == self.wall: # Draw vector to wall: renderer.set_pen(0x0000FF) renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) # Draw renderer.set_pen(0xFF00FF) renderer.push_state() renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1]) renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0], self.wall.along_wall_vector[1]) renderer.pop_state() # Draw heading (who knows, it might not be along_wall) renderer.set_pen(0xFF00FF) renderer.draw_arrow( 0, 0, arrow_length * cos(self.wall.heading_angle), arrow_length * sin(self.wall.heading_angle) ) # Important sensors renderer.set_pen(0) for v in self.wall.vectors: x, y, z = v renderer.push_state() renderer.translate(x, y) renderer.rotate(atan2(y, x)) renderer.draw_line(0.01, 0.01, -0.01, -0.01) renderer.draw_line(0.01, -0.01, -0.01, 0.01) renderer.pop_state() elif self.current == self.path: goal_angle = self.path.get_heading_angle(self.parameters) renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info): K3Supervisor.__init__(self, robot_pose, robot_info) #Add at least one controller self.gtg = self.add_controller('gotogoal.GoToGoal', self.parameters.gains) #Set default controller self.current = self.gtg
def process_state_info(self, state): """Sets parameters for supervisors and selects a supervisor This code is run every time the supervisor executes""" K3Supervisor.process_state_info(self, state) #Add some data to variable self.parameters #Below are some default parameters #------------------------------------- self.parameters.pose = self.pose_est self.parameters.ir_readings = self.robot.ir_sensors.readings
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" K3Supervisor.__init__(self, robot_pose, robot_info) #Add controllers ( go to goal is default) self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.avoidobstacles = self.get_controller('avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters) self.current = self.gtg self.old_omega = 0
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" K3Supervisor.__init__(self, robot_pose, robot_info) #Add controllers ( go to goal is default) self.ui_params.sensor_angles = [pose.theta for pose in robot_info.ir_sensors.poses] self.avoidobstacles = self.add_controller('avoidobstacles.AvoidObstacles', self.ui_params) self.gtg = self.add_controller('gotogoal.GoToGoal', self.ui_params.gains) self.hold = self.add_controller('hold.Hold', None) self.current = self.gtg
def process_state_info(self, state): """Update state parameters for the controllers and self""" K3Supervisor.process_state_info(self,state) # The pose for controllers self.parameters.pose = self.pose_est # Distance to the goal self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances()
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" K3Supervisor.__init__(self, robot_pose, robot_info) #Add controllers ( go to goal is default) self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.avoidobstacles = self.get_controller( 'avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters) self.current = self.gtg self.old_omega = 0
def process_state_info(self, state): """Update state parameters for the controllers and self""" K3Supervisor.process_state_info(self, state) # The pose for controllers self.parameters.pose = self.pose_est # Distance to the goal self.distance_from_goal = sqrt( (self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) # Sensor readings in real units self.parameters.sensor_distances = self.get_ir_distances()
def draw(self, renderer): K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) # Draw direction from obstacles renderer.set_pen(0xFF0000) arrow_length = self.robot_size * 5 renderer.draw_arrow(0, 0, arrow_length * cos(self.avoidobstacles.away_angle), arrow_length * sin(self.avoidobstacles.away_angle)) # Draw direction to goal renderer.set_pen(0x444444) goal_angle = atan2(self.parameters.goal.y - self.pose_est.y, self.parameters.goal.x - self.pose_est.x) \ - self.pose_est.theta renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle))
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.blending = self.create_controller('blending.Blending', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal self.add_controller(self.hold, (lambda: not self.at_goal(), self.blending)) self.add_controller(self.blending, (self.at_goal,self.hold)) # Start in the 'blending' state self.current = self.blending
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 away_angle = self.avoidobstacles.get_heading_angle(self.parameters) goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle), arrow_length * sin(goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0, 0, arrow_length * cos(away_angle), arrow_length * sin(away_angle))
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.blending = self.create_controller('blending.Blending', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal self.add_controller(self.hold, (lambda: not self.at_goal(), self.blending)) self.add_controller(self.blending, (self.at_goal, self.hold)) # Start in the 'blending' state self.current = self.blending
def draw(self, renderer): K3Supervisor.draw(self,renderer) renderer.set_pose(self.pose_est) # Draw direction from obstacles renderer.set_pen(0xFF0000) arrow_length = self.robot_size*5 renderer.draw_arrow(0,0, arrow_length*cos(self.avoidobstacles.away_angle), arrow_length*sin(self.avoidobstacles.away_angle)) # Draw direction to goal renderer.set_pen(0x444444) goal_angle = atan2(self.parameters.goal.y - self.pose_est.y, self.parameters.goal.x - self.pose_est.x) \ - self.pose_est.theta renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle))
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" K3Supervisor.__init__(self, robot_pose, robot_info) #Add controllers ( go to goal is default) self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.avoidobstacles = self.get_controller('avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.get_controller('hold.Hold', None) self.add_controller(self.hold) 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.gtg), ) self.current = self.gtg
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" K3Supervisor.__init__(self, robot_pose, robot_info) #Add controllers ( go to goal is default) self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] self.avoidobstacles = self.get_controller( 'avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.get_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.get_controller('hold.Hold', None) self.add_controller(self.hold) 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.gtg), ) self.current = self.gtg
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self, renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size * 5 # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.goal_angle), arrow_length * sin(self.blending.goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.away_angle), arrow_length * sin(self.blending.away_angle)) # Draw heading renderer.set_pen(0x0000FF) renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.heading_angle), arrow_length * sin(self.blending.heading_angle))
def draw(self, renderer): """Draw controller info""" K3Supervisor.draw(self,renderer) renderer.set_pose(self.pose_est) arrow_length = self.robot_size*5 away_angle = self.avoidobstacles.get_heading_angle(self.parameters) goal_angle = self.gtg.get_heading_angle(self.parameters) # Draw arrow to goal renderer.set_pen(0x00FF00) renderer.draw_arrow(0,0, arrow_length*cos(goal_angle), arrow_length*sin(goal_angle)) # Draw arrow away from obstacles renderer.set_pen(0xFF0000) renderer.draw_arrow(0,0, arrow_length*cos(away_angle), arrow_length*sin(away_angle))
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # 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.process_state_info(robot_info) #Add controllers self.gtg = self.create_controller('GoToGoal', self.parameters) self.avoidobstacles = self.create_controller('AvoidObstacles', self.parameters) self.wall = self.create_controller('FollowWall', self.parameters) self.hold = self.create_controller('Hold', None) # 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)) # Start in the 'go-to-goal' state self.current = self.gtg
def __init__(self, robot_pose, robot_info): """Create controllers and the state transitions""" K3Supervisor.__init__(self, robot_pose, robot_info) # Fill in poses for the controller self.parameters.sensor_poses = robot_info.ir_sensors.poses[:] # Add controllers self.avoidobstacles = self.create_controller('avoidobstacles.AvoidObstacles', self.parameters) self.gtg = self.create_controller('gotogoal.GoToGoal', self.parameters) self.hold = self.create_controller('hold.Hold', None) # Transitions if at goal/obstacle self.add_controller(self.hold) 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.gtg), ) # Start in the 'go-to-goal' state self.current = self.gtg
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 set_parameters(self,params): """Set parameters for itself and the controllers""" K3Supervisor.set_parameters(self,params) self.movingtopoint1.set_parameters(self.parameters) self.movingtopoint2.set_parameters(self.parameters)
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 set_parameters(self, params): K3Supervisor.set_parameters(self, params) self.gtg.set_parameters(self.parameters) self.avoidobstacles.set_parameters(self.parameters)
def set_parameters(self, params): """Set parameters for itself and the controllers""" K3Supervisor.set_parameters(self, params) self.movingtopoint1.set_parameters(self.parameters) self.movingtopoint2.set_parameters(self.parameters)
def set_parameters(self,params): K3Supervisor.set_parameters(self,params) self.gtg.set_parameters(self.parameters) self.avoidobstacles.set_parameters(self.parameters)
def set_parameters(self, params): """Set parameters for itself and the controllers""" K3Supervisor.set_parameters(self, params) self.blending.set_parameters(self.parameters)
def set_parameters(self,params): """Set parameters for itself and the controllers""" K3Supervisor.set_parameters(self,params) self.blending.set_parameters(self.parameters)