<commit_before>/**
 * Copyright 2017 Harold Bruintjes
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "Transfer.h"
#include "ThreeplConnection.h"
#include "types/ptr_array.h"

#include <libpurple/debug.h>

#define G_STDIO_NO_WRAP_ON_UNIX 1
#include <glib/gstdio.h>
#include <fcntl.h>

#undef G_STDIO_NO_WRAP_ON_UNIX

PrplTransfer::PrplTransfer(ceema::BlobAPI& api, PurpleConnection* gc, const char *who, PurpleXferType xfer_type) :
    m_blobAPI(api)
{
    m_xfer = purple_xfer_new(purple_connection_get_account(gc), xfer_type, who);
    m_xfer->data = this;
    purple_xfer_ref(m_xfer);

    purple_xfer_set_init_fnc(m_xfer, &PrplTransfer::xfer_init_cb);

    purple_xfer_set_start_fnc(m_xfer, &PrplTransfer::xfer_start_cb);
    purple_xfer_set_end_fnc(m_xfer, &PrplTransfer::xfer_end_cb);

    purple_xfer_set_cancel_send_fnc(m_xfer, &PrplTransfer::xfer_cancel_cb);
    purple_xfer_set_cancel_recv_fnc(m_xfer, &PrplTransfer::xfer_cancel_cb);
    purple_xfer_set_request_denied_fnc(m_xfer, &PrplTransfer::xfer_request_denied_cb);

    purple_xfer_set_read_fnc(m_xfer, &PrplTransfer::xfer_read_cb);
    purple_xfer_set_write_fnc(m_xfer, &PrplTransfer::xfer_write_cb);
    purple_xfer_set_ack_fnc(m_xfer, &PrplTransfer::xfer_ack_cb);

    // Take over file handling for encryption/decryption
    m_ops = *purple_xfer_get_ui_ops(m_xfer);
    m_ops.ui_read = &PrplTransfer::ui_read;
    m_ops.ui_write = &PrplTransfer::ui_write;
    m_ops.data_not_sent = &PrplTransfer::data_not_sent;
    m_xfer->ui_ops = &m_ops;

    purple_xfer_ui_ready(m_xfer);
}

PrplTransfer::~PrplTransfer() {
    purple_xfer_unref(m_xfer);
}

gssize PrplTransfer::on_ui_write(const guchar *buffer, gssize size) {
    m_data.insert(m_data.end(), buffer, buffer+size);

    purple_xfer_ui_ready(m_xfer);

    return size;
}

gssize PrplTransfer::on_ui_read(guchar **buffer, gssize size) {
    gssize avail = std::min(size, (gssize)m_data.size());
    *buffer = static_cast<guchar*>(g_malloc(avail));
    std::copy(m_data.begin(), m_data.begin()+avail, *buffer);
    m_data.erase(m_data.begin(), m_data.begin()+avail);

    purple_xfer_ui_ready(m_xfer);

    return avail;
}

void PrplTransfer::on_data_not_sent(const guchar *buffer, gsize size) {
    m_data.insert(m_data.begin(), buffer, buffer+size);
}

PrplUploadTransfer::PrplUploadTransfer(ceema::BlobAPI& api, PurpleConnection* gc, const char *who) :
        PrplTransfer(api, gc, who, PURPLE_XFER_SEND) {
}

PrplUploadTransfer::~PrplUploadTransfer() {
}

ssize_t PrplUploadTransfer::read(unsigned char* buffer, std::size_t expected) {
    // CURL requests data, indicate purple transfer is ready to fill the buffer
    if (m_buffer.empty() && purple_xfer_get_status(xfer()) == PURPLE_XFER_STATUS_STARTED) {
        purple_xfer_prpl_ready(xfer());
    }
    if (m_buffer.empty()) {
        // EOF
        return 0;
    }

    auto copy_size = std::min(expected, m_buffer.size());
    std::copy(m_buffer.begin(), m_buffer.begin()+copy_size, buffer);
    m_buffer.erase(m_buffer.begin(), m_buffer.begin()+copy_size);

    return copy_size;
}

void PrplUploadTransfer::on_xfer_init() {
    // When sending file, create a temporary copy, encrypt it (using mapped files)
    // and point the local filename to it instead
    GError* err = nullptr;

    const char* filename = purple_xfer_get_local_filename(xfer());

    m_blobData.filename = purple_xfer_get_filename(xfer());
    m_blobData.localFilename = filename;

    gint localfile = g_open(filename, O_RDONLY, 0);

    GMappedFile * mapped_localfile = g_mapped_file_new_from_fd(localfile, FALSE, &err);
    const unsigned char* localdata = reinterpret_cast<const unsigned char*>(g_mapped_file_get_contents(mapped_localfile));
    auto localdata_array = ceema::ptr_array<const unsigned char>(localdata, g_mapped_file_get_length(mapped_localfile));

    //TODO: could encrypt into new memory buffer, or reuse the memory mapped file?
    m_data.insert(m_data.begin(), localdata_array.begin(), localdata_array.end());

    bool ok = this->encrypt();
    LOG_DBG("Encrypt: " << ok);

    purple_xfer_set_size(xfer(), m_data.size());

    g_mapped_file_unref(mapped_localfile);

    g_close(localfile, &err);

    purple_xfer_prepare_thumbnail(xfer(), "jpeg");

    gconstpointer thumb;
    gsize thumb_size;
    if ((thumb = purple_xfer_get_thumbnail(xfer(), &thumb_size))) {
        ceema::byte_vector vec;
        vec.insert(vec.begin(), static_cast<unsigned char const *>(thumb), static_cast<unsigned char const *>(thumb) + thumb_size);
        get_thumb(std::move(vec)).next([this](ceema::future<ceema::blob_id> fut) {
            m_blobData.hasThumb = false;
            try {
                m_blobData.thumb = fut.get();
                m_blobData.hasThumb = true;
            } catch(std::exception& e) {
                LOG_DBG("Thumbnail upload exception: " << e.what());
                // Continue, no thumbnail is not critical
            }

            purple_xfer_start(xfer(), -1, NULL, 0);
        });
    } else {
        m_blobData.hasThumb = false;
        // Initiate the transfer immediately
        purple_xfer_start(xfer(), -1, NULL, 0);
    }
}

void PrplUploadTransfer::on_xfer_start() {
    api().upload(this);
}

gssize PrplUploadTransfer::on_xfer_write(const guchar * buffer, size_t size) {
    m_buffer.insert(m_buffer.end(), buffer, buffer + size);
    return size;
}

PrplDownloadTransfer::PrplDownloadTransfer(ceema::BlobAPI& api, ceema::blob_id id, ceema::blob_size size,
                                           PurpleConnection* gc, const char *who) :
        PrplTransfer(api, gc, who, PURPLE_XFER_RECEIVE), m_id(id) {
    purple_xfer_set_size(xfer(), size);
}

PrplDownloadTransfer::~PrplDownloadTransfer() {
}

ssize_t PrplDownloadTransfer::write(const unsigned char* buffer, std::size_t available) {
    if (purple_xfer_get_status(xfer()) != PURPLE_XFER_STATUS_STARTED) {
        // error
        return -1;
    }

    m_buffer.insert(m_buffer.end(), buffer, buffer + available);

    // Buffer has data, inform purple
    purple_xfer_prpl_ready(xfer());

    return available;
}

void PrplDownloadTransfer::on_xfer_start() {
    api().downloadFile(this, m_id);
}

gssize PrplDownloadTransfer::on_xfer_read(guchar ** buffer) {
    size_t size = m_buffer.size();
    purple_debug_info("threepl", "on_xfer_read %lu\n", size);

    *buffer = static_cast<guchar*>(g_malloc(size));
    std::copy(m_buffer.begin(), m_buffer.end(), *buffer);
    m_buffer.clear();

    return size;
}

void PrplDownloadTransfer::on_xfer_end() {
    // When receiving a file, upon completion decrypt it

    FILE* localfile = g_fopen(purple_xfer_get_local_filename(xfer()), "wb");

    bool ok = decrypt();
    LOG_DBG("Decrypt: " << ok);

    ::fwrite(m_data.data(), 1, m_data.size(), localfile);
    ::fclose(localfile);

    PrplTransfer::on_xfer_end();
}

void PrplTransfer::xfer_request_denied_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_request_denied_cb\n");
    // User decided to deny the incoming file
    // Cancel and cleanup transfer data
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_request_denied();
}

void PrplTransfer::xfer_init_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "threepl_transfer_init_cb\n");
    // Prepare the transfer to it can be presented to the user
    // Must be implemented
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_init();
}

void PrplTransfer::xfer_cancel_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_cancel_cb\n");
    // User decided to cancel the transfer
    // Do cleanup of transfer
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_cancel();
}

void PrplTransfer::xfer_start_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_start_cb\n");
    // xfer has been stated, FP's have been initialized etc. Waiting for data
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_start();
}

void PrplTransfer::xfer_end_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_end_cb\n");
    // called on normal ending (purple_xfer_end), when all data has been transmitted
    // Cleanup transfer
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_end();
}

void PrplTransfer::xfer_ack_cb(PurpleXfer *xfer, const guchar * data, size_t size) {
    purple_debug_info("threepl", "xfer_ack_cb\n");
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_ack(data, size);
}

gssize PrplTransfer::xfer_read_cb(guchar ** data, PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_read_cb\n");
    return static_cast<PrplTransfer*>(xfer->data)->on_xfer_read(data);
}

gssize PrplTransfer::xfer_write_cb(const guchar * data, size_t size, PurpleXfer *xfer) {
    purple_debug_error("threepl", "xfer_write_cb %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_xfer_write(data, size);
}

gssize PrplTransfer::ui_write(PurpleXfer *xfer, const guchar *buffer, gssize size) {
    purple_debug_error("threepl", "ui_write %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_ui_write(buffer, size);
}

gssize PrplTransfer::ui_read(PurpleXfer *xfer, guchar **buffer, gssize size) {
    purple_debug_error("threepl", "ui_read %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_ui_read(buffer, size);
}

void PrplTransfer::data_not_sent(PurpleXfer *xfer, const guchar *buffer, gsize size) {
    purple_debug_error("threepl", "data_not_sent %d\n", (int)size);
    static_cast<PrplTransfer*>(xfer->data)->on_data_not_sent(buffer, size);
}<commit_msg>Transfer.cpp: added more debug information while receiving a file<commit_after>/**
 * Copyright 2017 Harold Bruintjes
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "Transfer.h"
#include "ThreeplConnection.h"
#include "types/ptr_array.h"

#include <libpurple/debug.h>

#define G_STDIO_NO_WRAP_ON_UNIX 1
#include <glib/gstdio.h>
#include <fcntl.h>

#undef G_STDIO_NO_WRAP_ON_UNIX

PrplTransfer::PrplTransfer(ceema::BlobAPI& api, PurpleConnection* gc, const char *who, PurpleXferType xfer_type) :
    m_blobAPI(api)
{
    m_xfer = purple_xfer_new(purple_connection_get_account(gc), xfer_type, who);
    m_xfer->data = this;
    purple_xfer_ref(m_xfer);

    purple_xfer_set_init_fnc(m_xfer, &PrplTransfer::xfer_init_cb);

    purple_xfer_set_start_fnc(m_xfer, &PrplTransfer::xfer_start_cb);
    purple_xfer_set_end_fnc(m_xfer, &PrplTransfer::xfer_end_cb);

    purple_xfer_set_cancel_send_fnc(m_xfer, &PrplTransfer::xfer_cancel_cb);
    purple_xfer_set_cancel_recv_fnc(m_xfer, &PrplTransfer::xfer_cancel_cb);
    purple_xfer_set_request_denied_fnc(m_xfer, &PrplTransfer::xfer_request_denied_cb);

    purple_xfer_set_read_fnc(m_xfer, &PrplTransfer::xfer_read_cb);
    purple_xfer_set_write_fnc(m_xfer, &PrplTransfer::xfer_write_cb);
    purple_xfer_set_ack_fnc(m_xfer, &PrplTransfer::xfer_ack_cb);

    // Take over file handling for encryption/decryption
    m_ops = *purple_xfer_get_ui_ops(m_xfer);
    m_ops.ui_read = &PrplTransfer::ui_read;
    m_ops.ui_write = &PrplTransfer::ui_write;
    m_ops.data_not_sent = &PrplTransfer::data_not_sent;
    m_xfer->ui_ops = &m_ops;

    purple_xfer_ui_ready(m_xfer);
}

PrplTransfer::~PrplTransfer() {
    purple_xfer_unref(m_xfer);
}

gssize PrplTransfer::on_ui_write(const guchar *buffer, gssize size) {
    m_data.insert(m_data.end(), buffer, buffer+size);

    purple_xfer_ui_ready(m_xfer);

    return size;
}

gssize PrplTransfer::on_ui_read(guchar **buffer, gssize size) {
    gssize avail = std::min(size, (gssize)m_data.size());
    *buffer = static_cast<guchar*>(g_malloc(avail));
    std::copy(m_data.begin(), m_data.begin()+avail, *buffer);
    m_data.erase(m_data.begin(), m_data.begin()+avail);

    purple_xfer_ui_ready(m_xfer);

    return avail;
}

void PrplTransfer::on_data_not_sent(const guchar *buffer, gsize size) {
    m_data.insert(m_data.begin(), buffer, buffer+size);
}

PrplUploadTransfer::PrplUploadTransfer(ceema::BlobAPI& api, PurpleConnection* gc, const char *who) :
        PrplTransfer(api, gc, who, PURPLE_XFER_SEND) {
}

PrplUploadTransfer::~PrplUploadTransfer() {
}

ssize_t PrplUploadTransfer::read(unsigned char* buffer, std::size_t expected) {
    // CURL requests data, indicate purple transfer is ready to fill the buffer
    if (m_buffer.empty() && purple_xfer_get_status(xfer()) == PURPLE_XFER_STATUS_STARTED) {
        purple_xfer_prpl_ready(xfer());
    }
    if (m_buffer.empty()) {
        // EOF
        return 0;
    }

    auto copy_size = std::min(expected, m_buffer.size());
    std::copy(m_buffer.begin(), m_buffer.begin()+copy_size, buffer);
    m_buffer.erase(m_buffer.begin(), m_buffer.begin()+copy_size);

    return copy_size;
}

void PrplUploadTransfer::on_xfer_init() {
    // When sending file, create a temporary copy, encrypt it (using mapped files)
    // and point the local filename to it instead
    GError* err = nullptr;

    const char* filename = purple_xfer_get_local_filename(xfer());

    m_blobData.filename = purple_xfer_get_filename(xfer());
    m_blobData.localFilename = filename;

    gint localfile = g_open(filename, O_RDONLY, 0);

    GMappedFile * mapped_localfile = g_mapped_file_new_from_fd(localfile, FALSE, &err);
    const unsigned char* localdata = reinterpret_cast<const unsigned char*>(g_mapped_file_get_contents(mapped_localfile));
    auto localdata_array = ceema::ptr_array<const unsigned char>(localdata, g_mapped_file_get_length(mapped_localfile));

    //TODO: could encrypt into new memory buffer, or reuse the memory mapped file?
    m_data.insert(m_data.begin(), localdata_array.begin(), localdata_array.end());

    bool ok = this->encrypt();
    LOG_DBG("Encrypt: " << ok);

    purple_xfer_set_size(xfer(), m_data.size());

    g_mapped_file_unref(mapped_localfile);

    g_close(localfile, &err);

    purple_xfer_prepare_thumbnail(xfer(), "jpeg");

    gconstpointer thumb;
    gsize thumb_size;
    if ((thumb = purple_xfer_get_thumbnail(xfer(), &thumb_size))) {
        ceema::byte_vector vec;
        vec.insert(vec.begin(), static_cast<unsigned char const *>(thumb), static_cast<unsigned char const *>(thumb) + thumb_size);
        get_thumb(std::move(vec)).next([this](ceema::future<ceema::blob_id> fut) {
            m_blobData.hasThumb = false;
            try {
                m_blobData.thumb = fut.get();
                m_blobData.hasThumb = true;
            } catch(std::exception& e) {
                LOG_DBG("Thumbnail upload exception: " << e.what());
                // Continue, no thumbnail is not critical
            }

            purple_xfer_start(xfer(), -1, NULL, 0);
        });
    } else {
        m_blobData.hasThumb = false;
        // Initiate the transfer immediately
        purple_xfer_start(xfer(), -1, NULL, 0);
    }
}

void PrplUploadTransfer::on_xfer_start() {
    api().upload(this);
}

gssize PrplUploadTransfer::on_xfer_write(const guchar * buffer, size_t size) {
    m_buffer.insert(m_buffer.end(), buffer, buffer + size);
    return size;
}

PrplDownloadTransfer::PrplDownloadTransfer(ceema::BlobAPI& api, ceema::blob_id id, ceema::blob_size size,
                                           PurpleConnection* gc, const char *who) :
        PrplTransfer(api, gc, who, PURPLE_XFER_RECEIVE), m_id(id) {
    purple_xfer_set_size(xfer(), size);
}

PrplDownloadTransfer::~PrplDownloadTransfer() {
}

ssize_t PrplDownloadTransfer::write(const unsigned char* buffer, std::size_t available) {
    if (purple_xfer_get_status(xfer()) != PURPLE_XFER_STATUS_STARTED) {
        // error
        return -1;
    }

    m_buffer.insert(m_buffer.end(), buffer, buffer + available);

    // Buffer has data, inform purple
    purple_xfer_prpl_ready(xfer());

    return available;
}

void PrplDownloadTransfer::on_xfer_start() {
    purple_debug_info("threepl", "start download\n");
    api().downloadFile(this, m_id);
}

gssize PrplDownloadTransfer::on_xfer_read(guchar ** buffer) {
    size_t size = m_buffer.size();
    purple_debug_info("threepl", "on_xfer_read %lu\n", size);

    *buffer = static_cast<guchar*>(g_malloc(size));
    std::copy(m_buffer.begin(), m_buffer.end(), *buffer);
    m_buffer.clear();

    return size;
}

void PrplDownloadTransfer::on_xfer_end() {
    // When receiving a file, upon completion decrypt it

    LOG_DBG("Open downloaded file: ");
    FILE* localfile = g_fopen(purple_xfer_get_local_filename(xfer()), "wb");
    LOG_DBG("Open downloaded file: " << (localfile != NULL));

    bool ok = decrypt();
    LOG_DBG("Decrypt: " << ok);

    ::fwrite(m_data.data(), 1, m_data.size(), localfile);
    ::fclose(localfile);

    PrplTransfer::on_xfer_end();
}

void PrplTransfer::xfer_request_denied_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_request_denied_cb\n");
    // User decided to deny the incoming file
    // Cancel and cleanup transfer data
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_request_denied();
}

void PrplTransfer::xfer_init_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "threepl_transfer_init_cb\n");
    // Prepare the transfer to it can be presented to the user
    // Must be implemented
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_init();
}

void PrplTransfer::xfer_cancel_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_cancel_cb\n");
    // User decided to cancel the transfer
    // Do cleanup of transfer
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_cancel();
}

void PrplTransfer::xfer_start_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_start_cb\n");
    // xfer has been stated, FP's have been initialized etc. Waiting for data
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_start();
}

void PrplTransfer::xfer_end_cb(PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_end_cb\n");
    // called on normal ending (purple_xfer_end), when all data has been transmitted
    // Cleanup transfer
    // xfer will unref itself
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_end();
}

void PrplTransfer::xfer_ack_cb(PurpleXfer *xfer, const guchar * data, size_t size) {
    purple_debug_info("threepl", "xfer_ack_cb\n");
    static_cast<PrplTransfer*>(xfer->data)->on_xfer_ack(data, size);
}

gssize PrplTransfer::xfer_read_cb(guchar ** data, PurpleXfer *xfer) {
    purple_debug_info("threepl", "xfer_read_cb\n");
    return static_cast<PrplTransfer*>(xfer->data)->on_xfer_read(data);

    size_t all_read = purple_xfer_get_bytes_sent (xfer);
    purple_debug_info("threepl", "xfer_read_overall: %lu\n", all_read);
}

gssize PrplTransfer::xfer_write_cb(const guchar * data, size_t size, PurpleXfer *xfer) {
    purple_debug_error("threepl", "xfer_write_cb %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_xfer_write(data, size);
}

gssize PrplTransfer::ui_write(PurpleXfer *xfer, const guchar *buffer, gssize size) {
    purple_debug_error("threepl", "ui_write %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_ui_write(buffer, size);
}

gssize PrplTransfer::ui_read(PurpleXfer *xfer, guchar **buffer, gssize size) {
    purple_debug_error("threepl", "ui_read %d\n", (int)size);
    return static_cast<PrplTransfer*>(xfer->data)->on_ui_read(buffer, size);
}

void PrplTransfer::data_not_sent(PurpleXfer *xfer, const guchar *buffer, gsize size) {
    purple_debug_error("threepl", "data_not_sent %d\n", (int)size);
    static_cast<PrplTransfer*>(xfer->data)->on_data_not_sent(buffer, size);
}<|endoftext|>


<commit_before>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_control(int, int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 3000);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 3000);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 49) || (gui_key == 65)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;
    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;
	int local_hr = 0, local_hl = 0; //local variables for pwm control

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {

   		if((high_right != local_hr)||(high_left != local_hl)){
   			local_hr = high_right;
   			local_hl = high_left;
   			pwm_control(local_hr, local_hl);
   		}

		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours

		std::vector<std::vector<cv::Point>> road(1);
		std::vector<cv::Vec4i> hierarchy;

		contour_frame = blur_frame;

		cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
		if(contours.size() > 1){

			std::vector<int> contour_indexes(contours.size());
			// create index vector
			for(unsigned int i = 0; i < contours.size(); i++){
				contour_indexes[i] = i;
			}

			// sorting index vector acording to controur area
			std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

			cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(0, 255 ,0), 2);

			//draw_childrens(cam_frame, get_contour_childrens(contour_indexes[0], hierarchy), cv::Scalar(0, 0, 255));
			//road[0] = contours[get_closest_children(contour_indexes[0], contours, hierarchy)];

			cv::Rect obstacle;
			if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
				cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
				std::cout << cv::Point(obstacle.x + (obstacle.width / 2), obstacle.y + (obstacle.height / 2)) << std::endl;
			}

			//if(get_closest_children(contour_indexes[0], contours, hierarchy) > 0){
			//	cv::rectangle(cam_frame, cv::boundingRect(contours[get_closest_children(contour_indexes[0], contours, hierarchy)]), cv::Scalar(255));
			//}

			cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(255, 0, 0), 2);

			if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
				cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
			}
			//draw_childrens(cam_frame, get_contour_childrens(contour_indexes[1], hierarchy), cv::Scalar(255, 0, 255));
		}
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_control(int h_r, int h_l){
	for(int i = 0; i < 2; i++){
		pwm_right.high();
		pwm_left.high();
		usleep(std::min(h_r, h_l));
		if(h_r > h_l)
			pwm_left.low();
		else
			pwm_right.low();
		usleep(abs(h_r - h_l));
		if(h_r > h_l)
			pwm_right.low();
		else
			pwm_left.low();
		usleep(period - std::max(h_r, h_l));
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<commit_msg>separated pwm control<commit_after>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int static high_right = 600, high_left = 600; //PWM high time in us
int period = 20000; //PWM period

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_right(int);
void pwm_left(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 49) || (gui_key == 65)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;
    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;
	int local_hr = 0, local_hl = 0; //local variables for pwm control

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {

   		if(high_right != local_hr){
   			if(abs(high_righ - local_hr) > 5){
   				local_hr = high_righ;
   				pwm_right(local_hr);
   				}
   			}

   		if(high_left != local_hl){
   			if(abs(high_left - local_hl) > 5){
   				local_hl = high_left;
   				pwm_left(local_hl);
   			}
   		}

		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours

		std::vector<std::vector<cv::Point>> road(1);
		std::vector<cv::Vec4i> hierarchy;

		contour_frame = blur_frame;

		cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
		if(contours.size() > 1){

			std::vector<int> contour_indexes(contours.size());
			// create index vector
			for(unsigned int i = 0; i < contours.size(); i++){
				contour_indexes[i] = i;
			}

			// sorting index vector acording to controur area
			std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

			cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(0, 255 ,0), 2);

			//draw_childrens(cam_frame, get_contour_childrens(contour_indexes[0], hierarchy), cv::Scalar(0, 0, 255));
			//road[0] = contours[get_closest_children(contour_indexes[0], contours, hierarchy)];

			cv::Rect obstacle;
			if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
				cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
				std::cout << cv::Point(obstacle.x + (obstacle.width / 2), obstacle.y + (obstacle.height / 2)) << std::endl;
			}

			//if(get_closest_children(contour_indexes[0], contours, hierarchy) > 0){
			//	cv::rectangle(cam_frame, cv::boundingRect(contours[get_closest_children(contour_indexes[0], contours, hierarchy)]), cv::Scalar(255));
			//}

			cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[0], 20, true);
			cv::drawContours(cam_frame, road, -1, cv::Scalar(255, 0, 0), 2);

			if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
				cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
			}
			//draw_childrens(cam_frame, get_contour_childrens(contour_indexes[1], hierarchy), cv::Scalar(255, 0, 255));
		}
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_right(int h_r){
	pwm_righ.high();
	usleep(h_r);
	pwm_righ.low();
	usleep(period - h_r);
}

void pwm_left(int h_l){
	pwm_left.high();
	usleep(h_l);
	pwm_left.low();
	usleep(period - h_l);
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<|endoftext|>


<commit_before>#ifndef _TILES_STATE_HPP_
#define _TILES_STATE_HPP_

#include <boost/functional/hash.hpp>

#include <cassert>
#include <iostream>

#include "TilesTypes.hpp"

class TilesState15
{
private:
  std::size_t hash_value;
  TileArray tiles;

public:
  TilesState15(const TileArray &tiles)
    : hash_value(compute_hash(tiles))
    , tiles(tiles)
  {
    assert(tiles[get_blank()] == 0);
  }

  inline Tile operator ()(unsigned i, unsigned j) const
  {
    assert(i < 4);
    assert(j < 4);
    return tiles[i * 4 + j];
  }

  inline bool operator ==(const TilesState15 &other) const
  {
    return
      hash_value == other.hash_value &&
      tiles == other.tiles;
  }

  inline bool operator !=(const TilesState15 &other) const
  {
    return !(*this == other);
  }

  inline TileIndex get_blank() const
  {
    return find_blank_index(tiles);
  }

  inline TileIndex get_blank_row() const
  {
    return get_blank() / 4;
  }

  inline TileIndex get_blank_col() const
  {
    return get_blank() % 4;
  }

  inline const TileArray & get_tiles() const
  {
    return tiles;
  }

  inline size_t get_hash_value() const
  {
    return hash_value;
  }

  TilesState15 move_blank_up() const
  {
    assert(get_blank_row() > 0);
    return move_blank(-4);
  }

  TilesState15 move_blank_down() const
  {
    assert(get_blank_row() < 3);
    return move_blank(4);
  }

  TilesState15 move_blank_left() const
  {
    assert(get_blank_col() > 0);
    return move_blank(-1);
  }

  TilesState15 move_blank_right() const
  {
    assert(get_blank_col() < 3);
    return move_blank(1);
  }

  bool valid() const;

private:
  inline TilesState15 move_blank(TileIndex new_blank_offset) const
  {
    TileIndex blank_index = get_blank();
    TileIndex new_blank_index = blank_index + new_blank_offset;
    TilesState15 new_state(*this);
    std::swap(new_state.tiles[blank_index], new_state.tiles[new_blank_index]);
    new_state.hash_value = compute_hash(new_state.tiles);

    return new_state;
  }

  static std::size_t compute_hash(const TileArray &tiles)
  {
    return boost::hash_range(tiles.begin(), tiles.end());
  }

  static TileIndex find_blank_index(const TileArray &tiles)
  {
    for (TileIndex i = 0; i < tiles.size(); i += 1)
      if (tiles[i] == 0)
        return i;

    assert(false);
    return 0;
  }
};

inline std::size_t hash_value(TilesState15 const &tiles)
{
  return tiles.get_hash_value();
}

std::ostream & operator <<(std::ostream &o, const TilesState15 &tiles);

#endif  /* !_TILES_STATE_HPP_ */
<commit_msg>Added preprocessor flag:  CACHE_TILES_HASH_VALUE<commit_after>#ifndef _TILES_STATE_HPP_
#define _TILES_STATE_HPP_

