<commit_before>#include <utility> // pair

#include <realm/array_binary.hpp>
#include <realm/array_blob.hpp>
#include <realm/array_integer.hpp>
#include <realm/impl/destroy_guard.hpp>

using namespace realm;

void ArrayBinary::init_from_mem(MemRef mem) noexcept
{
    Array::init_from_mem(mem);
    ref_type offsets_ref = get_as_ref(0);
    ref_type blob_ref = get_as_ref(1);

    m_offsets.init_from_ref(offsets_ref);
    m_blob.init_from_ref(blob_ref);

    if (!legacy_array_type()) {
        ref_type nulls_ref = get_as_ref(2);
        m_nulls.init_from_ref(nulls_ref);
    }
}

void ArrayBinary::add(BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_7(value.size(), ==, 0, ||, value.data(), !=, 0);

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    m_blob.add(value.data(), value.size(), add_zero_term);
    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    size_t offset = stored_size;
    if (!m_offsets.is_empty())
        offset += to_size_t(m_offsets.back());
    m_offsets.add(offset);

    if (!legacy_array_type())
        m_nulls.add(value.is_null());
}

void ArrayBinary::set(size_t ndx, BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_3(ndx, <, m_offsets.size());
    REALM_ASSERT_3(value.size(), == 0 ||, value.data());

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    size_t start = ndx ? to_size_t(m_offsets.get(ndx-1)) : 0;
    size_t current_end = to_size_t(m_offsets.get(ndx));
    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    int_fast64_t diff =  (start + stored_size) - current_end;
    m_blob.replace(start, current_end, value.data(), value.size(), add_zero_term);
    m_offsets.adjust(ndx, m_offsets.size(), diff);

    if (!legacy_array_type())
        m_nulls.set(ndx, value.is_null());
}

void ArrayBinary::insert(size_t ndx, BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_3(ndx, <=, m_offsets.size());
    REALM_ASSERT_3(value.size(), == 0 ||, value.data());

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    size_t pos = ndx ? to_size_t(m_offsets.get(ndx-1)) : 0;
    m_blob.insert(pos, value.data(), value.size(), add_zero_term);

    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    m_offsets.insert(ndx, pos + stored_size);
    m_offsets.adjust(ndx+1, m_offsets.size(), stored_size);

    if (!legacy_array_type())
        m_nulls.insert(ndx, value.is_null());
}

void ArrayBinary::erase(size_t ndx)
{
    REALM_ASSERT_3(ndx, <, m_offsets.size());

    size_t start = ndx ? to_size_t(m_offsets.get(ndx-1)) : 0;
    size_t end = to_size_t(m_offsets.get(ndx));

    m_blob.erase(start, end);
    m_offsets.erase(ndx);
    m_offsets.adjust(ndx, m_offsets.size(), int64_t(start) - end);

    if (!legacy_array_type())
        m_nulls.erase(ndx);
}

BinaryData ArrayBinary::get(const char* header, size_t ndx, Allocator& alloc) noexcept
{
    // Column *may* be nullable if top has 3 refs (3'rd being m_nulls). Else, if it has 2, it's non-nullable
    // See comment in legacy_array_type() and also in array_binary.hpp.
    size_t siz = Array::get_size_from_header(header);
    REALM_ASSERT_7(siz, ==, 2, ||, siz, ==, 3);

    if (siz == 3) {
        std::pair<int64_t, int64_t> p = get_two(header, 1);
        const char* nulls_header = alloc.translate(to_ref(p.second));
        int64_t n = ArrayInteger::get(nulls_header, ndx);
        // 0 or 1 is all that is ever written to m_nulls; any other content would be a bug
        REALM_ASSERT_3(n == 1, ||, n == 0);
        bool null = (n != 0);
        if (null)
            return BinaryData(0, 0);
    }

    std::pair<int64_t, int64_t> p = get_two(header, 0);
    const char* offsets_header = alloc.translate(to_ref(p.first));
    const char* blob_header = alloc.translate(to_ref(p.second));
    size_t begin, end;
    if (ndx) {
        p = get_two(offsets_header, ndx-1);
        begin = to_size_t(p.first);
        end   = to_size_t(p.second);
    }
    else {
        begin = 0;
        end   = to_size_t(Array::get(offsets_header, ndx));
    }
    BinaryData bd = BinaryData(ArrayBlob::get(blob_header, begin), end - begin);
    return bd;
}

// FIXME: Not exception safe (leaks are possible).
ref_type ArrayBinary::bptree_leaf_insert(size_t ndx, BinaryData value, bool add_zero_term,
                                         TreeInsertBase& state)
{
    size_t leaf_size = size();
    REALM_ASSERT_3(leaf_size, <=, REALM_MAX_BPNODE_SIZE);
    if (leaf_size < ndx)
        ndx = leaf_size;
    if (REALM_LIKELY(leaf_size < REALM_MAX_BPNODE_SIZE)) {
        insert(ndx, value, add_zero_term); // Throws
        return 0; // Leaf was not split
    }

    // Split leaf node
    ArrayBinary new_leaf(get_alloc());
    new_leaf.create(); // Throws
    if (ndx == leaf_size) {
        new_leaf.add(value, add_zero_term); // Throws
        state.m_split_offset = ndx;
    }
    else {
        for (size_t i = ndx; i != leaf_size; ++i)
            new_leaf.add(get(i)); // Throws
        truncate(ndx); // Throws
        add(value, add_zero_term); // Throws
        state.m_split_offset = ndx + 1;
    }
    state.m_split_size = leaf_size + 1;
    return new_leaf.get_ref();
}

