Simulable.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2024  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;
 
        virtual mrpt::math::TPose3D getRelativePose() const { return getPose(); }
 
        mrpt::math::TPose3D getPoseNoLock() const;
 
        mrpt::math::TTwist2D getTwist() const;
 
        mrpt::math::TVector3D getLinearAcceleration() const;
 
        void setPose(const mrpt::math::TPose3D& p, bool notifyChange = true) const;
 
        virtual void setRelativePose(const mrpt::math::TPose3D& p) { setPose(p); }
 
        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;
 
        bool hadCollision() const;
 
        void resetCollisionFlag();
 
        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() {}
 
        virtual std::optional<float> getElevationAt(
                [[maybe_unused]] const mrpt::math::TPoint2D& worldXY) const
        {
                return std::nullopt;
        }
 
   protected:
        std::string name_;
 
        b2Body* b2dBody_ = nullptr;
 
        bool parseSimulable(const JointXMLnode<>& node, const ParseSimulableParams& p = {});
 
        void internalHandlePublish(const TSimulContext& context);
 
        virtual void notifySimulableSetPose([[maybe_unused]] const mrpt::math::TPose3D& newPose)
        {
                // Default: do nothing
        }
 
   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