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