SensorBase.cpp

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2023  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */

#include <mrpt/core/format.h>
#include <mvsim/Sensors/CameraSensor.h>
#include <mvsim/Sensors/DepthCameraSensor.h>
#include <mvsim/Sensors/IMU.h>
#include <mvsim/Sensors/LaserScanner.h>
#include <mvsim/Sensors/Lidar3D.h>
#include <mvsim/VehicleBase.h>
#include <mvsim/World.h>

#include <map>
#include <rapidxml.hpp>
#include <rapidxml_print.hpp>
#include <rapidxml_utils.hpp>
#include <sstream>      // std::stringstream
#include <string>

#include "parse_utils.h"
#include "xml_utils.h"

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
#include <mvsim/mvsim-msgs/GenericObservation.pb.h>
#include <mvsim/mvsim-msgs/ObservationLidar2D.pb.h>
#endif

using namespace mvsim;

TClassFactory_sensors mvsim::classFactory_sensors;

// Explicit registration calls seem to be one (the unique?) way to assure
// registration takes place:
void register_all_sensors()
{
        static bool done = false;
        if (done)
                return;
        else
                done = true;

        REGISTER_SENSOR("laser", LaserScanner)
        REGISTER_SENSOR("rgbd_camera", DepthCameraSensor)
        REGISTER_SENSOR("camera", CameraSensor)
        REGISTER_SENSOR("lidar3d", Lidar3D)
        REGISTER_SENSOR("imu", IMU)
}

static auto gAllSensorsOriginViz = mrpt::opengl::CSetOfObjects::Create();
static auto gAllSensorsFOVViz = mrpt::opengl::CSetOfObjects::Create();
static std::mutex gAllSensorVizMtx;

mrpt::opengl::CSetOfObjects::Ptr SensorBase::GetAllSensorsOriginViz()
{
        auto lck = mrpt::lockHelper(gAllSensorVizMtx);
        return gAllSensorsOriginViz;
}

mrpt::opengl::CSetOfObjects::Ptr SensorBase::GetAllSensorsFOVViz()
{
        auto lck = mrpt::lockHelper(gAllSensorVizMtx);
        return gAllSensorsFOVViz;
}

void SensorBase::RegisterSensorFOVViz(const mrpt::opengl::CSetOfObjects::Ptr& o)
{
        auto lck = mrpt::lockHelper(gAllSensorVizMtx);
        gAllSensorsFOVViz->insert(o);
}
void SensorBase::RegisterSensorOriginViz(
        const mrpt::opengl::CSetOfObjects::Ptr& o)
{
        auto lck = mrpt::lockHelper(gAllSensorVizMtx);
        gAllSensorsOriginViz->insert(o);
}

SensorBase::SensorBase(Simulable& vehicle)
        : VisualObject(vehicle.getSimulableWorldObject()),
          Simulable(vehicle.getSimulableWorldObject()),
          vehicle_(vehicle)
{
}

SensorBase::~SensorBase() = default;

SensorBase::Ptr SensorBase::factory(
        Simulable& parent, const rapidxml::xml_node<char>* root)
{
        register_all_sensors();

        using namespace std;
        using namespace rapidxml;

        if (!root) throw runtime_error("[SensorBase::factory] XML node is nullptr");
        if (0 != strcmp(root->name(), "sensor"))
                throw runtime_error(mrpt::format(
                        "[SensorBase::factory] XML root element is '%s' ('sensor' "
                        "expected)",
                        root->name()));

        // Get "class" attrib:
        const xml_attribute<>* sensor_class = root->first_attribute("class");
        if (!sensor_class || !sensor_class->value())
                throw runtime_error(
                        "[VehicleBase::factory] Missing mandatory attribute 'class' in "
                        "node <sensor>");

        const string sName = string(sensor_class->value());

        // Class factory:
        auto we = classFactory_sensors.create(sName, parent, root);

        if (!we)
                throw runtime_error(mrpt::format(
                        "[SensorBase::factory] Unknown sensor type '%s'", root->name()));

        // parse the optional visual model:
        we->parseVisual(*root);

        return we;
}

bool SensorBase::parseSensorPublish(
        const rapidxml::xml_node<char>* node,
        const std::map<std::string, std::string>& varValues)
{
        MRPT_START

        if (node == nullptr) return false;

        // Parse XML params:
        {
                TParameterDefinitions params;
                params["publish_topic"] = TParamEntry("%s", &publishTopic_);

                parse_xmlnode_children_as_param(*node, params, varValues);
        }

        // Parse the "enabled" attribute:
        {
                bool publishEnabled = true;
                TParameterDefinitions auxPar;
                auxPar["enabled"] = TParamEntry("%bool", &publishEnabled);
                parse_xmlnode_attribs(*node, auxPar, varValues);

                // Reset publish topic if enabled==false
                if (!publishEnabled) publishTopic_.clear();
        }

        return true;
        MRPT_END
}

