CSkeletonTracker.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/comms/CSerialPort.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/obs/CObservationSkeleton.h>
#include <mrpt/poses/CPose3D.h>

namespace mrpt::hwdrivers
{
// clang-format off
/** A class for grabbing mrpt::obs::CObservationSkeleton from a PrimeSense
  *camera.
  *  It connects to a PrimeSense camera and tries to detect users while
  *recording the positions of their skeletons' joints along time.
  *
  *  See also the application "rawlog-grabber" for a ready-to-use application to
  *gather data from this sensor.
  *
  *  \code
  *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
  * -------------------------------------------------------
  *   [supplied_section_name]
  *    driver           = CSkeletonTracker
  *    sensorLabel      = <label>           ; Label of the sensor
  *    grab_decimation  = 1                 ; [int] Grab skeletons in 1 out of 'grab_decimation' frames.
  *    show_preview     = 1                 ; [bool] {0,1} Opens a display window to show the recorded skeleton.
  *    pose_x           = 0                 ; [double] Sensor 3D position relative to the robot (meters)
  *    pose_y           = 0
  *    pose_z           = 0
  *    pose_yaw         = 0                 ; [double] Angles in degrees
  *    pose_pitch       = 0
  *    pose_roll        = 0
  *
  *  \endcode
  * \ingroup mrpt_hwdrivers_grp
  */  // clang-format on
class CSkeletonTracker : public hwdrivers::CGenericSensor
{
    enum JOINT
    {
        HEAD = 0,
        NECK,
        TORSO,
        LEFT_SHOULDER,
        LEFT_ELBOW,
        LEFT_HAND,
        LEFT_HIP,
        LEFT_KNEE,
        LEFT_FOOT,
        RIGHT_SHOULDER,
        RIGHT_ELBOW,
        RIGHT_HAND,
        RIGHT_HIP,
        RIGHT_KNEE,
        RIGHT_FOOT,
        NONE
    };

#define NUM_JOINTS 15  // number of joints
#define NUM_LINES 14  // number of lines joining joints

    DEFINE_GENERIC_SENSOR(CSkeletonTracker)
   protected:
    /** Opaque pointers to specific NITE data */
    void* /* nite::SkeletonState* */ m_skeletons_ptr{nullptr};
    void* /* nite::userTracker* */ m_userTracker_ptr{nullptr};

    /** Timestamp management */
    uint32_t m_timeStartUI{};
    mrpt::system::TTimeStamp m_timeStartTT;

    /** Sensor pose */
    mrpt::poses::CPose3D m_sensorPose;
    /** Number of detected users */
    int m_nUsers{0};

    /** Preview window management */
    bool m_showPreview{false};
    mrpt::gui::CDisplayWindow3D::Ptr m_win;
    /** Lines between joints */
    std::vector<std::pair<JOINT, JOINT>> m_linesToPlot;
    /** Joint angles when no skeleton has been detected */
    std::vector<double> m_joint_theta;

    /** Timeout counter (for internal use only) */
    unsigned int m_toutCounter{0};

    /** See the class documentation at the top for expected parameters */
    void loadConfig_sensorSpecific(
        const mrpt::config::CConfigFileBase& configSource,
        const std::string& iniSection) override;

    /** Displays real-time info for the captured skeleton */
    void processPreview(const mrpt::obs::CObservationSkeleton::Ptr& obs);
    void processPreviewNone();

   public:
    /** Constructor
     */
    CSkeletonTracker();

    /** Destructor
     */
    ~CSkeletonTracker() override;

    /** This method will be invoked at a minimum rate of "process_rate" (Hz)
     *  \exception This method must throw an exception with a descriptive
     * message if some critical error is found.
     */
    void doProcess() override;

    /** Connects to the PrimeSense camera and prepares it to get skeleton data
     */
    void initialize() override;

    /** Set/unset preview */
    inline void setPreview(const bool setPreview = true)
    {
        m_showPreview = setPreview;
    }
};  // end of class

}  // namespace mrpt::hwdrivers