MemRef ArrayBinary::create_array(size_t size, Allocator& alloc, BinaryData values)
{
    // Only null and zero-length non-null allowed as initialization value
    REALM_ASSERT(values.size() == 0);
    Array top(alloc);
    _impl::DeepArrayDestroyGuard dg(&top);
    top.create(type_HasRefs); // Throws

    _impl::DeepArrayRefDestroyGuard dg_2(alloc);
    {
        bool context_flag = false;
        int64_t value = 0;
        MemRef mem = ArrayInteger::create_array(type_Normal, context_flag, size, value, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }
    {
        size_t blobs_size = 0;
        MemRef mem = ArrayBlob::create_array(blobs_size, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }
    {
        // Always create a m_nulls array, regardless if its column is marked as nullable or not. NOTE: This is new
        // - existing binary arrays from earier versions of core will not have this third array. All methods on ArrayBinary
        // must thus check if this array exists before trying to access it. If it doesn't, it must be interpreted as if its
        // column isn't nullable.
        bool context_flag = false;
        int64_t value = values.is_null() ? 1 : 0;
        MemRef mem = ArrayInteger::create_array(type_Normal, context_flag, size, value, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }

    dg.release();
    return top.get_mem();
}

MemRef ArrayBinary::slice(size_t offset, size_t size, Allocator& target_alloc) const
{
    REALM_ASSERT(is_attached());

    ArrayBinary slice(target_alloc);
    _impl::ShallowArrayDestroyGuard dg(&slice);
    slice.create(); // Throws
    size_t begin = offset;
    size_t end   = offset + size;
    for (size_t i = begin; i != end; ++i) {
        BinaryData value = get(i);
        slice.add(value); // Throws
    }
    dg.release();
    return slice.get_mem();
}

#ifdef REALM_DEBUG

void ArrayBinary::to_dot(std::ostream& out, bool, StringData title) const
{
    ref_type ref = get_ref();

    out << "subgraph cluster_binary" << ref << " {" << std::endl;
    out << " label = \"ArrayBinary";
    if (title.size() != 0)
        out << "\\n'" << title << "'";
    out << "\";" << std::endl;

    Array::to_dot(out, "binary_top");
    m_offsets.to_dot(out, "offsets");
    m_blob.to_dot(out, "blob");

    out << "}" << std::endl;
}

#endif // REALM_DEBUG
<commit_msg>Avoid unnecessary casting and potential issues on 32-bit architectures casting from unsigned, underflowed 32-bit values to signed 64-bit values.<commit_after>#include <utility> // pair

#include <realm/array_binary.hpp>
#include <realm/array_blob.hpp>
#include <realm/array_integer.hpp>
#include <realm/impl/destroy_guard.hpp>

using namespace realm;

void ArrayBinary::init_from_mem(MemRef mem) noexcept
{
    Array::init_from_mem(mem);
    ref_type offsets_ref = get_as_ref(0);
    ref_type blob_ref = get_as_ref(1);

    m_offsets.init_from_ref(offsets_ref);
    m_blob.init_from_ref(blob_ref);

    if (!legacy_array_type()) {
        ref_type nulls_ref = get_as_ref(2);
        m_nulls.init_from_ref(nulls_ref);
    }
}

void ArrayBinary::add(BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_7(value.size(), ==, 0, ||, value.data(), !=, 0);

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    m_blob.add(value.data(), value.size(), add_zero_term);
    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    size_t offset = stored_size;
    if (!m_offsets.is_empty())
        offset += to_size_t(m_offsets.back());
    m_offsets.add(offset);

    if (!legacy_array_type())
        m_nulls.add(value.is_null());
}

void ArrayBinary::set(size_t ndx, BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_3(ndx, <, m_offsets.size());
    REALM_ASSERT_3(value.size(), == 0 ||, value.data());

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    int_fast64_t start = ndx ? m_offsets.get(ndx-1) : 0;
    int_fast64_t current_end = m_offsets.get(ndx);
    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    int_fast64_t diff = (start + stored_size) - current_end;
    m_blob.replace(start, current_end, value.data(), value.size(), add_zero_term);
    m_offsets.adjust(ndx, m_offsets.size(), diff);

    if (!legacy_array_type())
        m_nulls.set(ndx, value.is_null());
}

void ArrayBinary::insert(size_t ndx, BinaryData value, bool add_zero_term)
{
    REALM_ASSERT_3(ndx, <=, m_offsets.size());
    REALM_ASSERT_3(value.size(), == 0 ||, value.data());

    if (value.is_null() && legacy_array_type())
        throw LogicError(LogicError::column_not_nullable);

    size_t pos = ndx ? to_size_t(m_offsets.get(ndx-1)) : 0;
    m_blob.insert(pos, value.data(), value.size(), add_zero_term);

    size_t stored_size = value.size();
    if (add_zero_term)
        ++stored_size;
    m_offsets.insert(ndx, pos + stored_size);
    m_offsets.adjust(ndx+1, m_offsets.size(), stored_size);

    if (!legacy_array_type())
        m_nulls.insert(ndx, value.is_null());
}

void ArrayBinary::erase(size_t ndx)
{
    REALM_ASSERT_3(ndx, <, m_offsets.size());

    size_t start = ndx ? to_size_t(m_offsets.get(ndx-1)) : 0;
    size_t end = to_size_t(m_offsets.get(ndx));

    m_blob.erase(start, end);
    m_offsets.erase(ndx);
    m_offsets.adjust(ndx, m_offsets.size(), int64_t(start) - end);

    if (!legacy_array_type())
        m_nulls.erase(ndx);
}

BinaryData ArrayBinary::get(const char* header, size_t ndx, Allocator& alloc) noexcept
{
    // Column *may* be nullable if top has 3 refs (3'rd being m_nulls). Else, if it has 2, it's non-nullable
    // See comment in legacy_array_type() and also in array_binary.hpp.
    size_t siz = Array::get_size_from_header(header);
    REALM_ASSERT_7(siz, ==, 2, ||, siz, ==, 3);

    if (siz == 3) {
        std::pair<int64_t, int64_t> p = get_two(header, 1);
        const char* nulls_header = alloc.translate(to_ref(p.second));
        int64_t n = ArrayInteger::get(nulls_header, ndx);
        // 0 or 1 is all that is ever written to m_nulls; any other content would be a bug
        REALM_ASSERT_3(n == 1, ||, n == 0);
        bool null = (n != 0);
        if (null)
            return BinaryData(0, 0);
    }

    std::pair<int64_t, int64_t> p = get_two(header, 0);
    const char* offsets_header = alloc.translate(to_ref(p.first));
    const char* blob_header = alloc.translate(to_ref(p.second));
    size_t begin, end;
    if (ndx) {
        p = get_two(offsets_header, ndx-1);
        begin = to_size_t(p.first);
        end   = to_size_t(p.second);
    }
    else {
        begin = 0;
        end   = to_size_t(Array::get(offsets_header, ndx));
    }
    BinaryData bd = BinaryData(ArrayBlob::get(blob_header, begin), end - begin);
    return bd;
}

// FIXME: Not exception safe (leaks are possible).
ref_type ArrayBinary::bptree_leaf_insert(size_t ndx, BinaryData value, bool add_zero_term,
                                         TreeInsertBase& state)
{
    size_t leaf_size = size();
    REALM_ASSERT_3(leaf_size, <=, REALM_MAX_BPNODE_SIZE);
    if (leaf_size < ndx)
        ndx = leaf_size;
    if (REALM_LIKELY(leaf_size < REALM_MAX_BPNODE_SIZE)) {
        insert(ndx, value, add_zero_term); // Throws
        return 0; // Leaf was not split
    }

    // Split leaf node
    ArrayBinary new_leaf(get_alloc());
    new_leaf.create(); // Throws
    if (ndx == leaf_size) {
        new_leaf.add(value, add_zero_term); // Throws
        state.m_split_offset = ndx;
    }
    else {
        for (size_t i = ndx; i != leaf_size; ++i)
            new_leaf.add(get(i)); // Throws
        truncate(ndx); // Throws
        add(value, add_zero_term); // Throws
        state.m_split_offset = ndx + 1;
    }
    state.m_split_size = leaf_size + 1;
    return new_leaf.get_ref();
}

MemRef ArrayBinary::create_array(size_t size, Allocator& alloc, BinaryData values)
{
    // Only null and zero-length non-null allowed as initialization value
    REALM_ASSERT(values.size() == 0);
    Array top(alloc);
    _impl::DeepArrayDestroyGuard dg(&top);
    top.create(type_HasRefs); // Throws

    _impl::DeepArrayRefDestroyGuard dg_2(alloc);
    {
        bool context_flag = false;
        int64_t value = 0;
        MemRef mem = ArrayInteger::create_array(type_Normal, context_flag, size, value, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }
    {
        size_t blobs_size = 0;
        MemRef mem = ArrayBlob::create_array(blobs_size, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }
    {
        // Always create a m_nulls array, regardless if its column is marked as nullable or not. NOTE: This is new
        // - existing binary arrays from earier versions of core will not have this third array. All methods on ArrayBinary
        // must thus check if this array exists before trying to access it. If it doesn't, it must be interpreted as if its
        // column isn't nullable.
        bool context_flag = false;
        int64_t value = values.is_null() ? 1 : 0;
        MemRef mem = ArrayInteger::create_array(type_Normal, context_flag, size, value, alloc); // Throws
        dg_2.reset(mem.m_ref);
        int64_t v = from_ref(mem.m_ref);
        top.add(v); // Throws
        dg_2.release();
    }

    dg.release();
    return top.get_mem();
}

MemRef ArrayBinary::slice(size_t offset, size_t size, Allocator& target_alloc) const
{
    REALM_ASSERT(is_attached());

    ArrayBinary slice(target_alloc);
    _impl::ShallowArrayDestroyGuard dg(&slice);
    slice.create(); // Throws
    size_t begin = offset;
    size_t end   = offset + size;
    for (size_t i = begin; i != end; ++i) {
        BinaryData value = get(i);
        slice.add(value); // Throws
    }
    dg.release();
    return slice.get_mem();
}

#ifdef REALM_DEBUG

void ArrayBinary::to_dot(std::ostream& out, bool, StringData title) const
{
    ref_type ref = get_ref();

    out << "subgraph cluster_binary" << ref << " {" << std::endl;
    out << " label = \"ArrayBinary";
    if (title.size() != 0)
        out << "\\n'" << title << "'";
    out << "\";" << std::endl;

    Array::to_dot(out, "binary_top");
    m_offsets.to_dot(out, "offsets");
    m_blob.to_dot(out, "blob");

    out << "}" << std::endl;
}

#endif // REALM_DEBUG
<|endoftext|>


<commit_before>#include "window.hh"

#include "assert.hh"
#include "highlighter_registry.hh"
#include "hooks_manager.hh"
#include "context.hh"

#include <algorithm>
#include <sstream>

namespace Kakoune
{

Window::Window(Buffer& buffer)
    : Editor(buffer),
      m_position(0, 0),
      m_dimensions(0, 0)
{
    HighlighterRegistry& registry = HighlighterRegistry::instance();

    GlobalHooksManager::instance().run_hook("WinCreate", buffer.name(),
                                            Context(*this));

    registry.add_highlighter_to_window(*this, "expand_tabs", HighlighterParameters());
    registry.add_highlighter_to_window(*this, "highlight_selections", HighlighterParameters());
}

template<typename Iterator>
static DisplayCoord measure_string(Iterator begin, Iterator end)
{
    DisplayCoord result(0, 0);
    while (begin != end)
    {
        if (*begin == '\n')
        {
            ++result.line;
            result.column = 0;
        }
        else
            ++result.column;
        ++begin;
    }
    return result;
}

static DisplayCoord measure_string(const String& string)
{
    return measure_string(string.begin(), string.end());
}

BufferIterator Window::iterator_at(const DisplayCoord& window_pos) const
{
    if (m_display_buffer.begin() == m_display_buffer.end())
        return buffer().begin();

    if (DisplayCoord(0,0) <= window_pos)
    {
        for (auto atom_it = m_display_buffer.begin();
             atom_it != m_display_buffer.end(); ++atom_it)
        {
            if (window_pos < atom_it->coord())
            {
                return (--atom_it)->iterator_at(window_pos);
            }
        }
    }

    return buffer().iterator_at(m_position + BufferCoord(window_pos));
}

DisplayCoord Window::line_and_column_at(const BufferIterator& iterator) const
{
    if (m_display_buffer.begin() == m_display_buffer.end())
        return DisplayCoord(0, 0);

    if (iterator >= m_display_buffer.front().begin() and
        iterator <  m_display_buffer.back().end())
    {
        for (auto& atom : m_display_buffer)
        {
            if (atom.end() > iterator)
            {
                assert(atom.begin() <= iterator);
                return atom.line_and_column_at(iterator);
            }
        }
    }
    BufferCoord coord = buffer().line_and_column_at(iterator);
    return DisplayCoord(coord.line - m_position.line,
                        coord.column - m_position.column);
}

void Window::update_display_buffer()
{
    scroll_to_keep_cursor_visible_ifn();

    m_display_buffer.clear();

    BufferIterator begin = buffer().iterator_at(m_position);
    BufferIterator end = buffer().iterator_at(m_position +
                                              BufferCoord(m_dimensions.line, m_dimensions.column))+2;
    if (begin == end)
        return;

    m_display_buffer.append(DisplayAtom(DisplayCoord(0,0), begin, end));

    m_highlighters(m_display_buffer);
    m_display_buffer.check_invariant();
}

void Window::set_dimensions(const DisplayCoord& dimensions)
{
    m_dimensions = dimensions;
}

void Window::scroll_to_keep_cursor_visible_ifn()
{
    DisplayCoord cursor = line_and_column_at(selections().back().last());
    if (cursor.line < 0)
    {
        m_position.line = std::max(m_position.line + cursor.line, 0);
    }
    else if (cursor.line >= m_dimensions.line)
    {
        m_position.line += cursor.line - (m_dimensions.line - 1);
    }

    if (cursor.column < 0)
    {
        m_position.column = std::max(m_position.column + cursor.column, 0);
    }
    else if (cursor.column >= m_dimensions.column)
    {
        m_position.column += cursor.column - (m_dimensions.column - 1);
    }
}

std::string Window::status_line() const
{
    BufferCoord cursor = buffer().line_and_column_at(selections().back().last());
    std::ostringstream oss;
    oss << buffer().name();
    if (buffer().is_modified())
        oss << " [+]";
    oss << " -- " << cursor.line+1 << "," << cursor.column+1
        << " -- " << selections().size() << " sel -- ";
    if (is_editing())
        oss << "[Insert]";
    return oss.str();
}

void Window::on_incremental_insertion_end()
{
    push_selections();
    hooks_manager().run_hook("InsertEnd", "", Context(*this));
    pop_selections();
}

}
<commit_msg>remove unused measure_string from window.cc<commit_after>#include "window.hh"

#include "assert.hh"
#include "highlighter_registry.hh"
#include "hooks_manager.hh"
#include "context.hh"

#include <algorithm>
#include <sstream>

namespace Kakoune
{

Window::Window(Buffer& buffer)
    : Editor(buffer),
      m_position(0, 0),
      m_dimensions(0, 0)
{
    HighlighterRegistry& registry = HighlighterRegistry::instance();

    GlobalHooksManager::instance().run_hook("WinCreate", buffer.name(),
                                            Context(*this));

    registry.add_highlighter_to_window(*this, "expand_tabs", HighlighterParameters());
    registry.add_highlighter_to_window(*this, "highlight_selections", HighlighterParameters());
}

BufferIterator Window::iterator_at(const DisplayCoord& window_pos) const
{
    if (m_display_buffer.begin() == m_display_buffer.end())
        return buffer().begin();

    if (DisplayCoord(0,0) <= window_pos)
    {
        for (auto atom_it = m_display_buffer.begin();
             atom_it != m_display_buffer.end(); ++atom_it)
        {
            if (window_pos < atom_it->coord())
            {
                return (--atom_it)->iterator_at(window_pos);
            }
        }
    }

    return buffer().iterator_at(m_position + BufferCoord(window_pos));
}

DisplayCoord Window::line_and_column_at(const BufferIterator& iterator) const
{
    if (m_display_buffer.begin() == m_display_buffer.end())
        return DisplayCoord(0, 0);

    if (iterator >= m_display_buffer.front().begin() and
        iterator <  m_display_buffer.back().end())
    {
        for (auto& atom : m_display_buffer)
        {
            if (atom.end() > iterator)
            {
                assert(atom.begin() <= iterator);
                return atom.line_and_column_at(iterator);
            }
        }
    }
    BufferCoord coord = buffer().line_and_column_at(iterator);
    return DisplayCoord(coord.line - m_position.line,
                        coord.column - m_position.column);
}

void Window::update_display_buffer()
{
    scroll_to_keep_cursor_visible_ifn();

    m_display_buffer.clear();

    BufferIterator begin = buffer().iterator_at(m_position);
    BufferIterator end = buffer().iterator_at(m_position +
                                              BufferCoord(m_dimensions.line, m_dimensions.column))+2;
    if (begin == end)
        return;

    m_display_buffer.append(DisplayAtom(DisplayCoord(0,0), begin, end));

    m_highlighters(m_display_buffer);
    m_display_buffer.check_invariant();
}

void Window::set_dimensions(const DisplayCoord& dimensions)
{
    m_dimensions = dimensions;
}

void Window::scroll_to_keep_cursor_visible_ifn()
{
    DisplayCoord cursor = line_and_column_at(selections().back().last());
    if (cursor.line < 0)
    {
        m_position.line = std::max(m_position.line + cursor.line, 0);
    }
    else if (cursor.line >= m_dimensions.line)
    {
        m_position.line += cursor.line - (m_dimensions.line - 1);
    }

    if (cursor.column < 0)
    {
        m_position.column = std::max(m_position.column + cursor.column, 0);
    }
    else if (cursor.column >= m_dimensions.column)
    {
        m_position.column += cursor.column - (m_dimensions.column - 1);
    }
}

std::string Window::status_line() const
{
    BufferCoord cursor = buffer().line_and_column_at(selections().back().last());
    std::ostringstream oss;
    oss << buffer().name();
    if (buffer().is_modified())
        oss << " [+]";
    oss << " -- " << cursor.line+1 << "," << cursor.column+1
        << " -- " << selections().size() << " sel -- ";
    if (is_editing())
        oss << "[Insert]";
    return oss.str();
}

void Window::on_incremental_insertion_end()
{
    push_selections();
    hooks_manager().run_hook("InsertEnd", "", Context(*this));
    pop_selections();
}

}
<|endoftext|>


<commit_before>/********************************************************************
  Software License Agreement (BSD License)

  Copyright (c) 2016, Syler Wagner.
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:

  1. Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
  2. 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.
  3. Neither the name of the copyright holder 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.
 ********************************************************************/

/**
 * takeoff_server.cpp
 * Actionlib implementation of UAV takeoff
 *
 * @author  Syler Wagner  <syler@mit.edu>
 * @date    2016-08-15    creation
 **/

#include <ros/ros.h>
#include <pid_controller.h>
#include <actionlib/server/simple_action_server.h>
#include <mavpro/TakeoffAction.h>

//$ mavros services
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/CommandTOL.h>		//$ takeoff/landing command
#include <mavros_msgs/CommandBool.h> 	//$ arming command

//$ message types
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/TwistStamped.h>

 typedef actionlib::SimpleActionServer<mavpro::TakeoffAction> TakeoffActionServer;

 class TakeoffServer
 {
 public:
 	TakeoffServer() :
 	_n(NULL),
 	_as(NULL)
 	{
 		_n = new ros::NodeHandle("~");

		// //# for comanding the UAV
		// _vel_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1);

 		_as = new TakeoffActionServer(ros::NodeHandle(), "takeoff", boost::bind(&TakeoffServer::executeCb, this, _1), false);

		//# for keeping track of current altitude
 		_alt_sub = _n->subscribe<std_msgs::Float64>("/mavros/global_position/rel_alt", 1, &TakeoffServer::altCallback, this);
 		_state_sub = _n->subscribe<mavros_msgs::State>("/mavros/state", 1, &TakeoffServer::stateCallback, this);
 		_rc_sub = _n->subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 1, &TakeoffServer::radioCallback, this);

 		_rc_pub = _n->advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 1);

		//$ set parameter
 		_n->param("controller_frequency", _controller_frequency, 10.0);
 		_n->param("z_tolerance", _z_tolerance, 2.0);

 		ROS_WARN("Done initializing takeoff_server.");
 		_as->start();
 	}

 	~TakeoffServer()
 	{
 		if (_as != NULL)
 		{
 			delete _as;
 		}
 		if (_n != NULL)
 		{
 			delete _n;
 		}
 	}

 private:

 	TakeoffActionServer* _as;
 	mavpro::TakeoffResult _result;

 	ros::NodeHandle* _n;
 	ros::Subscriber _alt_sub;
 	ros::Subscriber _state_sub;
 	ros::Subscriber _rc_sub;

 	ros::Publisher _rc_pub;

 	double _controller_frequency;
 	double _z_tolerance;

 	double _current_alt;
 	std::string _mode;
	bool _armed;	//$ current arming status
	int _throttle_pwm;

	bool checkGCSID()
	{
		int mavros_sys_id, uav_gcs_id;

		_n->getParam("mavros/system_id", mavros_sys_id);

		ros::ServiceClient param_client = _n->serviceClient<mavros_msgs::ParamGet>("/mavros/param/get");

		mavros_msgs::ParamGet srv;
		srv.request.param_id = "SYSID_MYGCS";

		if (param_client.call(srv))
		{
			uav_gcs_id = srv.response.value.integer;
		}

		if (mavros_sys_id == uav_gcs_id)
		{
			return true;
		}
		else
		{
			ROS_ERROR("mavros system_id: %d", mavros_sys_id);
			ROS_ERROR("fcu SYSID_MYGCS: %d", uav_gcs_id);
			return false;
		}
	}

	bool setMode(const std::string mode)
	{
		ros::ServiceClient mode_client = _n->serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
		mavros_msgs::SetMode srv;

		srv.request.base_mode = 0;
		srv.request.custom_mode = mode;

		bool mode_changed = false;

		ros::Rate r(_controller_frequency);

		while ((!mode_changed) && _n->ok())
		{

			int num_calls = 0;

			if (mode_client.call(srv))
			{
				if (_mode == mode)
				{
					mode_changed = true;
					ROS_WARN("Changed to %s mode", mode.c_str());

				}
			}
			else
			{
				ROS_ERROR("Failed to switch to %s mode", mode.c_str());
				num_calls += 1;
			}

			r.sleep();

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to change mode 5 times, aborting.");
				break;
			}
		}

		return mode_changed;
	}

	void executeCb(const mavpro::TakeoffGoalConstPtr& goal)
	{

		ROS_INFO("takeoff_server has received takeoff goal");

		ros::Rate r(_controller_frequency);
		ros::NodeHandle n;

		while (n.ok())
		{

			if (_as->isPreemptRequested())
			{
				//$ TODO?
				ROS_INFO("takeoff_server preempting the current goal");
				_as->setPreempted();

				// return from execute after preempting
				return;
			}

			//for timing that gives real time even in simulation
			ros::WallTime start = ros::WallTime::now();

			//$ actually try to land
			bool done = executeTakeoff(goal->altitude);

			if (done)
			{
				return;
			}
			ros::WallDuration t_diff = ros::WallTime::now() - start;
			ROS_DEBUG_NAMED("takeoff_server","Takeoff time: %.9f\n", t_diff.toSec());

			r.sleep();
		}

		//$ if the node is killed then we'll abort and return
		_result.success = false;
		_as->setAborted(_result, "Aborting on the goal because the node has been killed");
		return;
	}

	bool executeArm(bool arming_value)
	{
		//$ TODO

		ros::ServiceClient arming_client = _n->serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
		mavros_msgs::CommandBool srv;

		srv.request.value = arming_value;

		bool arm_status_changed = false;

		while ((!arm_status_changed) && _n->ok())
		{

			int num_calls = 0;

			if (arming_client.call(srv))
			{
				ROS_WARN("Arming status changed");
				arm_status_changed = true;
			}
			else
			{
				ROS_ERROR("Failed to arm");
				num_calls += 1;
			}

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to arm 5 times, aborting.");
				break;
			}
		}

		return arm_status_changed;
	}

	void overrideThrottle(int pwm) {

		ros::Rate r(_controller_frequency);

		ROS_WARN("Overriding throttle to PWM %d", pwm);

		mavros_msgs::OverrideRCIn rc_override;
		int NO_CHANGE = 65535; 	//$ leaves RC channel value unchanged
		int RELEASE = 0;			//$ releases RC channel

		ros::WallTime start = ros::WallTime::now();

		while ((_throttle_pwm != pwm) && _n->ok())
		{
			//$ throttle is channel 2 (C++ is zero indexed)
			if (pwm == 0)	//$ clear previous override values
			{
				for (int i = 0; i < 8; i++)
				{
					rc_override.channels[i] = RELEASE;
				}
				rc_override.channels[2] = pwm;
			}
			else			//$ set override for throttle channel
			{
				for (int i = 0; i < 8; i++)
				{
					rc_override.channels[i] = NO_CHANGE;
				}
				rc_override.channels[2] = pwm;
			}

			_rc_pub.publish(rc_override);
			r.sleep();
		}
		ros::WallDuration t_diff = ros::WallTime::now() - start;
		ROS_WARN("Throttle override time: %.9f\n", t_diff.toSec());
	}

	bool isGoalReached(const double goal)
	{
		ROS_INFO("goal: %4.1f, current alt: %4.1f, error: %4.1f", goal, _current_alt, fabs(goal - _current_alt));

		if (fabs(goal - _current_alt) <= _z_tolerance)
		{
			return true;
		}
		else
		{
			return false;
		}
	}

	bool executeTakeoff(const double goal)
	{

		ros::Rate r(10);

		if (!_armed)
		{
			setMode("STABILIZE");
			overrideThrottle(1000);
			r.sleep();
			executeArm(true);
			setMode("GUIDED");
		}

		 //$ publish feedback
		mavpro::TakeoffFeedback feedback;
		feedback.altitude = _current_alt;
		_as->publishFeedback(feedback);

		 //$ check if goal has been reached
		if (isGoalReached(goal))
		{
			ROS_ERROR("SUCCESS");
			ROS_DEBUG_NAMED("takeoff_server", "Goal reached!");
			// resetState();

			_result.success = true;
			_as->setSucceeded(_result, "Goal reached.");
			return true;
		}

		mavros_msgs::CommandTOL srv;

		ros::ServiceClient takeoff_client = _n->serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");

		srv.request.min_pitch = 5;
		srv.request.yaw = 0;
		srv.request.latitude = 0;
		srv.request.longitude = 0;
		srv.request.altitude = goal;

		bool takeoff_call_success = false;

		while ((!takeoff_call_success) && _n->ok())
		{

			int num_calls = 0;

			if (takeoff_client.call(srv))
			{
				ROS_WARN("Called mavros takeoff service");
				takeoff_call_success = true;
			}
			else
			{
				ROS_ERROR("Failed to call mavros takeoff service");
				num_calls += 1;
			}

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to call mavros takeoff service 5 times, aborting.");
				break;
			}
		}

		 //$ not done yet
		return false;
	}

	void altCallback(const std_msgs::Float64::ConstPtr& rel_alt_msg)
	{
		_current_alt = rel_alt_msg->data;
	}

	void stateCallback(const mavros_msgs::State::ConstPtr& state_msg)
	{
		_armed = state_msg->armed;
		_mode = state_msg->mode;
	}

	void radioCallback(const mavros_msgs::RCIn::ConstPtr& radio_msg)
	{
		_throttle_pwm = radio_msg->channels[2];
	}

	void preemptCb()
	{
		ROS_DEBUG_NAMED("takeoff_server", "Preempted!");
		 // set the action state to preempted
		_as->setPreempted();
	}

 }; //$ end of mode_clientass TakeoffServer

 int main(int argc, char **argv) {

 	ros::init(argc, argv, "takeoff_server");

 	TakeoffServer land;

 	ros::spin();
 	return 0;
 }

