Simulable.h

Go to the documentation of this file.

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

#pragma once

#include <Box2D/Dynamics/b2Body.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mvsim/basic_types.h>

#include <shared_mutex>

namespace mvsim
{
class Client;

class Simulable
{
   public:
        using Ptr = std::shared_ptr<Simulable>;

        virtual void simul_pre_timestep(const TSimulContext& context);

        virtual void simul_post_timestep(const TSimulContext& context);

        virtual void poses_mutex_lock() = 0;
        virtual void poses_mutex_unlock() = 0;

        virtual void apply_force(
                const mrpt::math::TVector2D& force,
                const mrpt::math::TPoint2D& applyPoint = mrpt::math::TPoint2D(0, 0));

        mrpt::math::TTwist2D getVelocityLocal() const;

        mrpt::math::TPose3D getPose() const
        {
                m_q_mtx.lock_shared();
                mrpt::math::TPose3D ret = m_q;
                m_q_mtx.unlock_shared();
                return ret;
        }

        mrpt::math::TTwist2D getTwist() const
        {
                m_q_mtx.lock_shared();
                mrpt::math::TTwist2D ret = m_dq;
                m_q_mtx.unlock_shared();
                return ret;
        }

        void setPose(const mrpt::math::TPose3D& p) const
        {
                m_q_mtx.lock();
                const_cast<mrpt::math::TPose3D&>(m_q) = p;
                m_q_mtx.unlock();
        }

        void setTwist(const mrpt::math::TTwist2D& dq) const
        {
                m_q_mtx.lock();
                const_cast<mrpt::math::TTwist2D&>(m_dq) = dq;
                m_q_mtx.unlock();
        }

        mrpt::poses::CPose2D getCPose2D() const;

        const mrpt::math::TTwist2D& getVelocity() const { return m_dq; }

        const std::string& getName() const { return m_name; }

        void setName(const std::string& s) { m_name = s; }

        bool isInCollision() const { return m_isInCollision; }

        bool hadCollision() const { return m_hadCollisionFlag; }

        void resetCollisionFlag() { m_hadCollisionFlag = false; }

        virtual void registerOnServer(mvsim::Client& c);

        const b2Body* b2d_body() const { return m_b2d_body; }
        b2Body* b2d_body() { return m_b2d_body; }

   protected:
        std::string m_name;

        b2Body* m_b2d_body = nullptr;

        bool parseSimulable(const rapidxml::xml_node<char>* node);

        void internalHandlePublish(const TSimulContext& context);

   private:
        mutable std::shared_mutex m_q_mtx;

        mrpt::math::TPose3D m_q = mrpt::math::TPose3D::Identity();

        mrpt::math::TTwist2D m_dq{0, 0, 0};

        bool m_isInCollision = false;

        bool m_hadCollisionFlag = false;

        std::string publishPoseTopic_;
        double publishPosePeriod_ = 100e-3;      
        double publishPoseLastTime_ = 0;
};
}  // namespace mvsim