def release_camera(self):
if self.have_camera():
mgr.global_resources.release("Camera")
self.camera = None
else:
dg.print("Warning (camera): release_camera called while not \
acquired.")
def register_unit(unitname):
dg.print("Registering unit name {}...".format(unitname))
cfg.overall_config.set_unitname(unitname)
try:
main_hostname = cfg.overall_config.main_hostname()
except cfg.ConfigurationError as e:
dg.print(str(e))
return False
main_ip = socket.gethostbyname(main_hostname)
global client_conn, command_server # needed to keep client_conn alive
try:
client_conn = rpyc.connect(main_ip, 18868)
except ConnectionRefusedError as e:
dg.print(str(e))
return False
# unit_ip = client_conn._config['endpoints'][0]
unit_hostname, _ = client_conn._channel.stream.sock.getsockname()
unit_ip = socket.gethostbyname(unit_hostname)
cfg.overall_config.set_ip(unit_ip)
dg.print("Found main unit '{}'".format(
client_conn.root.get_service_name()))
client_conn.root.register_unit_name(unitname)
command_server = ThreadedServer(CommandService, port=18869)
Thread(target=command_server.start, args=[]).start()
client_conn.root.register_command_server(unitname, 18869)
dg.print("Registered.")
Thread(target=wait_for_close, args=[command_server]).start()
return True
def activate(self):
self.acquire_motors("Wheels")
if not self.have_acquired("Wheels"):
dg.print("Cannot access wheel motors for PID control.")
self.begin_actuate()
with self.condition:
self.condition.notify()
def store_received(self, recvd_list):
'''Store values received from remote, after checking that they are of the right format (4 angle values; three for the arm servos and 1 for the gripper). Once the values are checked, send them to the motors using the Actuator notification system. Return a reply in the form of a string to be sent back. If the values are invalid, return None.'''
if len(recvd_list) != 4:
return None
try:
thet_1 = int(recvd_list[0])
thet_2 = int(recvd_list[1])
thet_3 = int(recvd_list[2])
gripval = int(recvd_list[3])
except (ValueError, IndexError) as e:
dg.print(str(e))
return None
else:
# all values must be in the right range
if thet_1 <= 180 and thet_1 >= -180\
and thet_2 <= 180 and thet_2 >= -180\
and thet_3 <= 180 and thet_3 >= -180\
and gripval <= 180 and gripval >= -180:
with self.condition: # acquire lock
self.angle_1 = thet_1
self.angle_2 = thet_2
self.angle_3 = thet_3
self.angle_grp = gripval
self.condition.notify() # tell the actuator thread
# that values are ready.
return 'ACK'
else:
return 'ERR:RANGE' # values in wrong range
def before_start_call(self):
'''Call before the opmode's start() is executed.'''
with self.opmode_lock:
if not self.is_stopped():
raise OpModeError(
'Cannot start {}: mode already active'.format(self.name))
self.opmode_state = State.STARTING
dg.print("Starting {} mode...".format(self.name))
def set_values(self, values):
self.angle_grab = values[0]
self.angle_top = values[1]
self.angle_middle = values[2]
self.angle_bottom = values[3]
dg.print("Mock arm motors: values received: {}, {}, {}, {}".format(
values[0], values[1], values[2], values[3]))
self._set_angle()
def register_name(self, name):
if name in type(self).goals_list:
dg.print(
"Warning: goal name '{}' already taken. Skipping...".format(
name))
return
type(self).goals_list[name] = self
self.name = name
def activate(self):
self.acquire_motors("Arm")
if not self.have_acquired("Arm"):
dg.print("Cannot access arm motors for inverse kinematics.")
self.begin_actuate()
with self.condition:
self.condition.notify() # provide initial angles to motors
self.end_x, self.end_y, self.gamma = self._forward_kinematics(
self.angle_bottom, self.angle_middle, self.angle_top)
def set_values(self, values):
x = values[0]
y = values[1]
dg.print("Mock wheel motors: values received: {}, {}".format(x, y))
v_left = (y / self._ymax) + (1 / 2 * (x / self._xmax))
v_right = (y / self._ymax) - (1 / 2 * (x / self._xmax))
self._setMotorRight(v_right)
self._setMotorLeft(v_left)
def register_name(self, name):
'''Store opmode instance in opmode_list.'''
if name in type(self).opmodes_list:
dg.print(
'WARNING: Operational mode name {} already registered. Skipping...'
.format(name))
return
type(self).opmodes_list[name] = self
self.name = name
def begin_stream(self, source=None):
'''Stream source (default is direct camera output) to the specified \
port.'''
if self.streaming:
raise CameraUserError(
'Already streaming: cannot start new stream.')
if not self.have_camera():
raise CameraUserError('Camera not acquired.')
host = cfg.overall_config.get_connected_ip()
if source is None:
strm_port = cfg.cam_config.stream_port()
strm_framerate = cfg.cam_config.stream_framerate()
strm_width = cfg.cam_config.stream_frame_width()
strm_height = cfg.cam_config.stream_frame_height()
src_framerate = cfg.cam_config.capture_framerate()
src_width = cfg.cam_config.capture_frame_width()
src_height = cfg.cam_config.capture_frame_height()
else:
strm_port = source.stream_port()
strm_framerate = source.stream_framerate()
strm_width = source.stream_frame_width()
strm_height = source.stream_frame_height()
src_framerate = source.capture_framerate()
src_width = source.capture_frame_width()
src_height = source.capture_frame_height()
device = cfg.cam_config.device()
if device == 'rpicamsrc':
compressor = 'omxh264enc'
tune = ' '
elif device == 'v4l2src' or device == 'avfvideosrc' \
or device == "autovideosrc":
compressor = 'x264enc'
tune = ' tune=zerolatency '
comm = 'appsrc ! videoconvert ! video/x-raw,width='+str(src_width)+\
',height='+str(src_height)+',framerate='+str(src_framerate)+\
'/1,format=I420 ! '+compressor+tune+'! rtph264pay config-interval=1 pt=96 ! \
gdppay ! tcpserversink host=' +host+' port='+str(strm_port)+' sync=false'
self.stream_writer = cv2.VideoWriter(comm, cv2.CAP_GSTREAMER, 0,
strm_framerate,
(strm_width, strm_height), True)
self.streaming = True
if source is None:
thread = Thread(target=self._stream, args=())
else:
thread = Thread(target=self._stream_nondefault, args=[source])
thread.start()
dg.print("Using camera device '{}' and encoder '{}'".format(
device, compressor))
dg.print("Streaming on IP address {} and port {}".format(
host, strm_port))
def acquire_camera(self):
'''Get shared access to the camera.'''
if self.have_camera():
raise CameraUserError('Already have camera access: cannot \
reacquire.')
try:
self.camera = mgr.global_resources.get_shared("Camera")
except mgr.ResourceError as e:
dg.print(str(e))
raise CameraUserError('Could not get access to camera.')
def test_print_closed(self):
self.assertEqual(Diagnostics.state, DiagState.CLOSED)
Diagnostics.socket = Mock()
Diagnostics.buf = deque(maxlen=20)
self.assertEqual(len(Diagnostics.buf), 0)
Diagnostics.print("hi")
self.assertEqual(len(Diagnostics.buf), 1)
Diagnostics.print("hi again")
self.assertEqual(len(Diagnostics.buf), 2)
Diagnostics.socket.reply.assert_not_called()
self.assertEqual(Diagnostics.state, DiagState.CLOSED)
def on_disconnect(self, conn):
key_to_remove = None
for key in self.__class__.unit_list.keys():
_conn, _, _ = self.__class__.unit_list[key]
if _conn == conn:
dg.print("Unit {} disconnected.".format(key))
key_to_remove = key
if key_to_remove is not None:
self.__class__.unit_list.pop(key_to_remove)
else:
dg.print("Warning: unknown unit disconnected.")
def before_stop_call(self):
'''Call before the opmode's stop() is executed.'''
with self.opmode_lock:
if self.is_stopped():
raise OpModeError('Cannot stop {}: already stopped'.format(
self.name))
elif self.is_stopping():
raise OpModeError(
'Cannot stop {}: busy processing previous stop request.'.
format(self.name))
self.opmode_state = State.STOPPING
dg.print("Stopping {} mode...".format(self.name))
def disconnect_internal(self):
'''Close the socket and set the connection state to closed'''
with self.state_lock:
if self.state == ConnState.CLOSED:
return
dg.print("{}: disconnecting...".format(self.__class__.__name__))
with self.state_lock:
self.state = ConnState.CLOSED
if self.socket is not None:
self.socket.close()
self.socket = None
dg.print("{}: disconnected.".format(self.__class__.__name__))
def test_print_established_error(self):
self.assertEqual(Diagnostics.state, DiagState.CLOSED)
Diagnostics.socket = Mock()
Diagnostics.state = DiagState.ESTABLISHED
Diagnostics.buf = deque(maxlen=20)
Diagnostics.socket.reply.side_effect = TcpSocketError
self.assertEqual(len(Diagnostics.buf), 0)
#with self.assertRaises(DiagnosticsError):
Diagnostics.print("hi")
#self.assertEqual(len(Diagnostics.buf), 0)
Diagnostics.socket.reply.assert_called_with("hi")
self.assertEqual(Diagnostics.state, DiagState.CLOSED)
def exposed_accept(self, command):
dg.print("Command received from main unit: {}".format(command))
try:
parsed_args = parse_entry(command)
Thread(target=launcher.call_action, args=[parsed_args]).start()
with cli_wait:
res = cli_wait.wait(10) # wait for notification from mode.
if not res:
return 'ERR'
return 'ACK'
except CommandError as e:
dg.print(e)
def find_obj(self):
frame = self.camera.get_frame()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
objects = []
try:
objects = self.cascade.detectMultiScale(gray, 1.1,
50) #1.1,250 # 1.1,500
except Exception as e:
dg.print(str(e))
for (x, y, w, h) in objects:
cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
self.update_frame(frame) # for streaming
return objects
def _setMotorLeft(self, power):
int(power)
if power < 0:
pwm = int(self.duty_cycle * (1 + power))
if pwm > self.duty_cycle:
pwm = self.duty_cycle
elif power > 0:
pwm = int(self.duty_cycle * power)
if pwm > self.duty_cycle:
pwm = self.duty_cycle
else:
pwm = 0
dg.print("Pwm value for left wheel motors: {}".format(pwm))
def store_received(self, recvd_list):
if len(recvd_list) != 3:
dg.print("Wrong length of recvd list: actual is {}".format(
len(recvd_list)))
return None
try:
thet_1 = int(recvd_list[0])
thet_2 = int(recvd_list[1])
thet_3 = int(recvd_list[2])
except (ValueError, IndexError) as e:
dg.print(str(e))
return None
else:
try:
self.clisock.write("{},{},{}".format(thet_1, thet_2, thet_3))
self.clisock.read() # perform the read to stay in sync
time.sleep(0.02) # strange error without this
return 'ACK'
except ClientSocketError as e:
# self.run_on_connection_interrupted()
dg.print("ERROR: " + str(e))
dg.print("Bad values: {},{},{}".format(recvd_list[0],
recvd_list[1],
recvd_list[2]))
return None
def test_print_established(self):
self.assertEqual(Diagnostics.state, DiagState.CLOSED)
Diagnostics.socket = Mock()
Diagnostics.state = DiagState.ESTABLISHED
Diagnostics.buf = deque(maxlen=20)
self.assertEqual(len(Diagnostics.buf), 0)
Diagnostics.print("hi")
self.assertEqual(len(Diagnostics.buf), 0)
Diagnostics.socket.reply.assert_called_with("hi")
Diagnostics.print("hi again")
self.assertEqual(len(Diagnostics.buf), 0)
Diagnostics.socket.reply.assert_called_with("hi again")
self.assertEqual(Diagnostics.state, DiagState.ESTABLISHED)
Diagnostics.state = DiagState.CLOSED
def release_motors(self, motor_set):
'''Remove unique access to a set of motors so that they may be used
elsewhere.'''
with self.list_lock:
if motor_set in self.motor_list:
self.motor_list.pop(motor_set)
mgr.global_resources.release(motor_set)
with self.condition:
self.release_was_called = True
self.condition.notify()
else:
dg.print(
"Warning (actuator): release_motors called on resource not acquired."
)
def _inverse_kinematics(self, x, y, gamma):
'''
x and y are measured relative to the base servo. They are the coordinates of the tip of the gripper.
positive x is in the forward driving direction, positive y is away from the ground.
gamma is the orientation of the gripper. gamma=0 means the gripper segment is parallel to the ground.
Note that self.gamma is the current orientation, while gamma is the orientation to set.
gamma=-pi/2 means the gripper is pointing at the ground.
angles theta1,theta2,theta3 are the servo angles at base,middle,top along the arm.
they are assumed to be -pi/2 < theta < pi/2.
angles are converted to and returned in degrees.
'''
arm_l1 = self.arm_l1
arm_l2 = self.arm_l2
arm_l3 = self.arm_l3
x2R = x - arm_l3 * math.cos(
gamma) # x for the tip of the second segment
y2R = y - arm_l3 * math.sin(
gamma) # y for the tip of the second segment
ct2 = (((x2R**2) + (y2R**2)) -
((arm_l1**2) +
(arm_l2**2))) / (2 * arm_l1 * arm_l2) # cos(theta2)
#print("GAMMA: {}".format(math.degrees(gamma)))
#print("COSTHETA2: {}".format(ct2))
try:
if gamma > 0: # gripper looking up
# print(ct2)
thet2 = math.acos(
ct2) # elbow down configuration for 2R manipulator problem
else:
# print(ct2)
thet2 = -math.acos(ct2) # elbow up configuration
st2 = math.sin(thet2)
except Exception as e:
dg.print("Sample unreachable")
return None
A = arm_l1 + arm_l2 * ct2
B = arm_l2 * st2
AB2 = A**2 + B**2
ct1 = (x2R * A + y2R * B) / AB2
st1 = (y2R * A - x2R * B) / AB2
if ct1 > 0: # principle angles
thet1 = math.asin(st1)
else:
thet1 = math.pi - math.asin(st1)
thet3 = gamma - thet2 - thet1
return (math.degrees(thet1), math.degrees(thet2), math.degrees(thet3))
def __init__(self):
Resource.__init__(self)
self.policy = Policy.UNIQUE
self.register_name("Arm")
dg.print("Mock arm motors settings:")
servo1_pin = cfg.motor_config.get_pin("Arm", "Servo1")
servo2_pin = cfg.motor_config.get_pin("Arm", "Servo2")
servo3_pin = cfg.motor_config.get_pin("Arm", "Servo3")
gripper_pin = cfg.motor_config.get_pin("Arm", "Gripper")
dg.print(" Servo 1 pin: {}".format(servo1_pin))
dg.print(" Servo 2 pin: {}".format(servo2_pin))
dg.print(" Servo 3 pin: {}".format(servo3_pin))
dg.print(" Gripper pin: {}".format(gripper_pin))
def stop(self, args):
if self.connection_active():
try:
self.disconnect()
except ReceiverError as e:
raise OpModeError(str(e))
redirect.stop()
try:
if self.clisock.is_open(
): # only way to check if unit is reachable
unit.send_command(self.attached_unit, "STOP DepCamera")
self.clisock.close()
except unit.UnitError as e:
dg.print(
"Warning: sending command 'STOP DepCamera' to unit {} failed.".
format(self.attached_unit))
def send_command(unitname, command):
dg.print("Sending command '{}' to unit {}".format(command, unitname))
try:
comm = MainService.unit_list[unitname][2]
except KeyError:
raise UnitError("Unit '{}' not found.".format(unitname))
try:
res = comm.root.accept(command)
dg.print("{} returned '{}' in response to {}".format(
unitname, res, command))
except Exception as e: # probably some communication error
raise UnitError(
"Could not send command to unit {}: ".format(unitname) + str(e))
if res == 'ERR':
raise UnitError("Error during {}'s processing of command '{}'".format(
unitname, command))
def acquire_motors(self, motor_type):
'''Get unique access to a set of motors such as wheel motors or
robotic arm motors.'''
try:
with self.list_lock:
motor_set = mgr.global_resources.get_unique(motor_type)
if motor_set is not None:
self.motor_list[motor_type] = motor_set
else:
dg.print(
"{}: warning (acquire_motors): could not get unique access to {}"
.format(self.__class__.__name__, motor_type))
except mgr.ResourceError as e:
dg.print(str(e))
raise ActuatorError('{}: Could not get access to motors.'.format(
self.__class__.__name__))
def set_values(self, values):
if (values[0] > self.servo1_lim_upper
or values[0] < self.servo1_lim_lower):
return
if (values[1] > self.servo2_lim_upper
or values[1] < self.servo2_lim_lower):
return
if (values[2] > self.servo3_lim_upper
or values[2] < self.servo3_lim_lower):
return
offset = 135
top_angle = values[0] + offset
middle_angle = values[1] + offset
bottom_angle = values[2] + offset
dg.print("Mock deployable camera motors: values received: {}, {}, {}".
format(values[0], values[1], values[2]))
time.sleep(0.02)
def __init__(self):
Resource.__init__(self)
self.policy = Policy.UNIQUE
self.register_name("Wheels")
self._ymax = 100
self._xmax = 100
self.duty_cycle = 4095
dg.print("Mock wheel motors settings:")
left_pwm_pin = cfg.motor_config.get_pwm_pin("Wheels", "Left")
left_digital_pin = cfg.motor_config.get_digital_pin("Wheels", "Left")
right_pwm_pin = cfg.motor_config.get_pwm_pin("Wheels", "Right")
right_digital_pin = cfg.motor_config.get_digital_pin("Wheels", "Right")
dg.print(" Left PWM pin: {}".format(left_pwm_pin))
dg.print(" Left digital pin: {}".format(left_digital_pin))
dg.print(" Right PWM pin: {}".format(right_pwm_pin))
dg.print(" Right digital pin: {}".format(right_digital_pin))