#include <boost/functional/hash.hpp>

#include <cassert>
#include <iostream>

#include "TilesTypes.hpp"

class TilesState15
{
private:
  TileArray tiles;
#ifdef CACHE_TILES_HASH_VALUE
  std::size_t hash_value;
#endif

public:
  TilesState15(const TileArray &tiles)
    : tiles(tiles)
#ifdef CACHE_TILES_HASH_VALUE
    , hash_value(compute_hash(tiles))
#endif
  {
    assert(tiles[get_blank()] == 0);
  }

  inline Tile operator ()(unsigned i, unsigned j) const
  {
    assert(i < 4);
    assert(j < 4);
    return tiles[i * 4 + j];
  }

  inline bool operator ==(const TilesState15 &other) const
  {
    return
#ifdef CACHE_TILES_HASH_VALUE
      hash_value == other.hash_value &&
#endif
      tiles == other.tiles;
  }

  inline bool operator !=(const TilesState15 &other) const
  {
    return !(*this == other);
  }

  inline TileIndex get_blank() const
  {
    return find_blank_index(tiles);
  }

  inline TileIndex get_blank_row() const
  {
    return get_blank() / 4;
  }

  inline TileIndex get_blank_col() const
  {
    return get_blank() % 4;
  }

  inline const TileArray & get_tiles() const
  {
    return tiles;
  }