<commit_msg>Changed default z tolerance for takeoff_server #3<commit_after>/********************************************************************
  Software License Agreement (BSD License)

  Copyright (c) 2016, Syler Wagner.
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:

  1. Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
  2. 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.
  3. Neither the name of the copyright holder 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.
 ********************************************************************/

/**
 * takeoff_server.cpp
 * Actionlib implementation of UAV takeoff
 *
 * @author  Syler Wagner  <syler@mit.edu>
 * @date    2016-08-21    creation
 **/

#include <ros/ros.h>
#include <pid_controller.h>
#include <actionlib/server/simple_action_server.h>
#include <mavpro/TakeoffAction.h>

//$ mavros services
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/CommandTOL.h>		//$ takeoff/landing command
#include <mavros_msgs/CommandBool.h> 	//$ arming command

//$ message types
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/TwistStamped.h>

 typedef actionlib::SimpleActionServer<mavpro::TakeoffAction> TakeoffActionServer;

 class TakeoffServer
 {
 public:
 	TakeoffServer() :
 	_n(NULL),
 	_as(NULL)
 	{
 		_n = new ros::NodeHandle("~");

		// //# for comanding the UAV
		// _vel_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1);

 		_as = new TakeoffActionServer(ros::NodeHandle(), "takeoff", boost::bind(&TakeoffServer::executeCb, this, _1), false);

		//# for keeping track of current altitude
 		_alt_sub = _n->subscribe<std_msgs::Float64>("/mavros/global_position/rel_alt", 1, &TakeoffServer::altCallback, this);
 		_state_sub = _n->subscribe<mavros_msgs::State>("/mavros/state", 1, &TakeoffServer::stateCallback, this);
 		_rc_sub = _n->subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 1, &TakeoffServer::radioCallback, this);

 		_rc_pub = _n->advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 1);

		//$ set parameter
 		_n->param("controller_frequency", _controller_frequency, 10.0);
 		_n->param("z_tolerance", _z_tolerance, 1);

 		ROS_WARN("Done initializing takeoff_server.");
 		_as->start();
 	}

 	~TakeoffServer()
 	{
 		if (_as != NULL)
 		{
 			delete _as;
 		}
 		if (_n != NULL)
 		{
 			delete _n;
 		}
 	}

 private:

 	TakeoffActionServer* _as;
 	mavpro::TakeoffResult _result;

 	ros::NodeHandle* _n;
 	ros::Subscriber _alt_sub;
 	ros::Subscriber _state_sub;
 	ros::Subscriber _rc_sub;

 	ros::Publisher _rc_pub;

 	double _controller_frequency;
 	double _z_tolerance;

 	double _current_alt;
 	std::string _mode;
	bool _armed;	//$ current arming status
	int _throttle_pwm;

	bool checkGCSID()
	{
		int mavros_sys_id, uav_gcs_id;

		_n->getParam("mavros/system_id", mavros_sys_id);

		ros::ServiceClient param_client = _n->serviceClient<mavros_msgs::ParamGet>("/mavros/param/get");

		mavros_msgs::ParamGet srv;
		srv.request.param_id = "SYSID_MYGCS";

		if (param_client.call(srv))
		{
			uav_gcs_id = srv.response.value.integer;
		}

		if (mavros_sys_id == uav_gcs_id)
		{
			return true;
		}
		else
		{
			ROS_ERROR("mavros system_id: %d", mavros_sys_id);
			ROS_ERROR("fcu SYSID_MYGCS: %d", uav_gcs_id);
			return false;
		}
	}

	bool setMode(const std::string mode)
	{
		ros::ServiceClient mode_client = _n->serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
		mavros_msgs::SetMode srv;

		srv.request.base_mode = 0;
		srv.request.custom_mode = mode;

		bool mode_changed = false;

		ros::Rate r(_controller_frequency);

		while ((!mode_changed) && _n->ok())
		{

			int num_calls = 0;

			if (mode_client.call(srv))
			{
				if (_mode == mode)
				{
					mode_changed = true;
					ROS_WARN("Changed to %s mode", mode.c_str());

				}
			}
			else
			{
				ROS_ERROR("Failed to switch to %s mode", mode.c_str());
				num_calls += 1;
			}

			r.sleep();

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to change mode 5 times, aborting.");
				break;
			}
		}

		return mode_changed;
	}

	void executeCb(const mavpro::TakeoffGoalConstPtr& goal)
	{

		ROS_INFO("takeoff_server has received takeoff goal");

		ros::Rate r(_controller_frequency);
		ros::NodeHandle n;

		while (n.ok())
		{

			if (_as->isPreemptRequested())
			{
				//$ TODO?
				ROS_INFO("takeoff_server preempting the current goal");
				_as->setPreempted();

				// return from execute after preempting
				return;
			}

			//for timing that gives real time even in simulation
			ros::WallTime start = ros::WallTime::now();

			//$ actually try to land
			bool done = executeTakeoff(goal->altitude);

			if (done)
			{
				return;
			}
			ros::WallDuration t_diff = ros::WallTime::now() - start;
			ROS_DEBUG_NAMED("takeoff_server","Takeoff time: %.9f\n", t_diff.toSec());

			r.sleep();
		}

		//$ if the node is killed then we'll abort and return
		_result.success = false;
		_as->setAborted(_result, "Aborting on the goal because the node has been killed");
		return;
	}

	bool executeArm(bool arming_value)
	{
		//$ TODO

		ros::ServiceClient arming_client = _n->serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
		mavros_msgs::CommandBool srv;

		srv.request.value = arming_value;

		bool arm_status_changed = false;

		while ((!arm_status_changed) && _n->ok())
		{

			int num_calls = 0;

			if (arming_client.call(srv))
			{
				ROS_WARN("Arming status changed");
				arm_status_changed = true;
			}
			else
			{
				ROS_ERROR("Failed to arm");
				num_calls += 1;
			}

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to arm 5 times, aborting.");
				break;
			}
		}

		return arm_status_changed;
	}

	void overrideThrottle(int pwm) {

		ros::Rate r(_controller_frequency);

		ROS_WARN("Overriding throttle to PWM %d", pwm);

		mavros_msgs::OverrideRCIn rc_override;
		int NO_CHANGE = 65535; 	//$ leaves RC channel value unchanged
		int RELEASE = 0;			//$ releases RC channel

		ros::WallTime start = ros::WallTime::now();

		while ((_throttle_pwm != pwm) && _n->ok())
		{
			//$ throttle is channel 2 (C++ is zero indexed)
			if (pwm == 0)	//$ clear previous override values
			{
				for (int i = 0; i < 8; i++)
				{
					rc_override.channels[i] = RELEASE;
				}
				rc_override.channels[2] = pwm;
			}
			else			//$ set override for throttle channel
			{
				for (int i = 0; i < 8; i++)
				{
					rc_override.channels[i] = NO_CHANGE;
				}
				rc_override.channels[2] = pwm;
			}

			_rc_pub.publish(rc_override);
			r.sleep();
		}
		ros::WallDuration t_diff = ros::WallTime::now() - start;
		ROS_WARN("Throttle override time: %.9f\n", t_diff.toSec());
	}

	bool isGoalReached(const double goal)
	{
		ROS_INFO("goal: %4.1f, current alt: %4.1f, error: %4.1f", goal, _current_alt, fabs(goal - _current_alt));

		if (fabs(goal - _current_alt) <= _z_tolerance)
		{
			return true;
		}
		else
		{
			return false;
		}
	}

	bool executeTakeoff(const double goal)
	{

		ros::Rate r(10);

		if (!_armed)
		{
			setMode("STABILIZE");
			overrideThrottle(1000);
			r.sleep();
			executeArm(true);
			setMode("GUIDED");
		}

		 //$ publish feedback
		mavpro::TakeoffFeedback feedback;
		feedback.altitude = _current_alt;
		_as->publishFeedback(feedback);

		 //$ check if goal has been reached
		if (isGoalReached(goal))
		{
			ROS_ERROR("SUCCESS");
			ROS_DEBUG_NAMED("takeoff_server", "Goal reached!");
			// resetState();

			_result.success = true;
			_as->setSucceeded(_result, "Goal reached.");
			return true;
		}

		mavros_msgs::CommandTOL srv;

		ros::ServiceClient takeoff_client = _n->serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");

		srv.request.min_pitch = 5;
		srv.request.yaw = 0;
		srv.request.latitude = 0;
		srv.request.longitude = 0;
		srv.request.altitude = goal;

		bool takeoff_call_success = false;

		while ((!takeoff_call_success) && _n->ok())
		{

			int num_calls = 0;

			if (takeoff_client.call(srv))
			{
				ROS_WARN("Called mavros takeoff service");
				takeoff_call_success = true;
			}
			else
			{
				ROS_ERROR("Failed to call mavros takeoff service");
				num_calls += 1;
			}

			if (num_calls > 5)
			{
				ROS_ERROR("Tried to call mavros takeoff service 5 times, aborting.");
				break;
			}
		}

		 //$ not done yet
		return false;
	}

	void altCallback(const std_msgs::Float64::ConstPtr& rel_alt_msg)
	{
		_current_alt = rel_alt_msg->data;
	}

	void stateCallback(const mavros_msgs::State::ConstPtr& state_msg)
	{
		_armed = state_msg->armed;
		_mode = state_msg->mode;
	}

	void radioCallback(const mavros_msgs::RCIn::ConstPtr& radio_msg)
	{
		_throttle_pwm = radio_msg->channels[2];
	}

	void preemptCb()
	{
		ROS_DEBUG_NAMED("takeoff_server", "Preempted!");
		 // set the action state to preempted
		_as->setPreempted();
	}

 }; //$ end of mode_clientass TakeoffServer

 int main(int argc, char **argv) {

 	ros::init(argc, argv, "takeoff_server");

 	TakeoffServer land;

 	ros::spin();
 	return 0;
 }

