<commit_before>#include "LineKickPlanner.hpp"
#include <Configuration.hpp>
#include <Geometry2d/Util.hpp>
#include <motion/TrapezoidalMotion.hpp>
#include "EscapeObstaclesPathPlanner.hpp"
#include "RRTPlanner.hpp"
#include "motion/TrapezoidalMotion.hpp"
#include "CompositePath.hpp"
#include "MotionInstant.hpp"

using namespace std;
using namespace Geometry2d;

namespace Planning {

bool LineKickPlanner::shouldReplan(const PlanRequest& planRequest) const {
    const MotionConstraints& motionConstraints = planRequest.constraints.mot;
    const Geometry2d::ShapeSet& obstacles = planRequest.obstacles;
    const Path* prevPath = planRequest.prevPath.get();

    const auto& command =
        dynamic_cast<const PivotCommand&>(*planRequest.motionCommand);

    if (!prevPath) {
        return true;
    } else {
        // TODO ashaw37: make this better
        float radius = command.radius;
        auto pivotPoint = command.pivotPoint;
        auto pivotTarget = command.pivotTarget;
        auto endTarget =
            pivotPoint + (pivotPoint - pivotTarget).normalized(radius);
        float targetChange = (prevPath->end().motion.pos - endTarget).mag();

        // if (targetChange > SingleRobotPathPlanner::goalChangeThreshold()) {
        //     return true;
        // }
    }
    return false;
}

std::unique_ptr<Path> LineKickPlanner::run(PlanRequest& planRequest) {
    const float ApproachSpeed = 1.0;

    const float ballAvoidDistance = 0.05;

    auto prevAnglePath =
        dynamic_cast<AngleFunctionPath*>(planRequest.prevPath.get());

    const auto& command =
        dynamic_cast<const LineKickCommand&>(*planRequest.motionCommand);

    const MotionInstant& startInstant = planRequest.start;
    const auto& motionConstraints = planRequest.constraints.mot;
    const auto& rotationConstraints = planRequest.constraints.rot;
    auto& obstacles = planRequest.obstacles;
    auto& systemState = planRequest.systemState;
    const auto& ball = systemState.ball;
    const auto& robotConstraints = planRequest.constraints;
    auto& dynamicObstacles = planRequest.dynamicObstacles;

    auto ballObstacles = obstacles;
    const RJ::Time curTime = RJ::now();
    ballObstacles.add(
        make_shared<Circle>(ball.predict(curTime).pos, ballAvoidDistance));
    unique_ptr<Path> prevPath;
    if (prevAnglePath && prevAnglePath->path) {
        prevPath = std::move(prevAnglePath->path);
    }

    if (!prevPath) {
        finalApproach = false;
    }

    if (finalApproach && prevPath && targetKickPos) {
        RJ::Seconds timeIntoPath = curTime - prevPath->startTime();

        MotionInstant target = prevPath->end().motion;
        RJ::Time time = ball.estimateTimeTo(*targetKickPos);
        auto timeLeft = prevPath->getSlowedDuration() - timeIntoPath;

        if (timeLeft < RJ::Seconds(-0.3) || timeLeft > RJ::Seconds(5.0)) {
            finalApproach = false;
            prevPath = nullptr;
        } else if (timeLeft < RJ::Seconds(0)) {
            prevPath->setDebugText("reuse past done " +
                                   QString::number(timeLeft.count()));
            return make_unique<AngleFunctionPath>(
                std::move(prevPath),
                angleFunctionForCommandType(FacePointCommand(command.target)));
        } else {
            RJ::Seconds timeForBall = time - curTime;
            prevPath->slow(timeLeft/timeForBall, timeIntoPath);
            prevPath->setDebugText("reuse final slow " +
                                   QString::number(timeForBall.count()) + " " +
                                   QString::number(timeLeft.count()));
            return make_unique<AngleFunctionPath>(
                std::move(prevPath),
                angleFunctionForCommandType(FacePointCommand(command.target)));
        }
    }

    if (ball.vel.mag() < 0.2) {
        MotionInstant target(ball.pos);
        target.vel = (command.target - target.pos).normalized(ApproachSpeed);
        auto ballPath = ball.path(curTime);
        unique_ptr<Path> path;
        if (std::abs(target.vel.angleBetween((target.pos - startInstant.pos))) >
            DegreesToRadians(10)) {
            target.pos -=
                target.vel.normalized(ballAvoidDistance*2 + Robot_Radius);
            if (prevPath &&
                    target.pos.distTo(prevPath->end().motion.pos) < SingleRobotPathPlanner::goalPosChangeThreshold() &&
                    reusePathCount < 20) {
                target.vel = prevPath->end().motion.vel;
                reusePathCount++;
            } else {
                reusePathCount = 0;
            }

            auto command = std::make_unique<PathTargetCommand>(target);
            auto request = PlanRequest(systemState,
                startInstant, std::move(command), robotConstraints, std::move(prevPath), ballObstacles,
                dynamicObstacles, planRequest.shellID);
            path = rrtPlanner.run(request);
        } else {

            if (prevPath &&
                target.pos.distTo(prevPath->end().motion.pos) < SingleRobotPathPlanner::goalPosChangeThreshold() &&
                reusePathCount < 20) {
                target.vel = prevPath->end().motion.vel;
                reusePathCount++;
            } else {
                reusePathCount = 0;
            }

            target.pos += target.vel.normalized(Robot_Radius/2);
            auto command = std::make_unique<PathTargetCommand>(target);
            auto request = PlanRequest(systemState,
                                       startInstant, std::move(command), robotConstraints, std::move(prevPath), obstacles,
                                       dynamicObstacles, planRequest.shellID);

            path = rrtPlanner.run(request);
        }
        targetKickPos = boost::none;
        return make_unique<AngleFunctionPath>(
            std::move(path),
            angleFunctionForCommandType(FacePointCommand(command.target)));
    }

    if (prevPath && targetKickPos) {
        auto timeInto = RJ::now() - prevPath->startTime();
        auto timeLeft = prevPath->getDuration() - timeInto;

        MotionInstant target;
        RJ::Time time = ball.estimateTimeTo(*targetKickPos, &target.pos);
        RJ::Seconds timeToHit = time - curTime;
        if (timeLeft < RJ::Seconds(1.0)) {
            target.vel = (command.target - target.pos).normalized(ApproachSpeed);
            target.pos -= target.vel.normalized(Robot_Radius + Ball_Radius * 2);

            if (true) {
                vector<Point> points{startInstant.pos, target.pos};
                finalApproach = true;
                auto path = RRTPlanner::generatePath(points, obstacles,
                                                     motionConstraints, startInstant.vel, target.vel);

                if (path) {
                    path->setDebugText(
                            "FinalPath" + QString::number(path->getSlowedDuration().count()) +
                            " " + QString::number(timeToHit.count()) + " " +
                            QString::number(time.time_since_epoch().count()));
                    //path->slow(timeToHit / path->getDuration());

                    return make_unique<AngleFunctionPath>(
                            std::move(path),
                            angleFunctionForCommandType(FacePointCommand(command.target)));
                }
            }
        }

        if (!prevPath->hit(obstacles, timeInto) && timeLeft-1000ms < timeToHit && reusePathCount < 10) {
            reusePathCount++;
            Point nearPoint;
            prevPath->setDebugText("Reuse prevPath");
            if (ball.estimateTimeTo(prevPath->end().motion.pos, &nearPoint) >= RJ::now() + timeLeft - 1000ms) {
                return make_unique<AngleFunctionPath>(
                        std::move(prevPath),
                        angleFunctionForCommandType(FacePointCommand(command.target)));
            }
        }
    }

    std::unique_ptr<Path> partialPath = nullptr;
    RJ::Seconds partialPathTime = 0ms;
    auto tmpStartInstant = startInstant;
    const auto partialReplanLeadTime = RRTPlanner::getPartialReplanLeadTime();
    if (prevPath) {
        auto timeInto = RJ::now() - prevPath->startTime();
        if (timeInto < prevPath->getDuration() - partialReplanLeadTime*2) {
            partialPath = prevPath->subPath(0ms, timeInto + partialReplanLeadTime);
            partialPathTime = partialReplanLeadTime;
            tmpStartInstant = partialPath->end().motion;
        }
    }

    QString debug = "";
    for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) {
        auto tempObstacles = obstacles;
        MotionInstant ballNow = ball.predict(curTime + t);
        MotionInstant target(ballNow.pos);
        targetKickPos = target.pos;
        target.vel = (command.target - target.pos).normalized(ApproachSpeed);
        target.pos -= target.vel.normalized(Robot_Radius + Ball_Radius * 2);

        vector<Point> points{tmpStartInstant.pos, target.pos};
        if (std::abs(target.vel.angleBetween((target.pos - tmpStartInstant.pos))) >
            DegreesToRadians(60)) {
            auto dist = target.pos.distTo(tmpStartInstant.pos);
            points = {tmpStartInstant.pos,
                      target.pos -
                          target.vel.normalized(Robot_Radius * 2.0 + Ball_Radius * 2.0),
                      target.pos};

            //tempObstacles.add(
            //    make_shared<Circle>(ballNow.pos, Robot_Radius + Ball_Radius));
            //debug = "additional ";
        }
        if (Geometry2d::Segment(ball.pos, target.pos).distTo(tmpStartInstant.pos) <
            Robot_Radius) {
            debug = "meh";
            // break;
        }

        std::unique_ptr<Path> path = RRTPlanner::generatePath(
            points, tempObstacles, motionConstraints, tmpStartInstant.vel, target.vel);
        RJ::Seconds hitTime;

        if (path) {
            if (path->getDuration() + partialPathTime <= t) {
                if (path->hit(tempObstacles, RJ::Seconds::zero(), &hitTime)) {
                    continue;
                }

                if (partialPath) {
                    path = make_unique<CompositePath>(std::move(partialPath), std::move(path));
                    path->setStartTime(prevPath->startTime());
                }

                float multiplier = t / path->getDuration();
                path->setDebugText(
                    "FoundPath" + debug +
                    QString::number(path->getDuration().count()));
                // if (path->getDuration() < 0.7) {
                //    path->slow(multiplier);

                //    path->setDebugText(debug +
                //    QString::number(path->getDuration()) + " slow");
                //}
                reusePathCount=0;
                return make_unique<AngleFunctionPath>(
                    std::move(path), angleFunctionForCommandType(
                                         FacePointCommand(command.target)));
            }
        }
    }

    MotionInstant ballNow = ball.predict(curTime);
    MotionInstant target(ballNow.pos);
    target.vel = (command.target - target.pos).normalized(ApproachSpeed);
    target.pos -= target.vel.normalized(Robot_Radius * 3);

    // obstacles.add(make_shared<Circle>(target.pos, ballAvoidDistance));
    auto ballPath = ball.path(curTime);
    dynamicObstacles.push_back(DynamicObstacle(ballPath.get(), Ball_Radius));
    std::unique_ptr<MotionCommand> rrtCommand =
        std::make_unique<PathTargetCommand>(target);

    auto request = PlanRequest(systemState, startInstant, std::move(rrtCommand),
                               robotConstraints, std::move(prevPath), obstacles,
                               dynamicObstacles, planRequest.shellID);
    auto path = rrtPlanner.run(request);
    path->setDebugText("Gives ups");