void SensorBase::reportNewObservation(
        const std::shared_ptr<mrpt::obs::CObservation>& obs,
        const TSimulContext& context)
{
        if (!obs) return;

        // Notify the world:
        world_->dispatchOnObservation(vehicle_, obs);

        // Publish:
#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        if (!publishTopic_.empty())
        {
                mvsim_msgs::GenericObservation msg;
                msg.set_unixtimestamp(mrpt::Clock::toDouble(obs->timestamp));
                msg.set_sourceobjectid(vehicle_.getName());

                std::vector<uint8_t> serializedData;
                mrpt::serialization::ObjectToOctetVector(obs.get(), serializedData);

                msg.set_mrptserializedobservation(
                        serializedData.data(), serializedData.size());

                context.world->commsClient().publishTopic(publishTopic_, msg);
        }
#endif

        // Save to .rawlog:
        if (!save_to_rawlog_.empty())
        {
                if (!rawlog_io_)
                {
                        rawlog_io_ = std::make_shared<mrpt::io::CFileGZOutputStream>(
                                save_to_rawlog_);
                }

                auto arch = mrpt::serialization::archiveFrom(*rawlog_io_);
                arch << *obs;
        }
}

void SensorBase::reportNewObservation_lidar_2d(
        const std::shared_ptr<mrpt::obs::CObservation2DRangeScan>& obs,
        const TSimulContext& context)
{
        using namespace std::string_literals;

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        if (publishTopic_.empty()) return;

        mvsim_msgs::ObservationLidar2D msg;
        msg.set_unixtimestamp(mrpt::Clock::toDouble(obs->timestamp));
        msg.set_sourceobjectid(vehicle_.getName());

        msg.set_aperture(obs->aperture);
        for (size_t i = 0; i < obs->getScanSize(); i++)
        {
                msg.add_scanranges(obs->getScanRange(i));
                msg.add_validranges(obs->getScanRangeValidity(i));
        }

        msg.set_ccw(obs->rightToLeft);
        msg.set_maxrange(obs->maxRange);

        const auto& p = obs->sensorPose;
        auto sp = msg.mutable_sensorpose();
        sp->set_x(p.x());
        sp->set_y(p.y());
        sp->set_z(p.z());
        sp->set_yaw(p.yaw());
        sp->set_pitch(p.pitch());
        sp->set_roll(p.roll());

        context.world->commsClient().publishTopic(publishTopic_ + "_scan"s, msg);
#endif
}

void SensorBase::registerOnServer(mvsim::Client& c)
{
        // Default base stuff:
        Simulable::registerOnServer(c);

#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF)
        // Topic:
        if (!publishTopic_.empty())
                c.advertiseTopic<mvsim_msgs::GenericObservation>(publishTopic_);
#endif
}

void SensorBase::loadConfigFrom(const rapidxml::xml_node<char>* root)
{
        // Attribs:
        TParameterDefinitions attribs;
        attribs["name"] = TParamEntry("%s", &name_);
        parse_xmlnode_attribs(*root, attribs, {}, "[SensorBase]");

        varValues_ = world_->user_defined_variables();
        varValues_["NAME"] = name_;
        varValues_["PARENT_NAME"] = vehicle_.getName();

        // Parse common sensor XML params:
        this->parseSensorPublish(root->first_node("publish"), varValues_);

        TParameterDefinitions params;
        params["sensor_period"] = TParamEntry("%lf", &sensor_period_);
        params["save_to_rawlog"] = TParamEntry("%s", &save_to_rawlog_);

        // Parse XML params:
        parse_xmlnode_children_as_param(*root, params, varValues_);
}

void SensorBase::make_sure_we_have_a_name(const std::string& prefix)
{
        if (!name_.empty()) return;

        size_t nextIdx = 0;
        if (auto v = dynamic_cast<VehicleBase*>(&vehicle_); v)
                nextIdx = v->getSensors().size() + 1;

        name_ = mrpt::format(
                "%s%u", prefix.c_str(), static_cast<unsigned int>(nextIdx));
}

bool SensorBase::should_simulate_sensor(const TSimulContext& context)
{
        // to fix edge cases with sensor period a multiple of simulation timestep:
        const double timeEpsilon = 1e-6;

        if (context.simul_time <
                sensor_last_timestamp_ + sensor_period_ - timeEpsilon)
                return false;

        if ((context.simul_time - sensor_last_timestamp_) >= 2 * sensor_period_)
        {
                std::cout
                        << "[mvsim::SensorBase] WARNING: "
                           "At least one sensor sample has been lost due to too coarse "
                           "discrete time steps. sensor_period="
                        << sensor_period_ << " [s], (simul_time - sensor_last_timestamp)="
                        << (context.simul_time - sensor_last_timestamp_) << " [s]."
                        << std::endl;
        }

        sensor_last_timestamp_ = context.simul_time;

        return true;
}