def confirm_identity(self, max_retries=10):
"""
Confirms the device we're connected to is the device we expected.
"""
assert self.running and self.connected and self._serial
for _ in xrange(max_retries):
self.log('Confirming device (attempt %i of %i)...' %
(_ + 1, max_retries))
self._write_packet(Packet(c.ID_IDENTIFY))
ret = self._read_raw_packet()
print('confirm identity read:', ret)
if ret == c.ID_IDENTIFY + ' ' + self.name.upper():
return True
elif ret and ret[0] == c.ID_LOG:
self.log('init log:', ret)
elif not ret:
# No response, may have timed out, wait a little.
time.sleep(0.1)
else:
# Invalid or garbled response. Try again, incase device is correct
# by response was garbled.
self.log('Invalid identity:', ret)
return False
def _read_raw_packet(self):
"""
Reads a raw line of data from the Arduino.
"""
with self._serial_read_lock:
data = (self._serial.readline() or '').strip()
# self._serial.reset_input_buffer()
if data:
self.print('read raw data:', data)
if data[0] == c.ID_HASH:
# Check for new hash.
try:
self.expected_hash = int(data[1:].strip())
except (TypeError, ValueError) as e:
traceback.print_exc(file=sys.stderr)
elif self.expected_hash:
# Validate packet.
packet = Packet.from_string(data)
if packet.hash != self.expected_hash:
# if self.verbose:
self.print('Invalid packet read due to hash mismatch:',
packet)
data = ''
if data:
# Record a successful read.
self.last_read = time.time()
return data
def force_sensors(self):
"""
Triggers all sensors to report their current value, even if it hasn't changed
since last report.
"""
packet = Packet(id=c.ID_FORCE_SENSORS)
self.outgoing_queue.put(packet)
def packet_read_callback(self, msg):
packet = Packet.from_ros_message(msg)
# print 'packet:', packet.id_name
if packet.id == c.ID_GET_VALUE and packet.data[0] == c.ID_BATTERY_CHARGE_RATIO:
# Calculate x.
dt = datetime.now()
if self.first_dt is None:
self.first_dt = dt
# Calculate y.
charge_ratio = float(packet.parameters[1])
print 'charge_ratio:', charge_ratio
self.data_points_x.append((dt - self.first_dt).total_seconds())
self.data_points_y.append(charge_ratio)
if len(self.data_points_x) > self.max_data_points:
self.data_points_x = self.data_points_x[1:]
self.data_points_y = self.data_points_y[1:]
if len(self.data_points_x) > 60:
# Calculate linear regression estimate.
m, b, r_value, p_value, std_err = \
linregress(self.data_points_x, self.data_points_y)
print 'linregress:', m, b, r_value, p_value, std_err
# y = mx+b => (y-b)/m = x => -b/m = x
x_at_zero = -b/m
# Ignore if it's junk because there's not enough data.
remaining_seconds = x_at_zero - self.data_points_x[-1]
print 'remaining_seconds:', remaining_seconds
if remaining_seconds < 0:
return
# Perform a moving average over that, to smooth out swings.
_ma = self.ma
if self.ma is None:
self.ma = remaining_seconds
else:
self.ma = self.ma * self.ma_ratio + remaining_seconds * (1 - self.ma_ratio)
_td = -timedelta(seconds=self.ma)
print 'optimist: charge will end in %s' % humanize.naturaltime(_td)
# Clamp down to find the most likely worst-case scenario.
if _ma is not None and len(self.data_points_x) > 100:
self.ma = min(_ma, self.ma)
_td = -timedelta(seconds=self.ma)
print 'pessimist: charge will end in %s' % humanize.naturaltime(_td)
msg = msgs.RemainingTime()
msg.header.stamp = rospy.Time.now()
msg.remaining_seconds = rospy.Duration(secs=self.ma)
msg.error = std_err
self.remaining_time_pub.publish(msg)
def _write_data_to_arduino(self, retries=5):
"""
Continually writes data from outgoing queue to the Arduino.
"""
while self.running:
# Check heartbeat.
if self.last_ping + 1 <= time.time():
self.last_ping = time.time()
self.last_ping_dt = datetime.now()
# self.print('Queuing ping.')
self.outgoing_queue.put(Packet(c.ID_PING))
# Sending pending commands.
if not self.outgoing_queue.empty():
packet = self.outgoing_queue.get()
ack_success = False
for attempt in xrange(retries):
self.print(
'Sending: %s, attempt %i, (%i packets remaining)' %
(packet, attempt, self.outgoing_queue.qsize()))
sent_time = time.time()
self._write_packet(packet)
t0 = time.time() - sent_time
# self.print('Sent secs:', t0, ' self.write_time:', self.write_time)
if not self.running:
ack_success = True
break
elif packet.id in c.ACK_IDS:
# Wait for acknowledgement.
if self._wait_for_ack(packet.id, sent_time):
ack_success = True
break
else:
self.print(
'Timed out waiting for ack of packet %s, on attempt %i.'
% (packet, attempt))
self.ack_failure_count += 1
else:
# Don't wait for acknowledgement.
break
if packet.id in c.ACK_IDS:
with self.ack_queue_lock:
self.ack_queue[packet] = ack_success
self.print('Write thread exited.')
def shutdown(self):
"""
Called when ROS issues a shutdown event.
"""
self.log("Disconnecting the %s arduino..." % self.name)
# Automatically signal device to stop.
self.outgoing_queue.put(Packet(c.ID_ALL_STOP))
t0 = time.time()
while not self.outgoing_queue.empty():
time.sleep(1)
self.log('Waiting for write queue to clear...')
if time.time() - t0 > 5:
break
self.disconnect()
def _ros_to_arduino_handler(self, req, packet_id):
"""
Handles ROS service requests and forwards them to the Arduino.
"""
t0 = time.time()
self.log('Received service request: %s %s' % (req, packet_id))
# Convert ROS message to Arduino packet.
t1 = time.time()
input_formats = self.service_formats
parameters = input_formats[packet_id]
data = []
for arg_name, arg_type in parameters:
value = getattr(req, arg_name)
data.append(str(value))
packet = Packet(id=packet_id, data=' '.join(data))
t2 = time.time()
print('t_packet:', t2 - t1)
# Queue for sending.
self.outgoing_queue.put(packet)
t3 = time.time()
# If this packet requires acknowledgement, then block until received or timed out.
ack_success = True
if packet.id in c.ACK_IDS:
self.print('waiting for ack...')
while 1:
with self.ack_queue_lock:
if packet in self.ack_queue:
ack_success = self.ack_queue.pop(packet)
self.print('ack received:', ack_success)
break
time.sleep(0.01)
t4 = time.time()
print('t_ack:', time.time() - t3)
print('t_total:', time.time() - t0)
# Return service response.
if not ack_success:
raise Exception('ack failed')
name = type(req).__name__.replace('Request', '')
resp_cls = getattr(srvs, name + 'Response')
return resp_cls()
def _read_packet(self):
"""
Reads a packet from the Arduino.
"""
data = self._read_raw_packet()
if data:
packet = Packet.from_string(data)
# Keep a receipt of when we receive acknowledgments so the write thread
# knows when to stop waiting.
if packet.data == c.OK:
with self._acks_lock:
self._acks[packet.id] = time.time()
# Record pong so we know Arduino is still alive.
# if packet.id == c.ID_PONG:
# self.last_pong = time.time()
self.read_count += 1
if packet.non_get_id in c.ALL_IDS:
self.packet_counts[packet.non_get_id] += 2
return packet
def all_stop(self):
"""
Signals the Arduino to stop all motors.
"""
self.all_stopping = True
self.outgoing_queue.put(Packet(c.ID_ALL_STOP))