    targetKickPos = boost::none;
    return make_unique<AngleFunctionPath>(
        std::move(path),
        angleFunctionForCommandType(FacePointCommand(command.target)));
}
}
<commit_msg>make line kick kick farther<commit_after>#include "LineKickPlanner.hpp"
#include <Configuration.hpp>
#include <Geometry2d/Util.hpp>
#include <motion/TrapezoidalMotion.hpp>
#include "EscapeObstaclesPathPlanner.hpp"
#include "RRTPlanner.hpp"
#include "motion/TrapezoidalMotion.hpp"
#include "CompositePath.hpp"
#include "MotionInstant.hpp"

using namespace std;
using namespace Geometry2d;

namespace Planning {

bool LineKickPlanner::shouldReplan(const PlanRequest& planRequest) const {
    const MotionConstraints& motionConstraints = planRequest.constraints.mot;
    const Geometry2d::ShapeSet& obstacles = planRequest.obstacles;
    const Path* prevPath = planRequest.prevPath.get();

    const auto& command =
        dynamic_cast<const PivotCommand&>(*planRequest.motionCommand);

    if (!prevPath) {
        return true;
    } else {
        // TODO ashaw37: make this better
        float radius = command.radius;
        auto pivotPoint = command.pivotPoint;
        auto pivotTarget = command.pivotTarget;
        auto endTarget =
            pivotPoint + (pivotPoint - pivotTarget).normalized(radius);
        float targetChange = (prevPath->end().motion.pos - endTarget).mag();

        // if (targetChange > SingleRobotPathPlanner::goalChangeThreshold()) {
        //     return true;
        // }
    }
    return false;
}

std::unique_ptr<Path> LineKickPlanner::run(PlanRequest& planRequest) {
    const float ApproachSpeed = 1.0;

    const float ballAvoidDistance = 0.05;

    auto prevAnglePath =
        dynamic_cast<AngleFunctionPath*>(planRequest.prevPath.get());

    const auto& command =
        dynamic_cast<const LineKickCommand&>(*planRequest.motionCommand);

    const MotionInstant& startInstant = planRequest.start;
    const auto& motionConstraints = planRequest.constraints.mot;
    const auto& rotationConstraints = planRequest.constraints.rot;
    auto& obstacles = planRequest.obstacles;
    auto& systemState = planRequest.systemState;
    const auto& ball = systemState.ball;
    const auto& robotConstraints = planRequest.constraints;
    auto& dynamicObstacles = planRequest.dynamicObstacles;

    auto ballObstacles = obstacles;
    const RJ::Time curTime = RJ::now();
    ballObstacles.add(
        make_shared<Circle>(ball.predict(curTime).pos, ballAvoidDistance));
    unique_ptr<Path> prevPath;
    if (prevAnglePath && prevAnglePath->path) {
        prevPath = std::move(prevAnglePath->path);
    }

    if (!prevPath) {
        finalApproach = false;
    }

    if (finalApproach && prevPath && targetKickPos) {
        RJ::Seconds timeIntoPath = curTime - prevPath->startTime();

        MotionInstant target = prevPath->end().motion;
        RJ::Time time = ball.estimateTimeTo(*targetKickPos);
        auto timeLeft = prevPath->getSlowedDuration() - timeIntoPath;

        if (timeLeft < RJ::Seconds(-0.3) || timeLeft > RJ::Seconds(5.0)) {
            finalApproach = false;
            prevPath = nullptr;
        } else if (timeLeft < RJ::Seconds(0)) {
            prevPath->setDebugText("reuse past done " +
                                   QString::number(timeLeft.count()));
            return make_unique<AngleFunctionPath>(
                std::move(prevPath),
                angleFunctionForCommandType(FacePointCommand(command.target)));
        } else {
            RJ::Seconds timeForBall = time - curTime;
            prevPath->slow(timeLeft/timeForBall, timeIntoPath);
            prevPath->setDebugText("reuse final slow " +
                                   QString::number(timeForBall.count()) + " " +
                                   QString::number(timeLeft.count()));
            return make_unique<AngleFunctionPath>(
                std::move(prevPath),
                angleFunctionForCommandType(FacePointCommand(command.target)));
        }
    }

    if (ball.vel.mag() < 0.2) {
        MotionInstant target(ball.pos);
        target.vel = (command.target - target.pos).normalized(ApproachSpeed);
        auto ballPath = ball.path(curTime);
        unique_ptr<Path> path;
        if (std::abs(target.vel.angleBetween((target.pos - startInstant.pos))) >
            DegreesToRadians(10)) {
            target.pos -=
                target.vel.normalized(ballAvoidDistance*2 + Robot_Radius);
            if (prevPath &&
                    target.pos.distTo(prevPath->end().motion.pos) < SingleRobotPathPlanner::goalPosChangeThreshold() &&
                    reusePathCount < 20) {
                target.vel = prevPath->end().motion.vel;
                reusePathCount++;
            } else {
                reusePathCount = 0;
            }

            auto command = std::make_unique<PathTargetCommand>(target);
            auto request = PlanRequest(systemState,
                startInstant, std::move(command), robotConstraints, std::move(prevPath), ballObstacles,
                dynamicObstacles, planRequest.shellID);
            path = rrtPlanner.run(request);
        } else {

            if (prevPath &&
                target.pos.distTo(prevPath->end().motion.pos) < SingleRobotPathPlanner::goalPosChangeThreshold() &&
                reusePathCount < 20) {
                target.vel = prevPath->end().motion.vel;
                reusePathCount++;
            } else {
                reusePathCount = 0;
            }

            target.pos += target.vel.normalized(Robot_Radius);
            auto command = std::make_unique<PathTargetCommand>(target);
            auto request = PlanRequest(systemState,
                                       startInstant, std::move(command), robotConstraints, std::move(prevPath), obstacles,
                                       dynamicObstacles, planRequest.shellID);

            path = rrtPlanner.run(request);
        }
        targetKickPos = boost::none;
        return make_unique<AngleFunctionPath>(
            std::move(path),
            angleFunctionForCommandType(FacePointCommand(command.target)));
    }

    if (prevPath && targetKickPos) {
        auto timeInto = RJ::now() - prevPath->startTime();
        auto timeLeft = prevPath->getDuration() - timeInto;

        MotionInstant target;
        RJ::Time time = ball.estimateTimeTo(*targetKickPos, &target.pos);
        RJ::Seconds timeToHit = time - curTime;
        if (timeLeft < RJ::Seconds(1.0)) {
            target.vel = (command.target - target.pos).normalized(ApproachSpeed);
            target.pos -= target.vel.normalized(Robot_Radius + Ball_Radius * 2);

            if (true) {
                vector<Point> points{startInstant.pos, target.pos};
                finalApproach = true;
                auto path = RRTPlanner::generatePath(points, obstacles,
                                                     motionConstraints, startInstant.vel, target.vel);

                if (path) {
                    path->setDebugText(
                            "FinalPath" + QString::number(path->getSlowedDuration().count()) +
                            " " + QString::number(timeToHit.count()) + " " +
                            QString::number(time.time_since_epoch().count()));
                    //path->slow(timeToHit / path->getDuration());

                    return make_unique<AngleFunctionPath>(
                            std::move(path),
                            angleFunctionForCommandType(FacePointCommand(command.target)));
                }
            }
        }

        if (!prevPath->hit(obstacles, timeInto) && timeLeft-1000ms < timeToHit && reusePathCount < 10) {
            reusePathCount++;
            Point nearPoint;
            prevPath->setDebugText("Reuse prevPath");
            if (ball.estimateTimeTo(prevPath->end().motion.pos, &nearPoint) >= RJ::now() + timeLeft - 1000ms) {
                return make_unique<AngleFunctionPath>(
                        std::move(prevPath),
                        angleFunctionForCommandType(FacePointCommand(command.target)));
            }
        }
    }

    std::unique_ptr<Path> partialPath = nullptr;
    RJ::Seconds partialPathTime = 0ms;
    auto tmpStartInstant = startInstant;
    const auto partialReplanLeadTime = RRTPlanner::getPartialReplanLeadTime();
    if (prevPath) {
        auto timeInto = RJ::now() - prevPath->startTime();
        if (timeInto < prevPath->getDuration() - partialReplanLeadTime*2) {
            partialPath = prevPath->subPath(0ms, timeInto + partialReplanLeadTime);
            partialPathTime = partialReplanLeadTime;
            tmpStartInstant = partialPath->end().motion;
        }
    }

    QString debug = "";
    for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) {
        auto tempObstacles = obstacles;
        MotionInstant ballNow = ball.predict(curTime + t);
        MotionInstant target(ballNow.pos);
        targetKickPos = target.pos;
        target.vel = (command.target - target.pos).normalized(ApproachSpeed);
        target.pos -= target.vel.normalized(Robot_Radius + Ball_Radius * 2);

        vector<Point> points{tmpStartInstant.pos, target.pos};
        if (std::abs(target.vel.angleBetween((target.pos - tmpStartInstant.pos))) >
            DegreesToRadians(60)) {
            auto dist = target.pos.distTo(tmpStartInstant.pos);
            points = {tmpStartInstant.pos,
                      target.pos -
                          target.vel.normalized(Robot_Radius * 2.0 + Ball_Radius * 2.0),
                      target.pos};

            //tempObstacles.add(
            //    make_shared<Circle>(ballNow.pos, Robot_Radius + Ball_Radius));
            //debug = "additional ";
        }
        if (Geometry2d::Segment(ball.pos, target.pos).distTo(tmpStartInstant.pos) <
            Robot_Radius) {
            debug = "meh";
            // break;
        }

        std::unique_ptr<Path> path = RRTPlanner::generatePath(
            points, tempObstacles, motionConstraints, tmpStartInstant.vel, target.vel);
        RJ::Seconds hitTime;

        if (path) {
            if (path->getDuration() + partialPathTime <= t) {
                if (path->hit(tempObstacles, RJ::Seconds::zero(), &hitTime)) {
                    continue;
                }

                if (partialPath) {
                    path = make_unique<CompositePath>(std::move(partialPath), std::move(path));
                    path->setStartTime(prevPath->startTime());
                }

                float multiplier = t / path->getDuration();
                path->setDebugText(
                    "FoundPath" + debug +
                    QString::number(path->getDuration().count()));
                // if (path->getDuration() < 0.7) {
                //    path->slow(multiplier);

                //    path->setDebugText(debug +
                //    QString::number(path->getDuration()) + " slow");
                //}
                reusePathCount=0;
                return make_unique<AngleFunctionPath>(
                    std::move(path), angleFunctionForCommandType(
                                         FacePointCommand(command.target)));
            }
        }
    }

    MotionInstant ballNow = ball.predict(curTime);
    MotionInstant target(ballNow.pos);
    target.vel = (command.target - target.pos).normalized(ApproachSpeed);
    target.pos -= target.vel.normalized(Robot_Radius * 3);

    // obstacles.add(make_shared<Circle>(target.pos, ballAvoidDistance));
    auto ballPath = ball.path(curTime);
    dynamicObstacles.push_back(DynamicObstacle(ballPath.get(), Ball_Radius));
    std::unique_ptr<MotionCommand> rrtCommand =
        std::make_unique<PathTargetCommand>(target);

    auto request = PlanRequest(systemState, startInstant, std::move(rrtCommand),
                               robotConstraints, std::move(prevPath), obstacles,
                               dynamicObstacles, planRequest.shellID);
    auto path = rrtPlanner.run(request);
    path->setDebugText("Gives ups");

