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)