  inline size_t get_hash_value() const
  {
#ifdef CACHE_TILES_HASH_VALUE
    return hash_value;
#else
    return compute_hash(tiles);
#endif
  }

  TilesState15 move_blank_up() const
  {
    assert(get_blank_row() > 0);
    return move_blank(-4);
  }

  TilesState15 move_blank_down() const
  {
    assert(get_blank_row() < 3);
    return move_blank(4);
  }

  TilesState15 move_blank_left() const
  {
    assert(get_blank_col() > 0);
    return move_blank(-1);
  }

  TilesState15 move_blank_right() const
  {
    assert(get_blank_col() < 3);
    return move_blank(1);
  }

  bool valid() const;

private:
  inline TilesState15 move_blank(TileIndex new_blank_offset) const
  {
    TileIndex blank_index = get_blank();
    TileIndex new_blank_index = blank_index + new_blank_offset;
    TilesState15 new_state(*this);
    std::swap(new_state.tiles[blank_index], new_state.tiles[new_blank_index]);
#ifdef CACHE_TILES_HASH_VALUE
    new_state.hash_value = compute_hash(new_state.tiles);
#endif

    return new_state;
  }

  static std::size_t compute_hash(const TileArray &tiles)
  {
    return boost::hash_range(tiles.begin(), tiles.end());
  }

