CRovio.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/img/TCamera.h>
#include <mrpt/obs/CObservationImage.h>

#include <mrpt/hwdrivers/CGenericSensor.h>

#include <thread>

namespace mrpt::hwdrivers
{
/** A class to interface a Rovio robot (manufactured by WowWee).
 *  Supports: Simple motion commands, video streaming.
 * \ingroup mrpt_hwdrivers_grp
 */
class CRovio
{
   private:
    std::thread m_videoThread;
    bool m_videothread_must_exit;
    bool m_videothread_initialized_done;
    bool m_videothread_initialized_error;
    bool m_videothread_finished;

    mrpt::obs::CObservationImage::Ptr buffer_img;
    std::mutex buffer_img_cs;

    /** This function takes a frame and waits until getLastImage ask for it, and
     * so on.
     */
    void thread_video();

    bool send_cmd_action(int act, int speed);

    bool path_management(int act);

    bool path_management(int act, const std::string& path_name);

    bool general_command(int act, std::string& response, std::string& errormsg);

   public:
    struct TOptions
    {
        std::string IP;
        std::string user;
        std::string password;

        mrpt::img::TCamera cameraParams;  // Mat. cam. preguntar paco

        TOptions();
    } options;

    enum status
    {
        idle,
        driving_home,
        docking,
        executing_path,
        recording_path
    };

    struct TRovioState
    {
        status state;
        unsigned int nss;  // Navigation Signal Strength
        unsigned int wss;  // Wifi Signal Strength
    };

    struct TEncoders
    {
        int left;
        int right;
        int rear;
        TEncoders()
        {
            left = 0;
            right = 0;
            rear = 0;
        }
    } encoders;

    /** Establish Connection with Rovio and log in its system: Important, fill
     * out "options" members *BEFORE* calling this method.
     *  \exception std::runtime On errors
     */
    void initialize();  // string &errormsg_out, std::string
    // url_out="150.214.109.134", std::string
    // user_out="admin", std::string
    // password_out="investigacion");

    /** move send Rovio the command to move in the specified direcction
     * \param direction 'f'->forward, 'b'->backward, 'r'->right, 'l'->left
     * \return False on error
     */
    bool move(char direction, int speed = 5);

    /** rotate send Rovio the command to rotate in the specified direcction
     * 'r'->right, 'l'->left
     * \return False on error
     */
    bool rotate(char direction, int speed = 5);

    /**  Head positions
     * \return False on error
     */
    bool takeHeadUp();
    bool takeHeadMiddle();
    bool takeHeadDown();

    /*  Path commands */
    bool pathRecord();
    bool pathRecordAbort();
    bool pathRecordSave(const std::string& path_name);  // Repasar const
    bool pathDelete(const std::string& path_name);
    /** Get list of saved paths
     */
    bool pathGetList(std::string& path_list);
    bool pathRunForward();
    bool pathRunBackward();
    bool pathRunStop();
    bool pathRunPause();
    bool pathRename(const std::string& old_name, const std::string& new_name);

    /** goHome(bool dock) drives Rovio in front of charging station if the
     * paremeter dock is set to false, otherwise it also docks
     * \return False on error
     */
    bool goHome(bool dock, int speed = 5);

    /** Loads the rovio camera calibration parameters (of leave the default ones
     * if not found) (See CGenericSensor), then call to
     * "loadConfig_sensorSpecific"
     *  \exception This method throws an exception with a descriptive message
     * if some critical parameter is missing or has an invalid value.
     */
    void loadConfig(
        const mrpt::config::CConfigFileBase& configSource,
        const std::string& section);

    /** This function launchs a thread with the function "thread_video()" which
     * gets frames into a buffer.
     * After calling this method, images can be obtained with
     * getNextImageSync()
     * \return False on error
     * \sa getNextImageSync
     */
    bool retrieve_video();  // como la protejo para que no se llame dos
    // veces??????????????????????????????????????????????

    /** This function stops and joins the thread launched by "retrieve_video()".
     * \return False on error
     */
    bool stop_video();

    /** Returns the next frame from Rovio's live video stream, after starting
     * the live streaming with retrieve_video()
     * \return False on error
     * \sa retrieve_video, captureImageAsync
     */
    bool getNextImageSync(mrpt::obs::CObservationImage::Ptr& lastImage);

    /** Returns a snapshot from Rovio, if rectified is set true, the returned
     * image is rectified with the parameters of intrinsic_matrix and
     * distortion_matrix.
     * This function works asynchronously and does not need to have enabled the
     * live video streaming.
     * \return False on error
     * \sa captureImageSync
     */
    bool captureImageAsync(
        mrpt::img::CImage& out_img, bool recttified);  // string pict_name,

    /** Return true if video is streaming correctly \sa retrieve_video */
    bool isVideoStreamming() const;

    // Rovio State
    /** Returns a TRovioState with internal information of Rovio (State,
     * Navigation Signal Strength, Wifi Signal Strength)
     * \return False on error
     */
    bool getRovioState(TRovioState& state);

    /** Returns a TEncoders with information of Rovio encoders (since last read,
     * it seems Rovio is continuously reading with unknown sample time)
     * \return False on error
     */
    bool getEncoders(TEncoders& encoders);

    /** Returns the Rovio's pose
     * \return False on error
     */
    bool getPosition(mrpt::math::TPose2D& out_pose);

    CRovio();
    virtual ~CRovio();

};  // End of class

}  // namespace mrpt::hwdrivers