LaserScannerSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::ArmarXObjects::LaserScannerSimulation
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LaserScannerSimulation.h"
 
#include <chrono>
#include <cmath>
#include <cstddef>
#include <memory>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include "ArmarXCore/core/util/Throttler.h"
#include "ArmarXCore/core/time.h"
 
#include "RobotAPI/components/ArViz/Client/Elements.h"
 
#include "ArVizDrawer.h"
 
 
namespace armarx::laser_scanner_simulation
{
 
    static std::size_t nextMultipleOf(std::size_t value, std::size_t multiple)
    {
        if (multiple == 0)
        {
            return value;
        }
 
        std::size_t remainder = value % multiple;
        if (remainder == 0)
        {
            return value;
        }
 
        return value + multiple - remainder;
    }
 
    static std::vector<std::string> splitProperty(Property<std::string> prop)
    {
        std::string propString(prop.getValue());
        std::vector<std::string> result = Split(propString, ",");
        return result;
    }
 
    LaserScannerSimulation::~LaserScannerSimulation() = default;
 
    GridDimension LaserScannerSimulation::calculateGridDimension(
        const std::vector<VirtualRobot::BoundingBox>& boundingBoxes) const
    {
        simox::AxisAlignedBoundingBox box;
        for (const auto& boundingBox : boundingBoxes)
        {
            box.expand_to(boundingBox.getMin());
            box.expand_to(boundingBox.getMax());
        }
 
        const auto extents = box.extents();
 
        std::size_t gridSizeX = static_cast<std::size_t>(std::ceil(extents.x() / gridCellSize));
        gridSizeX             = nextMultipleOf(gridSizeX, sizeof(std::size_t) * CHAR_BIT);
 
        std::size_t gridSizeY = static_cast<std::size_t>(std::ceil(extents.y() / gridCellSize));
        gridSizeY             = nextMultipleOf(gridSizeY, sizeof(std::size_t) * CHAR_BIT);
 
        return {.originX = box.min_x(),
                .originY = box.min_y(),
                .sizeX   = std::max<std::size_t>(gridSizeX, 1),
                .sizeY   = std::max<std::size_t>(gridSizeY, 1)};
    }
 
    VirtualRobot::SceneObjectSetPtr LaserScannerSimulation::getCollisionObjects(
        const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects) const
    {
        VirtualRobot::SceneObjectSetPtr objects(new VirtualRobot::SceneObjectSet());
 
        for (auto& object : sceneObjects)
        {
            VirtualRobot::Robot* robot = dynamic_cast<VirtualRobot::Robot*>(object.get());
            if (robot == nullptr) // standard scene object
            {
                objects->addSceneObject(object);
            }
            else // passive robot
            {
                for (const auto& rns : robot->getRobotNodeSets())
                {
                    objects->addSceneObjects(rns);
                }
            }
        }
 
        return objects;
    }
 
    GridDimension LaserScannerSimulation::calculateGridDimension(
        const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects) const
    {
        VirtualRobot::SceneObjectSetPtr objectSet(new VirtualRobot::SceneObjectSet());
 
        for (auto& object : sceneObjects)
        {
            VirtualRobot::Robot* robot = dynamic_cast<VirtualRobot::Robot*>(object.get());
            if (robot == nullptr) // standard scene object
            {
                objectSet->addSceneObject(object);
            }
            else // passive robot
            {
                // for (const auto rns : robot->getRobotNodeSets())
                // {
                //     objects->addSceneObjects(rns);
                // }
                objectSet->addSceneObject(object); // This is not working properly
            }
        }
 
        // Create a occupancy grid
        return calculateGridDimension(objectSet);
    }
 