  static TileIndex find_blank_index(const TileArray &tiles)
  {
    for (TileIndex i = 0; i < tiles.size(); i += 1)
      if (tiles[i] == 0)
        return i;

    assert(false);
    return 0;
  }
};

inline std::size_t hash_value(TilesState15 const &tiles)
{
  return tiles.get_hash_value();
}

std::ostream & operator <<(std::ostream &o, const TilesState15 &tiles);

#endif  /* !_TILES_STATE_HPP_ */
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "interactive_marker.h"

#include "interactive_markers/tools.h"

#include "rviz/frame_manager.h"
#include "rviz/visualization_manager.h"
#include "rviz/selection/selection_manager.h"
#include "rviz/frame_manager.h"
#include "rviz/default_plugin/interactive_marker_display.h"
#include "rviz/render_panel.h"

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreResourceGroupManager.h>
#include <OGRE/OgreSubEntity.h>
#include <OGRE/OgreMath.h>

#include <boost/make_shared.hpp>
#include <wx/menu.h>

#include <ros/ros.h>

namespace rviz
{

InteractiveMarker::InteractiveMarker( InteractiveMarkerDisplay *owner, VisualizationManager *vis_manager, std::string topic_ns, std::string client_id ) :
  owner_(owner)
, vis_manager_(vis_manager)
, pose_changed_(false)
, time_since_last_feedback_(0)
, dragging_(false)
, pose_update_requested_(false)
, heart_beat_t_(0)
, topic_ns_(topic_ns)
, client_id_(client_id)
{
  ros::NodeHandle nh;
  std::string feedback_topic = topic_ns+"/feedback";
  feedback_pub_ = nh.advertise<visualization_msgs::InteractiveMarkerFeedback>( feedback_topic, 100, false );

  reference_node_ = vis_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();

  axes_node_ = reference_node_->createChildSceneNode();
  axes_ = new ogre_tools::Axes( vis_manager->getSceneManager(), axes_node_, 1, 0.05 );
}

InteractiveMarker::~InteractiveMarker()
{
  delete axes_;
  vis_manager_->getSceneManager()->destroySceneNode( axes_node_ );
  vis_manager_->getSceneManager()->destroySceneNode( reference_node_ );
}

void InteractiveMarker::reset()
{
  controls_.clear();
  menu_.reset();
  menu_entries_.clear();
}

void InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerPoseConstPtr message )
{
  Ogre::Vector3 position( message->pose.position.x, message->pose.position.y, message->pose.position.z );
  Ogre::Quaternion orientation( message->pose.orientation.w, message->pose.orientation.x,
      message->pose.orientation.y, message->pose.orientation.z );

  if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
  {
    orientation.w = 1;
  }

  reference_time_ = message->header.stamp;
  reference_frame_ = message->header.frame_id;
  frame_locked_ = (message->header.stamp == ros::Time(0));

  requestPoseUpdate( position, orientation );
}

bool InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerConstPtr message )
{
  reset();

  visualization_msgs::InteractiveMarker auto_message = *message;
  interactive_markers::autoComplete( auto_message );

  // copy values

  name_ = auto_message.name;
  description_ = auto_message.description;

  if ( auto_message.controls.size() == 0 )
  {
    owner_->setStatus( status_levels::Ok, name_, "Marker empty.");
    return false;
  }

  scale_ = auto_message.scale;

  reference_frame_ = auto_message.header.frame_id;
  reference_time_ = auto_message.header.stamp;
  frame_locked_ = (auto_message.header.stamp == ros::Time(0));

  position_ = Ogre::Vector3(
      auto_message.pose.position.x,
      auto_message.pose.position.y,
      auto_message.pose.position.z );

  orientation_ = Ogre::Quaternion(
      auto_message.pose.orientation.w,
      auto_message.pose.orientation.x,
      auto_message.pose.orientation.y,
      auto_message.pose.orientation.z );

  pose_changed_ =false;
  time_since_last_feedback_ = 0;

  // setup axes
  axes_->setPosition(position_);
  axes_->setOrientation(orientation_);
  axes_->set( scale_, scale_*0.05 );

  updateReferencePose();

  for ( unsigned i=0; i<auto_message.controls.size(); i++ )
  {
    controls_.push_back( boost::make_shared<InteractiveMarkerControl>(
        vis_manager_, auto_message.controls[i], reference_node_, this ) );
  }

  description_control_ = boost::make_shared<InteractiveMarkerControl>(
      vis_manager_, interactive_markers::makeTitle( auto_message ), reference_node_, this );
  controls_.push_back( description_control_ );

  //create menu
  if ( message->menu.size() > 0 )
  {
    menu_.reset( new wxMenu() );

    for ( unsigned i=0; i<message->menu.size(); i++ )
    {
      if ( message->menu[i].sub_entries.empty() )
      {
        // make top-level entry
        //ROS_INFO_STREAM("adding "<<menu_id);
        makeMenuEntry( menu_id, menu_, message->menu[i].entry.title );
        menu_entries_.push_back( message->menu[i].entry.command );
        menu_id++;
      }
      else
      {
        // make sub-menu
        wxMenu* sub_menu = new wxMenu();
        for ( unsigned j=0; j<message->menu[i].sub_entries.size(); j++ )
        {
          // make sub-menu entry
          makeMenuEntry( menu_id, sub_menu, message->menu[i].sub_entries[j].title );
          menu_entries_.push_back( message->menu[i].sub_entries[j].command );
          menu_id++;
        }
        sub_menu->Connect(wxEVT_COMMAND_MENU_SELECTED,
            (wxObjectEventFunction)&InteractiveMarker::handleMenuSelect, NULL, this);

        wxString menu_title = wxString::FromAscii(message->menu[i].entry.title.c_str());
        //ROS_INFO_STREAM("adding "<< menu_->AppendSubMenu( sub_menu, menu_title )->GetId() );
      }
    }
    menu_->Connect(wxEVT_COMMAND_MENU_SELECTED,
        (wxObjectEventFunction)&InteractiveMarker::handleMenuSelect, NULL, this);
  }

  owner_->setStatus( status_levels::Ok, name_, "OK");
  return true;
}