    targetKickPos = boost::none;
    return make_unique<AngleFunctionPath>(
        std::move(path),
        angleFunctionForCommandType(FacePointCommand(command.target)));
}
}
<|endoftext|>


<commit_before>#include "editor/changeset_wrapper.hpp"
#include "editor/osm_feature_matcher.hpp"

#include "indexer/feature.hpp"

#include "geometry/mercator.hpp"

#include "base/logging.hpp"
#include "base/macros.hpp"

#include "std/algorithm.hpp"
#include "std/random.hpp"
#include "std/sstream.hpp"

#include "private.h"

using editor::XMLFeature;

namespace
{
m2::RectD GetBoundingRect(vector<m2::PointD> const & geometry)
{
  m2::RectD rect;
  for (auto const & p : geometry)
  {
    auto const latLon = MercatorBounds::ToLatLon(p);
    rect.Add({latLon.lon, latLon.lat});
  }
  return rect;
}

bool OsmFeatureHasTags(pugi::xml_node const & osmFt)
{
  return osmFt.child("tag");
}

vector<string> const static kMainTags = {"amenity", "shop",      "tourism", "historic",
                                         "craft",   "emergency", "barrier", "highway",
                                         "office",  "entrance",  "building"};

string GetTypeForFeature(XMLFeature const & node)
{
  for (string const & key : kMainTags)
  {
    if (node.HasTag(key))
    {
      string const value = node.GetTagValue(key);
      if (value == "yes")
        return key;
      else if (key == "shop" || key == "office" || key == "building" || key == "entrance")
        return value + " " + key;  // "convenience shop"
      else
        return value;
    }
  }

  // Did not find any known tags.
  return node.HasAnyTags() ? "unknown object" : "empty object";
}

vector<m2::PointD> NaiveSample(vector<m2::PointD> const & source, size_t count)
{
  count = min(count, source.size());
  vector<m2::PointD> result;
  result.reserve(count);
  vector<size_t> indexes;
  indexes.reserve(count);

  mt19937 engine;
  uniform_int_distribution<> distrib(0, source.size());

  while (count--)
  {
    size_t index;
    do
    {
      index = distrib(engine);
    } while (find(begin(indexes), end(indexes), index) != end(indexes));
    result.push_back(source[index]);
    indexes.push_back(index);
  }

  return result;
}
}  // namespace

namespace pugi
{
string DebugPrint(xml_document const & doc)
{
  ostringstream stream;
  doc.print(stream, "  ");
  return stream.str();
}
}  // namespace pugi

namespace osm
{
ChangesetWrapper::ChangesetWrapper(TKeySecret const & keySecret,
                                   ServerApi06::TKeyValueTags const & comments) noexcept
    : m_changesetComments(comments),
      m_api(OsmOAuth::ServerAuth(keySecret))
{
}

ChangesetWrapper::~ChangesetWrapper()
{
  if (m_changesetId)
  {
    try
    {
      m_changesetComments["comment"] = GetDescription();
      m_api.UpdateChangeSet(m_changesetId, m_changesetComments);
      m_api.CloseChangeSet(m_changesetId);
    }
    catch (std::exception const & ex)
    {
      LOG(LWARNING, (ex.what()));
    }
  }
}

void ChangesetWrapper::LoadXmlFromOSM(ms::LatLon const & ll, pugi::xml_document & doc, double radiusInMeters)
{
  auto const response = m_api.GetXmlFeaturesAtLatLon(ll.lat, ll.lon, radiusInMeters);
  if (response.first != OsmOAuth::HTTP::OK)
    MYTHROW(HttpErrorException, ("HTTP error", response, "with GetXmlFeaturesAtLatLon", ll));

  if (pugi::status_ok != doc.load(response.second.c_str()).status)
    MYTHROW(
        OsmXmlParseException,
        ("Can't parse OSM server response for GetXmlFeaturesAtLatLon request", response.second));
}

void ChangesetWrapper::LoadXmlFromOSM(ms::LatLon const & min, ms::LatLon const & max, pugi::xml_document & doc)
{
  auto const response = m_api.GetXmlFeaturesInRect(min.lat, min.lon, max.lat, max.lon);
  if (response.first != OsmOAuth::HTTP::OK)
    MYTHROW(HttpErrorException, ("HTTP error", response, "with GetXmlFeaturesInRect", min, max));

  if (pugi::status_ok != doc.load(response.second.c_str()).status)
    MYTHROW(OsmXmlParseException,
            ("Can't parse OSM server response for GetXmlFeaturesInRect request", response.second));
}

XMLFeature ChangesetWrapper::GetMatchingNodeFeatureFromOSM(m2::PointD const & center)
{
  // Match with OSM node.
  ms::LatLon const ll = MercatorBounds::ToLatLon(center);
  pugi::xml_document doc;
  // Throws!
  LoadXmlFromOSM(ll, doc);

  pugi::xml_node const bestNode = GetBestOsmNode(doc, ll);
  if (bestNode.empty())
  {
    MYTHROW(OsmObjectWasDeletedException,
            ("OSM does not have any nodes at the coordinates", ll, ", server has returned:", doc));
  }

  if (!OsmFeatureHasTags(bestNode))
  {
    stringstream sstr;
    bestNode.print(sstr);
    LOG(LDEBUG, ("Node has no tags", sstr.str()));
    MYTHROW(EmptyFeatureException, ("Node has no tags"));
  }

  return XMLFeature(bestNode);
}

XMLFeature ChangesetWrapper::GetMatchingAreaFeatureFromOSM(vector<m2::PointD> const & geometry)
{
  auto const kSamplePointsCount = 3;
  bool hasRelation = false;
  // Try several points in case of poor osm response.
  for (auto const & pt : NaiveSample(geometry, kSamplePointsCount))
  {
    ms::LatLon const ll = MercatorBounds::ToLatLon(pt);
    pugi::xml_document doc;
    // Throws!
    LoadXmlFromOSM(ll, doc);

    if (doc.select_node("osm/relation"))
    {
      auto const rect = GetBoundingRect(geometry);
      LoadXmlFromOSM(ms::LatLon(rect.minY(), rect.minX()), ms::LatLon(rect.maxY(), rect.maxX()), doc);
      hasRelation = true;
    }

    pugi::xml_node const bestWayOrRelation = GetBestOsmWayOrRelation(doc, geometry);
    if (!bestWayOrRelation)
    {
      if (hasRelation)
        break;
      continue;
    }

    if (strcmp(bestWayOrRelation.name(), "relation") == 0)
    {
      stringstream sstr;
      bestWayOrRelation.print(sstr);
      LOG(LDEBUG, ("Relation is the best match", sstr.str()));
      MYTHROW(RelationFeatureAreNotSupportedException, ("Got relation as the best matching"));
    }

    if (!OsmFeatureHasTags(bestWayOrRelation))
    {
      stringstream sstr;
      bestWayOrRelation.print(sstr);
      LOG(LDEBUG, ("Way or relation has no tags", sstr.str()));
      MYTHROW(EmptyFeatureException, ("Way or relation has no tags"));
    }

    // TODO: rename to wayOrRelation when relations are handled.
    XMLFeature const way(bestWayOrRelation);
    ASSERT(way.IsArea(), ("Best way must be an area."));

    // AlexZ: TODO: Check that this way is really match our feature.
    // If we had some way to check it, why not to use it in selecting our feature?

    return way;
  }
  MYTHROW(OsmObjectWasDeletedException, ("OSM does not have any matching way for feature"));
}

void ChangesetWrapper::Create(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  // TODO(AlexZ): Think about storing/logging returned OSM ids.
  UNUSED_VALUE(m_api.CreateElement(node));
  m_created_types[GetTypeForFeature(node)]++;
}

void ChangesetWrapper::Modify(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  m_api.ModifyElement(node);
  m_modified_types[GetTypeForFeature(node)]++;
}

void ChangesetWrapper::Delete(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  m_api.DeleteElement(node);
  m_deleted_types[GetTypeForFeature(node)]++;
}

string ChangesetWrapper::TypeCountToString(TTypeCount const & typeCount)
{
  if (typeCount.empty())
    return string();

  // Convert map to vector and sort pairs by count, descending.
  vector<pair<string, size_t>> items;
  for (auto const & tc : typeCount)
    items.push_back(tc);

  sort(items.begin(), items.end(),
       [](pair<string, size_t> const & a, pair<string, size_t> const & b)
       {
         return a.second > b.second;
       });

  ostringstream ss;
  auto const limit = min(size_t(3), items.size());
  for (auto i = 0; i < limit; ++i)
  {
    if (i > 0)
    {
      // Separator: "A and B" for two, "A, B, and C" for three or more.
      if (limit > 2)
        ss << ", ";
      else
        ss << " ";
      if (i == limit - 1)
        ss << "and ";
    }

    auto & currentPair = items[i];
    // If we have more objects left, make the last one a list of these.
    if (i == limit - 1 && limit < items.size())
    {
      int count = 0;
      for (auto j = i; j < items.size(); ++j)
        count += items[j].second;
      currentPair = {"other object", count};
    }

    // Format a count: "a shop" for single shop, "4 shops" for multiple.
    if (currentPair.second == 1)
      ss << "a ";
    else
      ss << currentPair.second << ' ';
    ss << currentPair.first;
    if (currentPair.second > 1)
      ss << 's';
  }
  return ss.str();
}

string ChangesetWrapper::GetDescription() const
{
  string result;
  if (!m_created_types.empty())
    result = "Created " + TypeCountToString(m_created_types);
  if (!m_modified_types.empty())
  {
    if (!result.empty())
      result += "; ";
    result += "Updated " + TypeCountToString(m_modified_types);
  }
  if (!m_deleted_types.empty())
  {
    if (!result.empty())
      result += "; ";
    result += "Deleted " + TypeCountToString(m_deleted_types);
  }
  return result;
}

}  // namespace osm
<commit_msg>Use more ligheweight rand engine.<commit_after>#include "editor/changeset_wrapper.hpp"
#include "editor/osm_feature_matcher.hpp"

#include "indexer/feature.hpp"

#include "geometry/mercator.hpp"

#include "base/logging.hpp"
#include "base/macros.hpp"

#include "std/algorithm.hpp"
#include "std/random.hpp"
#include "std/sstream.hpp"

#include "private.h"

using editor::XMLFeature;

namespace
{
m2::RectD GetBoundingRect(vector<m2::PointD> const & geometry)
{
  m2::RectD rect;
  for (auto const & p : geometry)
  {
    auto const latLon = MercatorBounds::ToLatLon(p);
    rect.Add({latLon.lon, latLon.lat});
  }
  return rect;
}

bool OsmFeatureHasTags(pugi::xml_node const & osmFt)
{
  return osmFt.child("tag");
}

vector<string> const static kMainTags = {"amenity", "shop",      "tourism", "historic",
                                         "craft",   "emergency", "barrier", "highway",
                                         "office",  "entrance",  "building"};

string GetTypeForFeature(XMLFeature const & node)
{
  for (string const & key : kMainTags)
  {
    if (node.HasTag(key))
    {
      string const value = node.GetTagValue(key);
      if (value == "yes")
        return key;
      else if (key == "shop" || key == "office" || key == "building" || key == "entrance")
        return value + " " + key;  // "convenience shop"
      else
        return value;
    }
  }

  // Did not find any known tags.
  return node.HasAnyTags() ? "unknown object" : "empty object";
}

vector<m2::PointD> NaiveSample(vector<m2::PointD> const & source, size_t count)
{
  count = min(count, source.size());
  vector<m2::PointD> result;
  result.reserve(count);
  vector<size_t> indexes;
  indexes.reserve(count);

  minstd_rand engine;
  uniform_int_distribution<> distrib(0, source.size());

  while (count--)
  {
    size_t index;
    do
    {
      index = distrib(engine);
    } while (find(begin(indexes), end(indexes), index) != end(indexes));
    result.push_back(source[index]);
    indexes.push_back(index);
  }

  return result;
}
}  // namespace

