scene.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas */

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/collision.h>

#include <se2ez/core/robot.h>
#include <se2ez/core/scene.h>

using namespace se2ez;

///
/// Scene
///

Scene::RobotData::RobotData(const RobotPtr &r, const StatePtr &s) : robot(r), state(s)
{
}

Scene::Scene(const RobotPtr &arobot) : arobot_(arobot), mrobot_(std::make_shared<Robot>())
{
    mrobot_->addActiveRobot(arobot);
    mrobot_->compileTree();
}

///
/// Frame / Kinematic Tree Updating
///

void Scene::addRobot(const RobotPtr &robot, const std::string &name)
{
    auto state = std::make_shared<State>(robot);
    addRobot(robot, name, state);
}

void Scene::addRobot(const RobotPtr &robot, const std::string &name, const StatePtr &state)
{
    if (not robots_
                .emplace(std::piecewise_construct,  //
                         std::forward_as_tuple(name), std::forward_as_tuple(robot, state))
                .second)
        SE2EZ_WARN("Robot Named %1 already exists thus it is not added", name);

    dirty = true;
}

void Scene::compileScene()
{
    // Adding all the passive robots
    mrobot_->clear();
    for (const auto &entry : robots_)
        mrobot_->addFixedRobot(entry.second.robot, entry.second.state, entry.first);

    // Adding the active robot
    mrobot_->addActiveRobot(arobot_);

    // Disable all the collisions between passive frames
    for (const auto &r1 : robots_)
        for (const auto &f1 : r1.second.robot->getFrameNames())
            for (const auto &r2 : robots_)
                for (const auto &f2 : r2.second.robot->getFrameNames())
                    mrobot_->getACM()->disable(r1.first + ":" + f1, r2.first + ":" + f2);

    mrobot_->compileTree();
}

void Scene::removeRobot(const std::string &name)
{
    if (robots_.count(name))
        robots_.erase(name);
    else
        SE2EZ_WARN("Robot Named:%1, does not exist in Scene", name);

    dirty = true;
}

void Scene::clear()
{
    robots_.clear();
    dirty = true;
}

const RobotPtr &Scene::getActiveRobot()
{
    return arobot_;
}

const RobotPtr &Scene::getSceneRobot()
{
    if (dirty)
    {
        compileScene();
        dirty = false;
    }

    return mrobot_;
}

const std::string &Scene::getSignature()
{
    return getSceneRobot()->getSignature();
}

const RobotPtr &Scene::getRobot(const std::string &name)
{
    const auto &result = robots_.find(name);
    if (result == robots_.end())
        throw std::invalid_argument(log::format("Invalid robot name %1%!", name));

    return result->second.robot;
}

const std::vector<std::string> Scene::getRobotNames()
{
    std::vector<std::string> names;
    for (const auto &entry : robots_)
        names.emplace_back(entry.first);

    return names;
}

void Scene::setRandom()
{
    for (const auto &entry : robots_)
        entry.second.state->setRandom(entry.second.robot);

    dirty = true;
}