ElmoControllerMac.cpp

#include <boost/make_shared.hpp>
#include <boost/process.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/shared_array.hpp>
#include "logging.h"
#include "BrowserPlugin.h"
#include <json/json.h>
#include "ImageFileLoader.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem.hpp>

#include "ElmoControllerMac.h"

namespace bp = ::boost::process;
using std::getline;
using std::string;
using std::map;
using std::vector;

using boost::filesystem::path;
using boost::filesystem::exists;

using namespace boost::posix_time;

int ElmoControllerMac::refCount(0);

const int wait_milliseconds = 100;

namespace {
    map<long, string> DeviceTypes;
    map<string, long> DeviceStrings;
}

class NoDataException : public std::runtime_error {
public:
    NoDataException(const string& what_arg) : std::runtime_error(what_arg) {}
};

ElmoControllerMac::ElmoControllerMac(const ImageHandlerPtr& imageHandler) : CameraController(imageHandler), m_cameraListInit(false) {
    FBLOG_INFO("ElmoControllerMac", "Initializing Elmo controller. refCount: " << refCount);
    if (refCount++ == 0) {
    }
    
    path pluginPath (FB::BrowserPlugin::getFSPath());
    path ElmoPath = pluginPath.parent_path() / "ElmoShim.app" / "Contents" / "MacOS" / "ElmoShim";
    if (!exists(ElmoPath)) {
        FBLOG_WARN("ElmoControllerMac", "ELMO SDK not found; Elmo Support not present");
        m_cameraListInit = true;
        return;
    } else {
        ElmoShimPath = ElmoPath.string();
    }
    
    m_processThread = boost::thread(&ElmoControllerMac::runThread, this);
    
    boost::mutex::scoped_lock _l(m_mutex);
    
    while (!m_cameraListInit) {
        // We need to wait for the camera detection to finish (or fail) before continuing
        boost::posix_time::time_duration wait_duration = boost::posix_time::milliseconds(10);
        m_signal.timed_wait(_l, wait_duration);
    }
}

ElmoControllerMacPtr ElmoControllerMac::create(const ImageHandlerPtr& imageHandler) {
    return boost::make_shared<ElmoControllerMac>(imageHandler);
}

ElmoControllerMac::~ElmoControllerMac() {
    if (--refCount == 0) {
    }
    sendMessage(ElmoMessage_Shutdown);
    m_processThread.join();
}

std::string getLine(bp::pistream& is) {
    string resp;
    int i = 0;
    while (resp.empty()) {
        // The client will never intentionally send an empty string; if it did, there is something wrong.
        std::getline(is, resp, '\n');
        if (++i == 100) {
            // Timeout; this will never happen unless we've lost communications with the pipe
            FBLOG_ERROR("ElmoControllerMac", "Gave up polling data from the process after " << i << " attempts");
            throw NoDataException("Lost communications with the process");
        }
    }
    return resp;
}

std::string sendCommand(bp::postream& os, bp::pistream& is, const std::string& command, const std::string& arg = std::string()) {
    Json::Value root(Json::objectValue);
    root["command"] = command;
    Json::Value argsDoc(Json::arrayValue);
    if (!arg.empty()) { argsDoc.append(arg); }
    root["args"] = argsDoc;
    Json::FastWriter writer;
    std::string out = writer.write(root);
    //FBLOG_INFO("ElmoControllerMac", "Sending to ElmoShim: " << out);
    os << out;
    os.flush();
    return getLine(is);
}

void ElmoControllerMac::processImage(boost::process::pistream& is, const std::string& rep)
{
    //FBLOG_INFO("ElmoControllerMac", "Image frame detected");
    Json::Value root;
    Json::Reader rdr;
    if (rdr.parse(rep, root)) {
        if (root["message"] != "Success") {
            FBLOG_INFO("ElmoControllerMac", "Error getting frame from \"" << root["camera"].asString() << "\" (" << rep << ")");
            return;
        }
        // Valid json
        long size = root["size"].asInt();
        //FBLOG_INFO("ElmoControllerMac", "Received frame, " << size << " byte jpeg.");
        boost::shared_array<uint8_t> block(new uint8_t[size]);
        is.read((char *)block.get(), size);
        
        bool gray(false);
        int nwidth(640);
        int nheight(480);
        
        boost::shared_array<uint8_t> rgbData = ImageFileLoader::get()->loadImageData(block.get(), size, nwidth, nheight, gray);
        imageCallbackRGB24(rgbData, nwidth, nheight, false, true);
    }
}