<|endoftext|>


<commit_before>/**
 * Copyright (c) 2017 Darius Rückert
 * Licensed under the MIT License.
 * See LICENSE file for more information.
 */

#include "Queue.h"

#include "saiga/util/easylogging++.h"

namespace Saiga
{
namespace Vulkan
{
void Queue::create(vk::Device _device, uint32_t _queueFamilyIndex, uint32_t _queueIndex,
                   vk::CommandPoolCreateFlags commandPoolCreateFlags)
{
    device           = _device;
    queueFamilyIndex = _queueFamilyIndex;
    queueIndex       = _queueIndex;
    queue            = _device.getQueue(_queueFamilyIndex, _queueIndex);
    SAIGA_ASSERT(queue);
    commandPool.create(device, _queueFamilyIndex, commandPoolCreateFlags);
}

void Queue::waitIdle()
{
    submitMutex.lock();
    queue.waitIdle();
    submitMutex.unlock();
}

void Queue::destroy()
{
    waitIdle();

    LOG(INFO) << "Destroying queue with " << commandPools.size() << " command pools";
    for (auto& pool : commandPools)
    {
        LOG(INFO) << "Destroying command pool: " << static_cast<vk::CommandPool>(pool);
        pool.destroy();
    }

    commandPool.destroy();

    //    device.destroy(*this);
}

void Queue::submitAndWait(vk::CommandBuffer cmd)
{
    if (!cmd)
    {
        return;
    }
    vk::SubmitInfo submitInfo;
    submitInfo.commandBufferCount = 1;
    submitInfo.pCommandBuffers    = &cmd;
    vk::FenceCreateInfo fci{};
    submitMutex.lock();
    auto fence = device.createFence(fci);
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
    device.waitForFences(fence, VK_TRUE, std::numeric_limits<uint64_t>::max());
    device.destroyFence(fence);
}

vk::Fence Queue::submit(vk::CommandBuffer cmd)
{
    SAIGA_ASSERT(cmd, "invalid command buffer provided");

    vk::SubmitInfo submitInfo;
    submitInfo.commandBufferCount = 1;
    submitInfo.pCommandBuffers    = &cmd;
    auto fence                    = device.createFence({});
    submitMutex.lock();
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
    return fence;
}

CommandPool Queue::createCommandPool(vk::CommandPoolCreateFlags commandPoolCreateFlags)
{
    commandPoolCreationMutex.lock();

    CommandPool newCommandPool;
    newCommandPool.create(device, queueFamilyIndex, commandPoolCreateFlags);
    LOG(INFO) << "Creating command pool: " << static_cast<vk::CommandPool>(newCommandPool);
    commandPools.push_back(newCommandPool);
    commandPoolCreationMutex.unlock();
    return newCommandPool;
}

void Queue::submit(vk::SubmitInfo submitInfo, vk::Fence fence)
{
    submitMutex.lock();
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
}

}  // namespace Vulkan
}  // namespace Saiga
<commit_msg>Fence creation must be synchronized<commit_after>/**
 * Copyright (c) 2017 Darius Rückert
 * Licensed under the MIT License.
 * See LICENSE file for more information.
 */

