Simulable.h

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                                                           |
  +-------------------------------------------------------------------------+ */

#pragma once

#include <box2d/b2_body.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mvsim/basic_types.h>

#include <shared_mutex>

namespace mvsim
{
class Client;
class World;
class VisualObject;

struct ParseSimulableParams
{
        ParseSimulableParams() = default;

        bool init_pose_mandatory = true;
};

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

        Simulable(World* parent) : simulable_parent_(parent) {}

        virtual void simul_pre_timestep(const TSimulContext& context);

        virtual void simul_post_timestep(const TSimulContext& context);

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

        virtual VisualObject* meAsVisualObject() { return nullptr; }

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

        mrpt::math::TPose3D getPose() const;

        mrpt::math::TPose3D getPoseNoLock() const;

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

        mrpt::math::TVector3D getLinearAcceleration() const;

        void setPose(const mrpt::math::TPose3D& p) const;

        void setTwist(const mrpt::math::TTwist2D& dq) const;

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

        mrpt::poses::CPose3D getCPose3D() const;

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

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

        bool isInCollision() const { return isInCollision_; }

        bool hadCollision() const { return hadCollisionFlag_; }

        void resetCollisionFlag() { hadCollisionFlag_ = false; }

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

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

        World* getSimulableWorldObject() { return simulable_parent_; }
        const World* getSimulableWorldObject() const { return simulable_parent_; }

        virtual void freeOpenGLResources() {}

   protected:
        std::string name_;

        b2Body* b2dBody_ = nullptr;

        bool parseSimulable(
                const JointXMLnode<>& node, const ParseSimulableParams& p = {});

        void internalHandlePublish(const TSimulContext& context);

   private:
        World* simulable_parent_ = nullptr;

        mutable std::shared_mutex q_mtx_;

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

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

        mrpt::math::TVector3D ddq_lin_{0, 0, 0};

        mrpt::math::TPose3D former_q_;  

        // ============ ANIMATION VARIABLES ============
        mrpt::math::TPose3D initial_q_ = mrpt::math::TPose3D::Identity();

        std::optional<mrpt::poses::CPose3DInterpolator> anim_keyframes_path_;

        // ============ END OF ANIMATION VARIABLES ============

        bool isInCollision_ = false;

        bool hadCollisionFlag_ = false;

        std::string publishPoseTopic_;

        std::string publishRelativePoseTopic_;
        std::vector<std::string> publishRelativePoseOfOtherObjects_;

        double publishPosePeriod_ = 100e-3;      
        double publishPoseLastTime_ = 0;
};
}  // namespace mvsim