コード例 #1
0
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;
}
コード例 #2
0
void SystemState::drawShapeSet(const Geometry2d::ShapeSet& shapes,
                               const QColor& color, const QString& layer) {
    for (auto& shape : shapes.shapes()) {
        drawShape(shape, color, layer);
    }
}
コード例 #3
0
/**
 * 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();
}