namespace pugi
{
string DebugPrint(xml_document const & doc)
{
  ostringstream stream;
  doc.print(stream, "  ");
  return stream.str();
}
}  // namespace pugi

namespace osm
{
ChangesetWrapper::ChangesetWrapper(TKeySecret const & keySecret,
                                   ServerApi06::TKeyValueTags const & comments) noexcept
    : m_changesetComments(comments),
      m_api(OsmOAuth::ServerAuth(keySecret))
{
}

ChangesetWrapper::~ChangesetWrapper()
{
  if (m_changesetId)
  {
    try
    {
      m_changesetComments["comment"] = GetDescription();
      m_api.UpdateChangeSet(m_changesetId, m_changesetComments);
      m_api.CloseChangeSet(m_changesetId);
    }
    catch (std::exception const & ex)
    {
      LOG(LWARNING, (ex.what()));
    }
  }
}

void ChangesetWrapper::LoadXmlFromOSM(ms::LatLon const & ll, pugi::xml_document & doc, double radiusInMeters)
{
  auto const response = m_api.GetXmlFeaturesAtLatLon(ll.lat, ll.lon, radiusInMeters);
  if (response.first != OsmOAuth::HTTP::OK)
    MYTHROW(HttpErrorException, ("HTTP error", response, "with GetXmlFeaturesAtLatLon", ll));

  if (pugi::status_ok != doc.load(response.second.c_str()).status)
    MYTHROW(
        OsmXmlParseException,
        ("Can't parse OSM server response for GetXmlFeaturesAtLatLon request", response.second));
}

void ChangesetWrapper::LoadXmlFromOSM(ms::LatLon const & min, ms::LatLon const & max, pugi::xml_document & doc)
{
  auto const response = m_api.GetXmlFeaturesInRect(min.lat, min.lon, max.lat, max.lon);
  if (response.first != OsmOAuth::HTTP::OK)
    MYTHROW(HttpErrorException, ("HTTP error", response, "with GetXmlFeaturesInRect", min, max));

  if (pugi::status_ok != doc.load(response.second.c_str()).status)
    MYTHROW(OsmXmlParseException,
            ("Can't parse OSM server response for GetXmlFeaturesInRect request", response.second));
}

XMLFeature ChangesetWrapper::GetMatchingNodeFeatureFromOSM(m2::PointD const & center)
{
  // Match with OSM node.
  ms::LatLon const ll = MercatorBounds::ToLatLon(center);
  pugi::xml_document doc;
  // Throws!
  LoadXmlFromOSM(ll, doc);

  pugi::xml_node const bestNode = GetBestOsmNode(doc, ll);
  if (bestNode.empty())
  {
    MYTHROW(OsmObjectWasDeletedException,
            ("OSM does not have any nodes at the coordinates", ll, ", server has returned:", doc));
  }

  if (!OsmFeatureHasTags(bestNode))
  {
    stringstream sstr;
    bestNode.print(sstr);
    LOG(LDEBUG, ("Node has no tags", sstr.str()));
    MYTHROW(EmptyFeatureException, ("Node has no tags"));
  }

  return XMLFeature(bestNode);
}

XMLFeature ChangesetWrapper::GetMatchingAreaFeatureFromOSM(vector<m2::PointD> const & geometry)
{
  auto const kSamplePointsCount = 3;
  bool hasRelation = false;
  // Try several points in case of poor osm response.
  for (auto const & pt : NaiveSample(geometry, kSamplePointsCount))
  {
    ms::LatLon const ll = MercatorBounds::ToLatLon(pt);
    pugi::xml_document doc;
    // Throws!
    LoadXmlFromOSM(ll, doc);

    if (doc.select_node("osm/relation"))
    {
      auto const rect = GetBoundingRect(geometry);
      LoadXmlFromOSM(ms::LatLon(rect.minY(), rect.minX()), ms::LatLon(rect.maxY(), rect.maxX()), doc);
      hasRelation = true;
    }

    pugi::xml_node const bestWayOrRelation = GetBestOsmWayOrRelation(doc, geometry);
    if (!bestWayOrRelation)
    {
      if (hasRelation)
        break;
      continue;
    }

    if (strcmp(bestWayOrRelation.name(), "relation") == 0)
    {
      stringstream sstr;
      bestWayOrRelation.print(sstr);
      LOG(LDEBUG, ("Relation is the best match", sstr.str()));
      MYTHROW(RelationFeatureAreNotSupportedException, ("Got relation as the best matching"));
    }

    if (!OsmFeatureHasTags(bestWayOrRelation))
    {
      stringstream sstr;
      bestWayOrRelation.print(sstr);
      LOG(LDEBUG, ("Way or relation has no tags", sstr.str()));
      MYTHROW(EmptyFeatureException, ("Way or relation has no tags"));
    }

    // TODO: rename to wayOrRelation when relations are handled.
    XMLFeature const way(bestWayOrRelation);
    ASSERT(way.IsArea(), ("Best way must be an area."));

    // AlexZ: TODO: Check that this way is really match our feature.
    // If we had some way to check it, why not to use it in selecting our feature?

    return way;
  }
  MYTHROW(OsmObjectWasDeletedException, ("OSM does not have any matching way for feature"));
}

void ChangesetWrapper::Create(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  // TODO(AlexZ): Think about storing/logging returned OSM ids.
  UNUSED_VALUE(m_api.CreateElement(node));
  m_created_types[GetTypeForFeature(node)]++;
}

void ChangesetWrapper::Modify(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  m_api.ModifyElement(node);
  m_modified_types[GetTypeForFeature(node)]++;
}

void ChangesetWrapper::Delete(XMLFeature node)
{
  if (m_changesetId == kInvalidChangesetId)
    m_changesetId = m_api.CreateChangeSet(m_changesetComments);

  // Changeset id should be updated for every OSM server commit.
  node.SetAttribute("changeset", strings::to_string(m_changesetId));
  m_api.DeleteElement(node);
  m_deleted_types[GetTypeForFeature(node)]++;
}

string ChangesetWrapper::TypeCountToString(TTypeCount const & typeCount)
{
  if (typeCount.empty())
    return string();

  // Convert map to vector and sort pairs by count, descending.
  vector<pair<string, size_t>> items;
  for (auto const & tc : typeCount)
    items.push_back(tc);

  sort(items.begin(), items.end(),
       [](pair<string, size_t> const & a, pair<string, size_t> const & b)
       {
         return a.second > b.second;
       });

  ostringstream ss;
  auto const limit = min(size_t(3), items.size());
  for (auto i = 0; i < limit; ++i)
  {
    if (i > 0)
    {
      // Separator: "A and B" for two, "A, B, and C" for three or more.
      if (limit > 2)
        ss << ", ";
      else
        ss << " ";
      if (i == limit - 1)
        ss << "and ";
    }

    auto & currentPair = items[i];
    // If we have more objects left, make the last one a list of these.
    if (i == limit - 1 && limit < items.size())
    {
      int count = 0;
      for (auto j = i; j < items.size(); ++j)
        count += items[j].second;
      currentPair = {"other object", count};
    }

    // Format a count: "a shop" for single shop, "4 shops" for multiple.
    if (currentPair.second == 1)
      ss << "a ";
    else
      ss << currentPair.second << ' ';
    ss << currentPair.first;
    if (currentPair.second > 1)
      ss << 's';
  }
  return ss.str();
}

string ChangesetWrapper::GetDescription() const
{
  string result;
  if (!m_created_types.empty())
    result = "Created " + TypeCountToString(m_created_types);
  if (!m_modified_types.empty())
  {
    if (!result.empty())
      result += "; ";
    result += "Updated " + TypeCountToString(m_modified_types);
  }
  if (!m_deleted_types.empty())
  {
    if (!result.empty())
      result += "; ";
    result += "Deleted " + TypeCountToString(m_deleted_types);
  }
  return result;
}

}  // namespace osm
<|endoftext|>


<commit_before>#include <string.h>
#include <stdlib.h>
#include <gtk/gtk.h>
#include <getopt.h>
#include <iostream>

#include <bot_vis/bot_vis.h>
#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <path_util/path_util.h>
#include <ConciseArgs>

#include <bot_frames/bot_frames_renderers.h>

//imported renderers
#include <bot_lcmgl_render/lcmgl_bot_renderer.h>
#include <laser_utils/renderer_laser.h>
#include <image_utils/renderer_cam_thumb.h>
#include <visualization/collections_renderer.hpp>
#include <octomap_utils/renderer_octomap.h>
#include <renderer_maps/MapsRenderer.hpp>
#include <renderer_data_control/DataControlRenderer.hpp>
#include <multisense/multisense_renderer.h>
#include <occ_map/occ_map_renderers.h>
// Individual Renderers:
#include <renderer_drc/renderer_scrollingplots.h>
#include <renderer_drc/renderer_driving.hpp>
#include <renderer_drc/renderer_walking.hpp>
#include <renderer_drc/renderer_status.hpp>

// block of renderers
#include <renderer_robot_state/renderer_robot_state.hpp>
#include <renderer_robot_plan/renderer_robot_plan.hpp>
#include <renderer_affordances/renderer_affordances.hpp>
#include <renderer_sticky_feet/renderer_sticky_feet.hpp>
#include <renderer_end_effector_goal/renderer_end_effector_goal.hpp>

#include "udp_util.h"
#include "RendererGroupUtil.hpp"

using namespace std;

static int
logplayer_remote_on_key_press(BotViewer *viewer, BotEventHandler *ehandler,
        const GdkEventKey *event)
{
    int keyval = event->keyval;

    switch (keyval)
    {
    case 'P':
    case 'p':
        udp_send_string("127.0.0.1", 53261, "PLAYPAUSETOGGLE");
        break;
    case 'N':
    case 'n':
        udp_send_string("127.0.0.1", 53261, "STEP");
        break;
    case '=':
    case '+':
        udp_send_string("127.0.0.1", 53261, "FASTER");
        break;
    case '_':
    case '-':
        udp_send_string("127.0.0.1", 53261, "SLOWER");
        break;
    case '[':
        udp_send_string("127.0.0.1", 53261, "BACK5");
        break;
    case ']':
        udp_send_string("127.0.0.1", 53261, "FORWARD5");
        break;
    default:
        return 0;
    }

    return 1;
}

static void on_collapse_all_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *viewer = (BotViewer*) user_data;
  for (unsigned int i = 0; i < viewer->renderers->len; ++i) {
    BotRenderer* renderer =
      (BotRenderer*)g_ptr_array_index(viewer->renderers, i);

    renderer->expanded = FALSE;
    if (renderer->expander) {
      gtk_expander_set_expanded (GTK_EXPANDER (renderer->expander),
                                 renderer->expanded);
    }
  }
}

