SensorBase.h

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/opengl/opengl_frwds.h>
#include <mrpt/poses/CPose3D.h>
#include <mvsim/ClassFactory.h>
#include <mvsim/Simulable.h>
#include <mvsim/VisualObject.h>
 
#include <memory>
 
namespace mvsim
{
class Simulable;
 
class SensorBase : public VisualObject, public Simulable
{
   public:
    using Ptr = std::shared_ptr<SensorBase>;
 
    SensorBase(Simulable& vehicle);
    virtual ~SensorBase();
 
    static SensorBase::Ptr factory(Simulable& parent, const rapidxml::xml_node<char>* xml_node);
 
    virtual void loadConfigFrom(const rapidxml::xml_node<char>* root);
 
    void registerOnServer(mvsim::Client& c) override;
 
    virtual void simulateOn3DScene(  //
        [[maybe_unused]] mrpt::opengl::COpenGLScene& gl_scene)
    {
    }
 
    // Get all sensors visuals API:
    static std::shared_ptr<mrpt::opengl::CSetOfObjects> GetAllSensorsOriginViz();
 
    static std::shared_ptr<mrpt::opengl::CSetOfObjects> GetAllSensorsFOVViz();
    static void RegisterSensorFOVViz(const std::shared_ptr<mrpt::opengl::CSetOfObjects>& o);
    static void RegisterSensorOriginViz(const std::shared_ptr<mrpt::opengl::CSetOfObjects>& o);
 
    double sensor_period() const { return sensor_period_; }
 
    Simulable& vehicle() { return vehicle_; }
    const Simulable& vehicle() const { return vehicle_; }
 
   protected:
    bool should_simulate_sensor(const TSimulContext& context);
 
    Simulable& vehicle_;  
 
    World* world() { return vehicle_.getSimulableWorldObject(); }
    const World* world() const { return vehicle_.getSimulableWorldObject(); }
 
    double sensor_period_ = 0.1;
 
    double sensor_last_timestamp_ = 0;
 
    std::string publishTopic_;
 
    std::map<std::string, std::string> varValues_;
 
    bool parseSensorPublish(
        const rapidxml::xml_node<char>* node, const std::map<std::string, std::string>& varValues);
 
    void reportNewObservation(
        const std::shared_ptr<mrpt::obs::CObservation>& obs, const TSimulContext& context);
 
    void reportNewObservation_lidar_2d(
        const std::shared_ptr<mrpt::obs::CObservation2DRangeScan>& obs,
        const TSimulContext& context);
 
    void make_sure_we_have_a_name(const std::string& prefix);
};
 
using TListSensors = std::vector<SensorBase::Ptr>;
 
// Class factory:
using TClassFactory_sensors = ClassFactory<SensorBase, Simulable&, const rapidxml::xml_node<char>*>;
 
extern TClassFactory_sensors classFactory_sensors;
 
#define DECLARES_REGISTER_SENSOR(CLASS_NAME) \
    DECLARES_REGISTER_CLASS2(CLASS_NAME, SensorBase, Simulable&, const rapidxml::xml_node<char>*)
 
#define REGISTER_SENSOR(TEXTUAL_NAME, CLASS_NAME) \
    REGISTER_CLASS2(TClassFactory_sensors, classFactory_sensors, TEXTUAL_NAME, CLASS_NAME)
}  // namespace mvsim