CRovio.cpp

Go to the documentation of this file.

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

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/comms/net_utils.h>
#include <mrpt/hwdrivers/CFFMPEG_InputStream.h>
#include <mrpt/hwdrivers/CRovio.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/obs/CObservationImage.h>

#include <thread>

using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::comms::net;
using namespace std;

CRovio::TOptions::TOptions()
    : IP("150.214.109.134"), user("admin"), password("investigacion")
// We could fill the camera matrices here instead of in initialization
{
    double cam_params[] = {1650.740234375,
                           0,
                           805.33190917968750,
                           0,
                           1640.6497802734375,
                           675.1627197265625,
                           0,
                           0,
                           1};
    cameraParams.intrinsicParams.loadFromArray(cam_params);
    cameraParams.setDistortionParamsFromValues(
        0.12792350351810455, -0.4786585867404937, 0.011172077618539333,
        -0.0037264013662934303);
    // cameraParams.focalLengthMeters =
}

/*-------------------------------------------------
                     INITIALIZE
  -----------------------------------------------*/
void CRovio::initialize()  // string &errormsg, string url_out, string user_out,
// string password_out)
{
    string response, errormsg;
    http_get(
        format("http://%s/rev.cgi?Cmd=nav&action=1", options.IP.c_str()),
        response, errormsg, 80, options.user, options.password);

    if (!response.empty())
        cout << "[CRovio::Initialize] Response:\n" << response << endl;

    if (!errormsg.empty())
        THROW_EXCEPTION_FMT("Error initializing Rovio: %s", errormsg.c_str());
}

/*-------------------------------------------------
        SEND MOVEMENT COMMAND (Manual Drive)
  -----------------------------------------------*/
bool CRovio::send_cmd_action(int direction, int speed)
{
    string response, errormsg;
    string command = format(
        "http://%s/rev.cgi?Cmd=nav&action=18&drive=%i&speed=%i",
        options.IP.c_str(), direction, speed);
    http_get(command, response, errormsg, 80, options.user, options.password);
    return errormsg.empty();
}

/*-------------------------------------------------
                  PATH MANAGEMENT
  -----------------------------------------------*/
bool CRovio::path_management(int act)
{
    string response, errormsg;
    string command;
    command =
        format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
    http_get(command, response, errormsg, 80, options.user, options.password);
    return errormsg.empty();
}

bool CRovio::path_management(int act, const string& path_name)
{
    string response, errormsg;
    string command;
    command = format(
        "http://%s/rev.cgi?Cmd=nav&action=%i&name=%s", options.IP.c_str(), act,
        path_name.c_str());
    http_get(command, response, errormsg, 80, options.user, options.password);
    return errormsg.empty();
}

/*-------------------------------------------------
                  GENERAL COMMAND
  -----------------------------------------------*/
bool CRovio::general_command(int act, string& response, string& errormsg)
{
    string command;
    command =
        format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
    http_get(command, response, errormsg, 80, options.user, options.password);
    return errormsg.empty();
}

/*-------------------------------------------------
                    MOVE ROBOT
  -----------------------------------------------*/
bool CRovio::move(char direction, int speed)
{
    switch (direction)
    {
        case 'f':  // Forward
            return send_cmd_action(1, speed);
        case 'b':  // Backward
            return send_cmd_action(2, speed);
        case 'l':  // Left
            return send_cmd_action(3, speed);
        case 'r':  // Right
            return send_cmd_action(4, speed);
        default:
            cout << "Error in parameter of move()";
            return false;
    }
}

bool CRovio::rotate(char direction, int speed)
{
    switch (direction)
    {
        case 'l':  // Left
            return send_cmd_action(5, speed);
        case 'r':  // Right
            return send_cmd_action(6, speed);
        default:
            cout << "Error in parameter of rotate()";
            return false;
    }
}
/*-------------------------------------------------
                    MOVE HEAD
  -----------------------------------------------*/
bool CRovio::takeHeadUp() { return send_cmd_action(11, 5); }
bool CRovio::takeHeadMiddle() { return send_cmd_action(13, 5); }
bool CRovio::takeHeadDown() { return send_cmd_action(12, 5); }
/*-------------------------------------------------
                  PATH COMMAND
  -----------------------------------------------*/
bool CRovio::pathRecord() { return path_management(2); }
bool CRovio::pathRecordAbort() { return path_management(3); }
bool CRovio::pathRecordSave(const string& path_name)
{
    return path_management(4, path_name);
}
bool CRovio::pathDelete(const string& path_name)
{
    return path_management(5, path_name);
}
bool CRovio::pathGetList(string& path_list)
{
    string error;
    general_command(6, path_list, error);
    return error.empty();
}
bool CRovio::pathRunForward() { return path_management(7); }
bool CRovio::pathRunBackward() { return path_management(8); }
bool CRovio::pathRunStop() { return path_management(9); }
bool CRovio::pathRunPause() { return path_management(10); }
bool CRovio::pathRename(const string& current_name, const string& new_name)
{
    string response, errormsg;
    string command = format(
        "http://%s/rev.cgi?Cmd=nav&action=11&name=%s&newname=%s",
        options.IP.c_str(), current_name.c_str(), new_name.c_str());
    http_get(command, response, errormsg, 80, options.user, options.password);
    return errormsg.empty();
}