// TBD - correct location for this code?
static void on_top_view_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *self = (BotViewer*) user_data;

  double eye[3];
  double look[3];
  double up[3];
  self->view_handler->get_eye_look(self->view_handler, eye, look, up);
  eye[0] = 0;
  eye[1] = 0;
  eye[2] = 10;
  look[0] = 0;
  look[1] = 0;
  look[2] = 0;
  up[0] = 0;
  up[1] = 10;
  up[2] = 0;
  self->view_handler->set_look_at(self->view_handler, eye, look, up);

  bot_viewer_request_redraw(self);
}

// TBD - correct location for this?
int start_spy_counter=0;
static void on_start_spy_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *self = (BotViewer*) user_data;
  if (start_spy_counter >0){ // is there a better way than this counter?
    int i = system ("bot-spy &> /dev/null &");
  }
  start_spy_counter++;
}

static void
destroy_renderers (BotViewer *viewer)
{
  for (unsigned int ridx = 0; ridx < viewer->renderers->len; ridx++) {
    BotRenderer *renderer =
      (BotRenderer*)g_ptr_array_index(viewer->renderers, ridx);
    if (renderer && renderer->destroy) {
      std::cout << "Destroying renderer \"" << renderer->name <<
        "\"..." << std::flush;
      renderer->destroy(renderer);
      std::cout << "Done" << std::endl;
    }
  }

  g_ptr_array_free(viewer->renderers, TRUE);
}

int main(int argc, char *argv[])
{
  setlinebuf(stdout);

  string config_file = ""; // leave this empty so force viewer to get it from the param server
  string role = "robot";
  ConciseArgs opt(argc, (char**)argv);
  opt.add(config_file, "c", "config_file","Robot cfg file");
  opt.add(role, "r", "role","Role - robot or base");
  opt.parse();
  std::cout << "config_file: " << config_file << "\n";
  std::cout << "role: " << role << "\n";
  string viewer_title = "(" + role + ") MIT DRC Viewer";
  string vis_config_file = ".bot-plugin-"+ role +"-drc-viewer";

  //todo: comment this section
  gtk_init(&argc, &argv);
  glutInit(&argc, argv);
  g_thread_init(NULL);

  string lcm_url="";
  std::string role_upper;
  for(short i = 0; i < role.size(); ++i)
     role_upper+= (std::toupper(role[i]));
  if((role.compare("robot") == 0) || (role.compare("base") == 0) ){
    for(short i = 0; i < role_upper.size(); ++i)
       role_upper[i] = (std::toupper(role_upper[i]));
    string env_variable_name = string("LCM_URL_DRC_" + role_upper);
    char* env_variable;
    env_variable = getenv (env_variable_name.c_str());
    if (env_variable!=NULL){
      //printf ("The env_variable is: %s\n",env_variable);
      lcm_url = string(env_variable);
    }else{
      std::cout << env_variable_name << " environment variable has not been set ["<< lcm_url <<"]\n";
      exit(-1);
    }
  }else{
    std::cout << "Role not understood, choose: robot or base\n";
    return 1;
  }

  lcm_t * lcm;
  lcm= lcm_create(lcm_url.c_str());// bot_lcm_get_global(lcm_url.c_str());

  bot_glib_mainloop_attach_lcm(lcm);
  BotParam * bot_param;
  if(config_file.size()) {
    fprintf(stderr,"Reading config from file\n");
    std::string config_path = std::string(getConfigPath()) +'/' + std::string(config_file);
    bot_param = bot_param_new_from_file(config_path.c_str());
    if (bot_param == NULL) {
      std::cerr << "Couldn't get bot param from file %s\n" << config_path << std::endl;
      exit(-1);
    }
  }else {
    bot_param = bot_param_new_from_server(lcm, 0);
    if (bot_param == NULL) {
      fprintf(stderr, "Couldn't get bot param from server.\n");
      return 1;
    }
  }

  BotFrames* bot_frames = bot_frames_new(lcm, bot_param);
  BotViewer* viewer = bot_viewer_new(viewer_title.c_str());

  //die cleanly for control-c etc :-)
  bot_gtk_quit_on_interrupt();

  // logplayer controls
  BotEventHandler *ehandler = (BotEventHandler*) calloc(1, sizeof(BotEventHandler));
  ehandler->name = "LogPlayer Remote";
  ehandler->enabled = 1;
  ehandler->key_press = logplayer_remote_on_key_press;
  bot_viewer_add_event_handler(viewer, ehandler, 0);

  // core renderers
  bot_viewer_add_stock_renderer(viewer, BOT_VIEWER_STOCK_RENDERER_GRID, 1);
  bot_lcmgl_add_renderer_to_viewer(viewer, lcm, 1);
  laser_util_add_renderer_to_viewer(viewer, 1, lcm, bot_param, bot_frames);
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );

  collections_add_renderer_to_viewer(viewer, 1, lcm);
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );

  // Block of Renderers:
  setup_renderer_robot_state(viewer, 0, lcm);
  setup_renderer_robot_plan(viewer, 0, lcm);
  setup_renderer_affordances(viewer, 0, lcm);
  setup_renderer_sticky_feet(viewer, 0, lcm,bot_param,bot_frames);
  setup_renderer_end_effector_goal(viewer, 0, lcm);

  // Individual Renderers:
  add_octomap_renderer_to_viewer(viewer, 1, lcm);
  maps_renderer_setup(viewer, 0, lcm, bot_param, bot_frames);
  data_control_renderer_setup(viewer, 0, lcm, bot_param, bot_frames);
  scrollingplots_add_renderer_to_viewer(viewer, 0, lcm);
  status_add_renderer_to_viewer(viewer, 0, lcm);
  setup_renderer_driving(viewer, 0, lcm, bot_param, bot_frames);
  setup_renderer_walking(viewer, 0,lcm,bot_param,bot_frames);
  occ_map_pixel_map_add_renderer_to_viewer(viewer, 0, "TERRAIN_COST", "PixelMap");

  add_cam_thumb_renderer_to_viewer(viewer, 0, lcm, bot_param, bot_frames);
  multisense_add_renderer_to_viewer(viewer, 0,lcm,bot_frames,"CAMERA", bot_param);

  // add custom TOP VIEW button
  GtkWidget *top_view_button;
  top_view_button = (GtkWidget *) gtk_tool_button_new_from_stock(GTK_STOCK_ZOOM_FIT);
  gtk_tool_button_set_label(GTK_TOOL_BUTTON(top_view_button), "Top View");
  gtk_tool_item_set_tooltip(GTK_TOOL_ITEM(top_view_button), viewer->tips, "Switch to Top View", NULL);
  gtk_toolbar_insert(GTK_TOOLBAR(viewer->toolbar), GTK_TOOL_ITEM(top_view_button), 4);
  gtk_widget_show(top_view_button);
  g_signal_connect(G_OBJECT(top_view_button), "clicked", G_CALLBACK(on_top_view_clicked), viewer);
  on_top_view_clicked(NULL, (void *) viewer);

  // add custom TOP VIEW button
  GtkWidget *start_spy_button;
  start_spy_button = (GtkWidget *) gtk_tool_button_new_from_stock(GTK_STOCK_FIND);
  gtk_tool_button_set_label(GTK_TOOL_BUTTON(start_spy_button), "Bot Spy");
  gtk_tool_item_set_tooltip(GTK_TOOL_ITEM(start_spy_button), viewer->tips, "Launch Bot Spy", NULL);
  gtk_toolbar_insert(GTK_TOOLBAR(viewer->toolbar), GTK_TOOL_ITEM(start_spy_button), 4);
  gtk_widget_show(start_spy_button);
  g_signal_connect(G_OBJECT(start_spy_button), "clicked", G_CALLBACK(on_start_spy_clicked), viewer);
  on_start_spy_clicked(NULL, (void *) viewer);

  // add custom "collapse all" button
  GtkToolItem *item = gtk_tool_button_new_from_stock (GTK_STOCK_CLEAR);
  gtk_tool_button_set_label (GTK_TOOL_BUTTON (item), "Collapse All");
  gtk_tool_item_set_is_important (GTK_TOOL_ITEM (item), TRUE);
  gtk_tool_item_set_tooltip (GTK_TOOL_ITEM (item), viewer->tips,
                             "Collapse all visible renderers", NULL);
  gtk_toolbar_insert (GTK_TOOLBAR (viewer->toolbar), item, -1);
  gtk_widget_show (GTK_WIDGET (item));
  g_signal_connect (G_OBJECT (item), "clicked",
                    G_CALLBACK (on_collapse_all_clicked), viewer);

  // add custom renderer groups menu
  RendererGroupUtil groupUtil(viewer, bot_param);
  groupUtil.setup();

  // load the renderer params from the config file.
  char *fname = g_build_filename(g_get_user_config_dir(), vis_config_file.c_str() , NULL);
  bot_viewer_load_preferences(viewer, fname);

  gtk_main();

  //save the renderer params to the config file.
  bot_viewer_save_preferences(viewer, fname);

  // clean up all renderers
  destroy_renderers(viewer);

  // remove reference
  bot_viewer_unref(viewer);
}
<commit_msg>added a new interface<commit_after>#include <string.h>
#include <stdlib.h>
#include <gtk/gtk.h>
#include <getopt.h>
#include <iostream>

#include <bot_vis/bot_vis.h>
#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <path_util/path_util.h>
#include <ConciseArgs>

#include <bot_frames/bot_frames_renderers.h>

//imported renderers
#include <bot_lcmgl_render/lcmgl_bot_renderer.h>
#include <laser_utils/renderer_laser.h>
#include <image_utils/renderer_cam_thumb.h>
#include <visualization/collections_renderer.hpp>
#include <octomap_utils/renderer_octomap.h>
#include <renderer_maps/MapsRenderer.hpp>
#include <renderer_data_control/DataControlRenderer.hpp>
#include <multisense/multisense_renderer.h>
#include <occ_map/occ_map_renderers.h>
// Individual Renderers:
#include <renderer_drc/renderer_scrollingplots.h>
#include <renderer_drc/renderer_driving.hpp>
#include <renderer_drc/renderer_walking.hpp>
#include <renderer_drc/renderer_status.hpp>

// block of renderers
#include <renderer_robot_state/renderer_robot_state.hpp>
#include <renderer_robot_plan/renderer_robot_plan.hpp>
#include <renderer_affordances/renderer_affordances.hpp>
#include <renderer_sticky_feet/renderer_sticky_feet.hpp>
#include <renderer_end_effector_goal/renderer_end_effector_goal.hpp>

#include "udp_util.h"
#include "RendererGroupUtil.hpp"

using namespace std;

static int
logplayer_remote_on_key_press(BotViewer *viewer, BotEventHandler *ehandler,
        const GdkEventKey *event)
{
    int keyval = event->keyval;

    switch (keyval)
    {
    case 'P':
    case 'p':
        udp_send_string("127.0.0.1", 53261, "PLAYPAUSETOGGLE");
        break;
    case 'N':
    case 'n':
        udp_send_string("127.0.0.1", 53261, "STEP");
        break;
    case '=':
    case '+':
        udp_send_string("127.0.0.1", 53261, "FASTER");
        break;
    case '_':
    case '-':
        udp_send_string("127.0.0.1", 53261, "SLOWER");
        break;
    case '[':
        udp_send_string("127.0.0.1", 53261, "BACK5");
        break;
    case ']':
        udp_send_string("127.0.0.1", 53261, "FORWARD5");
        break;
    default:
        return 0;
    }

    return 1;
}