#include "Queue.h"

#include "saiga/util/easylogging++.h"

namespace Saiga
{
namespace Vulkan
{
void Queue::create(vk::Device _device, uint32_t _queueFamilyIndex, uint32_t _queueIndex,
                   vk::CommandPoolCreateFlags commandPoolCreateFlags)
{
    device           = _device;
    queueFamilyIndex = _queueFamilyIndex;
    queueIndex       = _queueIndex;
    queue            = _device.getQueue(_queueFamilyIndex, _queueIndex);
    SAIGA_ASSERT(queue);
    commandPool.create(device, _queueFamilyIndex, commandPoolCreateFlags);
}

void Queue::waitIdle()
{
    submitMutex.lock();
    queue.waitIdle();
    submitMutex.unlock();
}

void Queue::destroy()
{
    waitIdle();

    LOG(INFO) << "Destroying queue with " << commandPools.size() << " command pools";
    for (auto& pool : commandPools)
    {
        LOG(INFO) << "Destroying command pool: " << static_cast<vk::CommandPool>(pool);
        pool.destroy();
    }

    commandPool.destroy();

    //    device.destroy(*this);
}

void Queue::submitAndWait(vk::CommandBuffer cmd)
{
    if (!cmd)
    {
        return;
    }
    vk::SubmitInfo submitInfo;
    submitInfo.commandBufferCount = 1;
    submitInfo.pCommandBuffers    = &cmd;
    vk::FenceCreateInfo fci{};
    submitMutex.lock();
    auto fence = device.createFence(fci);
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
    device.waitForFences(fence, VK_TRUE, std::numeric_limits<uint64_t>::max());
    device.destroyFence(fence);
}

vk::Fence Queue::submit(vk::CommandBuffer cmd)
{
    SAIGA_ASSERT(cmd, "invalid command buffer provided");

    vk::SubmitInfo submitInfo;
    submitInfo.commandBufferCount = 1;
    submitInfo.pCommandBuffers    = &cmd;
    submitMutex.lock();
    auto fence                    = device.createFence({});
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
    return fence;
}

CommandPool Queue::createCommandPool(vk::CommandPoolCreateFlags commandPoolCreateFlags)
{
    commandPoolCreationMutex.lock();

    CommandPool newCommandPool;
    newCommandPool.create(device, queueFamilyIndex, commandPoolCreateFlags);
    LOG(INFO) << "Creating command pool: " << static_cast<vk::CommandPool>(newCommandPool);
    commandPools.push_back(newCommandPool);
    commandPoolCreationMutex.unlock();
    return newCommandPool;
}

void Queue::submit(vk::SubmitInfo submitInfo, vk::Fence fence)
{
    submitMutex.lock();
    queue.submit(submitInfo, fence);
    submitMutex.unlock();
}

}  // namespace Vulkan
}  // namespace Saiga
<|endoftext|>


<commit_before>// Copyright 2013-2015 Stanford University
//
// 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 "src/target/cpu_info.h"

#include <cpuid.h>
#include <tuple>
#include <vector>

using namespace std;
using namespace x64asm;

namespace {

typedef vector<pair<int, Flag>> regbit_to_flags;
typedef tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags> level_to_flags;

// Mapping from cpuid instruction results to Flag
vector<pair<unsigned, level_to_flags>> cpuid_to_flags_ {
  // level 1
  make_pair(1, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {
    make_pair(0, Flag::FPU),
    make_pair(4, Flag::TSC),
    make_pair(5, Flag::MSR),
    make_pair(8, Flag::CX8),
    make_pair(11, Flag::SEP),
    make_pair(15, Flag::CMOV),
    make_pair(19, Flag::CLFLUSH),
    make_pair(23, Flag::MMX),
    make_pair(24, Flag::FXSR),
    make_pair(25, Flag::SSE),
    make_pair(26, Flag::SSE2),
  },
  // ecx
  {
    make_pair(0, Flag::PNI),
    make_pair(1, Flag::PCLMULQDQ),
    make_pair(3, Flag::MONITOR),
    make_pair(9, Flag::SSSE3),
    make_pair(12, Flag::FMA),
    make_pair(13, Flag::CX16),
    make_pair(19, Flag::SSE4_1),
    make_pair(20, Flag::SSE4_2),
    make_pair(22, Flag::MOVBE),
    make_pair(23, Flag::POPCNT),
    make_pair(25, Flag::AES),
    make_pair(26, Flag::XSAVE),
    make_pair(28, Flag::AVX),
    make_pair(29, Flag::F16C),
    make_pair(30, Flag::RDRAND),
  },
  // ebx
  {},
  // eax
  {}
  )),
  // level 7
  make_pair(7, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {},
  // ecx
  {},
  // ebx
  {
    make_pair(0, Flag::FSGSBASE),
    make_pair(3, Flag::BMI1),
    make_pair(4, Flag::HLE),
    make_pair(5, Flag::AVX2),
    make_pair(8, Flag::BMI2),
    make_pair(9, Flag::ERMS),
    make_pair(10, Flag::INVPCID),
    make_pair(11, Flag::RTM),
  },
  // eax
  {}
  )),
  // level 0x80000001
  make_pair(0x80000001, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {
    make_pair(11, Flag::SYSCALL),
    make_pair(27, Flag::RDTSCP),
  },
  // ecx
  {
    make_pair(0, Flag::LAHF_LM),
    make_pair(5, Flag::ABM),
  },
  // ebx
  {},
  // eax
  {}
  )),
  // level 0x8000000d
  make_pair(0x8000000d, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {},
  // ecx
  {},
  // ebx
  {},
  // eax
  {
    make_pair(0, Flag::XSAVEOPT),
  }
  )),
};

void insert_flag(FlagSet& result, regbit_to_flags info, unsigned reg) {
  for (auto regbit : info) {
    if (reg & (1 << regbit.first)) {
      result += regbit.second;
    }
  }
}

} // namespace

namespace stoke {

FlagSet CpuInfo::get_flags() {
  FlagSet result;

  unsigned max = __get_cpuid_max(0, NULL);

  unsigned eax = 0, ebx = 0, ecx = 0, edx = 0;
  for (auto level : cpuid_to_flags_) {
    // is this level supported?
    if (max < (level.first & 0x80000000)) {
      continue;
    }

    // get cpuid information
    __cpuid_count(level.first, 0, eax, ebx, ecx, edx);
    insert_flag(result, get<0>(level.second), edx);
    insert_flag(result, get<1>(level.second), ecx);
    insert_flag(result, get<2>(level.second), ebx);
    insert_flag(result, get<3>(level.second), eax);
  }

  // detect multi-byte nop support
  __cpuid_count(1, 0, eax, ebx, ecx, edx);
  unsigned int model = (eax >> 8) & 0x0F;
  // support is present if model is 1111 or 0110
  if ((model == 0x0F) || (model == 0x06)) {
    result += Flag::NOPL;
  }

  // TODO: correctly detect REP_GOOD
  result += Flag::REP_GOOD;

#if defined(NEHALEM_BUILD) || defined(SANDYBRIDGE_BUILD)
  result -= Flag::AVX2;
  result -= Flag::BMI1;
  result -= Flag::BMI2;
  result -= Flag::FMA;
#endif
#ifdef NEHALEM_BUILD
  result -= Flag::AVX;
#endif

  return result;
}

} // namespace stoke
<commit_msg>remove flag that's not available on older architectures<commit_after>// Copyright 2013-2015 Stanford University
//
// 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 "src/target/cpu_info.h"

#include <cpuid.h>
#include <tuple>
#include <vector>

using namespace std;
using namespace x64asm;