void InteractiveMarker::makeMenuEntry( unsigned menu_id, wxMenu* menu, const std::string &entry )
{
  if ( entry.find( "[x]" ) == 0 )
  {
    wxString menu_entry = wxString::FromAscii( entry.substr( 3 ).c_str() );
    menu->AppendCheckItem( menu_id, menu_entry )->Check( true );
  }
  else if ( entry.find( "[ ]" ) == 0 )
  {
    wxString menu_entry = wxString::FromAscii( entry.substr( 3 ).c_str() );
    menu->AppendCheckItem( menu_id, menu_entry )->Check( false );
  }
  else
  {
    wxString menu_entry = wxString::FromAscii( entry.c_str() );
    menu->Append( menu_id, menu_entry );
  }
}

void InteractiveMarker::updateReferencePose()
{
  Ogre::Vector3 reference_position;
  Ogre::Quaternion reference_orientation;

  // if we're frame-locked, we need to find out what the most recent transformation time
  // actually is so we send back correct feedback
  if ( frame_locked_ )
  {
    std::string fixed_frame = FrameManager::instance()->getFixedFrame();
    if ( reference_frame_ == fixed_frame )
    {
      // if the two frames are identical, we don't need to do anything.
      reference_time_ = ros::Time::now();
    }
    else
    {
      std::string error;
      int retval = FrameManager::instance()->getTFClient()->getLatestCommonTime(
          reference_frame_, fixed_frame, reference_time_, &error );
      if ( retval != tf::NO_ERROR )
      {
        std::ostringstream s;
        s <<"Error getting time of latest transform between " << reference_frame_
            << " and " << fixed_frame << ": " << error << " (error code: " << retval << ")";
        owner_->setStatus( status_levels::Error, name_, s.str() );
        reference_node_->setVisible( false );
        return;
      }
    }
  }

  if (!FrameManager::instance()->getTransform( reference_frame_, reference_time_,
      reference_position, reference_orientation ))
  {
    std::string error;
    FrameManager::instance()->transformHasProblems(reference_frame_, reference_time_, error);
    owner_->setStatus( status_levels::Error, name_, error);
    reference_node_->setVisible( false );
    return;
  }

  reference_node_->setPosition( reference_position );
  reference_node_->setOrientation( reference_orientation );
  reference_node_->setVisible( true, false );
}

void InteractiveMarker::update(float wall_dt)
{
  time_since_last_feedback_ += wall_dt;
  if ( frame_locked_ )
  {
    updateReferencePose();
  }
  if ( dragging_ )
  {
    if ( pose_changed_ )
    {
      publishPose();
    }
    else if ( time_since_last_feedback_ > 0.25 )
    {
      //send keep-alive so we don't use control over the marker
      visualization_msgs::InteractiveMarkerFeedback feedback;
      feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
      publishFeedback( feedback );
    }
  }
}

void InteractiveMarker::publishPose()
{
  visualization_msgs::InteractiveMarkerFeedback feedback;
  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
  publishFeedback( feedback );
  pose_changed_ = false;
}

void InteractiveMarker::requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation )
{
  if ( dragging_ )
  {
    pose_update_requested_ = true;
    requested_position_ = position;
    requested_orientation_ = orientation;
  }
  else
  {
    updateReferencePose();
    setPose( position, orientation );
  }
}

void InteractiveMarker::setPose( Ogre::Vector3 position, Ogre::Quaternion orientation )
{
  position_ = position;
  orientation_ = orientation;
  pose_changed_ = true;

  axes_->setPosition(position_);
  axes_->setOrientation(orientation_);

  std::list<InteractiveMarkerControlPtr>::iterator it;
  for ( it = controls_.begin(); it != controls_.end(); it++ )
  {
    (*it)->interactiveMarkerPoseChanged( position_, orientation_ );
  }
}

void InteractiveMarker::setShowDescription( bool show )
{
  if ( description_control_.get() )
  {
    description_control_->setVisible( show );
  }
}

void InteractiveMarker::setShowAxes( bool show )
{
  axes_node_->setVisible( show );
}

void InteractiveMarker::translate( Ogre::Vector3 delta_position )
{
  setPose( position_+delta_position, orientation_ );
}

void InteractiveMarker::rotate( Ogre::Quaternion delta_orientation )
{
  setPose( position_, delta_orientation * orientation_ );
}

void InteractiveMarker::startDragging()
{
  dragging_ = true;
  pose_changed_ = false;
}

void InteractiveMarker::stopDragging()
{
  if ( pose_changed_ )
  {
    publishPose();
  }
  pose_update_requested_ = false;
  dragging_ = false;
  if ( pose_update_requested_ )
  {
    updateReferencePose();
    setPose( requested_position_, requested_orientation_ );
  }
}

bool InteractiveMarker::handleMouseEvent(ViewportMouseEvent& event)
{
//  if (event.event.LeftDown())
//  {
//    visualization_msgs::InteractiveMarkerFeedback feedback;
//    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN;
//    publishFeedback( feedback );
//  }
//  if (event.event.LeftUp())
//  {
//    visualization_msgs::InteractiveMarkerFeedback feedback;
//    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP;
//    publishFeedback( feedback );
//  }

  if ( menu_.get() )
  {
    if ( event.event.RightDown() || event.event.RightIsDown() )
    {
      return true;
    }
    if ( event.event.RightUp() )
    {
      event.panel->PopupMenu( menu_.get(), event.event.GetX(), event.event.GetY() );
      return true;
    }
  }

  return false;
}

void InteractiveMarker::handleMenuSelect(wxCommandEvent &evt)
{
  if ( (unsigned)evt.GetId() < menu_entries_.size() )
  {
    visualization_msgs::InteractiveMarkerFeedback feedback;
    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
    feedback.command = menu_entries_[evt.GetId()];
    publishFeedback( feedback );
  }
}

void InteractiveMarker::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
{
  feedback.client_id = client_id_;
  feedback.marker_name = name_;

  if ( frame_locked_ )
  {
    feedback.header.frame_id = reference_frame_;
    feedback.header.stamp = reference_time_;
    feedback.pose.position.x = position_.x;
    feedback.pose.position.y = position_.y;
    feedback.pose.position.z = position_.z;
    feedback.pose.orientation.x = orientation_.x;
    feedback.pose.orientation.y = orientation_.y;
    feedback.pose.orientation.z = orientation_.z;
    feedback.pose.orientation.w = orientation_.w;
  }
  else
  {
    feedback.header.frame_id = vis_manager_->getFixedFrame();
    feedback.header.stamp = ros::Time::now();

    Ogre::Vector3 world_position = reference_node_->convertLocalToWorldPosition( position_ );
    Ogre::Quaternion world_orientation = reference_node_->convertLocalToWorldOrientation( orientation_ );

    feedback.pose.position.x = world_position.x;
    feedback.pose.position.y = world_position.y;
    feedback.pose.position.z = world_position.z;
    feedback.pose.orientation.x = world_orientation.x;
    feedback.pose.orientation.y = world_orientation.y;
    feedback.pose.orientation.z = world_orientation.z;
    feedback.pose.orientation.w = world_orientation.w;
  }

  feedback_pub_.publish( feedback );

  time_since_last_feedback_ = 0;
}

}
<commit_msg>bugfix to last commit<commit_after>/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "interactive_marker.h"

#include "interactive_markers/tools.h"

#include "rviz/frame_manager.h"
#include "rviz/visualization_manager.h"
#include "rviz/selection/selection_manager.h"
#include "rviz/frame_manager.h"
#include "rviz/default_plugin/interactive_marker_display.h"
#include "rviz/render_panel.h"

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreResourceGroupManager.h>
#include <OGRE/OgreSubEntity.h>
#include <OGRE/OgreMath.h>

