VisionRobots.hpp

Go to the documentation of this file.

/* 
 * File:   VisionRobots.h
 * Author: eugene
 *
 * Created on 1 December 2013, 8:02 PM
 */
 
#ifndef VisionRobots_DEFINED
#define VisionRobots_DEFINED
 
#include <SimpleShapes.h>
#include <string>
#include <iostream>
#include <algorithm>
#include <gu_util.h>
#include "wb_robot.h"
 
#define TOPLIMIT 5
#define BOTTOMLIMIT 5
 
namespace guWhiteboard {
class VisionRobots {
private:
    wb_robot topRobots[TOPLIMIT];
    wb_robot bottomRobots[BOTTOMLIMIT];
    size_t _frameNumber;
    uint32_t topCntr;
    uint32_t bottomCntr;
public:
    VisionRobots() : _frameNumber(0), topCntr(0), bottomCntr(0) {
    }
 
    VisionRobots(std::string s) {
        from_string(s);
    }
 
    void from_string(std::string &s)  {
        _frameNumber = 0;
        size_t n = static_cast<size_t>(-5);
        std::string command = "ROBOT";
        std::transform(s.begin(), s.end(), s.begin(), ::toupper);
        while(n!=std::string::npos) {
            n = s.find(command, n+5);
            if (n!=std::string::npos) {
                std::string t = s.substr(n+command.length()+1);
                wb_robot robotInfo;
                VisionCamera cam;
                if(s.substr(n-3, 3).find("TOP") != std::string::npos)
                    cam = Top;
                else
                    cam = Bottom;
    
                std::vector<std::string> com = components_of_string_separated(t, '(');
                GUPoint<int16_t> _topLeft(com.at(2).c_str());
                GUPoint<int16_t> _bottomRight(com.at(3).c_str());
 
                robotInfo.set_topLeft_X(_topLeft.x);
                robotInfo.set_topLeft_Y(_topLeft.y);
 
                robotInfo.set_bottomRight_X(_bottomRight.x);
                robotInfo.set_bottomRight_Y(_bottomRight.y);
 
                setRobot(robotInfo, cam);
            }
        }
    }
    void setRobot(wb_robot robotInfo, VisionCamera camera) {
        if(camera == Top && topCntr < TOPLIMIT) {
            topRobots[topCntr] = robotInfo;
            topRobots[topCntr++].set_visible(true);
        }
        else if( bottomCntr < BOTTOMLIMIT ) {
            bottomRobots[bottomCntr] = robotInfo;
            bottomRobots[bottomCntr++].set_visible(true);
        }
    }
    
    const wb_robot &robots(VisionCamera camera, int idx) const
    {
        if(camera == Top)
        {
            idx = idx >= TOPLIMIT ? TOPLIMIT-1 : idx;
            return topRobots[idx];
        }
        else
        {
            idx = idx >= BOTTOMLIMIT ? BOTTOMLIMIT-1 : idx;
            return bottomRobots[idx];
        }
    }
    
    wb_robot &robots(VisionCamera camera, int idx)
    {
        if(camera == Top)
        {
            idx = idx >= TOPLIMIT ? TOPLIMIT-1 : idx;
            return topRobots[idx];
        }
        else
        {
            idx = idx >= BOTTOMLIMIT ? BOTTOMLIMIT-1 : idx;
            return bottomRobots[idx];
        }
    }
    
    void Reset() {
        topCntr = bottomCntr = 0;
        for( int i = 0; i < TOPLIMIT; i++ )
        {
            topRobots[i].set_visible(false);
        }
        for( int i = 0; i < BOTTOMLIMIT; i++ )
        {
            bottomRobots[i].set_visible(false);
        }
    }
    
    void setFrameNumber(size_t fn) {
        _frameNumber = fn;
    }
    
    size_t frameNumber() const {
        return _frameNumber;
    }
    
    std::string description() {
        std::stringstream result;
        
        for (uint32_t i = 0; i < topCntr; i++) {
            result << "TopRobot:("
            << topRobots[i].topLeft_X() << "," << topRobots[i].topLeft_Y() << ")("
            << topRobots[i].bottomRight_X() << "," << topRobots[i].bottomRight_Y() << ") ";
        }
        for (uint32_t i = 0; i < bottomCntr; i++) {
            result << "BottomRobot:("
            << bottomRobots[i].topLeft_X() << "," << bottomRobots[i].topLeft_Y() << ")("
            << bottomRobots[i].bottomRight_X() << "," << bottomRobots[i].bottomRight_Y() << ") ";
        }
        return result.str();
    }
};
}
 
#endif  /* VisionRobots_DEFINED */