def __handle_set_backpack_lights(self, command: sml.Identifier):
"""
Handle a Soar set-backpack-lights action.
The Sour output should look like:
(I3 ^set-backpack-lights Vx)
(Vx ^color [color])
where [color] is a string indicating which color the lights should be set to. The colors
are "red", "blue", "green", "white", and "off".
:param command: Soar command object
:return: True if successful, False otherwise
"""
color_str = command.GetParameterValue("color")
if color_str not in COLORS:
print("Invalid backpack lights color {}".format(color_str))
return False
elif color_str == "red":
light = cozmo.lights.red_light
elif color_str == "green":
light = cozmo.lights.green_light
elif color_str == "blue":
light = cozmo.lights.blue_light
elif color_str == "white":
light = cozmo.lights.white_light
else:
light = cozmo.lights.off_light
self.r.set_all_backpack_lights(light=light)
command.AddStatusComplete()
return (None, None)
def handle_command(self, command: sml.Identifier, agent: sml.Agent):
"""
Handle a command produced by Soar.
This basically just maps command names to their cozmo methods.
:param command: A Soar command object
:return: True if successful, False otherwise
"""
comm_name = command.GetCommandName().lower()
if comm_name == "move-lift":
success = self.__handle_move_lift(command, agent)
elif comm_name == "go-to-object":
success = self.__handle_go_to_object(command, agent)
elif comm_name == "move-head":
success = self.__handle_move_head(command, agent)
elif comm_name == "turn-to-face":
success = self.__handle_turn_to_face(command, agent)
elif comm_name == "set-backpack-lights":
success = self.__handle_set_backpack_lights(command, agent)
elif comm_name == "drive-forward":
success = self.__handle_drive_forward(command, agent)
elif comm_name == "turn-in-place":
success = self.__handle_turn_in_place(command, agent)
elif comm_name == "pick-up-object":
success = self.__handle_pick_up_object(command, agent)
elif comm_name == "place-object-down":
success = self.__handle_place_object_down(command, agent)
elif comm_name == "place-on-object":
success = self.__handle_place_on_object(command, agent)
elif comm_name == "dock-with-cube":
success = self.__handle_dock_with_cube(command, agent)
else:
raise NotImplementedError(
"Error: Don't know how to handle command {}".format(comm_name)
)
if not success:
command.AddStatusComplete()
return True