namespace {

typedef vector<pair<int, Flag>> regbit_to_flags;
typedef tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags> level_to_flags;

// Mapping from cpuid instruction results to Flag
vector<pair<unsigned, level_to_flags>> cpuid_to_flags_ {
  // level 1
  make_pair(1, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {
    make_pair(0, Flag::FPU),
    make_pair(4, Flag::TSC),
    make_pair(5, Flag::MSR),
    make_pair(8, Flag::CX8),
    make_pair(11, Flag::SEP),
    make_pair(15, Flag::CMOV),
    make_pair(19, Flag::CLFLUSH),
    make_pair(23, Flag::MMX),
    make_pair(24, Flag::FXSR),
    make_pair(25, Flag::SSE),
    make_pair(26, Flag::SSE2),
  },
  // ecx
  {
    make_pair(0, Flag::PNI),
    make_pair(1, Flag::PCLMULQDQ),
    make_pair(3, Flag::MONITOR),
    make_pair(9, Flag::SSSE3),
    make_pair(12, Flag::FMA),
    make_pair(13, Flag::CX16),
    make_pair(19, Flag::SSE4_1),
    make_pair(20, Flag::SSE4_2),
    make_pair(22, Flag::MOVBE),
    make_pair(23, Flag::POPCNT),
    make_pair(25, Flag::AES),
    make_pair(26, Flag::XSAVE),
    make_pair(28, Flag::AVX),
    make_pair(29, Flag::F16C),
    make_pair(30, Flag::RDRAND),
  },
  // ebx
  {},
  // eax
  {}
  )),
  // level 7
  make_pair(7, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {},
  // ecx
  {},
  // ebx
  {
    make_pair(0, Flag::FSGSBASE),
    make_pair(3, Flag::BMI1),
    make_pair(4, Flag::HLE),
    make_pair(5, Flag::AVX2),
    make_pair(8, Flag::BMI2),
    make_pair(9, Flag::ERMS),
    make_pair(10, Flag::INVPCID),
    make_pair(11, Flag::RTM),
  },
  // eax
  {}
  )),
  // level 0x80000001
  make_pair(0x80000001, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {
    make_pair(11, Flag::SYSCALL),
    make_pair(27, Flag::RDTSCP),
  },
  // ecx
  {
    make_pair(0, Flag::LAHF_LM),
    make_pair(5, Flag::ABM),
  },
  // ebx
  {},
  // eax
  {}
  )),
  // level 0x8000000d
  make_pair(0x8000000d, make_tuple<regbit_to_flags, regbit_to_flags, regbit_to_flags, regbit_to_flags>(
    // edx
  {},
  // ecx
  {},
  // ebx
  {},
  // eax
  {
    make_pair(0, Flag::XSAVEOPT),
  }
  )),
};

void insert_flag(FlagSet& result, regbit_to_flags info, unsigned reg) {
  for (auto regbit : info) {
    if (reg & (1 << regbit.first)) {
      result += regbit.second;
    }
  }
}

} // namespace

namespace stoke {

FlagSet CpuInfo::get_flags() {
  FlagSet result;

  unsigned max = __get_cpuid_max(0, NULL);

  unsigned eax = 0, ebx = 0, ecx = 0, edx = 0;
  for (auto level : cpuid_to_flags_) {
    // is this level supported?
    if (max < (level.first & 0x80000000)) {
      continue;
    }

    // get cpuid information
    __cpuid_count(level.first, 0, eax, ebx, ecx, edx);
    insert_flag(result, get<0>(level.second), edx);
    insert_flag(result, get<1>(level.second), ecx);
    insert_flag(result, get<2>(level.second), ebx);
    insert_flag(result, get<3>(level.second), eax);
  }

  // detect multi-byte nop support
  __cpuid_count(1, 0, eax, ebx, ecx, edx);
  unsigned int model = (eax >> 8) & 0x0F;
  // support is present if model is 1111 or 0110
  if ((model == 0x0F) || (model == 0x06)) {
    result += Flag::NOPL;
  }

  // TODO: correctly detect REP_GOOD
  result += Flag::REP_GOOD;

#if defined(NEHALEM_BUILD) || defined(SANDYBRIDGE_BUILD)
  result -= Flag::AVX2;
  result -= Flag::BMI1;
  result -= Flag::BMI2;
  result -= Flag::FMA;
  result -= Flag::F16C;
#endif
#ifdef NEHALEM_BUILD
  result -= Flag::AVX;
#endif

  return result;
}

} // namespace stoke
<|endoftext|>


<commit_before>#include "include/buffer.h"
#include "include/encoding.h"

#include "gtest/gtest.h"
#include "stdlib.h"

#define MAX_TEST 1000000

TEST(BufferList, EmptyAppend) {
	bufferlist bl;
	bufferptr ptr;
	bl.push_back(ptr);
	ASSERT_EQ(bl.begin().end(), 1);
}

TEST(BufferList, TestPtrAppend) {
	bufferlist bl;
	char correct[MAX_TEST];
	int curpos = 0;
	int length = random() % 5 > 0 ? random() % 1000 : 0;
	while (curpos + length < MAX_TEST) {
		if (!length) {
			bufferptr ptr;
			bl.push_back(ptr);
		} else {
			char *current = correct + curpos;
			for (int i = 0; i < length; ++i) {
				char next = random() % 255;
				correct[curpos++] = next;
			}
			bufferptr ptr(current, length);
			bl.append(ptr);
		}
		length = random() % 5 > 0 ? random() % 1000 : 0;
	}
	ASSERT_EQ(memcmp(bl.c_str(), correct, curpos), 0);
}

TEST(BufferList, TestDirectAppend) {
	bufferlist bl;
	char correct[MAX_TEST];
	int curpos = 0;
	int length = random() % 5 > 0 ? random() % 1000 : 0;
	while (curpos + length < MAX_TEST) {
		char *current = correct + curpos;
		for (int i = 0; i < length; ++i) {
			char next = random() % 255;
			correct[curpos++] = next;
		}
		bl.append(current, length);
		length = random() % 5 > 0 ? random() % 1000 : 0;
	}
	ASSERT_EQ(memcmp(bl.c_str(), correct, curpos), 0);
}
<commit_msg>test/bufferlist.cc: retab for consistency<commit_after>#include "include/buffer.h"
#include "include/encoding.h"

#include "gtest/gtest.h"
#include "stdlib.h"

#define MAX_TEST 1000000

TEST(BufferList, EmptyAppend) {
  bufferlist bl;
  bufferptr ptr;
  bl.push_back(ptr);
  ASSERT_EQ(bl.begin().end(), 1);
}

TEST(BufferList, TestPtrAppend) {
  bufferlist bl;
  char correct[MAX_TEST];
  int curpos = 0;
  int length = random() % 5 > 0 ? random() % 1000 : 0;
  while (curpos + length < MAX_TEST) {
    if (!length) {
      bufferptr ptr;
      bl.push_back(ptr);
    } else {
      char *current = correct + curpos;
      for (int i = 0; i < length; ++i) {
        char next = random() % 255;
        correct[curpos++] = next;
      }
      bufferptr ptr(current, length);
      bl.append(ptr);
    }
    length = random() % 5 > 0 ? random() % 1000 : 0;
  }
  ASSERT_EQ(memcmp(bl.c_str(), correct, curpos), 0);
}

TEST(BufferList, TestDirectAppend) {
  bufferlist bl;
  char correct[MAX_TEST];
  int curpos = 0;
  int length = random() % 5 > 0 ? random() % 1000 : 0;
  while (curpos + length < MAX_TEST) {
    char *current = correct + curpos;
    for (int i = 0; i < length; ++i) {
      char next = random() % 255;
      correct[curpos++] = next;
    }
    bl.append(current, length);
    length = random() % 5 > 0 ? random() % 1000 : 0;
  }
  ASSERT_EQ(memcmp(bl.c_str(), correct, curpos), 0);
}
<|endoftext|>


<commit_before>#include "IntegralFieldDetector.h"
#include <cmath>

#include <Tools/Math/ConvexHull.h>

IntegralFieldDetector::IntegralFieldDetector()
:
  cameraID(CameraInfo::Bottom)
{
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:draw_grid", "", false);
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:draw_end_cell", "", false);
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:mark_field_polygon", "", false);
  getDebugParameterList().add(&params);
}

IntegralFieldDetector::~IntegralFieldDetector()
{
  getDebugParameterList().remove(&params);
}

void IntegralFieldDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;
  getFieldPercept().reset();
  endpoints.clear();

  // TODO: make it easier readable?
  if((cameraID==CameraInfo::Top)? params.set_whole_image_as_field_top: params.set_whole_image_as_field_bottom) {
    // skip field detection and set whole image under the horizon as field
    endpoints = {
      {0, 0},
      {0, static_cast<int>(getImage().height())-1},
      {static_cast<int>(getImage().width())-1, static_cast<int>(getImage().height())-1},
      {static_cast<int>(getImage().width())-1, 0},
      {0, 0} // TODO: do we need this?
    };

	// NOTE: we call convexHull here to make sure the points are in the same
	// order as with the regular calculations
    // convex hull calculation sorts endpoints in clockwise order
	// TODO: check is polygon needs first and last point to be the same
    std::vector<Vector2i> result = ConvexHull::convexHull(endpoints);

	// TODO: why is convexHull not producing Polygon directly?
    FieldPercept::FieldPoly fieldPoly;
    for(size_t i = 0; i < result.size(); i++)
    {
      fieldPoly.add(result[i]);
    }

	// TODO: is this needed at this place (params.set_whole_image_as_field)?
    getFieldPercept().setField(fieldPoly, getArtificialHorizon());

	// TODO: (MAGIC NUMBE) is this necessary?  if yes why is it not a parameter?
    if(fieldPoly.getArea() >= 5600) {
      getFieldPercept().valid = true;
    }

    DEBUG_REQUEST("Vision:IntegralFieldDetector:mark_field_polygon",
      IMAGE_DRAWING_CONTEXT;
      CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

      std::string color = getFieldPercept().valid ? "FFFF00" : "FF0000";
      PEN(color, 2);

      const FieldPercept::FieldPoly& poly = getFieldPercept().getValidField();
      for(size_t i = 1; i < poly.size(); i++)
      {
        LINE(poly[i-1].x, poly[i-1].y, poly[i].x, poly[i].y);
      }
    );
    return;
  }

  // TODO: maybe rename getBallDetectorIntegralImage to IntegralImage
  // TODO: isValid necessary?
  if(!getBallDetectorIntegralImage().isValid()) {
    return;
  }

  // NOTE: the facto may be different for top and bottom
  factor = getBallDetectorIntegralImage().FACTOR;
  const int width = getBallDetectorIntegralImage().getWidth();
  const int height = getBallDetectorIntegralImage().getHeight();

  // TODO: this gives the size of a cell in pixels, not the size of the grid itself
  // maybe it's better to have the number of cells, i.e., grid size, as parameter?
  const int grid_size = (cameraID==CameraInfo::Top ? params.grid_size_top:
                                                     params.grid_size_bottom) / factor;

  // TODO: better comments
  // determine number of grids
  const int n_cells_horizontal = width / grid_size;
  const int n_cells_vertical = height / grid_size;

  // pixels lost due to integer division
  // TODO: rest is expected to be less then n_cells
  int rest_H = width - (n_cells_horizontal * grid_size);
  int rest_V = height - (n_cells_vertical * grid_size);

  const int pixels_per_cell = grid_size * grid_size;
  const int min_green = (int)(pixels_per_cell*params.proportion_of_green);
  const int min_end_green = (int)(pixels_per_cell*params.end_proportion_of_green);

  bool first = true;
  bool green_found = false;
  bool former_green = false;
  Cell last_green_cell;
  Cell cell;
  // TODO: simplify calculations of cell bounds.
  // In perticular get rid of bilataral usage of cell.maxX and cell.minY
  // where the integer devision rest is added.
  // In short: Make everything double and calculate grid sizes and thresholds individually
  for (cell.minX=0; cell.minX + grid_size-1 < width; cell.minX = cell.maxX + 1)
  {
    // FIXME: check whether the -1 is correct here with the use of integral image
    cell.maxX = cell.minX + grid_size-1;
    int horizon_height = std::max(
          (int) (getArtificialHorizon().point(toImage(cell.maxX)).y),
          (int) (getArtificialHorizon().point(toImage(cell.minX)).y)
        );
    int min_scan_y = Math::clamp(toIntegral(horizon_height), 0, height-1);

    int skipped = 0;
    int successive_green = 0;
    int rest = rest_V; // copy, because we need it several times
    int cell_number = 1;
    // scan up
    for(cell.maxY = height-1; cell.maxY - grid_size + 1 >= min_scan_y; cell.maxY = cell.minY - 1)
    {
      // FIXME: check whether the -1 is correct here with the use of integral image
      cell.minY = cell.maxY - grid_size + 1;

      // calculate number of green pixels in the cell
      cell.sum_of_green = getBallDetectorIntegralImage().getSumForRect(
            cell.minX, cell.minY, cell.maxX, cell.maxY, 1);

      DEBUG_REQUEST("Vision:IntegralFieldDetector:draw_grid",
        IMAGE_DRAWING_CONTEXT;
        CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

        std::string color;
        if(cell.sum_of_green >= min_green) {
          color = "00FF00";
        } else {
          color = "FF00FF";
        }
        PEN(color, 1);
        BOX(toImage(cell.minX), toImage(cell.minY),
            toImage(cell.maxX), toImage(cell.maxY));
      );

      if(cell.sum_of_green >= min_green) {
        skipped = 0;
        ++successive_green;

        if (successive_green >= params.min_successive_green
            || cell_number == successive_green) {
          last_green_cell = cell;
          former_green = true;
          green_found = true;
        }
      } else {
        if(former_green && cell.sum_of_green > min_end_green) {
          last_green_cell = cell;
        }
        ++skipped;
        if (skipped > params.max_skip_cells) {
          successive_green = 0;
        }
        former_green = false;
      }
      // ensure cells will end on the upper image border
      if(rest > 0) {
        rest--;
        cell.minY--;
      }
      cell_number++;
    }

    if(green_found){
      if(first) { // find the left most endpoint
        Vector2i left_endpoint;
        find_endpoint(toImage(last_green_cell.minX), last_green_cell, left_endpoint);
        endpoints.push_back(left_endpoint);
        first = false;
      }
      Vector2i endpoint;
      find_endpoint(toImage((last_green_cell.minX + last_green_cell.maxX) / 2),
                    last_green_cell, endpoint);
      endpoints.push_back(endpoint);
    }

    // ensure cells will end on the right image border
    if(rest_H > 0) {
      rest_H--;
      cell.maxX++;
    }
  }

  if(green_found) { // find the right most endpoint
    Vector2i right_endpoint;
    find_endpoint((last_green_cell.maxX + 1) * factor - 1,
                  last_green_cell, right_endpoint);
    endpoints.push_back(right_endpoint);
  }

  if(endpoints.size() > 1) {
    create_field();
  }
}

void IntegralFieldDetector::find_endpoint(int x, const Cell& cell, Vector2i& endpoint) {
  double score = 0.;
  double best_score = 0.;
  int minY = toImage(cell.minY);
  endpoint.x = x;
  endpoint.y = toImage(cell.maxY);
  for (int y=endpoint.y; y>=minY; --y) {
    Pixel pixel = getImage().get(endpoint.x, y);
    if (getFieldColorPercept().greenHSISeparator.isColor(pixel)) {
      score += params.positive_score;
    } else {
      score += params.negative_score;
    }
    if(score >= best_score) {
      best_score = score;
      endpoint.y = y;
    }
  }
  DEBUG_REQUEST("Vision:IntegralFieldDetector:draw_end_cell",
    IMAGE_DRAWING_CONTEXT;
    CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

    PEN("1CFFE4", 1);
    BOX(toImage(cell.minX), toImage(cell.minY),
        toImage(cell.maxX), toImage(cell.maxY));
    PEN("FFFF00", 1);
    CIRCLE(endpoint.x, endpoint.y, 2);
  );
}

void IntegralFieldDetector::create_field() {
  Vector2i p1 = endpoints.front();
  p1.y = getImage().height() - 1;
  Vector2i p2 = endpoints.back();
  p2.y = getImage().height() - 1;

  endpoints.push_back(p1);
  endpoints.push_back(p2);
  std::vector<Vector2i> result = ConvexHull::convexHull(endpoints);

  FieldPercept::FieldPoly fieldPoly;
  for(size_t i = 0; i < result.size(); i++)
  {
    fieldPoly.add(result[i]);
  }

  // add field to percept
  getFieldPercept().setField(fieldPoly, getArtificialHorizon());
  // check result
  getFieldPercept().valid = fieldPoly.getArea() >= 5600;

  DEBUG_REQUEST("Vision:IntegralFieldDetector:mark_field_polygon",
    IMAGE_DRAWING_CONTEXT;
    CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

    std::string color = getFieldPercept().valid ? "FFFF00" : "FF0000";
    PEN(color, 2);

    const FieldPercept::FieldPoly& poly = getFieldPercept().getValidField();
    for(size_t i = 1; i < poly.size(); i++)
    {
      LINE(poly[i-1].x, poly[i-1].y, poly[i].x, poly[i].y);
    }
  );
}
<commit_msg>fix typos in comments<commit_after>#include "IntegralFieldDetector.h"
#include <cmath>

#include <Tools/Math/ConvexHull.h>

IntegralFieldDetector::IntegralFieldDetector()
:
  cameraID(CameraInfo::Bottom)
{
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:draw_grid", "", false);
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:draw_end_cell", "", false);
  DEBUG_REQUEST_REGISTER("Vision:IntegralFieldDetector:mark_field_polygon", "", false);
  getDebugParameterList().add(&params);
}

IntegralFieldDetector::~IntegralFieldDetector()
{
  getDebugParameterList().remove(&params);
}

void IntegralFieldDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;
  getFieldPercept().reset();
  endpoints.clear();

  // TODO: make it easier readable?
  if((cameraID==CameraInfo::Top)? params.set_whole_image_as_field_top: params.set_whole_image_as_field_bottom) {
    // skip field detection and set whole image under the horizon as field
    endpoints = {
      {0, 0},
      {0, static_cast<int>(getImage().height())-1},
      {static_cast<int>(getImage().width())-1, static_cast<int>(getImage().height())-1},
      {static_cast<int>(getImage().width())-1, 0},
      {0, 0} // TODO: do we need this?
    };

	// NOTE: we call convexHull here to make sure the points are in the same
	// order as with the regular calculations
    // convex hull calculation sorts endpoints in clockwise order
	// TODO: check if polygon needs first and last point to be the same
    std::vector<Vector2i> result = ConvexHull::convexHull(endpoints);

	// TODO: why is convexHull not producing Polygon directly?
    FieldPercept::FieldPoly fieldPoly;
    for(size_t i = 0; i < result.size(); i++)
    {
      fieldPoly.add(result[i]);
    }

	  // TODO: is this needed at this place (params.set_whole_image_as_field)?
    getFieldPercept().setField(fieldPoly, getArtificialHorizon());

	  // TODO: (MAGIC NUMBER) is this necessary?  if yes why is it not a parameter?
    if(fieldPoly.getArea() >= 5600) {
      getFieldPercept().valid = true;
    }

    DEBUG_REQUEST("Vision:IntegralFieldDetector:mark_field_polygon",
      IMAGE_DRAWING_CONTEXT;
      CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

      std::string color = getFieldPercept().valid ? "FFFF00" : "FF0000";
      PEN(color, 2);

      const FieldPercept::FieldPoly& poly = getFieldPercept().getValidField();
      for(size_t i = 1; i < poly.size(); i++)
      {
        LINE(poly[i-1].x, poly[i-1].y, poly[i].x, poly[i].y);
      }
    );
    return;
  }

  // TODO: maybe rename getBallDetectorIntegralImage to IntegralImage
  // TODO: isValid necessary?
  if(!getBallDetectorIntegralImage().isValid()) {
    return;
  }

  // NOTE: the facto may be different for top and bottom
  factor = getBallDetectorIntegralImage().FACTOR;
  const int width = getBallDetectorIntegralImage().getWidth();
  const int height = getBallDetectorIntegralImage().getHeight();

  // TODO: this gives the size of a cell in pixels, not the size of the grid itself
  // maybe it's better to have the number of cells, i.e., grid size, as parameter?
  const int grid_size = (cameraID==CameraInfo::Top ? params.grid_size_top:
                                                     params.grid_size_bottom) / factor;

  // TODO: better comments
  // determine number of grids
  const int n_cells_horizontal = width / grid_size;
  const int n_cells_vertical = height / grid_size;

  // pixels lost due to integer division
  // TODO: rest is expected to be less then n_cells
  int rest_H = width - (n_cells_horizontal * grid_size);
  int rest_V = height - (n_cells_vertical * grid_size);

  const int pixels_per_cell = grid_size * grid_size;
  const int min_green = (int)(pixels_per_cell*params.proportion_of_green);
  const int min_end_green = (int)(pixels_per_cell*params.end_proportion_of_green);

  bool first = true;
  bool green_found = false;
  bool former_green = false;
  Cell last_green_cell;
  Cell cell;
  // TODO: simplify calculations of cell bounds.
  // In perticular get rid of bilataral usage of cell.maxX and cell.minY
  // where the integer devision rest is added.
  // In short: Make everything double and calculate grid sizes and thresholds individually
  for (cell.minX=0; cell.minX + grid_size-1 < width; cell.minX = cell.maxX + 1)
  {
    // FIXME: check whether the -1 is correct here with the use of integral image
    cell.maxX = cell.minX + grid_size-1;
    int horizon_height = std::max(
          (int) (getArtificialHorizon().point(toImage(cell.maxX)).y),
          (int) (getArtificialHorizon().point(toImage(cell.minX)).y)
        );
    int min_scan_y = Math::clamp(toIntegral(horizon_height), 0, height-1);

    int skipped = 0;
    int successive_green = 0;
    int rest = rest_V; // copy, because we need it several times
    int cell_number = 1;
    // scan up
    for(cell.maxY = height-1; cell.maxY - grid_size + 1 >= min_scan_y; cell.maxY = cell.minY - 1)
    {
      // FIXME: check whether the -1 is correct here with the use of integral image
      cell.minY = cell.maxY - grid_size + 1;

      // calculate number of green pixels in the cell
      cell.sum_of_green = getBallDetectorIntegralImage().getSumForRect(
            cell.minX, cell.minY, cell.maxX, cell.maxY, 1);

      DEBUG_REQUEST("Vision:IntegralFieldDetector:draw_grid",
        IMAGE_DRAWING_CONTEXT;
        CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

        std::string color;
        if(cell.sum_of_green >= min_green) {
          color = "00FF00";
        } else {
          color = "FF00FF";
        }
        PEN(color, 1);
        BOX(toImage(cell.minX), toImage(cell.minY),
            toImage(cell.maxX), toImage(cell.maxY));
      );

      if(cell.sum_of_green >= min_green) {
        skipped = 0;
        ++successive_green;

        if (successive_green >= params.min_successive_green
            || cell_number == successive_green) {
          last_green_cell = cell;
          former_green = true;
          green_found = true;
        }
      } else {
        if(former_green && cell.sum_of_green > min_end_green) {
          last_green_cell = cell;
        }
        ++skipped;
        if (skipped > params.max_skip_cells) {
          successive_green = 0;
        }
        former_green = false;
      }
      // ensure cells will end on the upper image border
      if(rest > 0) {
        rest--;
        cell.minY--;
      }
      cell_number++;
    }

    if(green_found){
      if(first) { // find the left most endpoint
        Vector2i left_endpoint;
        find_endpoint(toImage(last_green_cell.minX), last_green_cell, left_endpoint);
        endpoints.push_back(left_endpoint);
        first = false;
      }
      Vector2i endpoint;
      find_endpoint(toImage((last_green_cell.minX + last_green_cell.maxX) / 2),
                    last_green_cell, endpoint);
      endpoints.push_back(endpoint);
    }

    // ensure cells will end on the right image border
    if(rest_H > 0) {
      rest_H--;
      cell.maxX++;
    }
  }

  if(green_found) { // find the right most endpoint
    Vector2i right_endpoint;
    find_endpoint((last_green_cell.maxX + 1) * factor - 1,
                  last_green_cell, right_endpoint);
    endpoints.push_back(right_endpoint);
  }

  if(endpoints.size() > 1) {
    create_field();
  }
}