static void on_collapse_all_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *viewer = (BotViewer*) user_data;
  for (unsigned int i = 0; i < viewer->renderers->len; ++i) {
    BotRenderer* renderer =
      (BotRenderer*)g_ptr_array_index(viewer->renderers, i);

    renderer->expanded = FALSE;
    if (renderer->expander) {
      gtk_expander_set_expanded (GTK_EXPANDER (renderer->expander),
                                 renderer->expanded);
    }
  }
}

// TBD - correct location for this code?
static void on_top_view_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *self = (BotViewer*) user_data;

  double eye[3];
  double look[3];
  double up[3];
  self->view_handler->get_eye_look(self->view_handler, eye, look, up);
  eye[0] = 0;
  eye[1] = 0;
  eye[2] = 10;
  look[0] = 0;
  look[1] = 0;
  look[2] = 0;
  up[0] = 0;
  up[1] = 10;
  up[2] = 0;
  self->view_handler->set_look_at(self->view_handler, eye, look, up);

  bot_viewer_request_redraw(self);
}

// TBD - correct location for this?
int start_spy_counter=0;
static void on_start_spy_clicked(GtkToggleToolButton *tb, void *user_data)
{
  BotViewer *self = (BotViewer*) user_data;
  if (start_spy_counter >0){ // is there a better way than this counter?
    int i = system ("bot-spy &> /dev/null &");
  }
  start_spy_counter++;
}

static void
destroy_renderers (BotViewer *viewer)
{
  for (unsigned int ridx = 0; ridx < viewer->renderers->len; ridx++) {
    BotRenderer *renderer =
      (BotRenderer*)g_ptr_array_index(viewer->renderers, ridx);
    if (renderer && renderer->destroy) {
      std::cout << "Destroying renderer \"" << renderer->name <<
        "\"..." << std::flush;
      renderer->destroy(renderer);
      std::cout << "Done" << std::endl;
    }
  }

  g_ptr_array_free(viewer->renderers, TRUE);
}

int main(int argc, char *argv[])
{
  setlinebuf(stdout);

  string config_file = ""; // leave this empty so force viewer to get it from the param server
  string role = "robot";
  ConciseArgs opt(argc, (char**)argv);
  opt.add(config_file, "c", "config_file","Robot cfg file");
  opt.add(role, "r", "role","Role - robot or base");
  opt.parse();
  std::cout << "config_file: " << config_file << "\n";
  std::cout << "role: " << role << "\n";
  string viewer_title = "(" + role + ") MIT DRC Viewer";
  string vis_config_file = ".bot-plugin-"+ role +"-drc-viewer";

  //todo: comment this section
  gtk_init(&argc, &argv);
  glutInit(&argc, argv);
  g_thread_init(NULL);

  string lcm_url="";
  std::string role_upper;
  for(short i = 0; i < role.size(); ++i)
     role_upper+= (std::toupper(role[i]));
  if((role.compare("robot") == 0) || (role.compare("base") == 0) ){
    for(short i = 0; i < role_upper.size(); ++i)
       role_upper[i] = (std::toupper(role_upper[i]));
    string env_variable_name = string("LCM_URL_DRC_" + role_upper);
    char* env_variable;
    env_variable = getenv (env_variable_name.c_str());
    if (env_variable!=NULL){
      //printf ("The env_variable is: %s\n",env_variable);
      lcm_url = string(env_variable);
    }else{
      std::cout << env_variable_name << " environment variable has not been set ["<< lcm_url <<"]\n";
      exit(-1);
    }
  }else{
    std::cout << "Role not understood, choose: robot or base\n";
    return 1;
  }

  lcm_t * lcm;
  lcm= lcm_create(lcm_url.c_str());// bot_lcm_get_global(lcm_url.c_str());

  bot_glib_mainloop_attach_lcm(lcm);
  BotParam * bot_param;
  if(config_file.size()) {
    fprintf(stderr,"Reading config from file\n");
    std::string config_path = std::string(getConfigPath()) +'/' + std::string(config_file);
    bot_param = bot_param_new_from_file(config_path.c_str());
    if (bot_param == NULL) {
      std::cerr << "Couldn't get bot param from file %s\n" << config_path << std::endl;
      exit(-1);
    }
  }else {
    bot_param = bot_param_new_from_server(lcm, 0);
    if (bot_param == NULL) {
      fprintf(stderr, "Couldn't get bot param from server.\n");
      return 1;
    }
  }

  BotFrames* bot_frames = bot_frames_new(lcm, bot_param);
  BotViewer* viewer = bot_viewer_new(viewer_title.c_str());

  //die cleanly for control-c etc :-)
  bot_gtk_quit_on_interrupt();

  // logplayer controls
  BotEventHandler *ehandler = (BotEventHandler*) calloc(1, sizeof(BotEventHandler));
  ehandler->name = "LogPlayer Remote";
  ehandler->enabled = 1;
  ehandler->key_press = logplayer_remote_on_key_press;
  bot_viewer_add_event_handler(viewer, ehandler, 0);

  // core renderers
  bot_viewer_add_stock_renderer(viewer, BOT_VIEWER_STOCK_RENDERER_GRID, 1);
  bot_lcmgl_add_renderer_to_viewer(viewer, lcm, 1);
  laser_util_add_renderer_to_viewer(viewer, 1, lcm, bot_param, bot_frames);
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );

  collections_add_renderer_to_viewer(viewer, 1, lcm);
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );
  bot_frames_add_renderer_to_viewer(viewer, 1, bot_frames );

  // Block of Renderers:
  setup_renderer_robot_state(viewer, 0, lcm);
  setup_renderer_robot_plan(viewer, 0, lcm);
  setup_renderer_affordances(viewer, 0, lcm);
  setup_renderer_sticky_feet(viewer, 0, lcm,bot_param,bot_frames);
  setup_renderer_end_effector_goal(viewer, 0, lcm);

  // Individual Renderers:
  add_octomap_renderer_to_viewer(viewer, 1, lcm);
  maps_renderer_setup(viewer, 0, lcm, bot_param, bot_frames);
  data_control_renderer_setup(viewer, 0, lcm, bot_param, bot_frames);
  scrollingplots_add_renderer_to_viewer(viewer, 0, lcm);
  status_add_renderer_to_viewer(viewer, 0, lcm);
  setup_renderer_driving(viewer, 0, lcm, bot_param, bot_frames);
  setup_renderer_walking(viewer, 0,lcm,bot_param,bot_frames);
  occ_map_pixel_map_add_renderer_to_viewer_lcm(viewer, 0, lcm, "PIXEL_MAP", "PixelMap");

  add_cam_thumb_renderer_to_viewer(viewer, 0, lcm, bot_param, bot_frames);
  multisense_add_renderer_to_viewer(viewer, 0,lcm,bot_frames,"CAMERA", bot_param);

  // add custom TOP VIEW button
  GtkWidget *top_view_button;
  top_view_button = (GtkWidget *) gtk_tool_button_new_from_stock(GTK_STOCK_ZOOM_FIT);
  gtk_tool_button_set_label(GTK_TOOL_BUTTON(top_view_button), "Top View");
  gtk_tool_item_set_tooltip(GTK_TOOL_ITEM(top_view_button), viewer->tips, "Switch to Top View", NULL);
  gtk_toolbar_insert(GTK_TOOLBAR(viewer->toolbar), GTK_TOOL_ITEM(top_view_button), 4);
  gtk_widget_show(top_view_button);
  g_signal_connect(G_OBJECT(top_view_button), "clicked", G_CALLBACK(on_top_view_clicked), viewer);
  on_top_view_clicked(NULL, (void *) viewer);

  // add custom TOP VIEW button
  GtkWidget *start_spy_button;
  start_spy_button = (GtkWidget *) gtk_tool_button_new_from_stock(GTK_STOCK_FIND);
  gtk_tool_button_set_label(GTK_TOOL_BUTTON(start_spy_button), "Bot Spy");
  gtk_tool_item_set_tooltip(GTK_TOOL_ITEM(start_spy_button), viewer->tips, "Launch Bot Spy", NULL);
  gtk_toolbar_insert(GTK_TOOLBAR(viewer->toolbar), GTK_TOOL_ITEM(start_spy_button), 4);
  gtk_widget_show(start_spy_button);
  g_signal_connect(G_OBJECT(start_spy_button), "clicked", G_CALLBACK(on_start_spy_clicked), viewer);
  on_start_spy_clicked(NULL, (void *) viewer);

  // add custom "collapse all" button
  GtkToolItem *item = gtk_tool_button_new_from_stock (GTK_STOCK_CLEAR);
  gtk_tool_button_set_label (GTK_TOOL_BUTTON (item), "Collapse All");
  gtk_tool_item_set_is_important (GTK_TOOL_ITEM (item), TRUE);
  gtk_tool_item_set_tooltip (GTK_TOOL_ITEM (item), viewer->tips,
                             "Collapse all visible renderers", NULL);
  gtk_toolbar_insert (GTK_TOOLBAR (viewer->toolbar), item, -1);
  gtk_widget_show (GTK_WIDGET (item));
  g_signal_connect (G_OBJECT (item), "clicked",
                    G_CALLBACK (on_collapse_all_clicked), viewer);

  // add custom renderer groups menu
  RendererGroupUtil groupUtil(viewer, bot_param);
  groupUtil.setup();

  // load the renderer params from the config file.
  char *fname = g_build_filename(g_get_user_config_dir(), vis_config_file.c_str() , NULL);
  bot_viewer_load_preferences(viewer, fname);

  gtk_main();

  //save the renderer params to the config file.
  bot_viewer_save_preferences(viewer, fname);

  // clean up all renderers
  destroy_renderers(viewer);

  // remove reference
  bot_viewer_unref(viewer);
}
<|endoftext|>


<commit_before>#include "core/meta/MetaPopulationApportionment.hpp"
#include "core/meta/BlanketResolver.hpp"

MetaPopulationApportionment::MetaPopulationApportionment(
	PopulationNode * metaNode,
	ApportionmentFunction * apportionment,
	AggregationFunction * aggregation
) : Apportionment(metaNode, apportionment, aggregation) {}

Genome * MetaPopulationApportionment::getOperableGenome(Genome * genome) {
	Genome resolved = BlanketResolver::resolveBlanket(genome);
	return new Genome(&resolved);
}

std::vector<unsigned int> MetaPopulationApportionment::getComponentIndices(
	Genome * upper,
	Genome * target
) {
	unsigned int headIndex = BlanketResolver::findHeadIndex(upper);
	return upper->getIndex<Genome*>(headIndex)
		->getFlattenedIndices(target);
}
<commit_msg>[MetaPopulationApportionment]: Fixed root node not getting fitness<commit_after>#include "core/meta/MetaPopulationApportionment.hpp"
#include "core/meta/BlanketResolver.hpp"

MetaPopulationApportionment::MetaPopulationApportionment(
	PopulationNode * metaNode,
	ApportionmentFunction * apportionment,
	AggregationFunction * aggregation
) : Apportionment(metaNode, apportionment, aggregation) {}

Genome * MetaPopulationApportionment::getOperableGenome(Genome * genome) {
	Genome resolved = BlanketResolver::resolveBlanket(genome);
	return new Genome(&resolved);
}

