Simulable.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 <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 getRefVelocityLocal() const;
 
    [[deprecated("Use getRefVelocityLocal() instead")]] mrpt::math::TTwist2D getVelocityLocal()
        const
    {
        return getRefVelocityLocal();
    }
 
    mrpt::math::TTwist2D getRefVelocityGlobal() const;
 
    mrpt::math::TTwist2D getComVelocityGlobal() const;
 
    mrpt::math::TPose3D getPose() const;
 
    virtual mrpt::math::TPose3D getRelativePose() const { return getPose(); }
 
    mrpt::math::TPose3D getPoseNoLock() 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 setRefVelocityLocal(const mrpt::math::TTwist2D& dq);
 
    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::TTwist2D dq_com_{0, 0, 0};
 
    mrpt::math::TVector3D ddq_lin_{0, 0, 0};
 
    mrpt::math::TPose3D former_q_;  
    mrpt::math::TTwist2D former_dq_{0, 0, 0};  
 
    // ============ 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