void ElmoControllerMac::runThread() {
    FBLOG_INFO("ElmoControllerMac", "Starting the ElmoShim");
    bp::context ctx;
    ctx.environment = bp::self::get_environment();
    ctx.stdout_behavior = bp::capture_stream();
    ctx.stdin_behavior = bp::capture_stream();
    ctx.stderr_behavior = bp::silence_stream();
    ctx.work_directory = path(ElmoShimPath).parent_path().string();
    vector<string> args = boost::assign::list_of("ElmoShim");
    bp::child c = bp::launch(ElmoShimPath, args, ctx);
    bp::pistream& is = c.get_stdout();
    bp::postream& os = c.get_stdin();
    FBLOG_INFO("ElmoControllerMac", "Started the ElmoShim");
    
    try {
        // First get a list of available cameras, which the
        // binary will output first thing
        boost::mutex::scoped_lock _l(m_mutex);
        m_cameraList.clear();
        string cameraList;
        std::getline(is, cameraList, '\n');
        FBLOG_WARN("ElmoControllerMac", "Camera List: " << cameraList);
        Json::Value list;
        Json::Reader rdr;
        if (rdr.parse(cameraList, list)) {
            for (int i = 0; i < list.size(); ++i) {
                m_cameraList.push_back(list[i].asString() );
            }
        } else {
            FBLOG_WARN("ElmoControllerMac", "Invalid JSON camera list: " << cameraList);
        }
        m_cameraListInit = true;
        m_signal.notify_all();
    } catch (...) {
        // If we got here, something seriously wrong went down; abort!
        m_cameraListInit = true;
        boost::mutex::scoped_lock _l(m_mutex);
        m_signal.notify_all();
        return;
    }
    
    // Begin the loop
    bool running = true;
    bool connected = false;
    string log;
    string current_camera;
    try {
        while (running) {
            ptime start(microsec_clock::local_time());
            ElmoMessageType inMsg;
            while (m_messageQ.try_pop(inMsg)) {
                switch(inMsg) {
                    case ElmoMessage_Shutdown:
                        FBLOG_WARN("ElmoControllerMac", "Shutdown requested");
                        running = false;
                        connected = false;
                        break;
                    case ElmoMessage_Connect: {
                        FBLOG_WARN("ElmoControllerMac", "Connect requested");
                        boost::mutex::scoped_lock _l(m_mutex);
                        current_camera = m_currentCamera;
                        FBLOG_WARN("ElmoControllerMac", "Connected to " << current_camera);
                        connected = true;
                    } break;
                    case ElmoMessage_Disconnect: {
                        FBLOG_WARN("ElmoControllerMac", "Disconnect requested");
                        current_camera = string();
                        connected = false;
                    }
                    default:
                        break;
                }
            };
            
            if (running && connected) {
                // Camera is connected
                log = sendCommand(os, is, "GetImage", current_camera );
                //FBLOG_WARN("ElmoControllerMac", "GetImage response: " << log);
                processImage(is, log);
            }

            // We want this to run not more than every [wait_milliseconds] ms
            // However, sometimes the draw may take that long -- or even longer
            // and we don't want to artificially make it slower
            ptime stop(microsec_clock::local_time());
            time_duration len(stop-start);
            long wait_time = wait_milliseconds - len.total_milliseconds();
            
            if (wait_time > 0) {
                boost::posix_time::time_duration wait_duration = boost::posix_time::milliseconds(wait_time);
                boost::mutex::scoped_lock _l(m_mutex);
                m_signal.timed_wait(_l, wait_duration);
            }
        }
    } catch (NoDataException &ex) {
        FBLOG_ERROR("ElmoControllerMac", "Lost connection to the ElmoShim: " << ex.what());
        // Break out of the loop, we seem to have lost our connection
    }
    try {
        log = sendCommand(os, is, "Shutdown");
        FBLOG_WARN("ElmoControllerMac", "Shutdown response: " << log);
    } catch (NoDataException &ex) {
        FBLOG_WARN("ElmoControllerMac", "Couldn't send shutdown; ElmoShim not responding");
    }
    
    c.terminate();
    c.wait();
}

vector<string> ElmoControllerMac::getCameraList() {
    return m_cameraList;
}

void ElmoControllerMac::connect(const string& str) {
    m_currentCamera = str;
    if (str.empty() && !m_cameraList.empty()) {
        m_currentCamera = *m_cameraList.begin();
    } else if (m_cameraList.empty()) {
        throw CameraError("No camera detected");
    }
    FBLOG_INFO("ElmoControllerMac", "Requested camera connection: " << str);
    sendMessage(ElmoMessage_Connect);
}

void ElmoControllerMac::disconnect() {
    m_currentCamera = "";
    FBLOG_INFO("ElmoControllerMac", "Requested camera disconnect.");
    sendMessage(ElmoMessage_Disconnect);
}

void ElmoControllerMac:: shutdown() {
    sendMessage(ElmoMessage_Shutdown);
}

ElmoControllerMac.h

#ifndef ELMOCONTROLLERMAC_H
#define ELMOCONTROLLERMAC_H

#include "CameraController.h"
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/process.hpp>
#include "SafeQueue.h"

FB_FORWARD_PTR(ElmoControllerMac);

enum ElmoMessageType {
    ElmoMessage_Redraw = 0,
    ElmoMessage_Shutdown,
    ElmoMessage_Connect,
    ElmoMessage_Disconnect
};

class ElmoControllerMac : public CameraController {
public:
    ElmoControllerMac(const ImageHandlerPtr& imageHandler);
public:
    static ElmoControllerMacPtr create(const ImageHandlerPtr& imageHandler);
    ~ElmoControllerMac();
    std::string getPrefix() { return "ELMO "; }
    virtual std::vector<std::string> getCameraList();
    virtual void connect(const std::string&);
    virtual void disconnect();
    void shutdown();
    
    void runThread();
    
    void processImage(boost::process::pistream& is, const std::string& rep);
    
    void sendMessage(const ElmoMessageType t) {
        m_messageQ.push(t);
        m_signal.notify_one();
    }
    
private:
    boost::thread m_processThread;
    boost::mutex m_mutex;
    boost::condition_variable m_signal;
    static int refCount;
    std::string m_currentCamera;
    std::vector<std::string> m_cameraList;
    bool m_cameraListInit;
    std::string ElmoShimPath;
    FB::SafeQueue<ElmoMessageType> m_messageQ;
};


#endif // ELMOCONTROLLERMAC_H