#include <boost/make_shared.hpp>
#include <wx/menu.h>

#include <ros/ros.h>

namespace rviz
{

InteractiveMarker::InteractiveMarker( InteractiveMarkerDisplay *owner, VisualizationManager *vis_manager, std::string topic_ns, std::string client_id ) :
  owner_(owner)
, vis_manager_(vis_manager)
, pose_changed_(false)
, time_since_last_feedback_(0)
, dragging_(false)
, pose_update_requested_(false)
, heart_beat_t_(0)
, topic_ns_(topic_ns)
, client_id_(client_id)
{
  ros::NodeHandle nh;
  std::string feedback_topic = topic_ns+"/feedback";
  feedback_pub_ = nh.advertise<visualization_msgs::InteractiveMarkerFeedback>( feedback_topic, 100, false );

  reference_node_ = vis_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();

  axes_node_ = reference_node_->createChildSceneNode();
  axes_ = new ogre_tools::Axes( vis_manager->getSceneManager(), axes_node_, 1, 0.05 );
}

InteractiveMarker::~InteractiveMarker()
{
  delete axes_;
  vis_manager_->getSceneManager()->destroySceneNode( axes_node_ );
  vis_manager_->getSceneManager()->destroySceneNode( reference_node_ );
}

void InteractiveMarker::reset()
{
  controls_.clear();
  menu_.reset();
  menu_entries_.clear();
}

void InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerPoseConstPtr message )
{
  Ogre::Vector3 position( message->pose.position.x, message->pose.position.y, message->pose.position.z );
  Ogre::Quaternion orientation( message->pose.orientation.w, message->pose.orientation.x,
      message->pose.orientation.y, message->pose.orientation.z );

  if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
  {
    orientation.w = 1;
  }

  reference_time_ = message->header.stamp;
  reference_frame_ = message->header.frame_id;
  frame_locked_ = (message->header.stamp == ros::Time(0));

  requestPoseUpdate( position, orientation );
}

bool InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerConstPtr message )
{
  reset();

  visualization_msgs::InteractiveMarker auto_message = *message;
  interactive_markers::autoComplete( auto_message );

  // copy values

  name_ = auto_message.name;
  description_ = auto_message.description;

  if ( auto_message.controls.size() == 0 )
  {
    owner_->setStatus( status_levels::Ok, name_, "Marker empty.");
    return false;
  }

  scale_ = auto_message.scale;

  reference_frame_ = auto_message.header.frame_id;
  reference_time_ = auto_message.header.stamp;
  frame_locked_ = (auto_message.header.stamp == ros::Time(0));

  position_ = Ogre::Vector3(
      auto_message.pose.position.x,
      auto_message.pose.position.y,
      auto_message.pose.position.z );

  orientation_ = Ogre::Quaternion(
      auto_message.pose.orientation.w,
      auto_message.pose.orientation.x,
      auto_message.pose.orientation.y,
      auto_message.pose.orientation.z );

  pose_changed_ =false;
  time_since_last_feedback_ = 0;

  // setup axes
  axes_->setPosition(position_);
  axes_->setOrientation(orientation_);
  axes_->set( scale_, scale_*0.05 );

  updateReferencePose();

  for ( unsigned i=0; i<auto_message.controls.size(); i++ )
  {
    controls_.push_back( boost::make_shared<InteractiveMarkerControl>(
        vis_manager_, auto_message.controls[i], reference_node_, this ) );
  }

  description_control_ = boost::make_shared<InteractiveMarkerControl>(
      vis_manager_, interactive_markers::makeTitle( auto_message ), reference_node_, this );
  controls_.push_back( description_control_ );

  //create menu
  if ( message->menu.size() > 0 )
  {
    unsigned menu_id = 0;
    menu_.reset( new wxMenu() );

    for ( unsigned i=0; i<message->menu.size(); i++ )
    {
      if ( message->menu[i].sub_entries.empty() )
      {
        // make top-level entry
        //ROS_INFO_STREAM("adding "<<menu_id);
        makeMenuEntry( menu_id, menu_, message->menu[i].entry.title );
        menu_entries_.push_back( message->menu[i].entry.command );
        menu_id++;
      }
      else
      {
        // make sub-menu
        wxMenu* sub_menu = new wxMenu();
        for ( unsigned j=0; j<message->menu[i].sub_entries.size(); j++ )
        {
          // make sub-menu entry
          makeMenuEntry( menu_id, sub_menu, message->menu[i].sub_entries[j].title );
          menu_entries_.push_back( message->menu[i].sub_entries[j].command );
          menu_id++;
        }
        sub_menu->Connect(wxEVT_COMMAND_MENU_SELECTED,
            (wxObjectEventFunction)&InteractiveMarker::handleMenuSelect, NULL, this);

        wxString menu_title = wxString::FromAscii(message->menu[i].entry.title.c_str());
        //ROS_INFO_STREAM("adding "<< menu_->AppendSubMenu( sub_menu, menu_title )->GetId() );
      }
    }
    menu_->Connect(wxEVT_COMMAND_MENU_SELECTED,
        (wxObjectEventFunction)&InteractiveMarker::handleMenuSelect, NULL, this);
  }

  owner_->setStatus( status_levels::Ok, name_, "OK");
  return true;
}

void InteractiveMarker::makeMenuEntry( unsigned menu_id, wxMenu* menu, const std::string &entry )
{
  if ( entry.find( "[x]" ) == 0 )
  {
    wxString menu_entry = wxString::FromAscii( entry.substr( 3 ).c_str() );
    menu->AppendCheckItem( menu_id, menu_entry )->Check( true );
  }
  else if ( entry.find( "[ ]" ) == 0 )
  {
    wxString menu_entry = wxString::FromAscii( entry.substr( 3 ).c_str() );
    menu->AppendCheckItem( menu_id, menu_entry )->Check( false );
  }
  else
  {
    wxString menu_entry = wxString::FromAscii( entry.c_str() );
    menu->Append( menu_id, menu_entry );
  }
}

void InteractiveMarker::updateReferencePose()
{
  Ogre::Vector3 reference_position;
  Ogre::Quaternion reference_orientation;

  // if we're frame-locked, we need to find out what the most recent transformation time
  // actually is so we send back correct feedback
  if ( frame_locked_ )
  {
    std::string fixed_frame = FrameManager::instance()->getFixedFrame();
    if ( reference_frame_ == fixed_frame )
    {
      // if the two frames are identical, we don't need to do anything.
      reference_time_ = ros::Time::now();
    }
    else
    {
      std::string error;
      int retval = FrameManager::instance()->getTFClient()->getLatestCommonTime(
          reference_frame_, fixed_frame, reference_time_, &error );
      if ( retval != tf::NO_ERROR )
      {
        std::ostringstream s;
        s <<"Error getting time of latest transform between " << reference_frame_
            << " and " << fixed_frame << ": " << error << " (error code: " << retval << ")";
        owner_->setStatus( status_levels::Error, name_, s.str() );
        reference_node_->setVisible( false );
        return;
      }
    }
  }

  if (!FrameManager::instance()->getTransform( reference_frame_, reference_time_,
      reference_position, reference_orientation ))
  {
    std::string error;
    FrameManager::instance()->transformHasProblems(reference_frame_, reference_time_, error);
    owner_->setStatus( status_levels::Error, name_, error);
    reference_node_->setVisible( false );
    return;
  }

  reference_node_->setPosition( reference_position );
  reference_node_->setOrientation( reference_orientation );
  reference_node_->setVisible( true, false );
}

void InteractiveMarker::update(float wall_dt)
{
  time_since_last_feedback_ += wall_dt;
  if ( frame_locked_ )
  {
    updateReferencePose();
  }
  if ( dragging_ )
  {
    if ( pose_changed_ )
    {
      publishPose();
    }
    else if ( time_since_last_feedback_ > 0.25 )
    {
      //send keep-alive so we don't use control over the marker
      visualization_msgs::InteractiveMarkerFeedback feedback;
      feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
      publishFeedback( feedback );
    }
  }
}

void InteractiveMarker::publishPose()
{
  visualization_msgs::InteractiveMarkerFeedback feedback;
  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
  publishFeedback( feedback );
  pose_changed_ = false;
}

void InteractiveMarker::requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation )
{
  if ( dragging_ )
  {
    pose_update_requested_ = true;
    requested_position_ = position;
    requested_orientation_ = orientation;
  }
  else
  {
    updateReferencePose();
    setPose( position, orientation );
  }
}

void InteractiveMarker::setPose( Ogre::Vector3 position, Ogre::Quaternion orientation )
{
  position_ = position;
  orientation_ = orientation;
  pose_changed_ = true;

  axes_->setPosition(position_);
  axes_->setOrientation(orientation_);

  std::list<InteractiveMarkerControlPtr>::iterator it;
  for ( it = controls_.begin(); it != controls_.end(); it++ )
  {
    (*it)->interactiveMarkerPoseChanged( position_, orientation_ );
  }
}

