def control_humanoid(): robot = Robot.get_inst(1) object_detector = ObjectDetector() try: # Loop until exit flag set to True(1) while not ExitControl.gen: # Get users desired action motion_num = input("Enter motion num: ") # Give access to full range of motions if motion_num == KeyWordControl.FULL_CONTROL: print("full control") motion_num = input("Enter motion num: ") # Print dictionary if motion_num == KeyWordControl.DICTIONARY: for key in DICTS.HUMANOID_FULL: print("{} :{} ".format(key, DICTS.HUMANOID_FULL[key])) motion_num = input("Enter motion num: ") try: motion_num = int(motion_num) except ValueError as e: print("{} is not a number, full control" " exit".format(motion_num)) print(e) if motion_num in DICTS.HUMANOID_FULL: robot.play_rcb4_motion(HexConst.HUNDRED_NUM[motion_num]) # Create keyboard remote elif motion_num == KeyWordControl.REMOTE: alert_window = AlertWindow("remote") alert_window.output("Enter a key") ExitControl.remote = 0 # Loop until exit flag set to 1 while not ExitControl.remote: # get users command command = cv2.waitKey(0) # Stop robot motion if command == R_Control.STOP: message = "Robot stop" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[1]) # Enable and disable detection motion command elif command == R_Control.DETECT_ON: message = "detect on" alert_window.output(message) Funcs.detect_on() elif command == R_Control.DETECT_OFF: message = "detect off" alert_window.output(message) Funcs.detect_off() # Move the robot elif command == R_Control.FORWARD: message = "forward" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[15]) elif command == R_Control.BACKWARD: message = "backward" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[16]) elif command == R_Control.LEFT: message = "left" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[17]) elif command == R_Control.RIGHT: message = "right" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[18]) # Turn the robot elif command == R_Control.TURN: message = "turn" alert_window.output(message) command = cv2.waitKey(0) if command == R_Control.LEFT: message = "turn left" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[19]) elif command == R_Control.RIGHT: message = "turn right" alert_window.output(message) robot.play_rcb4_motion(HexConst.HUNDRED_NUM[20]) else: message = "invalid turn" alert_window.output(message) # Exit program elif command == R_Control.EXIT: ExitControl.gen = 1 ExitControl.remote = 1 print("exiting") # Close remote elif command == R_Control.CLOSE: ExitControl.remote = 1 else: message = "invalid option" alert_window.output(message) # Close window object (close opencv window) del alert_window # Print dictionary elif motion_num == KeyWordControl.DICTIONARY: for key in DICTS.HUMANOID_MOTION: print(key, " : ", DICTS.HUMANOID_MOTION[key]) # Stop robot current motion elif (motion_num == KeyWordControl.STOP1 or motion_num == KeyWordControl.STOP2): robot.play_rcb4_motion(HexConst.HUNDRED_NUM[1]) # Start calibration elif motion_num == KeyWordControl.CALIBRATE: # Ensure calibrator thread is not already active if ("calibrator_thread" not in locals() or not calibrator_thread.is_alive()): print("starting calibrate") calibrator_thread = threading.Thread( target=object_detector.calibrate_filter) with Locks.CAM_LOCK: ExitControl.calibrate = 0 calibrator_thread.start() else: print("calibrate already active") # Stop Calibration elif motion_num == KeyWordControl.CALIBRATE_STOP: print("stopping calibrate") with Locks.CAM_LOCK: ExitControl.calibrate = 1 # Enable or disable ball detection from sending motion command elif motion_num == KeyWordControl.DETECT_ON: Funcs.detect_on() elif motion_num == KeyWordControl.DETECT_OFF: Funcs.detect_off() # Turn debug verbose on or off elif motion_num == KeyWordControl.DEBUG_ON: Debug.debug_on() elif motion_num == KeyWordControl.DEBUG_OFF: Debug.debug_off() # Create exit condition elif (motion_num == KeyWordControl.EXIT1 or motion_num == KeyWordControl.EXIT2): Funcs.exit_program() else: try: motion_num = int(motion_num) except ValueError: print("motion unknown try again") if motion_num in DICTS.HUMANOID_MOTION: robot.play_rcb4_motion(HexConst.HUNDRED_NUM[motion_num]) except Exception as e: print("Main program exiting with exception: \n", e) finally: with Locks.GEN_LOCK: # Set exit flag for all threads ExitControl.gen = 1 del robot del object_detector
def control_robot(self): # All relevant command numbers can be found in config.Conf # HUMANOID_FULL and SPIDER_FULL if not self.is_connected: self.logger.debug( "Robot control could not be started as camera is not " "connected") return from robot_control import Robot self.logger.debug("control_robot: Started") self.robot = Robot.get_inst(self.robot_type) rbt_hd_srch_thrd = threading.Thread(target=self.rbt_head_search) rbt_hd_track_thrd = threading.Thread(target=self.rbt_head_track) cmd_sent = None cmd_sent_time = 0 wait_time = 0 time.sleep(3) while ExitControl.gen and ExitControl.cam: success = False dur = time.time() - cmd_sent_time orig_wait_time = wait_time if self.cam_obj_dict[ObjDist.IS_FOUND]: if not rbt_hd_track_thrd.is_alive(): rbt_hd_track_thrd = threading.Thread( target=self.rbt_head_track) rbt_hd_track_thrd.start() self.search_turn = False self.last_non_search = time.time() if dur > wait_time: if ( # Within kick range Conf.KICK_DIST + Conf.KICK_RANGE > self.cam_obj_dict[ObjDist.AVG] > Conf.KICK_DIST - Conf.KICK_RANGE): cmd_sent = Conf.CMD_KICK elif self.cam_obj_dict[ObjDist.AVG] > Conf.KICK_DIST: cmd_sent = Conf.CMD_FORWARD else: cmd_sent = Conf.CMD_BACKWARD self.command = self.robot.get_command_num(cmd_sent) wait_time = self.robot.full_dict[self.command][1] cmd_sent_time = time.time() success = self.robot.send_command(self.command, auto=True) else: # Search for object non_search_dur = time.time() - self.last_non_search if non_search_dur < Conf.MAX_SEARCH_DUR: # Search in front of robot then turn robot around 180 if not rbt_hd_srch_thrd.is_alive(): rbt_hd_srch_thrd = threading.Thread( target=self.rbt_head_search) if self.search_turn: cmd_sent = Conf.CMD_RIGHT self.command = self.robot.get_command_num(cmd_sent) wait_time = self.robot.full_dict[self.command][1] self.robot.send_command(self.command, auto=True) temp_dur = 0 wait_start = time.time() temp_wait = wait_time / 10 while (temp_dur < wait_time and not self.cam_obj_dict[ObjDist.IS_FOUND] and self.robot.active_auto_control): time.sleep(temp_wait) temp_dur = time.time() - wait_start self.search_turn = False rbt_hd_srch_thrd.start() else: self.logger.warning( "Object not found in " f"{pretty_time(non_search_dur, is_raw=False)}" " and robot has stopped searching", log_type=Conf.LOG_CAM_STOP_SEARCH) if success and dur > orig_wait_time: self.logger.info(f"control_robot: Command sent: {cmd_sent}") if self.disp: if (self.action_request == Conf.CMD_RH_UP or self.action_request == Conf.CMD_RH_DOWN or self.action_request == Conf.CMD_RH_LEFT or self.action_request == Conf.CMD_RH_RIGHT): self.robot.send_head_command(self.action_request) elif self.action_request == Conf.CMD_CV_HEAD_DELTA_P: self.robot.head_delta_theta += 5 self.robot.logger.debug( "Camera: control_robot: head_delta_theta increased to" f" {self.robot.head_delta_theta} ") elif self.action_request == Conf.CMD_CV_HEAD_DELTA_M: self.robot.head_delta_theta -= 5 self.robot.logger.debug( "Camera: control_robot: head_delta_theta decreased to" f" {self.robot.head_delta_theta} ") elif self.action_request == Conf.CMD_VARS: self.robot.dump_status() elif self.action_request == Conf.CMD_VARS1: self.robot.dump_conf() elif self.action_request == Conf.CMD_VARS2: self.dump_status() elif self.action_request != "": # Send all viable commands if self.action_request == Conf.CMD_STOP: self.robot.active_auto_control = False self.robot.send_command(self.action_request) if self.robot.cam_request == Conf.CMD_VARS2: self.dump_status() elif self.action_request != "": temp = ( "control_robot: current action_request before erasing " f"--> {self.action_request}") print(temp) self.action_request = "" if self.robot.cam_request != "": self.robot.cam_request = "" time.sleep(.2) else: time.sleep(1)