def _handle_can_message(self, cob_id, data, stamp):
can_node_id = (cob_id & 0xff)
if can_node_id != self.can_node_id:
return
command = (cob_id >> 8) & 0xff
parser = g_vesc_msg_parsers.get(command, None)
if parser is None:
logger.warning(
'No parser for command :%x node id: %x',
command,
can_node_id,
)
return
logger.debug('can node id %02x', can_node_id)
parser(self._latest_state, data)
self._latest_stamp.CopyFrom(stamp)
if command == VESC_STATUS_MSG_5:
if self._last_tachometer_stamp is not None:
self._delta_time_seconds = (
self._latest_stamp.ToMicroseconds() -
self._last_tachometer_stamp.ToMicroseconds()) * 1e-6
self._average_delta_time = self._average_delta_time * (
0.9) + self._delta_time_seconds * 0.1
else:
self._last_tachometer_stamp = Timestamp()
self._last_tachometer_stamp.CopyFrom(self._latest_stamp)
# only log on the 5th vesc message, as we have complete state at that point.
event = make_event('%s/state' % self.name,
self._latest_state,
stamp=self._latest_stamp)
self._event_bus.send(event)
async def run():
# TODO(isherman): WaitForServices instead
await asyncio.sleep(3)
start_logging()
for i in range(N_MESSAGES):
event_bus.send(make_event(f'{MESSAGE_NAME_PREFIX}/{i}', Timestamp()))
await asyncio.sleep(1)
def start_logging():
event_bus.send(
make_event(
'logger/command',
LoggingCommand(record_start=LoggingCommand.RecordStart(
archive_path=os.path.join(
BLOBSTORE.bucket_relative_path(BUCKET_LOGS),
ARCHIVE_NAME), ), ),
), )
def io_stream_to_event_bus(self, stream, out_buffer, out_name):
while True:
line = yield from stream.readline()
if not line:
return
out_buffer.write(line)
out_buffer.flush()
event = make_event(
f'programd/{out_name}',
ProgramOutput(line=str(line, sys.stdout.encoding)),
)
self._event_bus.send(event)
def send(self, n_periods):
command = SteeringCommand()
if n_periods > self.rate_hz:
self.stop()
command.CopyFrom(self.joystick_manual_steer.command)
if self.cruise_control_steer.cruise_control_axis_active():
self._start_cruise_control()
if self.cruise_control_active:
command.CopyFrom(self.cruise_control_steer.update())
elif self.servo_active:
command.CopyFrom(self.servo_control_steer.update())
if self.new_goal:
command.reset_goal = True
logger.info('reset goal')
self.new_goal = False
else:
command.CopyFrom(self.joystick_manual_steer.update())
self._event_bus.send(make_event(_g_message_name, command))
async def send_status(self):
while not self.shutdown:
event = make_event('programd/status', self.status)
self._event_bus.send(event)
await asyncio.sleep(1)
def _command_loop(self, n_periods):
# n_periods is the number of periods since the last call. Should normally be 1.
now = Timestamp()
now.GetCurrentTime()
if (self.n_cycle % (5 * self.command_rate_hz)) == 0:
logger.info(
'\nright motor:\n %s\nleft motor:\n %s\n state:\n %s',
MessageToString(self.right_motor.get_state(),
as_one_line=True),
MessageToString(self.left_motor.get_state(), as_one_line=True),
MessageToString(self.tractor_state, as_one_line=True),
)
self.tractor_state.stamp.CopyFrom(now)
self.tractor_state.wheel_velocity_rads_left = self.left_motor.velocity_rads(
)
self.tractor_state.wheel_velocity_rads_right = self.right_motor.velocity_rads(
)
self.tractor_state.average_update_rate_left_motor = self.left_motor.average_update_rate(
)
self.tractor_state.average_update_rate_right_motor = self.right_motor.average_update_rate(
)
if self.config.topology == TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER:
self.tractor_state.wheel_veolcity_rads_left_aft = self.left_motor_aft.velocity_rads(
)
self.tractor_state.wheel_veolcity_rads_right_aft = self.right_motor_aft.velocity_rads(
)
self.tractor_state.average_update_rate_left_aft_motor = self.left_motor_aft.average_update_rate(
)
self.tractor_state.average_update_rate_right_aft_motor = self.right_motor_aft.average_update_rate(
)
if self._last_odom_stamp is not None:
dt = (now.ToMicroseconds() -
self._last_odom_stamp.ToMicroseconds()) * 1e-6
min_dt = 0.0
max_dt = 1.0 # 1 second
if dt < min_dt or dt > max_dt:
# this condition can occur if n_periods skipped is high
# or negative if for some reason the clock is non-monotonic - TODO(ethanrublee) should we use a monotonic clock?
logger.warn(
'odometry time delta out of bounds, clipping. n_period=%d dt=%f min_dt=%f max_dt=%f',
n_periods, dt, min_dt, max_dt)
self.tractor_state.dt = np.clip(dt, min_dt, max_dt)
tractor_pose_delta = self.kinematics.compute_tractor_pose_delta(
self.tractor_state.wheel_velocity_rads_left,
self.tractor_state.wheel_velocity_rads_right,
self.tractor_state.dt,
)
self.odom_pose_tractor = self.odom_pose_tractor.dot(
tractor_pose_delta)
self.odom_poses_tractor.push(self.odom_pose_tractor, now)
self.tractor_state.abs_distance_traveled += np.linalg.norm(
tractor_pose_delta.trans)
self.tractor_state.odometry_pose_base.a_pose_b.CopyFrom(
se3_to_proto(self.odom_pose_tractor))
self.tractor_state.odometry_pose_base.frame_a = 'odometry/wheel'
self.tractor_state.odometry_pose_base.frame_b = 'tractor/base'
self.event_bus.send(
make_event(
'pose/tractor/base',
self.tractor_state.odometry_pose_base,
stamp=now,
), )
self._last_odom_stamp = now
self.n_cycle += 1
brake_current = 10.0
steering_command = self.steering.get_steering_command()
if steering_command.brake > 0.0:
self.tractor_state.commanded_brake_current = brake_current
self.tractor_state.commanded_wheel_velocity_rads_left = 0.0
self.tractor_state.commanded_wheel_velocity_rads_right = 0.0
self.tractor_state.target_unicycle_velocity = 0.0
self.tractor_state.target_unicycle_angular_velocity = 0.0
self.right_motor.send_current_brake_command(brake_current)
self.left_motor.send_current_brake_command(brake_current)
if self.config.topology == TractorConfig.TOPOLOGY_FOUR_MOTOR_SKID_STEER:
self.right_motor_aft.send_current_brake_command(brake_current)
self.left_motor_aft.send_current_brake_command(brake_current)
self.move_to_goal_controller.reset()
elif steering_command.mode in (SteeringCommand.MODE_SERVO, ):
self._servo(steering_command)
elif steering_command.mode in (
SteeringCommand.MODE_JOYSTICK_MANUAL,
SteeringCommand.MODE_JOYSTICK_CRUISE_CONTROL):
self._command_velocity(steering_command.velocity,
steering_command.angular_velocity)
self.event_bus.send(make_event('tractor_state', self.tractor_state))