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)