void InteractiveMarker::setShowDescription( bool show )
{
  if ( description_control_.get() )
  {
    description_control_->setVisible( show );
  }
}

void InteractiveMarker::setShowAxes( bool show )
{
  axes_node_->setVisible( show );
}

void InteractiveMarker::translate( Ogre::Vector3 delta_position )
{
  setPose( position_+delta_position, orientation_ );
}

void InteractiveMarker::rotate( Ogre::Quaternion delta_orientation )
{
  setPose( position_, delta_orientation * orientation_ );
}

void InteractiveMarker::startDragging()
{
  dragging_ = true;
  pose_changed_ = false;
}

void InteractiveMarker::stopDragging()
{
  if ( pose_changed_ )
  {
    publishPose();
  }
  pose_update_requested_ = false;
  dragging_ = false;
  if ( pose_update_requested_ )
  {
    updateReferencePose();
    setPose( requested_position_, requested_orientation_ );
  }
}

bool InteractiveMarker::handleMouseEvent(ViewportMouseEvent& event)
{
//  if (event.event.LeftDown())
//  {
//    visualization_msgs::InteractiveMarkerFeedback feedback;
//    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN;
//    publishFeedback( feedback );
//  }
//  if (event.event.LeftUp())
//  {
//    visualization_msgs::InteractiveMarkerFeedback feedback;
//    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP;
//    publishFeedback( feedback );
//  }

  if ( menu_.get() )
  {
    if ( event.event.RightDown() || event.event.RightIsDown() )
    {
      return true;
    }
    if ( event.event.RightUp() )
    {
      event.panel->PopupMenu( menu_.get(), event.event.GetX(), event.event.GetY() );
      return true;
    }
  }

  return false;
}

void InteractiveMarker::handleMenuSelect(wxCommandEvent &evt)
{
  if ( (unsigned)evt.GetId() < menu_entries_.size() )
  {
    visualization_msgs::InteractiveMarkerFeedback feedback;
    feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
    feedback.command = menu_entries_[evt.GetId()];
    publishFeedback( feedback );
  }
}

void InteractiveMarker::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
{
  feedback.client_id = client_id_;
  feedback.marker_name = name_;

  if ( frame_locked_ )
  {
    feedback.header.frame_id = reference_frame_;
    feedback.header.stamp = reference_time_;
    feedback.pose.position.x = position_.x;
    feedback.pose.position.y = position_.y;
    feedback.pose.position.z = position_.z;
    feedback.pose.orientation.x = orientation_.x;
    feedback.pose.orientation.y = orientation_.y;
    feedback.pose.orientation.z = orientation_.z;
    feedback.pose.orientation.w = orientation_.w;
  }
  else
  {
    feedback.header.frame_id = vis_manager_->getFixedFrame();
    feedback.header.stamp = ros::Time::now();

    Ogre::Vector3 world_position = reference_node_->convertLocalToWorldPosition( position_ );
    Ogre::Quaternion world_orientation = reference_node_->convertLocalToWorldOrientation( orientation_ );

    feedback.pose.position.x = world_position.x;
    feedback.pose.position.y = world_position.y;
    feedback.pose.position.z = world_position.z;
    feedback.pose.orientation.x = world_orientation.x;
    feedback.pose.orientation.y = world_orientation.y;
    feedback.pose.orientation.z = world_orientation.z;
    feedback.pose.orientation.w = world_orientation.w;
  }

  feedback_pub_.publish( feedback );

  time_since_last_feedback_ = 0;
}

}
<|endoftext|>


<commit_before>// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:t -*-
// vim: ts=8 sw=2 smarttab
/*
 * Ceph - scalable distributed file system
 *
 * Copyright (C) 2004-2006 Sage Weil <sage@newdream.net>
 *
 * This is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License version 2.1, as published by the Free Software
 * Foundation.  See file COPYING.
 *
 */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>

#include <sys/stat.h>
#include <iostream>
#include <string>
using namespace std;

#include "config.h"

#include "osd/OSDMap.h"
#include "mon/MonMap.h"
#include "common/common_init.h"

void usage()
{
  cout << " usage: [--print] [--createsimple <numosd> [--clobber] [--pg_bits <bitsperosd>]] <mapfilename>" << std::endl;
  cout << "   --export-crush <file>   write osdmap's crush map to <file>" << std::endl;
  cout << "   --import-crush <file>   replace osdmap's crush map with <file>" << std::endl;
  exit(1);
}