std::vector<unsigned int> MetaPopulationApportionment::getComponentIndices(
	Genome * upper,
	Genome * target
) {
	Genome * head = upper->getIndex<Genome*>(
		BlanketResolver::findHeadIndex(upper)
	);

	return (head == target) ?
		std::vector<unsigned int>(1, 0) :
		head->getFlattenedIndices(target);
}
<|endoftext|>


<commit_before>#include "lcio.h"

#include "IO/LCWriter.h"

// position of record name for last random access record
#define LCSIO_RANDOMACCESS_SIZE 112

#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>

using namespace lcio ;

static std::vector<std::string> FILEN ;

/** Simple program that opens existing LCIO files and appends the records needed for direct access - if they don't exist.
 */

int main(int argc, char** argv ){

    // create sio writer
    LCWriter* lcWrt = LCFactory::getInstance()->createLCWriter()  ;

    // read file names from command line (only argument)
    if( argc < 2) {
      std::cout << " usage:  addRandomAccess <input-file1> [[input-file2],...]" << std::endl ;
      exit(1) ;
    }

    int firstFile = 1 ;
    bool force = false ;
    std::string firstArg( argv[1] ) ;

    if( firstArg == "-f" ){ // force: 'undocumented' feature for fixing files with broken TOC (SplitWriter)
      firstFile++ ;
      std::cout << "  ====== called with -f (force) : will create a TOC for direct access in any case ! " << std::endl ;
      force = true ;
    }

    for(int i=firstFile ; i < argc ; i++){
      FILEN.push_back( argv[i] )  ;
    }

    int nFiles = argc - firstFile ;

    for( int i=0 ; i <nFiles ; ++i ) {

      try{

	if( force) { // --------------------------------------------------------------------------------------------
	  //           if called with force  we rename the last LCIORandomAcces record so that it is ignored and the
	  //           file treated as an old one w/o direct access....

	  FILE* f = fopen( FILEN[i].c_str()  , "r+") ;

	  if( f != 0 ){

	    fseek( f ,  -( LCSIO_RANDOMACCESS_SIZE ) , SEEK_END ) ;
	    std::string bla("") ;
	    bla.resize(17) ;

	    int status = fread( &bla[0] , sizeof(char) , 16 , f ) ;

	    //	    std::cout << "  ====== read : [" << bla << "]" << std::endl ;

	    if( !strcmp( bla.c_str() , "LCIORandomAccess")  ){

	      status = fseek( f ,  -(16) , SEEK_CUR ) ;

	      bla = "LCIORandomIGNORE" ;

	      status = fwrite( &bla[0] , 1 , 16 , f ) ;

	      //	      std::cout << "  --------- wrote " << bla << " to file - bytes written " << status << std::endl;
	    }
	  }

	  fclose( f ) ;
	} //---------- end if(force) --------------------------------------------------------------------------------

	lcWrt->open( FILEN[i] , LCIO::WRITE_APPEND )  ;

	lcWrt->close() ;

      }catch(IOException& e){

	std::cout << " io error in file  " << FILEN[i] << " : " << e.what() << std::endl ;
      }
    }

    return 0 ;
}

<commit_msg>added <cstring> for compiling with gcc 4.4<commit_after>#include "lcio.h"

#include "IO/LCWriter.h"

// position of record name for last random access record
#define LCSIO_RANDOMACCESS_SIZE 112

#include <iostream>
#include <sstream>
#include <cstdio>
#include <cstdlib>
#include <cstring>

using namespace lcio ;

static std::vector<std::string> FILEN ;

/** Simple program that opens existing LCIO files and appends the records needed for direct access - if they don't exist.
 */

int main(int argc, char** argv ){

    // create sio writer
    LCWriter* lcWrt = LCFactory::getInstance()->createLCWriter()  ;

    // read file names from command line (only argument)
    if( argc < 2) {
      std::cout << " usage:  addRandomAccess <input-file1> [[input-file2],...]" << std::endl ;
      exit(1) ;
    }

    int firstFile = 1 ;
    bool force = false ;
    std::string firstArg( argv[1] ) ;

    if( firstArg == "-f" ){ // force: 'undocumented' feature for fixing files with broken TOC (SplitWriter)
      firstFile++ ;
      std::cout << "  ====== called with -f (force) : will create a TOC for direct access in any case ! " << std::endl ;
      force = true ;
    }

    for(int i=firstFile ; i < argc ; i++){
      FILEN.push_back( argv[i] )  ;
    }

    int nFiles = argc - firstFile ;

    for( int i=0 ; i <nFiles ; ++i ) {

      try{

	if( force) { // --------------------------------------------------------------------------------------------
	  //           if called with force  we rename the last LCIORandomAcces record so that it is ignored and the
	  //           file treated as an old one w/o direct access....

	  FILE* f = fopen( FILEN[i].c_str()  , "r+") ;

	  if( f != 0 ){

	    fseek( f ,  -( LCSIO_RANDOMACCESS_SIZE ) , SEEK_END ) ;
	    std::string bla("") ;
	    bla.resize(17) ;

	    int status = fread( &bla[0] , sizeof(char) , 16 , f ) ;

	    //	    std::cout << "  ====== read : [" << bla << "]" << std::endl ;

	    if( !strcmp( bla.c_str() , "LCIORandomAccess")  ){

	      status = fseek( f ,  -(16) , SEEK_CUR ) ;

	      bla = "LCIORandomIGNORE" ;

	      status = fwrite( &bla[0] , 1 , 16 , f ) ;

	      //	      std::cout << "  --------- wrote " << bla << " to file - bytes written " << status << std::endl;
	    }
	  }

	  fclose( f ) ;
	} //---------- end if(force) --------------------------------------------------------------------------------

	lcWrt->open( FILEN[i] , LCIO::WRITE_APPEND )  ;

	lcWrt->close() ;

      }catch(IOException& e){

	std::cout << " io error in file  " << FILEN[i] << " : " << e.what() << std::endl ;
      }
    }

    return 0 ;
}

<|endoftext|>


<commit_before>/* -*- C++ -*-

   This file is part of ThreadWeaver.

   Author: Mirko Boehm
   Copyright: (C) 2005-2014 Mirko Boehm
   Contact: mirko@kde.org
         http://www.kde.org
         http://creative-destruction.me

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Library General Public
   License as published by the Free Software Foundation; either
   version 2 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Library General Public License for more details.

   You should have received a copy of the GNU Library General Public License
   along with this library; see the file COPYING.LIB.  If not, write to
   the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
   Boston, MA 02110-1301, USA.
*/

#include <QtDebug>
#include <QFile>
#include <QFileInfo>
#include <QLocale>

#include <ThreadWeaver/ThreadWeaver>
#include <ThreadWeaver/Exception>

#include "Image.h"
#include "Model.h"

//const int Image::ThumbHeight = 75;
//const int Image::ThumbWidth = 120;
const int Image::ThumbHeight = 60;
const int Image::ThumbWidth = 80;
QReadWriteLock Image::Lock;
int Image::ProcessingOrder;

Image::Image(const QString inputFileName, const QString outputFileName, Model *model, int id)
    : m_inputFileName(inputFileName)
    , m_outputFileName(outputFileName)
    , m_model(model)
    , m_id(id)
{
}

Progress Image::progress() const
{
    return qMakePair(m_progress, Step_NumberOfSteps);
}

QString Image::description() const
{
    QReadLocker l(&Lock);
    return m_description;
}

QString Image::details() const
{
    QReadLocker l(&Lock);
    return m_details;
}

QString Image::details2() const
{
    QReadLocker l(&Lock);
    return m_details2;
}

int Image::processingOrder() const
{
    return m_processingOrder.loadAcquire();
}

const QString Image::inputFileName() const
{
    return m_inputFileName;
}

const QString Image::outputFileName() const
{
    return m_outputFileName;
}

QImage Image::thumbNail() const
{
    QReadLocker r(&Lock);
    return m_thumbnail;
}

void Image::loadFile()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    QFile file(m_inputFileName);
    if (!file.open(QIODevice::ReadOnly)) {
        error(Step_LoadFile, tr("Unable to load input file!"));
    }
    m_imageData = file.readAll();
    QFileInfo fi(file);
    QLocale locale;
    QString details2 = tr("%1kB").arg(locale.toString(fi.size()/1024));
    {
        QWriteLocker l(&Lock);
        m_description = fi.fileName();
        m_details2 = details2;
    }
    announceProgress(Step_LoadFile);
}

void Image::loadImage()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    const bool result = m_image.loadFromData(m_imageData);
    m_imageData.clear();
    if (!result) {
        error(Step_LoadImage, tr("Unable to parse image data!"));
    }
    QString details = tr("%1x%2 pixels")
            .arg(m_image.width())
            .arg(m_image.height());
    {
        QWriteLocker l(&Lock);
        m_details = details;
    }
    announceProgress(Step_LoadImage);
}

void Image::computeThumbNail()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    QImage thumb = m_image.scaled(ThumbWidth, ThumbHeight,  Qt::KeepAspectRatioByExpanding, Qt::SmoothTransformation);
    if (thumb.isNull()) {
        error(Step_ComputeThumbNail, tr("Unable to compute thumbnail!"));
    }
    {   // thumb is implicitly shared, no copy:
        QWriteLocker l(&Lock);
        m_thumbnail = thumb;
    }

    m_image = QImage();
    announceProgress(Step_ComputeThumbNail);
}

void Image::saveThumbNail()
{
    QImage thumb;
    {
        QReadLocker r(&Lock);
        thumb = m_thumbnail;
    }
    if (!thumb.save(m_outputFileName)) {
        error(Step_SaveThumbNail, tr("Unable to save output file!"));
    }
    announceProgress(Step_SaveThumbNail);
}

void Image::announceProgress(Steps step)
{
    m_progress.storeRelease(step);
    if (m_model) {
        m_model->progressChanged();
        m_model->elementChanged(m_id);
    }
}

void Image::error(Image::Steps step, const QString &message)
{
    m_failedStep.store(step);
    announceProgress(Step_Complete);
    throw ThreadWeaver::JobFailed(message);
}
<commit_msg>Use a QImageReader to be able to detect error causes.<commit_after>/* -*- C++ -*-

   This file is part of ThreadWeaver.

   Author: Mirko Boehm
   Copyright: (C) 2005-2014 Mirko Boehm
   Contact: mirko@kde.org
         http://www.kde.org
         http://creative-destruction.me

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Library General Public
   License as published by the Free Software Foundation; either
   version 2 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Library General Public License for more details.

   You should have received a copy of the GNU Library General Public License
   along with this library; see the file COPYING.LIB.  If not, write to
   the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
   Boston, MA 02110-1301, USA.
*/

#include <QtDebug>
#include <QFile>
#include <QFileInfo>
#include <QLocale>
#include <QImageReader>
#include <QBuffer>

#include <ThreadWeaver/ThreadWeaver>
#include <ThreadWeaver/Exception>

#include "Image.h"
#include "Model.h"

//const int Image::ThumbHeight = 75;
//const int Image::ThumbWidth = 120;
const int Image::ThumbHeight = 60;
const int Image::ThumbWidth = 80;
QReadWriteLock Image::Lock;
int Image::ProcessingOrder;

