C++ Stopwatch::getElapsedTime方法代码示例

string OriCommand::cmd_merge(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
    Stopwatch sw = Stopwatch();
    sw.start();
#endif /* DEBUG */

    FUSE_PLOG("Command: merge");

    string error;
    ObjectHash hash;
    strwstream resp;

    // Parse Command
    str.readHash(hash);

    RWKey::sp lock = priv->nsLock.writeLock();
    error = priv->merge(hash);
    lock.reset();

    if (error != "") {
        resp.writeUInt8(0);
        resp.writePStr(error);
    } else {
        resp.writeUInt8(1);
    }

#if defined(DEBUG) || defined(ORI_PERF)
    sw.stop();
    FUSE_PLOG("merge with: %s", hash.hex().c_str());
    FUSE_PLOG("merge elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */

    return resp.str();
}

string OriCommand::cmd_snapshot(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
    Stopwatch sw = Stopwatch();
    sw.start();
#endif /* DEBUG */

    FUSE_PLOG("Command: snapshot");

    uint8_t hasMsg, hasName;
    string msg, name;
    Commit c;
    strwstream resp;

    // Parse Command
    hasMsg = str.readUInt8();
    hasName = str.readUInt8();
    if (hasMsg) {
        str.readLPStr(msg);
        c.setMessage(msg);
    }
    if (hasName) {
        str.readLPStr(name);
        c.setSnapshot(name);
    }

    RWKey::sp lock = priv->nsLock.writeLock();
    ObjectHash hash = priv->commit(c);
    lock.reset();
    if (hash.isEmpty()) {
        resp.writeUInt8(0);
    } else {
        resp.writeUInt8(1);
        resp.writeHash(hash);
    }

#if defined(DEBUG) || defined(ORI_PERF)
    sw.stop();
    if (hash.isEmpty())
        FUSE_PLOG("snapshot not taken");
    FUSE_PLOG("snapshot result: %s", hash.hex().c_str());
    FUSE_PLOG("snapshot elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */

    return resp.str();
}

string OriCommand::cmd_status(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
    Stopwatch sw = Stopwatch();
    sw.start();
#endif /* DEBUG */

    FUSE_PLOG("Command: status");

    map<string, OriFileState::StateType> diff;
    map<string, OriFileState::StateType>::iterator it;
    strwstream resp;

    RWKey::sp lock = priv->nsLock.writeLock();
    diff = priv->getDiff();
    lock.reset();

    resp.writeUInt32(diff.size());
    for (it = diff.begin(); it != diff.end(); it++) {
        char type = '!';

        if (it->second == OriFileState::Created)
            type = 'A';
        else if (it->second == OriFileState::Modified)
            type = 'M';
        else if (it->second == OriFileState::Deleted)
            type = 'D';
        else
            ASSERT(false);

        resp.writeUInt8(type);
        resp.writeLPStr(it->first);
    }

#if defined(DEBUG) || defined(ORI_PERF)
    sw.stop();
    FUSE_PLOG("status elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */

    return resp.str();
}

void DouglasPeuckerApproximation::approximate(const std::vector <FrameWithId>& in,
        std::vector <FrameWithId>& out, double epsilon) {

#ifdef DEBUG
    Stopwatch stopwatch;
    stopwatch.start();
#endif
    std::vector <FrameWithId> inputCopy;
    inputCopy = in;
    out.clear();
    int counter = 0;
    douglasPeucker(inputCopy, out, counter, epsilon);
#ifdef DEBUG_0
    stopwatch.stop();
    LOG("Douglas-Peucker approximation algorithm:");
    LOG("  - input size: %u", inputCopy.size());
    LOG("  - epsilon: %f", epsilon);
    LOG("  - output size: %u", out.size());
    LOG("  - iterations : %d", counter);
    LOG("  - duration : %f ms", stopwatch.getElapsedTime());
#endif
}

void ChaikinCurveApproximation::approximate(const std::vector <FrameWithId>& in, std::vector <FrameWithId>& out, unsigned int lod) {
#ifdef DEBUG
    Stopwatch stopwatch;
    stopwatch.start();
#endif
    std::vector <FrameWithId> inputCopy;
    inputCopy = in;
    unsigned int counter = 0;
    for (unsigned int i = 0; i < lod; ++i) {
        out.clear();
        counter += chaikinCurve(inputCopy, out);
        inputCopy = out;
    }
#ifdef DEBUG  
    stopwatch.stop();
    LOG("Chaikin curve approximation algorithm:");
    LOG("  - input size: %lu", in.size());
    LOG("  - level of detail: %d", lod);
    LOG("  - output size: %lu", out.size());
    LOG("  - iterations : %d", counter);
    LOG("  - duration : %f ms", stopwatch.getElapsedTime());
#endif    
}

string OriCommand::cmd_pull(strstream &str)
{
#if defined(DEBUG) || defined(ORI_PERF)
    Stopwatch sw = Stopwatch();
    sw.start();
#endif /* DEBUG */

    FUSE_PLOG("Command: pull");

    bool success;
    string srcPath;
    string error;
    ObjectHash hash;
    RemoteRepo srcRepo = RemoteRepo();
    strwstream resp;

    // Parse Command
    str.readPStr(srcPath);

    if (srcPath == "") {
        map<string, Peer> peers = priv->getRepo()->getPeers();
        map<string, Peer>::iterator it = peers.find("origin");

        if (it == peers.end()) {
            error = "No default repository to pull from.";
            goto error;
        }
        srcPath = (*it).second.getUrl();
    }

    try {
        success = srcRepo.connect(srcPath);
    } catch (exception &e) {
        error = e.what();
        goto error;
    }
    if (success) {
        hash = srcRepo->getHead();

        // XXX: Change to a repo lock
        RWKey::sp lock = priv->nsLock.writeLock();
        priv->getRepo()->pull(srcRepo.get());
        // XXX: Refcounts need to be done incrementally or rebuilt after
        lock.reset();
    } else {
        error = "Connection failed!";
    }

error:
    if (hash.isEmpty()) {
        resp.writeUInt8(0);
        resp.writePStr(error);
    } else {
        resp.writeUInt8(1);
        resp.writeHash(hash);
    }

#if defined(DEBUG) || defined(ORI_PERF)
    sw.stop();
    FUSE_PLOG("pull up to: %s", hash.hex().c_str());
    FUSE_PLOG("pull elapsed %lluus", sw.getElapsedTime());
#endif /* DEBUG */

    return resp.str();
}

bool CollisionCheckingRos::collisionCheck(const std::vector <FrameWithId>& path,
        const FrameWithId& actualPose, double interpolationStep, unsigned int numberOfSteps) {

    bool collision = false;

    if (path.empty())
        return collision;

    costmap_2d::Costmap2D costmapCopy;
    std::vector <geometry_msgs::Point> orientedFootprint;

    costmap->getOrientedFootprint(orientedFootprint);
    costmap->clearRobotFootprint();
    costmap->getCostmapCopy(costmapCopy);
    base_local_planner::CostmapModel collisionChecker(costmapCopy);

    std::vector <FrameWithId> prunedPath;
    utilities::prunePath(path, actualPose, prunedPath);



    LinearInterpolation interpolator;
    std::vector <FrameWithId> interpolatedPath;
    size_t counter = interpolator.interpolate(prunedPath, interpolatedPath, interpolationStep, numberOfSteps);

#ifdef DEBUG
    Stopwatch stopwatch;
    stopwatch.start();
#endif


    size_t i;
    for (i = 0; i < counter; ++i) {
        const KDL::Frame& frame = interpolatedPath[i].getFrame();
        double r, p, y;
        frame.M.GetRPY(r, p, y);

        geometry_msgs::PoseStamped pose;
        conversions::frameToPoseStampedRos(frame, pose);

        std::vector <geometry_msgs::Point> orientedFootprint;
        costmap->getOrientedFootprint(frame.p.x(), frame.p.y(), 0, orientedFootprint);
        double circumscribedRadius = costmap->getCircumscribedRadius();
        double inscribedRadius = costmap->getInscribedRadius();

        double c = collisionChecker.footprintCost(pose.pose.position,
                orientedFootprint,
                inscribedRadius,
                circumscribedRadius);

        if (c < 0) {
            ROS_INFO("Collision at pose: %f, %f", pose.pose.position.x, pose.pose.position.y);
            collision = true;
            break;
        }

    }

#ifdef DEBUG_0  
    stopwatch.stop();
    LOG("Collision checking using costmap2d_ros:");
    LOG("  - collision: %d", collision);
    LOG("  - step size: %f", interpolationStep);
    LOG("  - iterations : %u", i);
    LOG("  - duration : %f ms", stopwatch.getElapsedTime());
#endif

    return collision;
}