class MoveRobotToNextPuckHandler(Handler): def __init__(self): self.initialized = False self.goal = None self.current_puck_color= None self.goal_tuple = None def initialize(self): self.pub = rospy.Publisher('movement_vectors_string', String, queue_size=1) self.sub = rospy.Subscriber('pucks', String, self.pucks_callback, queue_size=1) self.move_robot_handler = MoveRobotHandler() self.initialized = True def pucks_callback(self, data): self.handled_data["calculate_pucks_pub"].publish(True) pucks_dict = json.loads(data.data) self.goal_tuple = pucks_dict[self.current_puck_color][0]["center_position"] self.sub.unregister() def handle(self, handled_data=None): #handled_data["calculate_pucks_pub"].publish(True) self.handled_data = handled_data if not self.initialized: self.initialize() self.current_puck_color = handled_data['puck_colors'].pop(0) while self.goal_tuple is None or self.current_puck_color is None: pass handled_data["goal"] = create_pose(self.goal_tuple) handled_data["path_following_mode_pub"].publish("PUCK") handled_data = self.move_robot_handler.handle(handled_data) return handled_data def unregister(self): self.pub.unregister() self.sub.unregister() self.move_robot_handler.unregister()
class MoveRobotToNextCornerHandler(Handler): def __init__(self): self.initialized = False self.goal = None self.current_corner = None self.goal_tuple = None def initialize(self): self.sub = rospy.Subscriber('square', String, self.square_callback, queue_size=1) self.move_robot_handler = MoveRobotHandler() self.initialized = True def square_callback(self, data): square_dict = json.loads(data.data) self.goal_tuple = square_dict["corner_" + self.current_corner] self.sub.unregister() def handle(self, handled_data=None): handled_data["calculate_pucks_pub"].publish(True) if not self.initialized: self.initialize() self.current_corner = handled_data['corners'].pop(0) while self.goal_tuple is None or self.current_corner is None: pass handled_data["goal"] = create_pose(self.goal_tuple) handled_data["path_following_mode_pub"].publish("PUCK") handled_data = self.move_robot_handler.handle(handled_data) return handled_data def unregister(self): self.sub.unregister() self.move_robot_handler.unregister()
class MoveRobotToCommandPanelHandler(Handler): def __init__(self): self.initialized = False def initialize(self): self.move_robot_handler = MoveRobotHandler() self.initialized = True def handle(self, handled_data=None): if not self.initialized: self.initialize() handled_data["goal"] = create_pose(handled_data["COMMAND_PANEL"]) handled_data["path_following_mode_pub"].publish("CENTER") handled_data = self.move_robot_handler.handle(handled_data) return handled_data def unregister(self): self.move_robot_handler.unregister()