bool TrapezoidalPath::hit(const Geometry2d::ShapeSet& obstacles, float& hitTime,
float initialTime) const {
std::set<std::shared_ptr<Shape>> startHitSet = obstacles.hitSet(_startPos);
for (float t = initialTime; t < _duration; t += 0.1) {
auto instant = evaluate(t);
if (instant) {
for (auto& shape : obstacles.shapes()) {
// If the shape is in the original hitSet, it is ignored
if (startHitSet.find(shape) != startHitSet.end()) {
continue;
}
if (shape->hit(instant->motion.pos)) {
hitTime = t;
return true;
}
}
}
}
return false;
}
void SystemState::drawShapeSet(const Geometry2d::ShapeSet& shapes,
const QColor& color, const QString& layer) {
for (auto& shape : shapes.shapes()) {
drawShape(shape, color, layer);
}
}
/**
* program loop
*/
void Processor::run() {
vision.start();
// Create radio socket
_radio =
_simulation ? (Radio*)new SimRadio(_blueTeam) : (Radio*)new USBRadio();
Status curStatus;
bool first = true;
// main loop
while (_running) {
RJ::Time startTime = RJ::timestamp();
int delta_us = startTime - curStatus.lastLoopTime;
_framerate = 1000000.0 / delta_us;
curStatus.lastLoopTime = startTime;
_state.timestamp = startTime;
if (!firstLogTime) {
firstLogTime = startTime;
}
////////////////
// Reset
// Make a new log frame
_state.logFrame = std::make_shared<Packet::LogFrame>();
_state.logFrame->set_timestamp(RJ::timestamp());
_state.logFrame->set_command_time(startTime + Command_Latency);
_state.logFrame->set_use_our_half(_useOurHalf);
_state.logFrame->set_use_opponent_half(_useOpponentHalf);
_state.logFrame->set_manual_id(_manualID);
_state.logFrame->set_blue_team(_blueTeam);
_state.logFrame->set_defend_plus_x(_defendPlusX);
if (first) {
first = false;
Packet::LogConfig* logConfig =
_state.logFrame->mutable_log_config();
logConfig->set_generator("soccer");
logConfig->set_git_version_hash(git_version_hash);
logConfig->set_git_version_dirty(git_version_dirty);
logConfig->set_simulation(_simulation);
}
for (OurRobot* robot : _state.self) {
// overall robot config
switch (robot->hardwareVersion()) {
case Packet::RJ2008:
robot->config = robotConfig2008;
break;
case Packet::RJ2011:
robot->config = robotConfig2011;
break;
case Packet::RJ2015:
robot->config = robotConfig2015;
case Packet::Unknown:
robot->config =
robotConfig2011; // FIXME: defaults to 2011 robots
break;
}
// per-robot configs
robot->status = robotStatuses.at(robot->shell());
}
////////////////
// Inputs
// Read vision packets
vector<const SSL_DetectionFrame*> detectionFrames;
vector<VisionPacket*> visionPackets;
vision.getPackets(visionPackets);
for (VisionPacket* packet : visionPackets) {
SSL_WrapperPacket* log = _state.logFrame->add_raw_vision();
log->CopyFrom(packet->wrapper);
curStatus.lastVisionTime = packet->receivedTime;
if (packet->wrapper.has_detection()) {
SSL_DetectionFrame* det = packet->wrapper.mutable_detection();
// FIXME - Account for network latency
double rt = packet->receivedTime / 1000000.0;
det->set_t_capture(rt - det->t_sent() + det->t_capture());
det->set_t_sent(rt);
// Remove balls on the excluded half of the field
google::protobuf::RepeatedPtrField<SSL_DetectionBall>* balls =
det->mutable_balls();
for (int i = 0; i < balls->size(); ++i) {
float x = balls->Get(i).x();
// FIXME - OMG too many terms
if ((!_state.logFrame->use_opponent_half() &&
((_defendPlusX && x < 0) ||
(!_defendPlusX && x > 0))) ||
(!_state.logFrame->use_our_half() &&
((_defendPlusX && x > 0) ||
(!_defendPlusX && x < 0)))) {
balls->SwapElements(i, balls->size() - 1);
balls->RemoveLast();
--i;
}
}
// Remove robots on the excluded half of the field
google::protobuf::RepeatedPtrField<SSL_DetectionRobot>*
robots[2] = {det->mutable_robots_yellow(),
det->mutable_robots_blue()};
for (int team = 0; team < 2; ++team) {
for (int i = 0; i < robots[team]->size(); ++i) {
float x = robots[team]->Get(i).x();
if ((!_state.logFrame->use_opponent_half() &&
((_defendPlusX && x < 0) ||
(!_defendPlusX && x > 0))) ||
(!_state.logFrame->use_our_half() &&
((_defendPlusX && x > 0) ||
(!_defendPlusX && x < 0)))) {
robots[team]->SwapElements(
i, robots[team]->size() - 1);
robots[team]->RemoveLast();
--i;
}
}
}
detectionFrames.push_back(det);
}
}
// Read radio reverse packets
_radio->receive();
for (const Packet::RadioRx& rx : _radio->reversePackets()) {
_state.logFrame->add_radio_rx()->CopyFrom(rx);
curStatus.lastRadioRxTime = rx.timestamp();
// Store this packet in the appropriate robot
unsigned int board = rx.robot_id();
if (board < Num_Shells) {
// We have to copy because the RX packet will survive past this
// frame but LogFrame will not (the RadioRx in LogFrame will be
// reused).
_state.self[board]->radioRx().CopyFrom(rx);
_state.self[board]->radioRxUpdated();
}
}
_radio->clear();
_loopMutex.lock();
for (Joystick* joystick : _joysticks) {
joystick->update();
}
runModels(detectionFrames);
for (VisionPacket* packet : visionPackets) {
delete packet;
}
// Update gamestate w/ referee data
_refereeModule->updateGameState(blueTeam());
_refereeModule->spinKickWatcher();
string yellowname, bluename;
if (blueTeam()) {
bluename = _state.gameState.OurInfo.name;
yellowname = _state.gameState.TheirInfo.name;
} else {
yellowname = _state.gameState.OurInfo.name;
bluename = _state.gameState.TheirInfo.name;
}
_state.logFrame->set_team_name_blue(bluename);
_state.logFrame->set_team_name_yellow(yellowname);
// Run high-level soccer logic
_gameplayModule->run();
// recalculates Field obstacles on every run through to account for
// changing inset
if (_gameplayModule->hasFieldEdgeInsetChanged()) {
_gameplayModule->calculateFieldObstacles();
}
/// Collect global obstacles
Geometry2d::ShapeSet globalObstacles =
_gameplayModule->globalObstacles();
Geometry2d::ShapeSet globalObstaclesWithGoalZones = globalObstacles;
Geometry2d::ShapeSet goalZoneObstacles =
_gameplayModule->goalZoneObstacles();
globalObstaclesWithGoalZones.add(goalZoneObstacles);
// Build a plan request for each robot.
std::map<int, Planning::PlanRequest> requests;
for (OurRobot* r : _state.self) {
if (r && r->visible) {
if (_state.gameState.state == GameState::Halt) {
r->setPath(nullptr);
continue;
}
// Visualize local obstacles
for (auto& shape : r->localObstacles().shapes()) {
_state.drawShape(shape, Qt::black, "LocalObstacles");
}
auto& globalObstaclesForBot =
(r->shell() == _gameplayModule->goalieID() ||
r->isPenaltyKicker)
? globalObstacles
: globalObstaclesWithGoalZones;
// create and visualize obstacles
Geometry2d::ShapeSet fullObstacles =
r->collectAllObstacles(globalObstaclesForBot);
requests[r->shell()] = Planning::PlanRequest(
Planning::MotionInstant(r->pos, r->vel),
r->motionCommand()->clone(), r->motionConstraints(),
std::move(r->angleFunctionPath.path),
std::make_shared<ShapeSet>(std::move(fullObstacles)));
}
}
// Run path planner and set the path for each robot that was planned for
auto pathsById = _pathPlanner->run(std::move(requests));
for (auto& entry : pathsById) {
OurRobot* r = _state.self[entry.first];
auto& path = entry.second;
path->draw(&_state, Qt::magenta, "Planning");
r->setPath(std::move(path));
r->angleFunctionPath.angleFunction =
angleFunctionForCommandType(r->rotationCommand());
}
// Visualize obstacles
for (auto& shape : globalObstacles.shapes()) {
_state.drawShape(shape, Qt::black, "Global Obstacles");
}
// Run velocity controllers
for (OurRobot* robot : _state.self) {
if (robot->visible) {
if ((_manualID >= 0 && (int)robot->shell() == _manualID) ||
_state.gameState.halt()) {
robot->motionControl()->stopped();
} else {
robot->motionControl()->run();
}
}
}
////////////////
// Store logging information
// Debug layers
const QStringList& layers = _state.debugLayers();
for (const QString& str : layers) {
_state.logFrame->add_debug_layers(str.toStdString());
}
// Add our robots data to the LogFram
for (OurRobot* r : _state.self) {
if (r->visible) {
r->addStatusText();
Packet::LogFrame::Robot* log = _state.logFrame->add_self();
*log->mutable_pos() = r->pos;
*log->mutable_world_vel() = r->vel;
*log->mutable_body_vel() = r->vel.rotated(2 * M_PI - r->angle);
//*log->mutable_cmd_body_vel() = r->
// *log->mutable_cmd_vel() = r->cmd_vel;
// log->set_cmd_w(r->cmd_w);
log->set_shell(r->shell());
log->set_angle(r->angle);
if (r->radioRx().has_kicker_voltage()) {
log->set_kicker_voltage(r->radioRx().kicker_voltage());
}
if (r->radioRx().has_kicker_status()) {
log->set_charged(r->radioRx().kicker_status() & 0x01);
log->set_kicker_works(
!(r->radioRx().kicker_status() & 0x90));
}
if (r->radioRx().has_ball_sense_status()) {
log->set_ball_sense_status(
r->radioRx().ball_sense_status());
}
if (r->radioRx().has_battery()) {
log->set_battery_voltage(r->radioRx().battery());
}
log->mutable_motor_status()->Clear();
log->mutable_motor_status()->MergeFrom(
r->radioRx().motor_status());
if (r->radioRx().has_quaternion()) {
log->mutable_quaternion()->Clear();
log->mutable_quaternion()->MergeFrom(
r->radioRx().quaternion());
} else {
log->clear_quaternion();
}
for (const Packet::DebugText& t : r->robotText) {
log->add_text()->CopyFrom(t);
}
}
}
// Opponent robots
for (OpponentRobot* r : _state.opp) {
if (r->visible) {
Packet::LogFrame::Robot* log = _state.logFrame->add_opp();
*log->mutable_pos() = r->pos;
log->set_shell(r->shell());
log->set_angle(r->angle);
*log->mutable_world_vel() = r->vel;
*log->mutable_body_vel() = r->vel.rotated(2 * M_PI - r->angle);
}
}
// Ball
if (_state.ball.valid) {
Packet::LogFrame::Ball* log = _state.logFrame->mutable_ball();
*log->mutable_pos() = _state.ball.pos;
*log->mutable_vel() = _state.ball.vel;
}
////////////////
// Outputs
// Send motion commands to the robots
sendRadioData();
// Write to the log
_logger.addFrame(_state.logFrame);
_loopMutex.unlock();
// Store processing loop status
_statusMutex.lock();
_status = curStatus;
_statusMutex.unlock();
////////////////
// Timing
RJ::Time endTime = RJ::timestamp();
int lastFrameTime = endTime - startTime;
if (lastFrameTime < _framePeriod) {
// Use system usleep, not QThread::usleep.
//
// QThread::usleep uses pthread_cond_wait which sometimes fails to
// unblock.
// This seems to depend on how many threads are blocked.
::usleep(_framePeriod - lastFrameTime);
} else {
// printf("Processor took too long: %d us\n", lastFrameTime);
}
}
vision.stop();
}