Image::Image(const QString inputFileName, const QString outputFileName, Model *model, int id)
    : m_inputFileName(inputFileName)
    , m_outputFileName(outputFileName)
    , m_model(model)
    , m_id(id)
{
}

Progress Image::progress() const
{
    return qMakePair(m_progress, Step_NumberOfSteps);
}

QString Image::description() const
{
    QReadLocker l(&Lock);
    return m_description;
}

QString Image::details() const
{
    QReadLocker l(&Lock);
    return m_details;
}

QString Image::details2() const
{
    QReadLocker l(&Lock);
    return m_details2;
}

int Image::processingOrder() const
{
    return m_processingOrder.loadAcquire();
}

const QString Image::inputFileName() const
{
    return m_inputFileName;
}

const QString Image::outputFileName() const
{
    return m_outputFileName;
}

QImage Image::thumbNail() const
{
    QReadLocker r(&Lock);
    return m_thumbnail;
}

void Image::loadFile()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    QFile file(m_inputFileName);
    if (!file.open(QIODevice::ReadOnly)) {
        error(Step_LoadFile, tr("Unable to load input file!"));
    }
    m_imageData = file.readAll();
    QFileInfo fi(file);
    QLocale locale;
    QString details2 = tr("%1kB").arg(locale.toString(fi.size()/1024));
    {
        QWriteLocker l(&Lock);
        m_description = fi.fileName();
        m_details2 = details2;
    }
    announceProgress(Step_LoadFile);
}

void Image::loadImage()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    QBuffer in(&m_imageData);
    in.open(QIODevice::ReadOnly);
    QImageReader reader(&in);
    m_image = reader.read();
    m_imageData.clear();
    if (m_image.isNull()) {
        QWriteLocker l(&Lock);
        m_details = tr("%1!").arg(reader.errorString());
        error(Step_LoadImage, m_details);
    }
    QString details = tr("%1x%2 pixels")
            .arg(m_image.width())
            .arg(m_image.height());
    {
        QWriteLocker l(&Lock);
        m_details = details;
    }
    announceProgress(Step_LoadImage);
}

void Image::computeThumbNail()
{
    m_processingOrder.storeRelease(ProcessingOrder++);
    QImage thumb = m_image.scaled(ThumbWidth, ThumbHeight,  Qt::KeepAspectRatioByExpanding, Qt::SmoothTransformation);
    if (thumb.isNull()) {
        error(Step_ComputeThumbNail, tr("Unable to compute thumbnail!"));
    }
    {   // thumb is implicitly shared, no copy:
        QWriteLocker l(&Lock);
        m_thumbnail = thumb;
    }

    m_image = QImage();
    announceProgress(Step_ComputeThumbNail);
}

void Image::saveThumbNail()
{
    QImage thumb;
    {
        QReadLocker r(&Lock);
        thumb = m_thumbnail;
    }
    if (!thumb.save(m_outputFileName)) {
        error(Step_SaveThumbNail, tr("Unable to save output file!"));
    }
    announceProgress(Step_SaveThumbNail);
}

void Image::announceProgress(Steps step)
{
    m_progress.storeRelease(step);
    if (m_model) {
        m_model->progressChanged();
        m_model->elementChanged(m_id);
    }
}

void Image::error(Image::Steps step, const QString &message)
{
    m_failedStep.store(step);
    announceProgress(Step_Complete);
    throw ThreadWeaver::JobFailed(message);
}
<|endoftext|>


<commit_before>#include <tabulate/table.hpp>
using namespace tabulate;

int main() {

  Table logo;

  // Add rows
  logo.add_row({"tabulate for Modern C++"});

  // Format the table
  logo.format()
    .font_style({FontStyle::bold})
    .font_align(FontAlign::center)
    .font_color(Color::yellow);

  // Print the table
  std::cout << logo << std::endl;
}
<commit_msg>Updated logo sample<commit_after>#include <tabulate/table.hpp>
using namespace tabulate;

int main() {

  Table header;
  header.add_row({"tabulate", "for Modern C++"});

  // Format the table
  header[0][0].format()
    .font_style({FontStyle::bold, FontStyle::italic})
    .font_align(FontAlign::center)
    .font_color(Color::yellow)
    .border("")
    .corner("")
    .padding_left(17)
    .column_separator("");

  header[0][1].format()
    .padding(0)
    .border("")
    .corner("")
    .column_separator("")
    .font_color(Color::green)
    .font_style({FontStyle::bold});

  std::cout << header << std::endl;

  Table tagline;
  tagline.format()
    .border("")
    .corner("")
    .column_separator("")
    .color(Color::red)
    .font_style({FontStyle::bold});
  tagline.add_row({"tabulate", " is a library for printing aligned and formatted tabes"});
  tagline[0][0].format()
    .padding(0)
    .font_style({FontStyle::italic, FontStyle::underline});
  tagline[0][1].format()
    .padding_left(0);

  std::cout << tagline << std::endl;

  Table logo;

  logo.format()
    .font_style({FontStyle::bold});

  // Add rows
  logo.add_row({"Header-only Library"});
  logo.add_row({"Requires C++17"});
  logo.add_row({"MIT License"});

  logo[1].format()
    .font_color(Color::white)
    .font_background_color(Color::grey)
    .border_left_background_color(Color::grey)
    .border_right_background_color(Color::grey);

  // Print the table
  std::cout << logo << std::endl;
}
<|endoftext|>


<commit_before>/*
 * This file is open source software, licensed to you under the terms
 * of the Apache License, Version 2.0 (the "License").  See the NOTICE file
 * distributed with this work for additional information regarding copyright
 * ownership.  You may not use this file except in compliance with the License.
 *
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */
/*
 * Copyright 2014 Cloudius Systems
 */

#pragma once

#include "future.hh"
#include <experimental/optional>
#include <exception>

namespace seastar {

/// \addtogroup fiber-module
/// @{

namespace stdx = std::experimental;

/// Exception thrown when a \ref gate object has been closed
/// by the \ref gate::close() method.
class gate_closed_exception : public std::exception {
public:
    virtual const char* what() const noexcept override {
        return "gate closed";
    }
};

/// Facility to stop new requests, and to tell when existing requests are done.
///
/// When stopping a service that serves asynchronous requests, we are faced with
/// two problems: preventing new requests from coming in, and knowing when existing
/// requests have completed.  The \c gate class provides a solution.
class gate {
    size_t _count = 0;
    stdx::optional<promise<>> _stopped;
public:
    /// Registers an in-progress request.
    ///
    /// If the gate is not closed, the request is registered.  Otherwise,
    /// a \ref gate_closed_exception is thrown.
    void enter() {
        if (_stopped) {
            throw gate_closed_exception();
        }
        ++_count;
    }
    /// Unregisters an in-progress request.
    ///
    /// If the gate is closed, and there are no more in-progress requests,
    /// the \ref closed() promise will be fulfilled.
    void leave() {
        --_count;
        if (!_count && _stopped) {
            _stopped->set_value();
        }
    }
    /// Potentially stop an in-progress request.
    ///
    /// If the gate is already closed, a \ref gate_closed_exception is thrown.
    /// By using \ref enter() and \ref leave(), the program can ensure that
    /// no further requests are serviced. However, long-running requests may
    /// continue to run. The check() method allows such a long operation to
    /// voluntarily stop itself after the gate is closed, by making calls to
    /// check() in appropriate places. check() with throw an exception and
    /// bail out of the long-running code if the gate is closed.
    void check() {
        if (_stopped) {
            throw gate_closed_exception();
        }
    }
    /// Closes the gate.
    ///
    /// Future calls to \ref enter() will fail with an exception, and when
    /// all current requests call \ref leave(), the returned future will be
    /// made ready.
    future<> close() {
        assert(!_stopped && "seastar::gate::close() cannot be called more than once");
        _stopped = stdx::make_optional(promise<>());
        if (!_count) {
            _stopped->set_value();
        }
        return _stopped->get_future();
    }
};

/// Executes the function \c func making sure the gate \c g is properly entered
/// and later on, properly left.
///
/// \param func function to be executed
/// \param g the gate. Caller must make sure that it outlives this function.
/// \returns whatever \c func returns
///
/// \relates gate
template <typename Func>
inline
auto
with_gate(gate& g, Func&& func) {
    g.enter();
    return func().finally([&g] { g.leave(); });
}
/// @}

}
<commit_msg>core::gate: add a get_count() method<commit_after>/*
 * This file is open source software, licensed to you under the terms
 * of the Apache License, Version 2.0 (the "License").  See the NOTICE file
 * distributed with this work for additional information regarding copyright
 * ownership.  You may not use this file except in compliance with the License.
 *
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */
/*
 * Copyright 2014 Cloudius Systems
 */

#pragma once

#include "future.hh"
#include <experimental/optional>
#include <exception>

namespace seastar {

/// \addtogroup fiber-module
/// @{

namespace stdx = std::experimental;

/// Exception thrown when a \ref gate object has been closed
/// by the \ref gate::close() method.
class gate_closed_exception : public std::exception {
public:
    virtual const char* what() const noexcept override {
        return "gate closed";
    }
};

/// Facility to stop new requests, and to tell when existing requests are done.
///
/// When stopping a service that serves asynchronous requests, we are faced with
/// two problems: preventing new requests from coming in, and knowing when existing
/// requests have completed.  The \c gate class provides a solution.
class gate {
    size_t _count = 0;
    stdx::optional<promise<>> _stopped;
public:
    /// Registers an in-progress request.
    ///
    /// If the gate is not closed, the request is registered.  Otherwise,
    /// a \ref gate_closed_exception is thrown.
    void enter() {
        if (_stopped) {
            throw gate_closed_exception();
        }
        ++_count;
    }
    /// Unregisters an in-progress request.
    ///
    /// If the gate is closed, and there are no more in-progress requests,
    /// the \ref closed() promise will be fulfilled.
    void leave() {
        --_count;
        if (!_count && _stopped) {
            _stopped->set_value();
        }
    }
    /// Potentially stop an in-progress request.
    ///
    /// If the gate is already closed, a \ref gate_closed_exception is thrown.
    /// By using \ref enter() and \ref leave(), the program can ensure that
    /// no further requests are serviced. However, long-running requests may
    /// continue to run. The check() method allows such a long operation to
    /// voluntarily stop itself after the gate is closed, by making calls to
    /// check() in appropriate places. check() with throw an exception and
    /// bail out of the long-running code if the gate is closed.
    void check() {
        if (_stopped) {
            throw gate_closed_exception();
        }
    }
    /// Closes the gate.
    ///
    /// Future calls to \ref enter() will fail with an exception, and when
    /// all current requests call \ref leave(), the returned future will be
    /// made ready.
    future<> close() {
        assert(!_stopped && "seastar::gate::close() cannot be called more than once");
        _stopped = stdx::make_optional(promise<>());
        if (!_count) {
            _stopped->set_value();
        }
        return _stopped->get_future();
    }

    /// Returns a current number of registered in-progress requests.
    size_t get_count() const {
        return _count;
    }
};

/// Executes the function \c func making sure the gate \c g is properly entered
/// and later on, properly left.
///
/// \param func function to be executed
/// \param g the gate. Caller must make sure that it outlives this function.
/// \returns whatever \c func returns
///
/// \relates gate
template <typename Func>
inline
auto
with_gate(gate& g, Func&& func) {
    g.enter();
    return func().finally([&g] { g.leave(); });
}
/// @}

}
<|endoftext|>