/*-------------------------------------------------
                     GO HOME
  -----------------------------------------------*/
bool CRovio::goHome(bool dock, int speed)
{
    if (dock)
        return send_cmd_action(13, speed);
    else
        return send_cmd_action(12, speed);
}

/*-------------------------------------------------
                  CAMERA FUNCTIONS
  -----------------------------------------------*/
void CRovio::loadConfig(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& section)
{
    options.cameraParams.loadFromConfigFile(section, configSource);

    // Any other params??
}
/*-------------------------------------------------
                  VIDEO STREAMING
  -----------------------------------------------*/
void CRovio::thread_video()  // This function takes a frame and waits until
// getLastImage() ask for it, and so on.
{
    try
    {
        // obj -> this
        CFFMPEG_InputStream in_video;
        string video_url = format("rtsp://%s/webcam", options.IP.c_str());

        const bool open_ok = in_video.openURL(
            video_url, false /*grayscale*/, false /* verbose */);

        m_videothread_initialized_error = !open_ok;
        m_videothread_initialized_done = true;

        if (m_videothread_initialized_error)
        {
            std::cerr << "[CRovio] Error opening video stream: " << video_url
                      << std::endl;
            return;  // Error!
        }

        while (!m_videothread_must_exit)
        {
            CObservationImage::Ptr obs = std::make_shared<CObservationImage>();

            if (in_video.retrieveFrame(obs->image))
            {
                obs->cameraParams = options.cameraParams;

                // Critical section
                {
                    std::lock_guard<std::mutex> cs(this->buffer_img_cs);
                    this->buffer_img = obs;
                    // cout<<"[CRovio::threadVideo] Image grabbed\n";
                }
            }
            else
            {
                // obs.clear(); //If no image was copied, destroy the thisect.
                cout << "[CRovio::thread_video] Warning: the program doesn't "
                        "receive any image\n";
            }
            std::this_thread::sleep_for(10ms);
        }  // end while

        in_video.close();

        m_videothread_finished = true;
    }
    catch (const std::exception& e)  // que hace eactamente esto?
    {
        m_videothread_initialized_done = true;  // Just in case...
        m_videothread_finished = true;
        cout << "Error in thread_video thread";
        cerr << e.what() << endl;
    }
    catch (...)
    {
        m_videothread_initialized_done = true;  // Just in case...
        m_videothread_finished = true;
        cout << "Error in thread_video thread";
    }
}

bool CRovio::retrieve_video()
{
    if (m_videoThread.get_id() == std::thread::id())
    {
        m_videothread_initialized_done = false;
        m_videothread_initialized_error = false;
        m_videothread_must_exit = false;
        m_videothread_finished = false;

        m_videoThread =
            std::thread(&mrpt::hwdrivers::CRovio::thread_video, this);

        while (!m_videothread_initialized_done)
        {
            std::this_thread::sleep_for(10ms);
        }

        // Ok or error?
        if (m_videothread_initialized_error)
        {
            return false;
        }
        else
            return true;  // Grabbing video
    }
    else
        return true;
}

bool CRovio::isVideoStreamming() const
{
    return (m_videoThread.joinable() && !m_videothread_finished);
}

bool CRovio::stop_video()
{
    bool was_already_stop = true;
    m_videothread_must_exit = true;
    if (isVideoStreamming())
    {
        m_videoThread.join();
        was_already_stop = false;
    }

    return !was_already_stop;
}

bool CRovio::getNextImageSync(
    CObservationImage::Ptr&
        lastImage)  // This function grabbes the images taken by thread_video
{
    if (!isVideoStreamming()) return false;

    {
        std::lock_guard<std::mutex> cs(buffer_img_cs);
        if (!buffer_img) return false;

        lastImage = buffer_img;
    }

    return true;
}

/*-------------------------------------------------
                  CAPTURE PICTURE
  -----------------------------------------------*/
bool CRovio::captureImageAsync(CImage& picture, bool rectified)
{
    try
    {
        std::vector<uint8_t> resp;
        string errormsg;
        string MF =
            format("http://%s/Jpeg/CamImg[0000].jpg", options.IP.c_str());
        http_get(MF, resp, errormsg, 80, options.user, options.password);

        CMemoryStream stream(&resp[0], resp.size());
        picture.loadFromStreamAsJPEG(stream);
        if (rectified) picture.undistort(picture, options.cameraParams);
        return true;
    }
    catch (const std::exception& e)
    {
        cerr << e.what() << endl;
        return false;
    }
}

