LaserScannerSimulation.h

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
 */
 
#pragma once
 
#include <cmath>
#include <random>
#include <string>
 
#include <Eigen/Eigen>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include "ArmarXCore/core/util/Throttler.h"
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <ArmarXSimulation/components/ArmarXPhysicsWorldVisualization/ArmarXPhysicsWorldVisualization.h>
 
#include "ArVizDrawer.h"
#include "OccupancyGrid.h"
 
namespace armarx
{
    class Throttler;
}
 
namespace armarx::laser_scanner_simulation
{
 
    /**
     * @class LaserScannerSimulationPropertyDefinitions
     * @brief
     */
    class LaserScannerSimulationPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        LaserScannerSimulationPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
            defineOptionalProperty<std::string>("RobotStateComponentName",
                                                "RobotStateComponent",
                                                "Name of the RobotStateComponent to use.");
            defineOptionalProperty<std::string>(
                "DebugDrawerTopicName", "DebugDrawerUpdates", "Visualize the results here.");
            defineOptionalProperty<std::string>(
                "ReportVisuTopicName",
                "SimulatorVisuUpdates",
                "The topic on which the visualization updates are published.");
 
            defineOptionalProperty<bool>("TopicReplayerDummy",
                                         false,
                                         "Enable to serve the purpose of a topic replayer dummy");
            defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans in ms");
            defineOptionalProperty<int>(
                "VisuUpdateFrequency", 10, "Visualization update frequency (Hz) for laser scans");
            defineOptionalProperty<float>(
                "GridCellSize",
                20.0f,
                "Size of the grid cells used to generate the occupancy map in mm");
            defineOptionalProperty<std::string>(
                "Frames",
                "LaserScanner_1",
                "Name of the frames to attach the sensor to (e.g. Node1,Node2,Node3)");
            defineOptionalProperty<std::string>(
                "Devices",
                "127.0.0.1",
                "Name of the devices to simulate (e.g. Device1,Device2,Device3)");
            defineOptionalProperty<std::string>(
                "MinAngles",
                std::to_string(-M_PI),
                "Minumum angles to be reported in rad (e.g -2.35,-1.27,0)");
            defineOptionalProperty<std::string>(
                "MaxAngles",
                std::to_string(M_PI),
                "Maxiumum angles to be reported in rad (e.g 2.35,1.27,3.14)");
            defineOptionalProperty<std::string>(
                "Steps",
                "1081",
                "Number of single steps (angle, distance) per scan (e.g 1081,360,270)");
            defineOptionalProperty<std::string>(
                "NoiseStdDev",
                "40.0",
                "Noise is added to the distance of single steps (e.g 40,30,20)");
            defineOptionalProperty<bool>(
                "visualization.enable",
                false,
                "If enabled, useful information will be visualized in ArViz");
        }
    };
 
    struct LaserScannerSimUnit
    {
        std::string frame;
        float minAngle;
        float maxAngle;
        float noiseStdDev;
        int steps;
        SharedRobotNodeInterfacePrx frameNode;
    };
 
    struct GridDimension
    {
        float originX;
        float originY;
        std::size_t sizeX;
        std::size_t sizeY;
    };
 
    /**
     * @defgroup Component-LaserScannerSimulation LaserScannerSimulation
     * @ingroup ArmarXSimulation-Components
     * A description of the component LaserScannerSimulation.
     *
     * @class LaserScannerSimulation
     * @ingroup Component-LaserScannerSimulation
     * @brief Brief description of class LaserScannerSimulation.
     *
     * Detailed description of class LaserScannerSimulation.
     */
    class LaserScannerSimulation :
        virtual public armarx::SensorActorUnit,
        virtual public armarx::LaserScannerUnitInterface,
        virtual public armarx::ArVizComponentPluginUser
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "LaserScannerSimulation";
        }
 
        virtual ~LaserScannerSimulation() override;
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        std::string getReportTopicName(const Ice::Current&) const override;
 
        LaserScannerInfoSeq getConnectedDevices(const Ice::Current&) const override;
 
    private:
        void updateScanData();
 
        void fillOccupancyGrid(std::vector<VirtualRobot::SceneObjectPtr> const& sceneObjects);
 
        GridDimension calculateGridDimension(VirtualRobot::SceneObjectSetPtr const& objects) const;
        GridDimension
        calculateGridDimension(const std::vector<VirtualRobot::BoundingBox>& boundingBoxes) const;
        GridDimension
        calculateGridDimension(const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects) const;
 
        VirtualRobot::SceneObjectSetPtr
        getCollisionObjects(const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects) const;
 
        std::vector<VirtualRobot::BoundingBox>
        boundingBoxes(VirtualRobot::SceneObjectSetPtr const& objects) const;
 
        std::string topicName;
        LaserScannerUnitListenerPrx topic;
        std::string robotStateName;
        armarx::RobotStateComponentInterfacePrx robotState;
        armarx::SharedRobotInterfacePrx sharedRobot;
        std::string debugDrawerName;
        armarx::DebugDrawerInterfacePrx debugDrawer;
 
        ArmarXPhysicsWorldVisualizationPtr worldVisu;
 
        bool topicReplayerDummy = false;
        int updatePeriod = 25;
        float gridCellSize = 20.0f;
        int visuUpdateFrequency = 10;
 
        bool enableVisualization{false};
 
        float boxPosZ = 50.0f; // sensor height
 
        std::vector<LaserScannerSimUnit> scanners;
        PeriodicTask<LaserScannerSimulation>::pointer_type task;
 
        OccupancyGrid grid;
        std::mt19937 engine;
 
        LaserScannerInfoSeq connectedDevices;
 
        std::unique_ptr<ArVizDrawer> arvizDrawer;
 
        std::unique_ptr<Throttler> visuThrottler;
    };
 
} // namespace armarx::laser_scanner_simulation