    std::vector<VirtualRobot::BoundingBox>
    LaserScannerSimulation::boundingBoxes(VirtualRobot::SceneObjectSetPtr const& objects) const
    {
        std::vector<VirtualRobot::BoundingBox> objectBoudingBoxes;
 
        for (unsigned int objIndex = 0; objIndex < objects->getSize(); ++objIndex)
        {
            VirtualRobot::SceneObjectPtr object = objects->getSceneObject(objIndex);
 
            VirtualRobot::Robot* robot = dynamic_cast<VirtualRobot::Robot*>(object.get());
            if (robot == nullptr) // standard scene object
            {
                objectBoudingBoxes.push_back(object->getCollisionModel()->getBoundingBox());
            }
            else // passive robot
            {
                for (const auto& collisionModel : robot->getCollisionModels())
                {
                    if (collisionModel->getCollisionModelImplementation()->getPQPModel())
                    {
                        objectBoudingBoxes.push_back(collisionModel->getBoundingBox());
                    }
                }
            }
        }
 
        return objectBoudingBoxes;
    }
 
    GridDimension
    LaserScannerSimulation::calculateGridDimension(VirtualRobot::SceneObjectSetPtr const& objects) const
    {
        if (objects->getSize() == 0)
        {
            return {.originX = 0.0F, .originY = 0.0F, .sizeX = 1, .sizeY = 1};
        }
 
        const auto objectBoundingBoxes = boundingBoxes(objects);
        ARMARX_INFO << objectBoundingBoxes.size() << " bounding boxes";
 
        // arvizDrawer->drawBoundingBoxes(objectBoundingBoxes);
 
        return calculateGridDimension(objectBoundingBoxes);
    }
 
    void LaserScannerSimulation::fillOccupancyGrid(std::vector<VirtualRobot::SceneObjectPtr> const& sceneObjects)
    {
        // initialize grid
        const auto gridDim = calculateGridDimension(sceneObjects);
        grid.init(gridDim.sizeX, gridDim.sizeY, gridDim.originX, gridDim.originY, gridCellSize);
 
        ARMARX_INFO_S << "Creating grid with size (" << gridDim.sizeX << ", " << gridDim.sizeY
                      << ")";
 
        VirtualRobot::CollisionCheckerPtr collisionChecker =
            VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
        VirtualRobot::VisualizationFactoryPtr factory(new VirtualRobot::CoinVisualizationFactory());
 
        // collision checking by sliding an object with the grid step size over the grid map
        float boxSize = 1.1f * gridCellSize;
        VirtualRobot::VisualizationNodePtr boxVisu =
            factory->createBox(boxSize, boxSize, boxSize, VirtualRobot::VisualizationFactory::Color::Green());
        VirtualRobot::CollisionModelPtr boxCollisionModel(
            new VirtualRobot::CollisionModel(boxVisu, "", collisionChecker));
        VirtualRobot::SceneObjectPtr box(
            new VirtualRobot::SceneObject("my_box", boxVisu, boxCollisionModel));
 
        if (not collisionChecker)
        {
            ARMARX_WARNING_S << "No global collision checker found";
            return;
        }
 
        ARMARX_INFO_S << "Filling occupancy grid";
        const auto collisionObjects = getCollisionObjects(sceneObjects);
 
        for (std::size_t indexY = 0; indexY < grid.sizeY; ++indexY)
        {
            for (std::size_t indexX = 0; indexX < grid.sizeX; ++indexX)
            {
                const Eigen::Vector2f pos = grid.getCentralPosition(indexX, indexY);
                const Eigen::Vector3f boxPos(pos.x(), pos.y(), boxPosZ);
                const Eigen::Matrix4f boxPose =
                    Eigen::Affine3f(Eigen::Translation3f(boxPos)).matrix();
                box->setGlobalPose(boxPose);
                // - Check collisions with the other objects
                const bool collision = collisionChecker->checkCollision(box, collisionObjects);
                // - Mark each field as occupied or free
                grid.setOccupied(indexX, indexY, collision);
            }
        }
    }
 