void IntegralFieldDetector::find_endpoint(int x, const Cell& cell, Vector2i& endpoint) {
  double score = 0.;
  double best_score = 0.;
  int minY = toImage(cell.minY);
  endpoint.x = x;
  endpoint.y = toImage(cell.maxY);
  for (int y=endpoint.y; y>=minY; --y) {
    Pixel pixel = getImage().get(endpoint.x, y);
    if (getFieldColorPercept().greenHSISeparator.isColor(pixel)) {
      score += params.positive_score;
    } else {
      score += params.negative_score;
    }
    if(score >= best_score) {
      best_score = score;
      endpoint.y = y;
    }
  }
  DEBUG_REQUEST("Vision:IntegralFieldDetector:draw_end_cell",
    IMAGE_DRAWING_CONTEXT;
    CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

    PEN("1CFFE4", 1);
    BOX(toImage(cell.minX), toImage(cell.minY),
        toImage(cell.maxX), toImage(cell.maxY));
    PEN("FFFF00", 1);
    CIRCLE(endpoint.x, endpoint.y, 2);
  );
}

void IntegralFieldDetector::create_field() {
  Vector2i p1 = endpoints.front();
  p1.y = getImage().height() - 1;
  Vector2i p2 = endpoints.back();
  p2.y = getImage().height() - 1;

  endpoints.push_back(p1);
  endpoints.push_back(p2);
  std::vector<Vector2i> result = ConvexHull::convexHull(endpoints);

  FieldPercept::FieldPoly fieldPoly;
  for(size_t i = 0; i < result.size(); i++)
  {
    fieldPoly.add(result[i]);
  }

  // add field to percept
  getFieldPercept().setField(fieldPoly, getArtificialHorizon());
  // check result
  getFieldPercept().valid = fieldPoly.getArea() >= 5600;

  DEBUG_REQUEST("Vision:IntegralFieldDetector:mark_field_polygon",
    IMAGE_DRAWING_CONTEXT;
    CANVAS(((cameraID==CameraInfo::Top)? "ImageTop": "ImageBottom"));

    std::string color = getFieldPercept().valid ? "FFFF00" : "FF0000";
    PEN(color, 2);

    const FieldPercept::FieldPoly& poly = getFieldPercept().getValidField();
    for(size_t i = 1; i < poly.size(); i++)
    {
      LINE(poly[i-1].x, poly[i-1].y, poly[i].x, poly[i].y);
    }
  );
}
<|endoftext|>


<commit_before>#pragma once

#include <set>

// TODO use move semantics

namespace simple_graph {

int creations = 0;
int copies = 0;
int moves = 0;
int assigns = 0;

template<typename T>
class Vertex {
public:
    Vertex() : idx_(-1), data_() {
        ++creations;
    }
    Vertex(const Vertex<T> &v) {
        ++copies;
        idx_ = v.idx_;
        data_ = v.data_;
    }
    Vertex(Vertex<T> &&v) {
        ++moves;
        idx_ = v.idx_;
        std::swap(data_, v.data_);
    }
    explicit Vertex(int idx) : idx_(idx), data_() {
        ++creations;
    }
    Vertex(int idx, const T &data) : idx_(idx), data_(data) {}
    virtual ~Vertex() = default;

    int idx() const { return idx_; }
    T data() const { return data_; }

    bool operator<(const Vertex<T> &vertex) const {
        return idx_ < vertex.idx_;
    }

    Vertex &operator=(const Vertex<T> &v) {
        ++assigns;
        if (this != &v) {
            idx_ = v.idx_;
            data_ = v.data_;
        }
        return *this;
    }

    Vertex &operator=(Vertex<T> &&v) {
        ++moves;
        if (this != &v) {
            idx_ = v.idx_;
            std::swap(data_, v.data_);
        }
        return *this;
    }

private:
    int idx_;
    T data_;
};

template<typename T>
class Edge {
public:
    Edge() : idx1_(-1), idx2_(-1), weight_() {}
    Edge(int idx1, int idx2) : idx1_(idx1), idx2_(idx2), weight_(1) {}
    Edge(int idx1, int idx2, const T &weight) : idx1_(idx1), idx2_(idx2), weight_(weight) {}

    virtual ~Edge() = default;
    int idx1() const { return idx1_; }
    int idx2() const { return idx2_; }
    const T &weight() const { return weight_; }

private:
    int idx1_;
    int idx2_;
    T weight_;
};

template<typename V, typename E>
class Graph {
public:
    // TODO add constructor from std::initializer_list
    virtual ~Graph() = default;

    virtual int add_vertex(const Vertex<V> &vertex) = 0;
    virtual int add_vertex(Vertex<V> &&vertex) = 0;
    virtual int set_vertex(const Vertex<V> &vertex) = 0;
    virtual void rm_vertex(const Vertex<V> &vertex) = 0;
    // TODO measure performance
    virtual const std::set<int> &adjacent_vertices(int idx) const = 0;

    virtual const Vertex<V> &vertex(int idx) const = 0;
    virtual int vertex_num() const = 0;

    virtual int add_edge(const Edge<E> &edge) = 0;
    virtual const Edge<E> &edge(int idx1, int idx2) const = 0;
    virtual int rm_edge(const Edge<E> &edge) = 0;
};

}  // namespace simple_graph
<commit_msg>Add noexcept specifier for Vertex constructor and assignment operator<commit_after>#pragma once

#include <set>

// TODO use move semantics

namespace simple_graph {

int creations = 0;
int copies = 0;
int moves = 0;
int assigns = 0;

template<typename T>
class Vertex {
public:
    Vertex() : idx_(-1), data_() {
        ++creations;
    }
    Vertex(const Vertex<T> &v) {
        ++copies;
        idx_ = v.idx_;
        data_ = v.data_;
    }
    Vertex(Vertex<T> &&v) noexcept(noexcept(
                std::is_nothrow_move_constructible<T>::value
                    && std::is_nothrow_move_assignable<T>::value)) {
        ++moves;
        idx_ = v.idx_;
        std::swap(data_, v.data_);
    }
    explicit Vertex(int idx) : idx_(idx), data_() {
        ++creations;
    }
    Vertex(int idx, const T &data) : idx_(idx), data_(data) {}
    virtual ~Vertex() = default;

    int idx() const { return idx_; }
    T data() const { return data_; }

    bool operator<(const Vertex<T> &vertex) const {
        return idx_ < vertex.idx_;
    }

    Vertex &operator=(const Vertex<T> &v) {
        ++assigns;
        if (this != &v) {
            idx_ = v.idx_;
            data_ = v.data_;
        }
        return *this;
    }

    Vertex &operator=(Vertex<T> &&v) noexcept(noexcept(
                std::is_nothrow_move_constructible<T>::value
                    && std::is_nothrow_move_assignable<T>::value)) {
        ++moves;
        if (this != &v) {
            idx_ = v.idx_;
            std::swap(data_, v.data_);
        }
        return *this;
    }

private:
    int idx_;
    T data_;
};

template<typename T>
class Edge {
public:
    Edge() : idx1_(-1), idx2_(-1), weight_() {}
    Edge(int idx1, int idx2) : idx1_(idx1), idx2_(idx2), weight_(1) {}
    Edge(int idx1, int idx2, const T &weight) : idx1_(idx1), idx2_(idx2), weight_(weight) {}

    virtual ~Edge() = default;
    int idx1() const { return idx1_; }
    int idx2() const { return idx2_; }
    const T &weight() const { return weight_; }

private:
    int idx1_;
    int idx2_;
    T weight_;
};

template<typename V, typename E>
class Graph {
public:
    // TODO add constructor from std::initializer_list
    virtual ~Graph() = default;

    virtual int add_vertex(const Vertex<V> &vertex) = 0;
    virtual int add_vertex(Vertex<V> &&vertex) = 0;
    virtual int set_vertex(const Vertex<V> &vertex) = 0;
    virtual void rm_vertex(const Vertex<V> &vertex) = 0;
    // TODO measure performance
    virtual const std::set<int> &adjacent_vertices(int idx) const = 0;

    virtual const Vertex<V> &vertex(int idx) const = 0;
    virtual int vertex_num() const = 0;

    virtual int add_edge(const Edge<E> &edge) = 0;
    virtual const Edge<E> &edge(int idx1, int idx2) const = 0;
    virtual int rm_edge(const Edge<E> &edge) = 0;
};

}  // namespace simple_graph
<|endoftext|>