int main(int argc, const char **argv)
{
  vector<const char*> args;
  argv_to_vec(argc, argv, args);
  env_to_vec(args);
  DEFINE_CONF_VARS(usage);
  common_init(args, "osdmaptool", false);

  const char *me = argv[0];

  const char *fn = 0;
  bool print = false;
  bool createsimple = false;
  int num_osd = 0, num_dom = 0;
  int pg_bits = g_conf.osd_pg_bits;
  int lpg_bits = g_conf.osd_lpg_bits;
  bool clobber = false;
  bool modified = false;
  const char *export_crush = 0;
  const char *import_crush = 0;
  list<entity_addr_t> add, rm;
  const char *test_map_pg = 0;
  bool test_crush = false;

  FOR_EACH_ARG(args) {
    if (CONF_ARG_EQ("print", 'p')) {
      CONF_SAFE_SET_ARG_VAL(&print, OPT_BOOL);
    } else if (CONF_ARG_EQ("createsimple", '\0')) {
      createsimple = true;
      CONF_SAFE_SET_ARG_VAL(&num_osd, OPT_INT);
    } else if (CONF_ARG_EQ("clobber", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&clobber, OPT_BOOL);
    } else if (CONF_ARG_EQ("pg_bits", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&pg_bits, OPT_INT);
    } else if (CONF_ARG_EQ("lpg_bits", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&lpg_bits, OPT_INT);
    } else if (CONF_ARG_EQ("num_dom", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&num_dom, OPT_INT);
    } else if (CONF_ARG_EQ("export_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&export_crush, OPT_STR);
    } else if (CONF_ARG_EQ("import_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&import_crush, OPT_STR);
    } else if (CONF_ARG_EQ("test_map_pg", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&test_map_pg, OPT_STR);
    } else if (CONF_ARG_EQ("test_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&test_crush, OPT_BOOL);
    } else if (!fn)
      fn = args[i];
    else
      usage();
  }
  if (!fn) {
    cerr << me << ": must specify osdmap filename" << std::endl;
    usage();
  }

  OSDMap osdmap;
  bufferlist bl;

  cout << me << ": osdmap file '" << fn << "'" << std::endl;

  int r = 0;
  if (!(createsimple && clobber)) {
    r = bl.read_file(fn);
    osdmap.decode(bl);
  }
  char buf[80];
  if (!createsimple && r < 0) {
    cerr << me << ": couldn't open " << fn << ": " << strerror_r(errno, buf, sizeof(buf)) << std::endl;
    return -1;
  }
  else if (createsimple && !clobber && r == 0) {
    cerr << me << ": " << fn << " exists, --clobber to overwrite" << std::endl;
    return -1;
  }

  if (createsimple) {
    if (num_osd < 1) {
      cerr << me << ": osd count must be > 0" << std::endl;
      exit(1);
    }
    ceph_fsid_t fsid;
    memset(&fsid, 0, sizeof(ceph_fsid_t));
    osdmap.build_simple(0, fsid, num_osd, num_dom, pg_bits, lpg_bits, 0);
    modified = true;
  }

  if (import_crush) {
    bufferlist cbl;
    r = cbl.read_file(import_crush);
    if (r < 0) {
      cerr << me << ": error reading crush map from " << import_crush << std::endl;
      exit(1);
    }
    // validate
    CrushWrapper cw;
    bufferlist::iterator p = cbl.begin();
    cw.decode(p);

    // apply
    OSDMap::Incremental inc;
    inc.fsid = osdmap.get_fsid();
    inc.epoch = osdmap.get_epoch()+1;
    inc.crush = cbl;
    osdmap.apply_incremental(inc);
    cout << me << ": imported " << cbl.length() << " byte crush map from " << import_crush << std::endl;
    modified = true;
  }

  if (export_crush) {
    bufferlist cbl;
    osdmap.crush.encode(cbl);
    r = cbl.write_file(export_crush);
    if (r < 0) {
      cerr << me << ": error writing crush map to " << import_crush << std::endl;
      exit(1);
    }
    cout << me << ": exported crush map to " << export_crush << std::endl;
  }

  if (test_map_pg) {
    pg_t pgid;
    if (pgid.parse(test_map_pg) < 0) {
      cerr << me << ": failed to parse pg '" << test_map_pg
	   << "', r = " << r << std::endl;
      usage();
    }
    cout << " parsed '" << test_map_pg << "' -> " << pgid << std::endl;

    vector<int> acting;
    osdmap.pg_to_acting_osds(pgid, acting);
    cout << pgid << " maps to " << acting << std::endl;
  }
  if (test_crush) {
    int pass = 0;
    while (1) {
      cout << "pass " << ++pass << std::endl;

      hash_map<pg_t,vector<int> > m;
      for (map<int,pg_pool_t>::const_iterator p = osdmap.get_pools().begin();
	   p != osdmap.get_pools().end();
	   p++) {
	const pg_pool_t& pool = osdmap.get_pg_pool(p->first);
	for (int ps = 0; ps < pool.get_pg_num(); ps++) {
	  pg_t pgid(ps, p->first, -1);
	  for (int i=0; i<100; i++) {
	    cout << pgid << " attempt " << i << std::endl;

	    vector<int> r, s;
	    osdmap.pg_to_acting_osds(pgid, r);
	    //cout << pgid << " " << r << std::endl;
	    if (m.count(pgid)) {
	      if (m[pgid] != r) {
		cout << pgid << " had " << m[pgid] << " now " << r << std::endl;
		assert(0);
	      }
	    } else
	      m[pgid] = r;
	  }
	}
      }
    }
  }

  if (!print && !modified && !export_crush && !import_crush && !test_map_pg) {
    cerr << me << ": no action specified?" << std::endl;
    usage();
  }

  if (modified)
    osdmap.inc_epoch();

  if (print)
    osdmap.print(cout);

  if (modified) {
    bl.clear();
    osdmap.encode(bl);

    // write it out
    cout << me << ": writing epoch " << osdmap.get_epoch()
	 << " to " << fn
	 << std::endl;
    int r = bl.write_file(fn);
    if (r < 0) {
      cerr << "osdmaptool: error writing to '" << fn << "': " << strerror_r(-r, buf, sizeof(buf)) << std::endl;
      return 1;
    }
  }

  return 0;
}
<commit_msg>osdmaptool: more usage<commit_after>// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:t -*-
// vim: ts=8 sw=2 smarttab
/*
 * Ceph - scalable distributed file system
 *
 * Copyright (C) 2004-2006 Sage Weil <sage@newdream.net>
 *
 * This is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License version 2.1, as published by the Free Software
 * Foundation.  See file COPYING.
 *
 */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>

#include <sys/stat.h>
#include <iostream>
#include <string>
using namespace std;

#include "config.h"

#include "osd/OSDMap.h"
#include "mon/MonMap.h"
#include "common/common_init.h"

void usage()
{
  cout << " usage: [--print] [--createsimple <numosd> [--clobber] [--pg_bits <bitsperosd>]] <mapfilename>" << std::endl;
  cout << "   --export-crush <file>   write osdmap's crush map to <file>" << std::endl;
  cout << "   --import-crush <file>   replace osdmap's crush map with <file>" << std::endl;
  cout << "   --test-map-pg <pgid>    map a pgid to osds" << std::endl;
  exit(1);
}

int main(int argc, const char **argv)
{
  vector<const char*> args;
  argv_to_vec(argc, argv, args);
  env_to_vec(args);
  DEFINE_CONF_VARS(usage);
  common_init(args, "osdmaptool", false);

  const char *me = argv[0];

  const char *fn = 0;
  bool print = false;
  bool createsimple = false;
  int num_osd = 0, num_dom = 0;
  int pg_bits = g_conf.osd_pg_bits;
  int lpg_bits = g_conf.osd_lpg_bits;
  bool clobber = false;
  bool modified = false;
  const char *export_crush = 0;
  const char *import_crush = 0;
  list<entity_addr_t> add, rm;
  const char *test_map_pg = 0;
  bool test_crush = false;

  FOR_EACH_ARG(args) {
    if (CONF_ARG_EQ("print", 'p')) {
      CONF_SAFE_SET_ARG_VAL(&print, OPT_BOOL);
    } else if (CONF_ARG_EQ("createsimple", '\0')) {
      createsimple = true;
      CONF_SAFE_SET_ARG_VAL(&num_osd, OPT_INT);
    } else if (CONF_ARG_EQ("clobber", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&clobber, OPT_BOOL);
    } else if (CONF_ARG_EQ("pg_bits", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&pg_bits, OPT_INT);
    } else if (CONF_ARG_EQ("lpg_bits", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&lpg_bits, OPT_INT);
    } else if (CONF_ARG_EQ("num_dom", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&num_dom, OPT_INT);
    } else if (CONF_ARG_EQ("export_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&export_crush, OPT_STR);
    } else if (CONF_ARG_EQ("import_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&import_crush, OPT_STR);
    } else if (CONF_ARG_EQ("test_map_pg", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&test_map_pg, OPT_STR);
    } else if (CONF_ARG_EQ("test_crush", '\0')) {
      CONF_SAFE_SET_ARG_VAL(&test_crush, OPT_BOOL);
    } else if (!fn)
      fn = args[i];
    else
      usage();
  }
  if (!fn) {
    cerr << me << ": must specify osdmap filename" << std::endl;
    usage();
  }

  OSDMap osdmap;
  bufferlist bl;

  cout << me << ": osdmap file '" << fn << "'" << std::endl;

  int r = 0;
  if (!(createsimple && clobber)) {
    r = bl.read_file(fn);
    osdmap.decode(bl);
  }
  char buf[80];
  if (!createsimple && r < 0) {
    cerr << me << ": couldn't open " << fn << ": " << strerror_r(errno, buf, sizeof(buf)) << std::endl;
    return -1;
  }
  else if (createsimple && !clobber && r == 0) {
    cerr << me << ": " << fn << " exists, --clobber to overwrite" << std::endl;
    return -1;
  }

  if (createsimple) {
    if (num_osd < 1) {
      cerr << me << ": osd count must be > 0" << std::endl;
      exit(1);
    }
    ceph_fsid_t fsid;
    memset(&fsid, 0, sizeof(ceph_fsid_t));
    osdmap.build_simple(0, fsid, num_osd, num_dom, pg_bits, lpg_bits, 0);
    modified = true;
  }

  if (import_crush) {
    bufferlist cbl;
    r = cbl.read_file(import_crush);
    if (r < 0) {
      cerr << me << ": error reading crush map from " << import_crush << std::endl;
      exit(1);
    }
    // validate
    CrushWrapper cw;
    bufferlist::iterator p = cbl.begin();
    cw.decode(p);

    // apply
    OSDMap::Incremental inc;
    inc.fsid = osdmap.get_fsid();
    inc.epoch = osdmap.get_epoch()+1;
    inc.crush = cbl;
    osdmap.apply_incremental(inc);
    cout << me << ": imported " << cbl.length() << " byte crush map from " << import_crush << std::endl;
    modified = true;
  }

  if (export_crush) {
    bufferlist cbl;
    osdmap.crush.encode(cbl);
    r = cbl.write_file(export_crush);
    if (r < 0) {
      cerr << me << ": error writing crush map to " << import_crush << std::endl;
      exit(1);
    }
    cout << me << ": exported crush map to " << export_crush << std::endl;
  }

  if (test_map_pg) {
    pg_t pgid;
    if (pgid.parse(test_map_pg) < 0) {
      cerr << me << ": failed to parse pg '" << test_map_pg
	   << "', r = " << r << std::endl;
      usage();
    }
    cout << " parsed '" << test_map_pg << "' -> " << pgid << std::endl;

    vector<int> acting;
    osdmap.pg_to_acting_osds(pgid, acting);
    cout << pgid << " maps to " << acting << std::endl;
  }
  if (test_crush) {
    int pass = 0;
    while (1) {
      cout << "pass " << ++pass << std::endl;

      hash_map<pg_t,vector<int> > m;
      for (map<int,pg_pool_t>::const_iterator p = osdmap.get_pools().begin();
	   p != osdmap.get_pools().end();
	   p++) {
	const pg_pool_t& pool = osdmap.get_pg_pool(p->first);
	for (int ps = 0; ps < pool.get_pg_num(); ps++) {
	  pg_t pgid(ps, p->first, -1);
	  for (int i=0; i<100; i++) {
	    cout << pgid << " attempt " << i << std::endl;

	    vector<int> r, s;
	    osdmap.pg_to_acting_osds(pgid, r);
	    //cout << pgid << " " << r << std::endl;
	    if (m.count(pgid)) {
	      if (m[pgid] != r) {
		cout << pgid << " had " << m[pgid] << " now " << r << std::endl;
		assert(0);
	      }
	    } else
	      m[pgid] = r;
	  }
	}
      }
    }
  }

  if (!print && !modified && !export_crush && !import_crush && !test_map_pg) {
    cerr << me << ": no action specified?" << std::endl;
    usage();
  }

  if (modified)
    osdmap.inc_epoch();

  if (print)
    osdmap.print(cout);

  if (modified) {
    bl.clear();
    osdmap.encode(bl);

    // write it out
    cout << me << ": writing epoch " << osdmap.get_epoch()
	 << " to " << fn
	 << std::endl;
    int r = bl.write_file(fn);
    if (r < 0) {
      cerr << "osdmaptool: error writing to '" << fn << "': " << strerror_r(-r, buf, sizeof(buf)) << std::endl;
      return 1;
    }
  }

  return 0;
}
<|endoftext|>