    void LaserScannerSimulation::onInitComponent()
    {
        scanners.clear();
 
        // Necessary to initialize SoDB and Qt, SoQt::init (otherwise the application crashes at startup)
        VirtualRobot::init("SimulatorViewerApp");
        worldVisu = Component::create<ArmarXPhysicsWorldVisualization>(
                        getIceProperties(), getName() + "_PhysicsWorldVisualization");
        getArmarXManager()->addObject(worldVisu);
 
        enableVisualization = getProperty<bool>("visualization.enable").getValue();
        ARMARX_INFO << "Visualization will be " << (enableVisualization ? "enabled" : "disabled");
 
        topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
        ARMARX_INFO << "Reporting on topic \"" << topicName << "\"";
        offeringTopic(topicName);
 
        robotStateName = getProperty<std::string>("RobotStateComponentName").getValue();
        ARMARX_INFO << "Using RobotStateComponent \"" << robotStateName << "\"";
        usingProxy(robotStateName);
 
        debugDrawerName = getProperty<std::string>("DebugDrawerTopicName").getValue();
        ARMARX_INFO << "Using DebugDrawerTopic \"" << debugDrawerName << "\"";
        // No usingProxy for the DebugDrawerTopic? or is it a topic?
 
        topicReplayerDummy = getProperty<bool>("TopicReplayerDummy").getValue();
        updatePeriod       = getProperty<int>("UpdatePeriod").getValue();
        visuUpdateFrequency       = getProperty<int>("VisuUpdateFrequency").getValue();
        gridCellSize       = getProperty<float>("GridCellSize").getValue();
 
        auto framesStrings      = splitProperty(getProperty<std::string>("Frames"));
        auto deviceStrings      = splitProperty(getProperty<std::string>("Devices"));
        auto minAnglesStrings   = splitProperty(getProperty<std::string>("MinAngles"));
        auto maxAnglesStrings   = splitProperty(getProperty<std::string>("MaxAngles"));
        auto stepsStrings       = splitProperty(getProperty<std::string>("Steps"));
        auto noiseStrings       = splitProperty(getProperty<std::string>("NoiseStdDev"));
        std::size_t scannerSize = framesStrings.size();
        if (deviceStrings.size() == 1)
        {
            deviceStrings.resize(scannerSize, deviceStrings.front());
        }
        else if (deviceStrings.size() != scannerSize)
        {
            ARMARX_WARNING << "Unexpected size of property Devices (expected " << scannerSize
                           << " but got " << deviceStrings.size() << ")";
            return;
        }
        if (minAnglesStrings.size() == 1)
        {
            minAnglesStrings.resize(scannerSize, minAnglesStrings.front());
        }
        else if (minAnglesStrings.size() != scannerSize)
        {
            ARMARX_WARNING << "Unexpected size of property MinAngles (expected " << scannerSize
                           << " but got " << minAnglesStrings.size() << ")";
            return;
        }
        if (maxAnglesStrings.size() == 1)
        {
            maxAnglesStrings.resize(scannerSize, maxAnglesStrings.front());
        }
        else if (maxAnglesStrings.size() != scannerSize)
        {
            ARMARX_WARNING << "Unexpected size of property MaxAngles (expected " << scannerSize
                           << " but got " << maxAnglesStrings.size() << ")";
            return;
        }
        if (stepsStrings.size() == 1)
        {
            stepsStrings.resize(scannerSize, stepsStrings.front());
        }
        else if (stepsStrings.size() != scannerSize)
        {
            ARMARX_WARNING << "Unexpected size of property Steps (expected " << scannerSize
                           << " but got " << stepsStrings.size() << ")";
            return;
        }
        if (noiseStrings.size() == 1)
        {
            noiseStrings.resize(scannerSize, noiseStrings.front());
        }
        else if (noiseStrings.size() != scannerSize)
        {
            ARMARX_WARNING << "Unexpected size of property NoiseStdDev (expected " << scannerSize
                           << " but got " << noiseStrings.size() << ")";
            return;
        }
 
        scanners.reserve(scannerSize);
        connectedDevices.clear();
        for (std::size_t i = 0; i < scannerSize; ++i)
        {
            LaserScannerSimUnit scanner;
            scanner.frame = framesStrings[i];
            try
            {
                scanner.minAngle    = std::stof(minAnglesStrings[i]);
                scanner.maxAngle    = std::stof(maxAnglesStrings[i]);
                scanner.noiseStdDev = std::stof(noiseStrings[i]);
                scanner.steps       = std::stoi(stepsStrings[i]);
            }
            catch (std::exception const& ex)
            {
                ARMARX_INFO << "Scanner[" << i << "] Config error: " << ex.what();
                continue;
            }
 
            scanners.push_back(scanner);
 
            LaserScannerInfo info;
            info.device   = topicReplayerDummy ? deviceStrings[i] : scanner.frame;
            info.frame    = scanner.frame;
            info.minAngle = scanner.minAngle;
            info.maxAngle = scanner.maxAngle;
            info.stepSize = (info.maxAngle - info.minAngle) / scanner.steps;
            connectedDevices.push_back(info);
 
            ARMARX_INFO << "Scanner[" << i << "]: " << scanner.frame << ", " << scanner.minAngle
                        << ", " << scanner.maxAngle << ", " << scanner.steps;
        }
    }
 
