CRovio.cpp

Go to the documentation of this file.

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

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

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

#include <thread>

using namespace mrpt::utils;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
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::utils::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 =
                                mrpt::make_aligned_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 (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
        {
                vector_byte 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)  // Comprobar que las matrices existen y son
                        // correctas********************
                        picture.rectifyImageInPlace(options.cameraParams);
                // picture.saveToFile("0000.jpg");
                // cout<<"Response:\n"<<response<<endl;
                return true;
        }
        catch (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() {}
CRovio::~CRovio()
{
        if (isVideoStreamming()) stop_video();
}