def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" QBSwitchingSupervisorBase.__init__(self, robot_pose, robot_info) self.states = {} self.blending = self.create_controller("week5_solved.Blending", self.parameters) 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.add_controller(self.gtg, (self.at_goal, self.hold), (self.unsafe, self.blending)) self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.gtg), (self.safe, self.blending) ) self.add_controller(self.blending, (self.at_goal, self.hold), (self.free, self.gtg), (self.at_obstacle, self.avoidobstacles), )
def __init__(self, robot_pose, robot_info): """Creates an avoid-obstacle controller and go-to-goal controller""" QBSwitchingSupervisorBase.__init__(self, robot_pose, robot_info) self.states = {} self.blending = self.create_controller("week5_solved.Blending", self.parameters) 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.add_controller(self.gtg, (self.at_goal, self.hold), (self.unsafe, self.blending)) self.add_controller(self.avoidobstacles, (self.at_goal, self.hold), (self.free, self.gtg), (self.safe, self.blending)) self.add_controller( self.blending, (self.at_goal, self.hold), (self.free, self.gtg), (self.at_obstacle, self.avoidobstacles), )
def process_state_info(self, state): """Selects the best controller based on ir sensor readings Updates parameters.pose and parameters.ir_readings""" QBSwitchingSupervisorBase.process_state_info(self,state) self.distance_from_goal = sqrt((self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) self.distmin = min(self.parameters.sensor_distances)
def process_state_info(self, state): """Selects the best controller based on ir sensor readings Updates parameters.pose and parameters.ir_readings""" QBSwitchingSupervisorBase.process_state_info(self, state) self.distance_from_goal = sqrt( (self.pose_est.x - self.parameters.goal.x)**2 + (self.pose_est.y - self.parameters.goal.y)**2) self.distmin = min(self.parameters.sensor_distances)
def set_parameters(self,params): QBSwitchingSupervisorBase.set_parameters(self,params) self.blending.set_parameters(self.parameters)
def set_parameters(self, params): QBSwitchingSupervisorBase.set_parameters(self, params) self.blending.set_parameters(self.parameters)