    void LaserScannerSimulation::onConnectComponent()
    {
        if (topicReplayerDummy)
        {
            ARMARX_INFO << "Fake connect (component is used for topic replay)";
            return;
        }
        topic       = getTopic<LaserScannerUnitListenerPrx>(topicName);
        robotState  = getProxy<RobotStateComponentInterfacePrx>(robotStateName);
        sharedRobot = robotState->getSynchronizedRobot();
        debugDrawer = getTopic<DebugDrawerInterfacePrx>(debugDrawerName);
 
        for (LaserScannerSimUnit& scanner : scanners)
        {
            try
            {
                scanner.frameNode = sharedRobot->getRobotNode(scanner.frame);
            }
            catch (std::exception const& ex)
            {
                ARMARX_WARNING << "Error while querying robot frame: " << scanner.frame << " "
                               << ex.what();
                scanner.frameNode = nullptr;
            }
 
            if (!scanner.frameNode)
            {
                ARMARX_WARNING << "Tried to use a non-existing robot node as frame for the "
                               "laser scanner simulation: "
                               << scanner.frame;
            }
        }
 
        if (task)
        {
            task->stop();
        }
 
        if (enableVisualization)
        {
            arvizDrawer = std::make_unique<ArVizDrawer>(getArvizClient());
        }
 
        // Don't forget to sync the data otherwise there may be no objects
        std::vector<VirtualRobot::SceneObjectPtr> objects;
        while (objects.empty())
        {
            worldVisu->synchronizeVisualizationData();
            objects = worldVisu->getObjects();
            ARMARX_INFO << deactivateSpam(5) << "Got " << objects.size() << " objects from the simulator";
            if (objects.empty())
            {
                ARMARX_VERBOSE << deactivateSpam() << "Could not get any objects from the simulator after syncing";
                TimeUtil::MSSleep(100);
            }
        }
 
        std::vector<VirtualRobot::RobotPtr> robots;
        while (robots.empty())
        {
            worldVisu->synchronizeVisualizationData();
            robots = worldVisu->getRobots();
 
            if (robots.empty())
            {
                ARMARX_VERBOSE << deactivateSpam()
                               << "Could not get any robots from the simulator after syncing";
                TimeUtil::MSSleep(100);
            }
        }
 
        ARMARX_INFO << deactivateSpam(5) << "Got " << robots.size() << " robots from the simulator";
 
        // remove active robots as they might move
        robots.erase(std::remove_if(robots.begin(),
                                    robots.end(),
                                    [](const auto & r) -> bool { return not r->isPassive(); }),
                     robots.end());
 
        ARMARX_INFO << "Got " << objects.size() << " scene objects from the simulator";
 
        std::vector<VirtualRobot::SceneObjectPtr> validObjects;
        for (VirtualRobot::SceneObjectPtr const& o : objects)
        {
            VirtualRobot::CollisionModelPtr cm = o->getCollisionModel();
            if (!cm)
            {
                ARMARX_WARNING << "Scene object with no collision model: " << o->getName();
                continue;
            }
 
            const auto pqpModel = cm->getCollisionModelImplementation()->getPQPModel();
            if (pqpModel)
            {
                validObjects.push_back(o);
            }
            else
            {
                ARMARX_WARNING << "PQP model is not filled: " << o->getName();
            }
        }
 
        // robots consist of multiple collision models
        validObjects.insert(validObjects.end(), robots.begin(), robots.end());
 
        fillOccupancyGrid(validObjects);
 
        if (arvizDrawer)
        {
            arvizDrawer->drawOccupancyGrid(grid, boxPosZ);
        }
 
        visuThrottler = std::make_unique<Throttler>(visuUpdateFrequency);
 
        task = new PeriodicTask<LaserScannerSimulation>(this,
                &LaserScannerSimulation::updateScanData,
                updatePeriod,
                false,
                "LaserScannerSimUpdate");
        task->start();
    }
 