/*-------------------------------------------------
                     STATE
  -----------------------------------------------*/
bool CRovio::getRovioState(CRovio::TRovioState& status)
{
    MRPT_UNUSED_PARAM(status);
    size_t x_pos, /*y_pos, theta_pos,*/ lenght;
    string x_value, response, errormsg;
    mrpt::math::TPose2D pose;
    general_command(1, response, errormsg);  // Get report from Rovio to
    // response

    // Getting x value
    x_pos = response.find("x=");
    x_value = response.substr((x_pos + 2), 8);
    lenght = x_value.find('|');
    x_value = x_value.substr(0, lenght);
    char* x_char = new char[lenght];
    strcpy(x_char, x_value.c_str());
    pose.x = atof(x_char);

    string error;
    string state;
    general_command(1, state, error);

    return error.empty();
}

/*-------------------------------------------------
                     GET ENCODERS
  -----------------------------------------------*/
long convertToLong(char* sLong)
{
    char* result = strpbrk(sLong, "-0123456789ABCDEF");
    char* stop;
    return strtol(result, &stop, 16);
}

bool CRovio::getEncoders(CRovio::TEncoders& encoders)  // Revisar esto
{
    MRPT_UNUSED_PARAM(encoders);
    string resp, error, field;
    // string field_name[12]={"Packet length","Not Used","Left Wheel:Dir
    // Rotation","Left Wheel:Ticks","Right Wheel:Dir Rotation","Right
    // Wheel:Ticks","Rear Wheel:Dir Rotation","Rear Wheel:Ticks","Not
    // used","Head Position","Batery","Config Status"};
    size_t length;
    int* a_enc = new int[11];  // 12 encoder's fields
    long l_value;

    general_command(20, resp, error);  // get Encoders string to resp
    if (error.empty())
    {
        size_t pos = (resp.find("responses =") + 12);
        for (int i = 0; i <= 11; i++)
        {
            if ((i == 3) || (i == 5) || (i == 7))
                length = 4;
            else
                length = 2;

            field = resp.substr(pos, length);
            pos += length;

            /*---------- String to binary conv------------------*/
            char* cstr = new char[field.size() + 1];
            strcpy(cstr, field.c_str());
            l_value = convertToLong(cstr);

            if ((i == 2) || (i == 4) ||
                (i == 6))  // just interested in bit(2)-> "0000X0"
                l_value = ((l_value >> 1) & 0x1);  // This extracts the last but
            // one bit of l_value which
            // sustitutes l_value
            a_enc[i] = l_value;
        }
        // Upload the encoders value
        if (a_enc[2])
            this->encoders.left += a_enc[3];  // Esta esto bien asi? o deberia
        // usar encoders como parametro de
        // entrada
        else
            this->encoders.left -= a_enc[3];
        if (a_enc[4])
            this->encoders.left += a_enc[5];
        else
            this->encoders.left -= a_enc[5];
        if (a_enc[6])
            this->encoders.left += a_enc[7];
        else
            this->encoders.left -= a_enc[7];

        return true;
    }
    else
    {
        cout << "\n---------------------------------------------------" << endl;
        cout << "ERROR->" << error << endl;

        return false;
    }
}

/*-------------------------------------------------
        GET POSITION WITH NORTHSTAR SYSTEM
  -----------------------------------------------*/
bool CRovio::getPosition(mrpt::math::TPose2D& pose)
{
    size_t x_pos, y_pos, theta_pos, lenght;
    string x_value, y_value, theta_value, response, errormsg;
    general_command(1, response, errormsg);  // Get report from Rovio to
    // response

    // Getting x value
    x_pos = response.find("x=");
    x_value = response.substr((x_pos + 2), 8);
    lenght = x_value.find('|');
    x_value = x_value.substr(0, lenght);
    char* x_char = new char[lenght];
    strcpy(x_char, x_value.c_str());
    pose.x = atof(x_char);

    // Getting y value
    y_pos = response.find("y=");
    y_value = response.substr((y_pos + 2), 8);
    lenght = y_value.find('|');
    y_value = y_value.substr(0, lenght);
    char* y_char = new char[lenght];
    strcpy(y_char, y_value.c_str());
    pose.y = atof(y_char);

    // Getting theta value
    theta_pos = response.find("theta=");
    theta_value = response.substr((theta_pos + 6), 8);
    lenght = theta_value.find('|');
    theta_value = theta_value.substr(0, lenght);
    char* theta_char = new char[lenght];
    strcpy(theta_char, theta_value.c_str());
    pose.phi = atof(theta_char);

    return errormsg.empty();
}

CRovio::CRovio() = default;
CRovio::~CRovio()
{
    if (isVideoStreamming()) stop_video();
}