    void LaserScannerSimulation::onDisconnectComponent()
    {
        if (task)
        {
            task->stop();
            task = nullptr;
        }
    }
 
    void LaserScannerSimulation::onExitComponent()
    {
        if (worldVisu)
        {
            getArmarXManager()->removeObjectBlocking(worldVisu);
            worldVisu = nullptr;
        }
    }
 
    armarx::PropertyDefinitionsPtr LaserScannerSimulation::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
                   new LaserScannerSimulationPropertyDefinitions(getConfigIdentifier()));
    }
 
    std::string LaserScannerSimulation::getReportTopicName(const Ice::Current&) const
    {
        return topicName;
    }
 
    LaserScannerInfoSeq LaserScannerSimulation::getConnectedDevices(const Ice::Current&) const
    {
        return connectedDevices;
    }
 
    void LaserScannerSimulation::updateScanData()
    {
        auto startTime = armarx::Clock::Now();
        TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
 
        const bool updateVisu = enableVisualization and visuThrottler->check(TimeUtil::GetTime().toMicroSeconds());
 
        for (LaserScannerSimUnit const& scanner : scanners)
        {
            if (!scanner.frameNode)
            {
                continue;
            }
            PosePtr scannerPoseP        = PosePtr::dynamicCast(scanner.frameNode->getGlobalPose());
            Eigen::Matrix4f scannerPose = scannerPoseP->toEigen();
            Eigen::Vector2f position    = scannerPose.col(3).head<2>();
 
            Eigen::Matrix3f scannerRot = scannerPose.block<3, 3>(0, 0);
            Eigen::Vector2f yWorld(0.0f, 1.0f);
            Eigen::Vector2f yScanner = scannerRot.col(1).head<2>();
            float theta              = acos(yWorld.dot(yScanner));
            if (yScanner.x() >= 0.0f)
            {
                theta = -theta;
            }
 
            float minAngle = scanner.minAngle;
            float maxAngle = scanner.maxAngle;
            int scanSteps  = scanner.steps;
 
            LaserScan scan;
            scan.reserve(scanSteps);
 
            // those scan lines would go through the robot
            // const Interval skipInterval(M_PI_2f32, M_PIf32);
            const Interval skipInterval(0.0, M_PI_2);
 
            std::normal_distribution<float> dist(0.0f, scanner.noiseStdDev);
            for (int i = 0; i < scanSteps; ++i)
            {
                LaserScanStep step;
                step.angle    = minAngle + i * (maxAngle - minAngle) / (scanSteps - 1);
 
                if (skipInterval.contains(step.angle))
                {
                    continue;
                }
 
                const Eigen::Vector2f scanDirGlobal =
                    Eigen::Rotation2Df(step.angle + theta) * Eigen::Vector2f(0.0f, 1.0f);
                const float distance = grid.computeDistance(position, scanDirGlobal);
                if (distance > 0.0f)
                {
                    step.distance = distance + dist(engine);
                    scan.push_back(step);
                }
            }
 
            topic->reportSensorValues(scanner.frame, scanner.frame, scan, now);
 
            if (updateVisu)
            {
                arvizDrawer->prepareScan(scan, scanner.frame, Eigen::Affine3f(scannerPose));
            }
        }
 
        if (updateVisu)
        {
            arvizDrawer->drawScans();
        }
 
        auto endTime     = armarx::Clock::Now();
        auto timeElapsed = endTime - startTime;
 
        ARMARX_INFO << deactivateSpam(10)
                    << "Time to simulate laser scanners: " << timeElapsed.toMilliSecondsDouble()
                    << " ms";
    }
 
} // namespace armarx::laser_scanner_simulation