<commit_before>#ifndef VSMC_UTILITY_DISPATCH_HPP
#define VSMC_UTILITY_DISPATCH_HPP

#include <vsmc/internal/common.hpp>
#include <dispatch/dispatch.h>

namespace vsmc {

/// \brief Types of DispatchQueue
/// \ingroup Dispatch
enum DispatchQueueType {
    Main,    ///< The queue obtained through `dispatch_get_main_queue`
    Global,  ///< The queue obtained through `dispatch_get_gloal_queue`
    Private  ///< The queue created by `dispatch_queue_create`
};

template <DispatchQueueType> class DispatchQueue;

/// \brief Base class of Dispatch objects
/// \ingroup Dispatch
///
/// \details All Dispatch objects are reference counting shared objects
template <typename DispatchType>
class DispatchObject
{
    public :

    /// \brief Create a DispatchObject from its C-type object
    ///
    /// \details
    /// The original object will be retained by this object
    explicit DispatchObject (DispatchType object) : object_(object)
    {dispatch_retain(object);}

    DispatchObject (const DispatchObject &other) : object_(other.object_)
    {dispatch_retain(object_);}

    DispatchObject &operator= (const DispatchObject &other)
    {
        object_ = other.object_;
        dispatch_retain(object_);

        return *this;
    }

    ~DispatchObject () {dispatch_release(object_);}

    /// \brief Return the underlying Dispatch object
    const DispatchType get () const {return object_;}

    /// \brief If the object is non-NULL
    bool empty () const
    {
        if (object_)
            return false;
        else
            return true;
    }

    void *get_context () const {return dispatch_get_context(object_);}

    void set_context (void *ctx) {dispatch_set_context(object_, ctx);}

    void set_finalizer_f (dispatch_function_t finalizer)
    {dispatch_set_finalizer_t(object_, finalizer);}

    private :

    DispatchType object_;
}; // class DispatchObject

/// \brief Base class of DispatchQueue
/// \ingroup Dispatch
class DispatchQueueBase : public DispatchObject<dispatch_queue_t>
{
    public :

    void resume () {dispatch_resume(this->get());}

    void suspend () {dispatch_suspend(this->get());}

    const char *get_label () const
    {return dispatch_queue_get_label(this->get());}

#ifdef MAC_OS_X_VERSION_10_7
    void *get_specific (const void *key) const
    {return dispatch_queue_get_specific(this->get(), key);}

    void set_specific (const void *key, void *context,
            dispatch_function_t destructor)
    {dispatch_queue_set_specific(this->get(), key, context, destructor);}
#endif // MAC_OS_X_VERSION_10_7

    /// \brief Set this queue as the target queue for the object
    ///
    /// \details
    /// Note that set this queue as the target of an dispatch object will
    /// retain the queue.
    template <typename DispatchType>
    void set_as_target (const DispatchObject<DispatchType> &object) const
    {dispatch_set_target_queue(object.get(), this->get());}

    void after_f (dispatch_time_t when, void *context,
            dispatch_function_t f) const
    {dispatch_after_f(when, this->get(), context, f);}

    void apply_f (std::size_t iterations, void *context,
            void (*work) (void *, std::size_t)) const
    {dispatch_apply_f(iterations, this->get(), context, work);}

    void async_f (void *context, dispatch_function_t work) const
    {dispatch_async_f(this->get(), context, work);}

    void sync_f (void *context, dispatch_function_t work) const
    {dispatch_sync_f(this->get(), context, work);}

#ifdef MAC_OS_X_VERSION_10_7
    void barrier_async_f (void *context, dispatch_function_t work) const
    {dispatch_barrier_async_f(this->get(), context, work);}

    void barrier_sync_f (void *context, dispatch_function_t work) const
    {dispatch_barrier_sync_f(this->get(), context, work);}
#endif // MAC_OS_X_VERSION_10_7

#ifdef __BLOCKS__
    void after (dispatch_time_t when, dispatch_block_t block) const
    {dispatch_after(when, this->get(), block);}

    void apply (std::size_t iterations, void (^block) (std::size_t)) const
    {dispatch_apply(iterations, this->get(), block);}

    void async (dispatch_block_t block) const
    {dispatch_async(this->get(), block);}

    void sync (dispatch_block_t block) const
    {dispatch_sync(this->get(), block);}

#ifdef MAC_OS_X_VERSION_10_7
    void barrier_async (dispatch_block_t block) const
    {dispatch_barrier_async(this->get(), block);}

    void barrier_sync (dispatch_block_t block) const
    {dispatch_barrier_sync(this->get(), block);}

    void read (dispatch_fd_t fd, std::size_t length,
            void (^handler) (dispatch_data_t, int)) const
    {dispatch_read(fd, length, this->get(), handler);}

    void write (dispatch_fd_t fd, dispatch_data_t data,
            void (^handler) (dispatch_data_t, int)) const
    {dispatch_write(fd, data, this->get(), handler);}
#endif // MAC_OS_X_VERSION_10_7
#endif // __BLOCKS__

    protected :

    DispatchQueueBase (dispatch_queue_t queue) :
        DispatchObject<dispatch_queue_t>(queue) {}

    DispatchQueueBase (const DispatchQueueBase &other) :
        DispatchObject<dispatch_queue_t>(other) {}

    DispatchQueueBase &operator= (const DispatchQueueBase &other)
    {DispatchObject<dispatch_queue_t>::operator=(other); return *this;}
}; // class DispatchQueueBase

/// \brief The main dispatch queue (`dipatch_get_main_queue`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Main> : public DispatchQueueBase
{
    public :

    DispatchQueue () : DispatchQueueBase(dispatch_get_main_queue()) {}
}; // class DispatchQueue

/// \brief The global (concurrent) dispatch queue (`dispatch_get_gloal_queue`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Global> : public DispatchQueueBase
{
    public :

    DispatchQueue (dispatch_queue_priority_t priority =
            DISPATCH_QUEUE_PRIORITY_DEFAULT, unsigned long flags = 0) :
        DispatchQueueBase(dispatch_get_global_queue(priority, flags)) {}
}; // class DispatchQueue

/// \brief A private dispatch queue (`dispatch_queue_create`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Private> : public DispatchQueueBase
{
    public :

    DispatchQueue (const char *name, dispatch_queue_attr_t attr = NULL) :
        DispatchQueueBase(dispatch_queue_create(name, attr)) {}

    ~DispatchQueue () {dispatch_release(this->get());}
}; // class DispatchQueue

/// \brief A Dispatch group
/// \ingroup Dispatch
class DispatchGroup : public DispatchObject<dispatch_group_t>
{
    public :

    DispatchGroup () :
        DispatchObject<dispatch_group_t>(dispatch_group_create()) {}

    ~DispatchGroup () {dispatch_release(this->get());}

    void enter () {dispatch_group_enter(this->get());}

    void leave () {dispatch_group_leave(this->get());}

    long wait (dispatch_time_t timeout)
    {return dispatch_group_wait(this->get(), timeout);}

    template <DispatchQueueType Type>
    void async_f (const DispatchQueue<Type> &queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_async_f(this->get(), queue.get(), context, work);}

    void async_f (dispatch_queue_t queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_async_f(this->get(), queue, context, work);}

    template <DispatchQueueType Type>
    void notify_f (const DispatchQueue<Type> &queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_notify_f(this->get(), queue.get(), context, work);}

    void notify_f (dispatch_queue_t queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_notify_f(this->get(), queue, context, work);}

#ifdef __BLOCKS__
    template <DispatchQueueType Type>
    void async (const DispatchQueue<Type> &queue,
            dispatch_block_t block) const
    {dispatch_group_async(this->get(), queue.get(), block);}

    void async (dispatch_queue_t queue,
            dispatch_block_t block) const
    {dispatch_group_async(this->get(), queue, block);}

    template <DispatchQueueType Type>
    void notify (const DispatchQueue<Type> &queue,
            dispatch_block_t block) const
    {dispatch_group_notify(this->get(), queue.get(), block);}

    void notify (dispatch_queue_t queue,
            dispatch_block_t block) const
    {dispatch_group_notify(this->get(), queue, block);}
#endif // __BLOCKS__
}; // class DispatchGroup

/// \brief Base class of DispatchSource
/// \ingroup Dispatch
template <dispatch_source_type_t Type>
class DispatchSourceBase : public DispatchObject<dispatch_source_t>
{
    public :

    void resume () {dispatch_resume(this->get());}

    void suspend () {dispatch_suspend(this->get());}

    void cancel () {dispatch_source_cancel(this->get());}

    long test_cancel () const
    {return dispatch_source_test_cancel(this->get());}

    unsigned long get_data () const {dispatch_source_get_data(this->get());}

    uintptr_t get_handle () const {dispatch_source_get_handle(this->get());}

    unsigned long get_mask () const {dispatch_source_get_mask(this->get());}

    void set_cancel_handler_f (dispatch_function_t cancel_handler)
    {dispatch_source_set_cancel_handler_f(this->get(), cancel_handler);}

    void set_event_handler_f (dispatch_function_t event_handler)
    {dispatch_source_set_event_handler_f(this->get(), event_handler);}

#ifdef MAC_OS_X_VERSION_10_7
    void set_registration_handler_f (dispatch_function_t registration_handler)
    {
        dispatch_source_set_registration_handler_f(
                this->get(), registration_handler);
    }
#endif // MAC_OS_X_VERSION_10_7

#ifdef __BLOCKS__
    void set_cancel_handler (dispatch_block_t cancel_handler)
    {dispatch_source_set_cancel_handler(this->get(), cancel_handler);}

    void set_event_handler (dispatch_block_t event_handler)
    {dispatch_source_set_event_handler(this->get(), event_handler);}

#ifdef MAC_OS_X_VERSION_10_7
    void set_registration_handler (dispatch_block_t registration_handler)
    {
        dispatch_source_set_registration_handler(
                this->get(), registration_handler);
    }
#endif // MAC_OS_X_VERSION_10_7
#endif // __BLOCKS__

    DispatchSourceBase (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) : DispatchObject<dispatch_source_t>(
                dispatch_source_create(Type, handle, mask, queue)) {}

    DispatchSourceBase (const DispatchSourceBase &other) :
        DispatchObject<dispatch_source_t>(other) {}

    DispatchSourceBase &operator= (const DispatchSourceBase &other)
    {DispatchObject<dispatch_source_t>::operator=(other); return *this;}

    ~DispatchSourceBase () {dispatch_release(this->get());}
}; // class DispatchSourceBase

/// \brief A dispatch source
template <dispatch_source_type_t Type>
class DispatchSource : public DispatchSourceBase<Type>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) : DispatchSourceBase<Type>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) : DispatchSourceBase<Type>(
                handle, mask, queue) {}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_DATA_ADD> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>(
                handle, mask, queue) {}

    void merge_data (unsigned long value) const
    {dispatch_source_merge_data(this->get(), value);}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_DATA_OR> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>(
                handle, mask, queue) {}

    void merge_data (unsigned long value) const
    {dispatch_source_merge_data(this->get(), value);}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_TIMER> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue) {}

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue,
            dispatch_time_t start, uint64_t interval, uint64_t leeway) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue.get())
    {set_timer(start, interval, leeway);}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue,
            dispatch_time_t start, uint64_t interval, uint64_t leeway) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue)
    {set_timer(start, interval, leeway);}

    void set_timer (dispatch_time_t start, uint64_t interval, uint64_t leeway)
    {dispatch_source_set_timer(this->get(), start, interval, leeway);}
}; // class DispatchSource

} // namespace vsmc

#endif // VSMC_UTILITY_DISPATCH_HPP
<commit_msg>disable use dispatch_queue_priority_t in linux<commit_after>#ifndef VSMC_UTILITY_DISPATCH_HPP
#define VSMC_UTILITY_DISPATCH_HPP

#include <vsmc/internal/common.hpp>
#include <dispatch/dispatch.h>

namespace vsmc {

/// \brief Types of DispatchQueue
/// \ingroup Dispatch
enum DispatchQueueType {
    Main,    ///< The queue obtained through `dispatch_get_main_queue`
    Global,  ///< The queue obtained through `dispatch_get_gloal_queue`
    Private  ///< The queue created by `dispatch_queue_create`
};

template <DispatchQueueType> class DispatchQueue;

/// \brief Base class of Dispatch objects
/// \ingroup Dispatch
///
/// \details All Dispatch objects are reference counting shared objects
template <typename DispatchType>
class DispatchObject
{
    public :

    /// \brief Create a DispatchObject from its C-type object
    ///
    /// \details
    /// The original object will be retained by this object
    explicit DispatchObject (DispatchType object) : object_(object)
    {dispatch_retain(object);}

    DispatchObject (const DispatchObject &other) : object_(other.object_)
    {dispatch_retain(object_);}

    DispatchObject &operator= (const DispatchObject &other)
    {
        object_ = other.object_;
        dispatch_retain(object_);

        return *this;
    }

    ~DispatchObject () {dispatch_release(object_);}

    /// \brief Return the underlying Dispatch object
    const DispatchType get () const {return object_;}

    /// \brief If the object is non-NULL
    bool empty () const
    {
        if (object_)
            return false;
        else
            return true;
    }

    void *get_context () const {return dispatch_get_context(object_);}

    void set_context (void *ctx) {dispatch_set_context(object_, ctx);}

    void set_finalizer_f (dispatch_function_t finalizer)
    {dispatch_set_finalizer_t(object_, finalizer);}

    private :

    DispatchType object_;
}; // class DispatchObject

/// \brief Base class of DispatchQueue
/// \ingroup Dispatch
class DispatchQueueBase : public DispatchObject<dispatch_queue_t>
{
    public :

    void resume () {dispatch_resume(this->get());}

    void suspend () {dispatch_suspend(this->get());}

    const char *get_label () const
    {return dispatch_queue_get_label(this->get());}

#ifdef MAC_OS_X_VERSION_10_7
    void *get_specific (const void *key) const
    {return dispatch_queue_get_specific(this->get(), key);}

    void set_specific (const void *key, void *context,
            dispatch_function_t destructor)
    {dispatch_queue_set_specific(this->get(), key, context, destructor);}
#endif // MAC_OS_X_VERSION_10_7

    /// \brief Set this queue as the target queue for the object
    ///
    /// \details
    /// Note that set this queue as the target of an dispatch object will
    /// retain the queue.
    template <typename DispatchType>
    void set_as_target (const DispatchObject<DispatchType> &object) const
    {dispatch_set_target_queue(object.get(), this->get());}

    void after_f (dispatch_time_t when, void *context,
            dispatch_function_t f) const
    {dispatch_after_f(when, this->get(), context, f);}

    void apply_f (std::size_t iterations, void *context,
            void (*work) (void *, std::size_t)) const
    {dispatch_apply_f(iterations, this->get(), context, work);}

    void async_f (void *context, dispatch_function_t work) const
    {dispatch_async_f(this->get(), context, work);}

    void sync_f (void *context, dispatch_function_t work) const
    {dispatch_sync_f(this->get(), context, work);}

#ifdef MAC_OS_X_VERSION_10_7
    void barrier_async_f (void *context, dispatch_function_t work) const
    {dispatch_barrier_async_f(this->get(), context, work);}

    void barrier_sync_f (void *context, dispatch_function_t work) const
    {dispatch_barrier_sync_f(this->get(), context, work);}
#endif // MAC_OS_X_VERSION_10_7

#ifdef __BLOCKS__
    void after (dispatch_time_t when, dispatch_block_t block) const
    {dispatch_after(when, this->get(), block);}

    void apply (std::size_t iterations, void (^block) (std::size_t)) const
    {dispatch_apply(iterations, this->get(), block);}

    void async (dispatch_block_t block) const
    {dispatch_async(this->get(), block);}

    void sync (dispatch_block_t block) const
    {dispatch_sync(this->get(), block);}

#ifdef MAC_OS_X_VERSION_10_7
    void barrier_async (dispatch_block_t block) const
    {dispatch_barrier_async(this->get(), block);}

    void barrier_sync (dispatch_block_t block) const
    {dispatch_barrier_sync(this->get(), block);}

    void read (dispatch_fd_t fd, std::size_t length,
            void (^handler) (dispatch_data_t, int)) const
    {dispatch_read(fd, length, this->get(), handler);}

    void write (dispatch_fd_t fd, dispatch_data_t data,
            void (^handler) (dispatch_data_t, int)) const
    {dispatch_write(fd, data, this->get(), handler);}
#endif // MAC_OS_X_VERSION_10_7
#endif // __BLOCKS__

    protected :

    DispatchQueueBase (dispatch_queue_t queue) :
        DispatchObject<dispatch_queue_t>(queue) {}

    DispatchQueueBase (const DispatchQueueBase &other) :
        DispatchObject<dispatch_queue_t>(other) {}

    DispatchQueueBase &operator= (const DispatchQueueBase &other)
    {DispatchObject<dispatch_queue_t>::operator=(other); return *this;}
}; // class DispatchQueueBase

/// \brief The main dispatch queue (`dipatch_get_main_queue`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Main> : public DispatchQueueBase
{
    public :

    DispatchQueue () : DispatchQueueBase(dispatch_get_main_queue()) {}
}; // class DispatchQueue

/// \brief The global (concurrent) dispatch queue (`dispatch_get_gloal_queue`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Global> : public DispatchQueueBase
{
    public :

#ifdef MAC_OS_X_VERSION_10_7
    DispatchQueue (dispatch_queue_priority_t priority =
            DISPATCH_QUEUE_PRIORITY_DEFAULT, unsigned long flags = 0) :
        DispatchQueueBase(dispatch_get_global_queue(priority, flags)) {}
#else // MAC_OS_X_VERSION_10_7
    DispatchQueue (long priority = 0, unsigned long flags = 0) :
        DispatchQueueBase(dispatch_get_global_queue(priority, flags)) {}
#endif // MAC_OS_X_VERSION_10_7
}; // class DispatchQueue

/// \brief A private dispatch queue (`dispatch_queue_create`)
/// \ingroup Dispatch
template <>
class DispatchQueue<Private> : public DispatchQueueBase
{
    public :

    DispatchQueue (const char *name, dispatch_queue_attr_t attr = NULL) :
        DispatchQueueBase(dispatch_queue_create(name, attr)) {}

    ~DispatchQueue () {dispatch_release(this->get());}
}; // class DispatchQueue

/// \brief A Dispatch group
/// \ingroup Dispatch
class DispatchGroup : public DispatchObject<dispatch_group_t>
{
    public :

    DispatchGroup () :
        DispatchObject<dispatch_group_t>(dispatch_group_create()) {}

    ~DispatchGroup () {dispatch_release(this->get());}

    void enter () {dispatch_group_enter(this->get());}

    void leave () {dispatch_group_leave(this->get());}

    long wait (dispatch_time_t timeout)
    {return dispatch_group_wait(this->get(), timeout);}

    template <DispatchQueueType Type>
    void async_f (const DispatchQueue<Type> &queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_async_f(this->get(), queue.get(), context, work);}

    void async_f (dispatch_queue_t queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_async_f(this->get(), queue, context, work);}

    template <DispatchQueueType Type>
    void notify_f (const DispatchQueue<Type> &queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_notify_f(this->get(), queue.get(), context, work);}

    void notify_f (dispatch_queue_t queue, void *context,
            dispatch_function_t work) const
    {dispatch_group_notify_f(this->get(), queue, context, work);}

#ifdef __BLOCKS__
    template <DispatchQueueType Type>
    void async (const DispatchQueue<Type> &queue,
            dispatch_block_t block) const
    {dispatch_group_async(this->get(), queue.get(), block);}

    void async (dispatch_queue_t queue,
            dispatch_block_t block) const
    {dispatch_group_async(this->get(), queue, block);}

    template <DispatchQueueType Type>
    void notify (const DispatchQueue<Type> &queue,
            dispatch_block_t block) const
    {dispatch_group_notify(this->get(), queue.get(), block);}

    void notify (dispatch_queue_t queue,
            dispatch_block_t block) const
    {dispatch_group_notify(this->get(), queue, block);}
#endif // __BLOCKS__
}; // class DispatchGroup

/// \brief Base class of DispatchSource
/// \ingroup Dispatch
template <dispatch_source_type_t Type>
class DispatchSourceBase : public DispatchObject<dispatch_source_t>
{
    public :

    void resume () {dispatch_resume(this->get());}

    void suspend () {dispatch_suspend(this->get());}

    void cancel () {dispatch_source_cancel(this->get());}

    long test_cancel () const
    {return dispatch_source_test_cancel(this->get());}

    unsigned long get_data () const {dispatch_source_get_data(this->get());}

    uintptr_t get_handle () const {dispatch_source_get_handle(this->get());}

    unsigned long get_mask () const {dispatch_source_get_mask(this->get());}

    void set_cancel_handler_f (dispatch_function_t cancel_handler)
    {dispatch_source_set_cancel_handler_f(this->get(), cancel_handler);}

    void set_event_handler_f (dispatch_function_t event_handler)
    {dispatch_source_set_event_handler_f(this->get(), event_handler);}

#ifdef MAC_OS_X_VERSION_10_7
    void set_registration_handler_f (dispatch_function_t registration_handler)
    {
        dispatch_source_set_registration_handler_f(
                this->get(), registration_handler);
    }
#endif // MAC_OS_X_VERSION_10_7

#ifdef __BLOCKS__
    void set_cancel_handler (dispatch_block_t cancel_handler)
    {dispatch_source_set_cancel_handler(this->get(), cancel_handler);}

    void set_event_handler (dispatch_block_t event_handler)
    {dispatch_source_set_event_handler(this->get(), event_handler);}

#ifdef MAC_OS_X_VERSION_10_7
    void set_registration_handler (dispatch_block_t registration_handler)
    {
        dispatch_source_set_registration_handler(
                this->get(), registration_handler);
    }
#endif // MAC_OS_X_VERSION_10_7
#endif // __BLOCKS__

    DispatchSourceBase (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) : DispatchObject<dispatch_source_t>(
                dispatch_source_create(Type, handle, mask, queue)) {}

    DispatchSourceBase (const DispatchSourceBase &other) :
        DispatchObject<dispatch_source_t>(other) {}

    DispatchSourceBase &operator= (const DispatchSourceBase &other)
    {DispatchObject<dispatch_source_t>::operator=(other); return *this;}

    ~DispatchSourceBase () {dispatch_release(this->get());}
}; // class DispatchSourceBase

/// \brief A dispatch source
template <dispatch_source_type_t Type>
class DispatchSource : public DispatchSourceBase<Type>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) : DispatchSourceBase<Type>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) : DispatchSourceBase<Type>(
                handle, mask, queue) {}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_DATA_ADD> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_ADD>(
                handle, mask, queue) {}

    void merge_data (unsigned long value) const
    {dispatch_source_merge_data(this->get(), value);}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_DATA_OR> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_DATA_OR>(
                handle, mask, queue) {}

    void merge_data (unsigned long value) const
    {dispatch_source_merge_data(this->get(), value);}
}; // class DispatchSource

template <>
class DispatchSource<DISPATCH_SOURCE_TYPE_TIMER> :
    public DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>
{
    public :

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue.get()) {}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue) {}

    template <DispatchQueueType QType>
    DispatchSource (uintptr_t handle, unsigned long mask,
            const DispatchQueue<QType> &queue,
            dispatch_time_t start, uint64_t interval, uint64_t leeway) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue.get())
    {set_timer(start, interval, leeway);}

    DispatchSource (uintptr_t handle, unsigned long mask,
            dispatch_queue_t queue,
            dispatch_time_t start, uint64_t interval, uint64_t leeway) :
        DispatchSourceBase<DISPATCH_SOURCE_TYPE_TIMER>(
                handle, mask, queue)
    {set_timer(start, interval, leeway);}

    void set_timer (dispatch_time_t start, uint64_t interval, uint64_t leeway)
    {dispatch_source_set_timer(this->get(), start, interval, leeway);}
}; // class DispatchSource

} // namespace vsmc

#endif // VSMC_UTILITY_DISPATCH_HPP
<|endoftext|>


<commit_before>#pragma once

#include <gcl/io/policy.hpp>
#include <gcl/functional.hpp>
#include <gcl/cx/typeinfo.hpp>
// #include <gcl/mp/function_traits.hpp>

#include <unordered_map>
#include <functional>

// todo : serializable concept
// todo : add aggregate speciali rule

namespace gcl::io::serialization
{
    template <class io_policy = gcl::io::policy::binary>
    struct engine {
        template <typename on_deserialization_t>
        class in {
            using type_id_t = uint32_t;
            using storage_type = std::unordered_map<type_id_t, std::function<void()>>;
            std::istream&        input_stream;
            on_deserialization_t on_deserialize;
            storage_type         storage;

            template <typename T>
            auto generate_type_handler()
            {
                return std::pair{
                    gcl::cx::typeinfo::hashcode<T>(), std::function<void()>{[this]() {
                        T value;
                        io_policy::read(input_stream, value);
                        if (not input_stream.good())
                            throw std::runtime_error{
                                "serialization::engine::deserialize (value) / storage::mapped::operator() "};
                        on_deserialize(std::move(value));
                    }}};
            }

          public:
            in(std::istream& input, on_deserialization_t&& cb)
                : input_stream{input}
                , on_deserialize{std::forward<decltype(cb)>(cb)}
            {}

            template <typename... Ts>
            // requires ((std::default_initializable<Ts> and ...))
            in(std::istream& input, on_deserialization_t&& cb, std::tuple<Ts...>)
                : input_stream{input}
                , on_deserialize{std::forward<decltype(cb)>(cb)}
                , storage{generate_type_handler<Ts>()...}
            {}

            template <typename... Ts>
            void register_types()
            {
                (storage.insert(generate_type_handler<Ts>()), ...);
            }
            template <typename... Ts>
            void unregister_types()
            {
                (storage.erase(gcl::cx::typeinfo::hashcode<Ts>()), ...);
            }

            void deserialize()
            {
                type_id_t type_id_value;
                io_policy::read(input_stream, type_id_value);

                if (input_stream.eof())
                    return;
                if (not input_stream.good())
                    throw std::runtime_error{"serialization::engine::deserialize (typeid)"};

                try
                {
                    auto& handler = storage.at(type_id_value);
                    handler();
                }
                catch (const std::out_of_range&)
                {
                    throw std::runtime_error{
                        "serialization::engine::deserialize : unknown type extracted : cx-hash=" +
                        std::to_string(type_id_value)};
                }
            }
            template <std::size_t count = 1>
            void deserialize_n()
            {
                for (std::size_t i{0}; i < count and not input_stream.eof(); ++i)
                {
                    deserialize();
                }
            }
            void deserialize_all()
            {
                while (not input_stream.eof())
                {
                    deserialize();
                }
            }
        };
#if __clang__
        template <class on_deserialization_t, typename... Ts>
        in(std::istream&, on_deserialization_t&&, std::tuple<Ts...>) -> in<on_deserialization_t>;
#endif

        class out {
            std::ostream& output_stream;

          public:
            out(std::ostream& output)
                : output_stream{output}
            {}

            template <typename T>
            void serialize(T&& value) const
            {
                io_policy::write(output_stream, gcl::cx::typeinfo::hashcode<T>());
                io_policy::write(output_stream, std::forward<T>(value));
            }
            template <typename T>
            const out& operator<<(T&& value) const
            {
                serialize(std::forward<decltype(value)>(value));
                return *this;
            }
        };

        template <typename on_deserialization_t>
        using deserializer = in<on_deserialization_t>;
        using serializer = out;
    };
}

#include <sstream>

namespace gcl::io::tests::serialization
{
    static void test()
    {
        struct event_1 {};
        struct event_2 {};
        struct event_3 {};

        try
        {
            std::stringstream ss;

            using io_engine = typename gcl::io::serialization::engine<gcl::io::policy::binary>;
            {
                auto serializer = io_engine::out{ss};
                serializer << event_1{} << event_2{} << event_3{};
            }
            {
                using types = std::tuple<event_1, event_2, event_3>;

                int  call_counter{0};
                auto deserializer = io_engine::in{
                    ss,
                    gcl::functional::overload{
                        [&call_counter](event_3&&) mutable {
                            if (++call_counter != 3)
                                throw std::runtime_error{"gcl::io::tests::serialization::test : event_3"};
                        },
                        [&call_counter]<typename T>(T&& arg) mutable {
                            static_assert(not std::is_same_v<decltype(arg), event_3>);

                            if constexpr (std::is_same_v<T, event_1>)
                                if (++call_counter not_eq 1)
                                    throw std::runtime_error{"gcl::io::tests::serialization::test : event_1"};
                            if constexpr (std::is_same_v<T, event_2>)
                                if (++call_counter not_eq 2)
                                    throw std::runtime_error{"gcl::io::tests::serialization::test : event_2"};
                        }},
                    types{}};
                deserializer.deserialize_all();
            }
        }
        catch (const std::exception& error)
        {
            std::cerr << "error : " << error.what() << '\n';
        }
    }
}<commit_msg>[io::serialization] engine : add template restriction : T -> serializable<commit_after>#pragma once

// original POC : https://godbolt.org/z/P4a6voYqP

#include <gcl/io/policy.hpp>
#include <gcl/io/concepts.hpp>
#include <gcl/functional.hpp>
#include <gcl/cx/typeinfo.hpp>
// #include <gcl/mp/function_traits.hpp>

#include <unordered_map>
#include <functional>

// todo : serializable concept
// todo : add aggregate speciale rule

namespace gcl::io::serialization
{
    template <class io_policy = gcl::io::policy::binary>
    struct engine {
        template <typename on_deserialization_t>
        class in {
            using type_id_t = uint32_t;
            using storage_type = std::unordered_map<type_id_t, std::function<void()>>;
            std::istream&        input_stream;
            on_deserialization_t on_deserialize;
            storage_type         storage;

            template <gcl::io::concepts::serializable T>
            auto generate_type_handler()
            {
                return std::pair{
                    gcl::cx::typeinfo::hashcode<T>(), std::function<void()>{[this]() {
                        T value;
                        io_policy::read(input_stream, value);
                        if (not input_stream.good())
                            throw std::runtime_error{
                                "serialization::engine::deserialize (value) / storage::mapped::operator() "};
                        on_deserialize(std::move(value));
                    }}};
            }

          public:
            in(std::istream& input, on_deserialization_t&& cb)
                : input_stream{input}
                , on_deserialize{std::forward<decltype(cb)>(cb)}
            {}

            template <gcl::io::concepts::serializable ... Ts>
            in(std::istream& input, on_deserialization_t&& cb, std::tuple<Ts...>)
                : input_stream{input}
                , on_deserialize{std::forward<decltype(cb)>(cb)}
                , storage{generate_type_handler<Ts>()...}
            {}

            template <gcl::io::concepts::serializable ... Ts>
            void register_types()
            {
                (storage.insert(generate_type_handler<Ts>()), ...);
            }
            template <typename... Ts>
            void unregister_types()
            {
                (storage.erase(gcl::cx::typeinfo::hashcode<Ts>()), ...);
            }

            void deserialize()
            {
                type_id_t type_id_value;
                io_policy::read(input_stream, type_id_value);

                if (input_stream.eof())
                    return;
                if (not input_stream.good())
                    throw std::runtime_error{"serialization::engine::deserialize (typeid)"};

                try
                {
                    auto& handler = storage.at(type_id_value);
                    handler();
                }
                catch (const std::out_of_range&)
                {
                    throw std::runtime_error{
                        "serialization::engine::deserialize : unknown type extracted : cx-hash=" +
                        std::to_string(type_id_value)};
                }
            }
            template <std::size_t count = 1>
            void deserialize_n()
            {
                for (std::size_t i{0}; i < count and not input_stream.eof(); ++i)
                {
                    deserialize();
                }
            }
            void deserialize_n(std::size_t count)
            {
                for (std::size_t i{0}; i < count and not input_stream.eof(); ++i)
                {
                    deserialize();
                }
            }
            void deserialize_all()
            {
                while (not input_stream.eof())
                {
                    deserialize();
                }
            }
        };
#if __clang__
        template <class on_deserialization_t, typename... Ts>
        in(std::istream&, on_deserialization_t&&, std::tuple<Ts...>) -> in<on_deserialization_t>;
#endif

        class out {
            std::ostream& output_stream;

          public:
            out(std::ostream& output)
                : output_stream{output}
            {}

            template <typename T>
            void serialize(T&& value) const
            {
                io_policy::write(output_stream, gcl::cx::typeinfo::hashcode<T>());
                io_policy::write(output_stream, std::forward<T>(value));
            }
            template <typename T>
            const out& operator<<(T&& value) const
            {
                serialize(std::forward<decltype(value)>(value));
                return *this;
            }
        };

        template <typename on_deserialization_t>
        using deserializer = in<on_deserialization_t>;
        using serializer = out;
    };
}

#include <sstream>

namespace gcl::io::tests::serialization
{
    static void test()
    {
        struct event_1 {};
        struct event_2 {};
        struct event_3 {};

        try
        {
            std::stringstream ss;

            using io_engine = typename gcl::io::serialization::engine<gcl::io::policy::binary>;
            {
                auto serializer = io_engine::out{ss};
                serializer << event_1{} << event_2{} << event_3{};
            }
            {
                using types = std::tuple<event_1, event_2, event_3>;

                int  call_counter{0};
                auto deserializer = io_engine::in{
                    ss,
                    gcl::functional::overload{
                        [&call_counter](event_3&&) mutable {
                            if (++call_counter != 3)
                                throw std::runtime_error{"gcl::io::tests::serialization::test : event_3"};
                        },
                        [&call_counter]<typename T>(T&& arg) mutable {
                            static_assert(not std::is_same_v<decltype(arg), event_3>);

                            if constexpr (std::is_same_v<T, event_1>)
                                if (++call_counter not_eq 1)
                                    throw std::runtime_error{"gcl::io::tests::serialization::test : event_1"};
                            if constexpr (std::is_same_v<T, event_2>)
                                if (++call_counter not_eq 2)
                                    throw std::runtime_error{"gcl::io::tests::serialization::test : event_2"};
                        }},
                    types{}};
                deserializer.deserialize_all();
            }
        }
        catch (const std::exception& error)
        {
            std::cerr << "error : " << error.what() << '\n';
        }
    }
}<|endoftext|>


<commit_before>#ifndef IMAGE_CLOUD_COMMON_PIPELINE_POINTCLOUD_H_
#define IMAGE_CLOUD_COMMON_PIPELINE_POINTCLOUD_H_

#include <image_cloud/common/filter/pcl/common.hpp>
#include <image_cloud/common/filter/pcl/filter_depth_intensity.hpp>
#include <image_cloud/common/filter/pcl/segmentation.hpp>
#include <image_cloud/common/filter/pcl/depth_filter.hpp>
#include <image_cloud/common/filter/pcl/depth_edge.hpp>
#include <image_cloud/common/filter/pcl/normal_diff_filter.hpp>
#include <image_cloud/common/filter/pcl/range_borders.hpp>
#include <image_cloud/common/filter/pcl/remove_cluster_2d.hpp>
#include <image_cloud/common/filter/pcl/depth_filter_radius.hpp>
#include <image_cloud/common/filter/pcl/depth_filter_neighbors.hpp>
#include <image_cloud/common/filter/pcl/filter_depth_projection.hpp>
#include <image_cloud/common/filter/pcl/hit_same_point.hpp>
#include <image_cloud/common/filter/pcl/edge_image_plane.hpp>
#include <image_cloud/common/project2d.hpp>
#include <image_cloud/common/calibration/pipeline/enums.h>
#include <assert.h>
#include <opencv2/opencv.hpp>

namespace image_cloud{

template <typename PointT>
inline void
filter3d_switch(const pcl::PointCloud<PointT> &in_points,
		pcl::PointCloud<PointT> &out_points,
		const image_geometry::PinholeCameraModel &camera_model,
		pcl_filter::Filter3d filter,
		int rows=0,
		int cols=0){

	switch (filter)
	{
		case pcl_filter::OFF:
				out_points = in_points;
			break;
		case pcl_filter::DEPTH:
		{
			filter_3d::filter_depth_discontinuity(in_points, out_points); // epsilon
		}
		break;

		case pcl_filter::DEPTH_INTENSITY:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
									std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);
			filter_3d::filter_depth_intensity<PointT>(map, out_points);
		}
		break;
		case pcl_filter::DEPTH_EDGE:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
												std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);
			filter_3d::depth_edge<PointT>(map, out_points); // neighbors
		}
		break;
		case pcl_filter::NORMAL_DIFF:
		{
			filter_3d::normal_diff_filter<PointT>(in_points, out_points); // threshold
		}
		break;
		case pcl_filter::REMOVE_CLUSTER_2D:
		{
			filter_3d::remove_cluster_2d<PointT>(camera_model, in_points, out_points, rows, cols); // threshold
		}
		break;
		case pcl_filter::RANGE_BORDERS:
		{
			filter_3d::range_borders<PointT>(in_points, out_points); // threshold
		}
		break;
		case pcl_filter::DEPTH_RADIUS:
		{
			filter_3d::depth_discontinuity_radius<PointT>(in_points, out_points); // min neighbors
		}
		break;
		case pcl_filter::DEPTH_NEIGHBORS:
		{
			filter_3d::depth_filter_neighbors<PointT>(in_points, out_points); // max distance
		}
		break;
		case pcl_filter::DEPTH_EDGE_PROJECTION:
		{
			filter_3d::filter_depth_projection<PointT>(camera_model, in_points, out_points, rows, cols); // neighbors); // max distance
		}
		break;
		case pcl_filter::HIT_SAME_POINT:
		{
			filter_3d::hit_same_point<PointT>(camera_model, in_points, out_points, rows, cols);
		}
		break;
		case pcl_filter::OTHER:
		{
			filter_3d::segmentation<PointT>(in_points, out_points);
				//filtred = transformed;
		}
		break;
		case pcl_filter::DEPTH_INTENSITY_NORMAL_DIFF:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
									std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);

			pcl::PointCloud<PointT> temp_points;
			filter_3d::filter_depth_intensity<PointT>(map, temp_points);
			filter_3d::depth_discontinuity_radius<PointT>(temp_points, out_points, 0.5, 0.05, 70); // min neighbors

			//filter_3d::normal_diff_filter<PointT>(temp_points, out_points);
		}
		break;
		case pcl_filter::DEPTH_INTENSITY_AND_REMOVE_CLUSER_2D:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
			std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);

			pcl::PointCloud<PointT> temp_points;
			filter_3d::filter_depth_intensity<PointT>(map, temp_points);
			filter_3d::remove_cluster_2d<PointT>(camera_model, temp_points, out_points, rows, cols); // threshold
		}
		break;
		case pcl_filter::REMOVE_CLUSER_2D_RADIUS_SEARCH:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::remove_cluster_2d<PointT>(camera_model, in_points, temp_points, rows, cols); // threshold
			filter_3d::depth_discontinuity_radius<PointT>(temp_points, out_points); // min neighbors
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE:
		{
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, out_points, rows, cols);
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE_2D_RADIUS_SEARCH:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, temp_points, rows, cols);
			filter_3d::remove_cluster_2d<PointT>(camera_model,temp_points, out_points, rows, cols); // min neighbors
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE_NORMAL_DIFF:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, temp_points, rows, cols);
			filter_3d::normal_diff_filter<PointT>(temp_points, out_points, 0.001,0.1);
		}
		break;
		default:
			break;
	}
}

}

#endif
<commit_msg>added depth_edge_projection filter for aggregated pointclouds<commit_after>#ifndef IMAGE_CLOUD_COMMON_PIPELINE_POINTCLOUD_H_
#define IMAGE_CLOUD_COMMON_PIPELINE_POINTCLOUD_H_

#include <image_cloud/common/filter/pcl/common.hpp>
#include <image_cloud/common/filter/pcl/filter_depth_intensity.hpp>
#include <image_cloud/common/filter/pcl/segmentation.hpp>
#include <image_cloud/common/filter/pcl/depth_filter.hpp>
#include <image_cloud/common/filter/pcl/depth_edge.hpp>
#include <image_cloud/common/filter/pcl/normal_diff_filter.hpp>
#include <image_cloud/common/filter/pcl/range_borders.hpp>
#include <image_cloud/common/filter/pcl/remove_cluster_2d.hpp>
#include <image_cloud/common/filter/pcl/depth_filter_radius.hpp>
#include <image_cloud/common/filter/pcl/depth_filter_neighbors.hpp>
#include <image_cloud/common/filter/pcl/filter_depth_projection.hpp>
#include <image_cloud/common/filter/pcl/hit_same_point.hpp>
#include <image_cloud/common/filter/pcl/edge_image_plane.hpp>
#include <image_cloud/common/project2d.hpp>
#include <image_cloud/common/calibration/pipeline/enums.h>
#include <assert.h>
#include <opencv2/opencv.hpp>

namespace image_cloud{

template <typename PointT>
inline void
filter3d_switch(const pcl::PointCloud<PointT> &in_points,
		pcl::PointCloud<PointT> &out_points,
		const image_geometry::PinholeCameraModel &camera_model,
		pcl_filter::Filter3d filter,
		int rows=0,
		int cols=0){

	switch (filter)
	{
		case pcl_filter::OFF:
				out_points = in_points;
			break;
		case pcl_filter::DEPTH:
		{
			filter_3d::filter_depth_discontinuity(in_points, out_points); // epsilon
		}
		break;

		case pcl_filter::DEPTH_INTENSITY:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
									std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);
			filter_3d::filter_depth_intensity<PointT>(map, out_points);
		}
		break;
		case pcl_filter::DEPTH_EDGE:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
												std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);
			filter_3d::depth_edge<PointT>(map, out_points); // neighbors
		}
		break;
		case pcl_filter::NORMAL_DIFF:
		{
			filter_3d::normal_diff_filter<PointT>(in_points, out_points); // threshold
		}
		break;
		case pcl_filter::REMOVE_CLUSTER_2D:
		{
			filter_3d::remove_cluster_2d<PointT>(camera_model, in_points, out_points, rows, cols); // threshold
		}
		break;
		case pcl_filter::RANGE_BORDERS:
		{
			filter_3d::range_borders<PointT>(in_points, out_points); // threshold
		}
		break;
		case pcl_filter::DEPTH_RADIUS:
		{
			filter_3d::depth_discontinuity_radius<PointT>(in_points, out_points); // min neighbors
		}
		break;
		case pcl_filter::DEPTH_NEIGHBORS:
		{
			filter_3d::depth_filter_neighbors<PointT>(in_points, out_points); // max distance
		}
		break;
		case pcl_filter::DEPTH_EDGE_PROJECTION:
		{
			filter_3d::filter_depth_projection<PointT>(camera_model, in_points, out_points, rows, cols); // neighbors); // max distance
		}
		break;
		case pcl_filter::HIT_SAME_POINT:
		{
			filter_3d::hit_same_point<PointT>(camera_model, in_points, out_points, rows, cols);
		}
		break;
		case pcl_filter::OTHER:
		{
			filter_3d::segmentation<PointT>(in_points, out_points);
				//filtred = transformed;
		}
		break;
		case pcl_filter::DEPTH_INTENSITY_NORMAL_DIFF:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
									std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);

			pcl::PointCloud<PointT> temp_points;
			filter_3d::filter_depth_intensity<PointT>(map, temp_points);
			filter_3d::depth_discontinuity_radius<PointT>(temp_points, out_points, 0.5, 0.05, 70); // min neighbors

			//filter_3d::normal_diff_filter<PointT>(temp_points, out_points);
		}
		break;
		case pcl_filter::DEPTH_INTENSITY_AND_REMOVE_CLUSER_2D:
		{
			std::vector<std::vector<boost::shared_ptr<PointT> > > map(cols,
			std::vector<boost::shared_ptr<PointT> > (rows));
			Projected_pointcloud<PointT> projected_pointclouds;
			project2d::project_2d(camera_model, in_points, map, projected_pointclouds, cols, rows);

			pcl::PointCloud<PointT> temp_points;
			filter_3d::filter_depth_intensity<PointT>(map, temp_points);
			filter_3d::remove_cluster_2d<PointT>(camera_model, temp_points, out_points, rows, cols); // threshold
		}
		break;
		case pcl_filter::REMOVE_CLUSER_2D_RADIUS_SEARCH:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::remove_cluster_2d<PointT>(camera_model, in_points, temp_points, rows, cols); // threshold
			filter_3d::depth_discontinuity_radius<PointT>(temp_points, out_points); // min neighbors
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE:
		{
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, out_points, rows, cols);
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE_2D_RADIUS_SEARCH:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, temp_points, rows, cols);
			filter_3d::remove_cluster_2d<PointT>(camera_model,temp_points, out_points, rows, cols); // min neighbors
		}
		break;
		case pcl_filter::EDGE_IMAGE_PLANE_NORMAL_DIFF:
		{
			pcl::PointCloud<PointT> temp_points;
			filter_3d::edge_image_plane<PointT,uchar>(camera_model, in_points, temp_points, rows, cols);
			filter_3d::normal_diff_filter<PointT>(temp_points, out_points, 0.001,0.1);
		}
		break;
		case pcl_filter::DEPTH_EDGE_PROJECTION_AGGREGATED:
		{
			filter_3d::filter_depth_projection<PointT>(camera_model, in_points, out_points, rows, cols, 2, 1.5); // neighbors); // max distance
		}
		break;
		default:
			break;
	}
}

}

#endif
<|endoftext|>


<commit_before>#include "bloomierFilter.h"

BloomierFilter::BloomierFilter(size_t pM, size_t pK, size_t pQ){
	// mHashSeed = pHashSeed;
	mM = pM;;
	mK = pK;
	mQ = pQ;
	// mKeyDict = pKeyDict;
	mBitVectorSize = ceil(pQ / static_cast<float>(CHAR_BIT));
	mBloomierHash = new BloomierHash(pM, pK, mBitVectorSize);
	mOrderAndMatchFinder = new OrderAndMatchFinder(pM, pK, pQ, mBloomierHash);
	// vsize_t orderAndMatch = mOrderAndMatchFinder.find(); // datatype?
	// mByteSize = this->getByteSize(pQ);
	mTable = new bloomierTable(pM);
	mValueTable = new vvsize_t_p(pM);
	for (size_t i = 0; i < pM; ++i) {
		(*mTable)[i] = new bitVector(mBitVectorSize, 0);
		(*mValueTable)[i] = new vsize_t();
	}
    // mValueTable = new vvsize_t_p(pM);

    mEncoder = new Encoder(mBitVectorSize);
	mPiIndex = 0;
    // this->create(pKeyDict, orderAndMatch);
}

BloomierFilter::~BloomierFilter(){

}
void BloomierFilter::check() {
	std::cout << __LINE__ << std::endl;
	size_t sumTable = 0;
	for(size_t i = 0; i < mTable->size(); ++i) {
		sumTable += (*mTable)[i]->size();
	}
	std::cout << __LINE__ << std::endl;
	size_t sumValueTable = 0;
	for(size_t i = 0; i < mValueTable->size(); ++i) {
		sumValueTable += (*mValueTable)[i]->size();
	}
	std::cout << "sumTable: "  << sumTable << std::endl;
	std::cout << "sumValueTable: "  << sumValueTable << std::endl;

	std::cout << __LINE__ << std::endl;

}
bloomierTable* BloomierFilter::getTable() {
	return mTable;
}

void BloomierFilter::setTable(bloomierTable* pTable) {
	mTable = pTable;
}

vvsize_t_p* BloomierFilter::getValueTable() {
	return mValueTable;
}
void BloomierFilter::setValueTable(vvsize_t_p* pTable) {
	mValueTable = pTable;
}

void BloomierFilter::xorOperation(bitVector* pValue, bitVector* pMask, vsize_t* pNeighbors) {
	// std::cout << "41" << std::endl;
	// std::cout << __LINE__ << std::endl;

	this->xorBitVector(pValue, pMask);
	// std::cout << "44" << std::endl;
	// std::cout << __LINE__ << std::endl;

	for (auto it = pNeighbors->begin(); it != pNeighbors->end(); ++it) {
	// std::cout << "47" << std::endl;
	// std::cout << __LINE__ << std::endl;
		if (*it < pNeighbors->size()) {
			this->xorBitVector(pValue, (*mTable)[(*it)]);
		}
	// std::cout << __LINE__ << std::endl;

	// std::cout << "50" << std::endl;

	}
	// return pValue;
}
vsize_t* BloomierFilter::get(size_t pKey) {
	std::cout << __LINE__ << std::endl;

	check();
	std::cout << __LINE__ << std::endl;

	vsize_t* neighbors = mBloomierHash->getKNeighbors(pKey, mK, mM);
	std::cout << __LINE__ << ":)" << std::endl;

	bitVector* mask = mBloomierHash->getMask(pKey);
	std::cout << __LINE__ << ":(" << std::endl;

	bitVector* valueToGet = new bitVector(mBitVectorSize, 0);
	std::cout << __LINE__ << std::endl;

	this->xorOperation(valueToGet, mask, neighbors);
	std::cout << __LINE__ << std::endl;

	size_t h = mEncoder->decode(valueToGet);
	std::cout << __LINE__ << std::endl;

	if (h < neighbors->size()) {
	std::cout << __LINE__ << std::endl;

		size_t L = (*neighbors)[h];
	std::cout << __LINE__ << std::endl;

		if (L < mValueTable->size()) {
	std::cout << __LINE__ << std::endl;

			return (*mValueTable)[L];
		}
	}
	std::cout << __LINE__ << std::endl;

	check();
	std::cout << __LINE__ << std::endl;

	return new vsize_t();
}
bool BloomierFilter::set(size_t pKey, size_t pValue) {
	// std::cout << __LINE__ << std::endl;
	// check();
	// std::cout << __LINE__ << std::endl;

	vsize_t* neighbors = mBloomierHash->getKNeighbors(pKey, mK, mM);
	bitVector* mask = mBloomierHash->getMask(pKey);
	bitVector* valueToGet = new bitVector(mBitVectorSize, 0);
	this->xorOperation(valueToGet, mask, neighbors);
	size_t h = mEncoder->decode(valueToGet);
	// std::cout << __LINE__ << std::endl;
	if (h < neighbors->size()) {
		size_t L = (*neighbors)[h];
		if (L < mValueTable->size()) {
				vsize_t* v = ((*mValueTable)[L]);
			// if (v == NULL) {
			// 	v = new vsize_t();
			// }
	// std::cout << __LINE__ << std::endl;
			v->push_back(pValue);
	// std::cout << __LINE__ << std::endl;
			// (*mValueTable)[L] = v;
	// std::cout << __LINE__ << std::endl;
			return true;
		}
	// std::cout << __LINE__ << std::endl;
	} else {
	// std::cout << __LINE__ << std::endl;
		vsize_t* keys = new vsize_t (1, pKey);
		vvsize_t_p* values = new vvsize_t_p (1);
		(*values)[0] = new vsize_t(1, pValue);
		this->create(keys, values, mPiIndex);
	// std::cout << __LINE__ << std::endl;

		// check();
	// std::cout << __LINE__ << std::endl;

		return true;
	}
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;

	return false;
}

void BloomierFilter::create(vsize_t* pKeys, vvsize_t_p* pValues, size_t piIndex) {
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;
	// std::cout << "size if pKeys: " << pKeys->size() << std::endl;

	// std::cout << std::endl;
	mOrderAndMatchFinder->find(pKeys);
	// std::cout << "120" << std::endl;

    vsize_t* piVector = mOrderAndMatchFinder->getPiVector();
	// std::cout << "123" << std::endl;

	vsize_t* tauVector = mOrderAndMatchFinder->getTauVector();
	// std::cout << "piVector" << std::endl;
	// for (size_t i = 0; i < piVector->size(); ++i) {
		// std::cout << "\t" << (*piVector)[i] << std::endl;
	// }
	for (size_t i = piIndex; i < piVector->size(); ++i) {
		// size_t key = (*piList)[i];
		// size_t value = pAssignment[key];
		vsize_t* neighbors = mBloomierHash->getKNeighbors((*pKeys)[i], mK, mM);
		bitVector* mask = mBloomierHash->getMask((*pKeys)[i]);
		size_t l = (*tauVector)[i];
		size_t L = (*neighbors)[l];

		bitVector* encodeValue = mEncoder->encode(l);
		bitVector* valueToStore = new bitVector(mBitVectorSize, 0);
		this->xorBitVector(valueToStore, encodeValue);
		this->xorBitVector(valueToStore, mask);
		for (size_t j = 0; j < neighbors->size(); ++j) {
			if (j != l) {
				this->xorBitVector(valueToStore, (*mTable)[(*neighbors)[i]]);
			}
		}
		(*mTable)[L] = valueToStore;
		// std::cout << "size of piVector: " << piVector->size() << std::endl;
		// std::cout << "i: " << i << std::endl;
		// std::cout << "size of pvalues: " <<  pValues->size() << std::endl;
		(*mValueTable)[L] = (*pValues)[i-piIndex];
	}
	mPiIndex = mPiIndex + pKeys->size();
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;

}

void BloomierFilter::xorBitVector(bitVector* pResult, bitVector* pInput) {
	size_t length = std::min(pResult->size(), pInput->size());
	for (size_t i = 0; i < length; ++i) {
		(*pResult)[i] = (*pResult)[i] ^ (*pInput)[i];
	}
}<commit_msg>bug fix<commit_after>#include "bloomierFilter.h"

BloomierFilter::BloomierFilter(size_t pM, size_t pK, size_t pQ){
	// mHashSeed = pHashSeed;
	mM = pM;;
	mK = pK;
	mQ = pQ;
	// mKeyDict = pKeyDict;
	mBitVectorSize = ceil(pQ / static_cast<float>(CHAR_BIT));
	mBloomierHash = new BloomierHash(pM, pK, mBitVectorSize);
	mOrderAndMatchFinder = new OrderAndMatchFinder(pM, pK, pQ, mBloomierHash);
	// vsize_t orderAndMatch = mOrderAndMatchFinder.find(); // datatype?
	// mByteSize = this->getByteSize(pQ);
	mTable = new bloomierTable(pM);
	mValueTable = new vvsize_t_p(pM);
	for (size_t i = 0; i < pM; ++i) {
		(*mTable)[i] = new bitVector(mBitVectorSize, 0);
		(*mValueTable)[i] = new vsize_t();
	}
    // mValueTable = new vvsize_t_p(pM);

    mEncoder = new Encoder(mBitVectorSize);
	mPiIndex = 0;
    // this->create(pKeyDict, orderAndMatch);
}

BloomierFilter::~BloomierFilter(){

}
void BloomierFilter::check() {
	std::cout << __LINE__ << std::endl;
	size_t sumTable = 0;
	for(size_t i = 0; i < mTable->size(); ++i) {
		sumTable += (*mTable)[i]->size();
	}
	std::cout << __LINE__ << std::endl;
	size_t sumValueTable = 0;
	for(size_t i = 0; i < mValueTable->size(); ++i) {
		sumValueTable += (*mValueTable)[i]->size();
	}
	std::cout << "sumTable: "  << sumTable << std::endl;
	std::cout << "sumValueTable: "  << sumValueTable << std::endl;

	std::cout << __LINE__ << std::endl;

}
bloomierTable* BloomierFilter::getTable() {
	return mTable;
}

void BloomierFilter::setTable(bloomierTable* pTable) {
	mTable = pTable;
}

vvsize_t_p* BloomierFilter::getValueTable() {
	return mValueTable;
}
void BloomierFilter::setValueTable(vvsize_t_p* pTable) {
	mValueTable = pTable;
}

void BloomierFilter::xorOperation(bitVector* pValue, bitVector* pMask, vsize_t* pNeighbors) {
	// std::cout << "41" << std::endl;
	// std::cout << __LINE__ << std::endl;

	this->xorBitVector(pValue, pMask);
	// std::cout << "44" << std::endl;
	// std::cout << __LINE__ << std::endl;

	for (auto it = pNeighbors->begin(); it != pNeighbors->end(); ++it) {
	// std::cout << "47" << std::endl;
	// std::cout << __LINE__ << std::endl;
		if (*it < pNeighbors->size()) {
			this->xorBitVector(pValue, (*mTable)[(*it)]);
		}
	// std::cout << __LINE__ << std::endl;

	// std::cout << "50" << std::endl;

	}
	// return pValue;
}
vsize_t* BloomierFilter::get(size_t pKey) {
	std::cout << __LINE__ << std::endl;

	check();
	std::cout << __LINE__ << std::endl;

	vsize_t* neighbors = mBloomierHash->getKNeighbors(pKey, mK, mM);
	std::cout << __LINE__ << ":)" << std::endl;

	bitVector* mask = mBloomierHash->getMask(pKey);
	std::cout << __LINE__ << ":(" << std::endl;

	bitVector* valueToGet = new bitVector(mBitVectorSize, 0);
	std::cout << __LINE__ << std::endl;

	this->xorOperation(valueToGet, mask, neighbors);
	std::cout << __LINE__ << std::endl;

	size_t h = mEncoder->decode(valueToGet);
	std::cout << __LINE__ << std::endl;

	if (h < neighbors->size()) {
	std::cout << __LINE__ << std::endl;

		size_t L = (*neighbors)[h];
	std::cout << __LINE__ << std::endl;

		if (L < mValueTable->size()) {
	std::cout << __LINE__ << std::endl;

			return (*mValueTable)[L];
		}
	}
	std::cout << __LINE__ << std::endl;

	check();
	std::cout << __LINE__ << std::endl;

	return new vsize_t();
}
bool BloomierFilter::set(size_t pKey, size_t pValue) {
	// std::cout << __LINE__ << std::endl;
	// check();
	// std::cout << __LINE__ << std::endl;

	vsize_t* neighbors = mBloomierHash->getKNeighbors(pKey, mK, mM);
	bitVector* mask = mBloomierHash->getMask(pKey);
	bitVector* valueToGet = new bitVector(mBitVectorSize, 0);
	this->xorOperation(valueToGet, mask, neighbors);
	size_t h = mEncoder->decode(valueToGet);
	// std::cout << __LINE__ << std::endl;
	if (h < neighbors->size()) {
		size_t L = (*neighbors)[h];
		if (L < mValueTable->size()) {
				vsize_t* v = ((*mValueTable)[L]);
			// if (v == NULL) {
			// 	v = new vsize_t();
			// }
	// std::cout << __LINE__ << std::endl;
			v->push_back(pValue);
	// std::cout << __LINE__ << std::endl;
			// (*mValueTable)[L] = v;
	// std::cout << __LINE__ << std::endl;
			return true;
		}
	// std::cout << __LINE__ << std::endl;
	} else {
	// std::cout << __LINE__ << std::endl;
		vsize_t* keys = new vsize_t (1, pKey);
		vvsize_t_p* values = new vvsize_t_p (1);
		(*values)[0] = new vsize_t(1, pValue);
		this->create(keys, values, mPiIndex);
	// std::cout << __LINE__ << std::endl;

		// check();
	// std::cout << __LINE__ << std::endl;

		return true;
	}
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;

	return false;
}

void BloomierFilter::create(vsize_t* pKeys, vvsize_t_p* pValues, size_t piIndex) {
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;
	// std::cout << "size if pKeys: " << pKeys->size() << std::endl;

	// std::cout << std::endl;
	mOrderAndMatchFinder->find(pKeys);
	// std::cout << "120" << std::endl;

    vsize_t* piVector = mOrderAndMatchFinder->getPiVector();
	// std::cout << "123" << std::endl;

	vsize_t* tauVector = mOrderAndMatchFinder->getTauVector();
	// std::cout << "piVector" << std::endl;
	// for (size_t i = 0; i < piVector->size(); ++i) {
		// std::cout << "\t" << (*piVector)[i] << std::endl;
	// }
	for (size_t i = piIndex; i < piVector->size(); ++i) {
		// size_t key = (*piList)[i];
		// size_t value = pAssignment[key];
		vsize_t* neighbors = mBloomierHash->getKNeighbors((*pKeys)[i], mK, mM);
		bitVector* mask = mBloomierHash->getMask((*pKeys)[i]);
		size_t l = (*tauVector)[i];
		size_t L = (*neighbors)[l];

		bitVector* encodeValue = mEncoder->encode(l);
		// bitVector* valueToStore = new bitVector(mBitVectorSize, 0);
		this->xorBitVector((*mTable)[L], encodeValue);
		this->xorBitVector((*mTable)[L], mask);
		for (size_t j = 0; j < neighbors->size(); ++j) {
			if (j != l) {
				this->xorBitVector((*mTable)[L], (*mTable)[(*neighbors)[j]]);
			}
		}
		// (*mTable)[L] = valueToStore;
		// std::cout << "size of piVector: " << piVector->size() << std::endl;
		// std::cout << "i: " << i << std::endl;
		// std::cout << "size of pvalues: " <<  pValues->size() << std::endl;
		(*mValueTable)[L] = (*pValues)[i-piIndex];
	}
	mPiIndex = mPiIndex + pKeys->size();
	// std::cout << __LINE__ << std::endl;

	// check();
	// std::cout << __LINE__ << std::endl;

}

void BloomierFilter::xorBitVector(bitVector* pResult, bitVector* pInput) {
	size_t length = std::min(pResult->size(), pInput->size());
	for (size_t i = 0; i < length; ++i) {
		(*pResult)[i] = (*pResult)[i] ^ (*pInput)[i];
	}
}<|endoftext|>


<commit_before>#include "thruster_tortuga.h"

ThrusterTortugaNode::ThrusterTortugaNode(int argc, char **argv, int rate): TortugaNode(){
    ros::Rate loop_rate(rate);
    subscriber = n.subscribe("/qubo/thruster_input", 1000, &ThrusterTortugaNode::thrusterCallBack, this);

    ROS_DEBUG("Opening sensorboard");
    sensor_file = "/dev/sensor";
    fd = openSensorBoard(sensor_file.c_str());
    ROS_DEBUG("Opened sensorboard with fd %d.", fd);
    checkError(syncBoard(fd));
    ROS_DEBUG("Synced with the Board");

    //GH: This is not the correct use of checkError.
    //It should be called on the return value of a function call, not on the file descriptor.
    //checkError(fd);

    // Unsafe all the thrusters
    ROS_DEBUG("Unsafing all thrusters");
    for (int i = 6; i <= 11; i++) {
        checkError(setThrusterSafety(fd, i));
    }
    ROS_DEBUG("Unsafed all thrusters");

}

ThrusterTortugaNode::~ThrusterTortugaNode(){
    //Stop all the thrusters
    ROS_DEBUG("Stopping thrusters");
    readSpeedResponses(fd);
    setSpeeds(fd, 0, 0, 0, 0, 0, 0);
    ROS_DEBUG("Safing thrusters");
    // Safe all the thrusters
    for (int i = 0; i <= 5; i++) {
        checkError(setThrusterSafety(fd, i));
    }
    ROS_DEBUG("Safed thrusters");
    //Close the sensorboard
    close(fd);
}

void ThrusterTortugaNode::update(){
    //I think we need to initialize thrusters and stuff before this will work
    ros::spinOnce();
    // setSpeeds(fd, msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
    // ROS_DEBUG("fd = %x",fd);

    ROS_DEBUG("Setting thruster speeds");
    int retR = readSpeedResponses(fd);
    ROS_DEBUG("Read speed before: %x", retR);
    int retS = setSpeeds(fd, 128, 128, 128, 128, 128, 128);
    ROS_DEBUG("Set speed status: %x", retS);
    usleep(20*1000);
    int retA = readSpeedResponses(fd);
    ROS_DEBUG("Read speed after: %x", retA);

    ROS_DEBUG("thruster state = %x", readThrusterState(fd));
    ROS_DEBUG("set speed returns %x", retS);
    ROS_DEBUG("read speed returns %x", retR);
}

void ThrusterTortugaNode::publish(){
    // setSpeeds(fd, msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
}

void ThrusterTortugaNode::thrusterCallBack(const std_msgs::Int64MultiArray new_powers){
    //SG: TODO change this shit
  for(int i = 0 ;i < NUM_THRUSTERS; i++){
    thruster_powers.data[i] = new_powers.data[i];
  }
}

//ssh robot@192.168.10.12
<commit_msg>intialized power values<commit_after>#include "thruster_tortuga.h"

ThrusterTortugaNode::ThrusterTortugaNode(int argc, char **argv, int rate): TortugaNode(){
    ros::Rate loop_rate(rate);
    subscriber = n.subscribe("/qubo/thruster_input", 1000, &ThrusterTortugaNode::thrusterCallBack, this);

    ROS_DEBUG("Opening sensorboard");
    sensor_file = "/dev/sensor";
    fd = openSensorBoard(sensor_file.c_str());
    ROS_DEBUG("Opened sensorboard with fd %d.", fd);
    checkError(syncBoard(fd));
    ROS_DEBUG("Synced with the Board");

    //GH: This is not the correct use of checkError.
    //It should be called on the return value of a function call, not on the file descriptor.
    //checkError(fd);

    // Unsafe all the thrusters
    ROS_DEBUG("Unsafing all thrusters");
    for (int i = 6; i <= 11; i++) {
        checkError(setThrusterSafety(fd, i));
    }
    ROS_DEBUG("Unsafed all thrusters");

    for(int i = 0 ;i < NUM_THRUSTERS; i++){
      thruster_powers.data[i] = 0;
    }
}

}

ThrusterTortugaNode::~ThrusterTortugaNode(){
    //Stop all the thrusters
    ROS_DEBUG("Stopping thrusters");
    readSpeedResponses(fd);
    setSpeeds(fd, 0, 0, 0, 0, 0, 0);
    ROS_DEBUG("Safing thrusters");
    // Safe all the thrusters
    for (int i = 0; i <= 5; i++) {
        checkError(setThrusterSafety(fd, i));
    }
    ROS_DEBUG("Safed thrusters");
    //Close the sensorboard
    close(fd);
}

void ThrusterTortugaNode::update(){
    //I think we need to initialize thrusters and stuff before this will work
    ros::spinOnce();
    // setSpeeds(fd, msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
    // ROS_DEBUG("fd = %x",fd);

    ROS_DEBUG("Setting thruster speeds");
    int retR = readSpeedResponses(fd);
    ROS_DEBUG("Read speed before: %x", retR);
    int retS = setSpeeds(fd, 128, 128, 128, 128, 128, 128);
    ROS_DEBUG("Set speed status: %x", retS);
    usleep(20*1000);
    int retA = readSpeedResponses(fd);
    ROS_DEBUG("Read speed after: %x", retA);

    ROS_DEBUG("thruster state = %x", readThrusterState(fd));
    ROS_DEBUG("set speed returns %x", retS);
    ROS_DEBUG("read speed returns %x", retR);
}

void ThrusterTortugaNode::publish(){
    // setSpeeds(fd, msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
}

void ThrusterTortugaNode::thrusterCallBack(const std_msgs::Int64MultiArray new_powers){
    //SG: TODO change this shit
  for(int i = 0 ;i < NUM_THRUSTERS; i++){
    thruster_powers.data[i] = new_powers.data[i];
  }
}

//ssh robot@192.168.10.12
<|endoftext|>


<commit_before>/* Copyright 2007-2015 QReal Research Group
 *
 * 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 "pluginManagerImplementation.h"

#include <QtCore/QCoreApplication>

#include <qrkernel/logging.h>

using namespace qReal::details;

PluginManagerImplementation::PluginManagerImplementation(const QString &applicationDirPath
		, const QString &additionalPart)
	: mPluginsDir(QDir(applicationDirPath))
	, mApplicationDirectoryPath(applicationDirPath)
	, mAdditionalPart(additionalPart)
{
}

PluginManagerImplementation::~PluginManagerImplementation()
{
}

QList<QObject *> PluginManagerImplementation::loadAllPlugins()
{
	while (!mPluginsDir.isRoot() && !mPluginsDir.entryList(QDir::Dirs).contains("plugins")) {
		mPluginsDir.cdUp();
	}

	QList<QString> splittedDir = mAdditionalPart.split('/');
	for (const QString &partOfDirectory : splittedDir) {
		mPluginsDir.cd(partOfDirectory);
	}

	QList<QObject *> listOfPlugins;

	for (const QString &fileName : mPluginsDir.entryList(QDir::Files)) {
		QPair<QObject *, QString> const pluginAndError =  pluginLoadedByName(fileName);
		QObject * const pluginByName = pluginAndError.first;
		if (pluginByName) {
			listOfPlugins.append(pluginByName);
			mFileNameAndPlugin.insert(fileName, pluginByName);
		} else {
			QLOG_ERROR() << "Plugin loading failed:" << pluginAndError.second;
			qDebug() << "Plugin loading failed:" << pluginAndError.second;
		}
	}

	return listOfPlugins;
}

QPair<QObject *, QString> PluginManagerImplementation::pluginLoadedByName(const QString &pluginName)
{
	QPluginLoader *loader = new QPluginLoader(mPluginsDir.absoluteFilePath(pluginName), qApp);
	loader->load();
	QObject *plugin = loader->instance();
	QPair<QString, QPluginLoader*> pairOfLoader = qMakePair(pluginName, loader);
	mLoaders.append(pairOfLoader);

	if (plugin) {
		mFileNameAndPlugin.insert(loader->metaData()["IID"].toString(), plugin);
		return qMakePair(plugin, QString());
	}

	const QString loaderError = loader->errorString();

	// Unloading of plugins is currently (Qt 5.3) broken due to a bug in metatype system: calling Q_DECLARE_METATYPE
	// from plugin registers some data from plugin address space in Qt metatype system, which is not being updated
	// when plugin is unloaded and loaded again. Any subsequent calls to QVariant or other template classes/methods
	// which use metainformation will result in a crash. It is likely (but not verified) that qRegisterMetaType leads
	// to the same issue. Since we can not guarantee that plugin does not use Q_DECLARE_METATYPE or qRegisterMetaType
	// we shall not unload plugin at all, to be safe rather than sorry.
	//
	// But it seems also that metainformation gets deleted BEFORE plugins are unloaded on application exit, so we can
	// not call any metainformation-using code in destructors that get called on unloading. Since Qt classes themselves
	// are using such calls (QGraphicsViewScene, for example), random crashes on exit may be a symptom of this problem.
	//
	// EditorManager is an exception, because it really needs to unload editor plugins, to be able to load new versions
	// compiled by metaeditor. Editor plugins are generated, so we can guarantee to some extent that there will be no
	// metatype registrations.
	//
	// See:
	// http://stackoverflow.com/questions/19234703/using-q-declare-metatype-with-a-dll-that-may-be-loaded-multiple-times
	// (and http://qt-project.org/forums/viewthread/35587)
	// https://bugreports.qt-project.org/browse/QTBUG-32442

	delete loader;
	return qMakePair(nullptr, loaderError);
}

QString PluginManagerImplementation::unloadPlugin(const QString &pluginName)
{
	int count = 0;
	bool stateUnload = true;

	for (const QPair<QString, QPluginLoader *> &currentPair : mLoaders) {
		if (currentPair.first == pluginName) {
			stateUnload = currentPair.second->unload();
		}

		++count;
	}

	for (int j = 0; j < count; ++j) {
		mLoaders.removeFirst();
	}

	if (stateUnload) {
		return QString();
	}

	return QString("Plugin was not found");
}

QList<QString> PluginManagerImplementation::namesOfPlugins() const
{
	QList<QString> listOfNames;
	for (const QPair<QString, QPluginLoader *> &currentPair : mLoaders) {
		listOfNames.append(currentPair.first);
	}
	return listOfNames;
}

QString PluginManagerImplementation::fileName(QObject *plugin) const
{
	return mFileNameAndPlugin.key(plugin);
}

QObject *PluginManagerImplementation::pluginByName(const QString &pluginName) const
{
	return mFileNameAndPlugin[pluginName];
}
<commit_msg>small corrections<commit_after>/* Copyright 2007-2015 QReal Research Group
 *
 * 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 "pluginManagerImplementation.h"

#include <QtCore/QCoreApplication>

#include <qrkernel/logging.h>

using namespace qReal::details;

PluginManagerImplementation::PluginManagerImplementation(const QString &applicationDirPath
		, const QString &additionalPart)
	: mPluginsDir(QDir(applicationDirPath))
	, mApplicationDirectoryPath(applicationDirPath)
	, mAdditionalPart(additionalPart)
{
}

PluginManagerImplementation::~PluginManagerImplementation()
{
	const int count = mLoaders.length();
	for (int i = 0; i < count; ++i) {
		mLoaders.removeFirst();
	}
}

QList<QObject *> PluginManagerImplementation::loadAllPlugins()
{
	while (!mPluginsDir.isRoot() && !mPluginsDir.entryList(QDir::Dirs).contains("plugins")) {
		mPluginsDir.cdUp();
	}

	QList<QString> splittedDir = mAdditionalPart.split('/');
	for (const QString &partOfDirectory : splittedDir) {
		mPluginsDir.cd(partOfDirectory);
	}

	QList<QObject *> listOfPlugins;

	for (const QString &fileName : mPluginsDir.entryList(QDir::Files)) {
		QPair<QObject *, QString> const pluginAndError =  pluginLoadedByName(fileName);
		QObject * const pluginByName = pluginAndError.first;
		if (pluginByName) {
			listOfPlugins.append(pluginByName);
			mFileNameAndPlugin.insert(fileName, pluginByName);
		} else {
			QLOG_ERROR() << "Plugin loading failed:" << pluginAndError.second;
			qDebug() << "Plugin loading failed:" << pluginAndError.second;
		}
	}

	return listOfPlugins;
}

QPair<QObject *, QString> PluginManagerImplementation::pluginLoadedByName(const QString &pluginName)
{
	QPluginLoader *loader = new QPluginLoader(mPluginsDir.absoluteFilePath(pluginName), qApp);
	loader->load();
	QObject *plugin = loader->instance();
	QPair<QString, QPluginLoader*> pairOfLoader = qMakePair(pluginName, loader);
	mLoaders.append(pairOfLoader);

	if (plugin) {
		mFileNameAndPlugin.insert(loader->metaData()["IID"].toString(), plugin);
		return qMakePair(plugin, QString());
	}

	const QString loaderError = loader->errorString();

	// Unloading of plugins is currently (Qt 5.3) broken due to a bug in metatype system: calling Q_DECLARE_METATYPE
	// from plugin registers some data from plugin address space in Qt metatype system, which is not being updated
	// when plugin is unloaded and loaded again. Any subsequent calls to QVariant or other template classes/methods
	// which use metainformation will result in a crash. It is likely (but not verified) that qRegisterMetaType leads
	// to the same issue. Since we can not guarantee that plugin does not use Q_DECLARE_METATYPE or qRegisterMetaType
	// we shall not unload plugin at all, to be safe rather than sorry.
	//
	// But it seems also that metainformation gets deleted BEFORE plugins are unloaded on application exit, so we can
	// not call any metainformation-using code in destructors that get called on unloading. Since Qt classes themselves
	// are using such calls (QGraphicsViewScene, for example), random crashes on exit may be a symptom of this problem.
	//
	// EditorManager is an exception, because it really needs to unload editor plugins, to be able to load new versions
	// compiled by metaeditor. Editor plugins are generated, so we can guarantee to some extent that there will be no
	// metatype registrations.
	//
	// See:
	// http://stackoverflow.com/questions/19234703/using-q-declare-metatype-with-a-dll-that-may-be-loaded-multiple-times
	// (and http://qt-project.org/forums/viewthread/35587)
	// https://bugreports.qt-project.org/browse/QTBUG-32442

	delete loader;
	return qMakePair(nullptr, loaderError);
}

QString PluginManagerImplementation::unloadPlugin(const QString &pluginName)
{
	int count = 0;
	bool stateUnload = true;

	for (const QPair<QString, QPluginLoader *> &currentPair : mLoaders) {
		if (currentPair.first == pluginName) {
			stateUnload = currentPair.second->unload();
		}

		++count;
	}

	if (stateUnload) {
		return QString();
	}

	return QString("Plugin was not found");
}

QList<QString> PluginManagerImplementation::namesOfPlugins() const
{
	QList<QString> listOfNames;
	for (const QPair<QString, QPluginLoader *> &currentPair : mLoaders) {
		listOfNames.append(currentPair.first);
	}
	return listOfNames;
}

QString PluginManagerImplementation::fileName(QObject *plugin) const
{
	return mFileNameAndPlugin.key(plugin);
}

QObject *PluginManagerImplementation::pluginByName(const QString &pluginName) const
{
	return mFileNameAndPlugin[pluginName];
}
<|endoftext|>


<commit_before>/*******************************************************************************
 * c7a/data/data_manager.hpp
 *
 ******************************************************************************/

#ifndef C7A_DATA_DATA_MANAGER_HEADER
#define C7A_DATA_DATA_MANAGER_HEADER

#include <map>
#include "block_iterator.hpp"

namespace c7a {
namespace data {

//! Identification for DIAs
typedef int DIAId;

//! Stores in-memory data
//!
//! Future versions: Provide access to remote DIAs
class DataManager
{
public:

    //! returns iterator on requested partition
    //!
    //! \param id ID of the DIA
    template<class T>
    BlockIterator<T> getLocalBlocks(DIAId id) {
        const auto& block = data_[id];
        return BlockIterator<T>(block.cbegin(), block.cend());
    }

    //! returns true if the manager holds data of given DIA
    //!
    //! \param id ID of the DIA
    bool Contains(DIAId id) {
        return data_.find(id) != data_.end();
    }

    /*BlockEmitter<T> getLocalEmitter(DIAId) {

    }*/

private:

    std::map<DIAId, std::vector<Blob>> data_;
};

}
}

#endif // !C7A_DATA_DATA_MANAGER_HEADER

/******************************************************************************/
<commit_msg>implement getLocalEmitter<commit_after>/*******************************************************************************
 * c7a/data/data_manager.hpp
 *
 ******************************************************************************/

#ifndef C7A_DATA_DATA_MANAGER_HEADER
#define C7A_DATA_DATA_MANAGER_HEADER

#include <map>
#include <functional>
#include <stdexcept>
#include "block_iterator.hpp"

namespace c7a {
namespace data {

//! Identification for DIAs
typedef int DIAId;

//! function Signature for an emitt function
template<typename T>
using BlockEmitter = std::function<void(T)>;

//! Stores in-memory data
//!
//! Future versions: Provide access to remote DIAs
class DataManager
{
public:

    //! returns iterator on requested partition
    //!
    //! \param id ID of the DIA
    template<class T>
    BlockIterator<T> getLocalBlocks(DIAId id) {
        const auto& block = data_[id];
        return BlockIterator<T>(block.cbegin(), block.cend());
    }

    //! returns true if the manager holds data of given DIA
    //!
    //! \param id ID of the DIA
    bool Contains(DIAId id) {
        return data_.find(id) != data_.end();
    }

    template<class T>
    BlockEmitter<T> getLocalEmitter(DIAId id) {
        if (!Contains(id)) {
            throw std::runtime_error("target dia id unknown.");
        }
        auto& target = data_[id];
        return [&target](T elem){ target.push_back(Serialize(elem)); };
    }

private:

    std::map<DIAId, std::vector<Blob>> data_;
};

}
}

#endif // !C7A_DATA_DATA_MANAGER_HEADER

/******************************************************************************/
<|endoftext|>


<commit_before>#include <array>
#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "OsMock.hpp"
#include "mission/sail.hpp"
#include "mock/HasStateMock.hpp"
#include "mock/comm.hpp"
#include "obc/telecommands/sail.hpp"
#include "os/os.hpp"

using telecommunication::downlink::DownlinkAPID;
using testing::ElementsAre;
using testing::Eq;
using testing::Return;
using testing::ReturnRef;
using testing::_;

namespace
{
    class StopSailDeploymentTelecommandTest : public testing::Test
    {
      protected:
        StopSailDeploymentTelecommandTest();
        testing::NiceMock<HasStateMock<SystemState>> stateContainer;
        testing::NiceMock<TransmitterMock> transmitter;

        testing::NiceMock<OSMock> os;
        OSReset guard;

        obc::telecommands::StopSailDeployment telecommand{stateContainer};
    };

    StopSailDeploymentTelecommandTest::StopSailDeploymentTelecommandTest()
    {
        this->guard = InstallProxy(&os);
        ON_CALL(os, TakeSemaphore(_, _)).WillByDefault(Return(OSResult::Success));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldDisableDeploymentBeforeItBegins)
    {
        SystemState state;
        auto& persistentState = state.PersistentState;
        persistentState.Set(state::SailState(state::SailOpeningState::Waiting));
        EXPECT_CALL(stateContainer, MockGetState()).WillOnce(ReturnRef(state));

        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0x22, 0))));

        std::array<std::uint8_t, 1> frame{0x22};

        telecommand.Handle(this->transmitter, frame);

        state::SailState sailState;
        persistentState.Get(sailState);
        ASSERT_THAT(sailState.CurrentState(), Eq(state::SailOpeningState::OpeningStopped));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldStopOpening)
    {
        SystemState state;
        auto& persistentState = state.PersistentState;
        persistentState.Set(state::SailState(state::SailOpeningState::Opening));
        EXPECT_CALL(stateContainer, MockGetState()).WillOnce(ReturnRef(state));

        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0x22, 0))));

        std::array<std::uint8_t, 1> frame{0x22};

        telecommand.Handle(this->transmitter, frame);

        state::SailState sailState;
        persistentState.Get(sailState);
        ASSERT_THAT(sailState.CurrentState(), Eq(state::SailOpeningState::OpeningStopped));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldRespondWithErrorOnInvalidFrame)
    {
        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0, 255))));

        telecommand.Handle(this->transmitter, gsl::span<const uint8_t>());
    }
}
<commit_msg>[telecommunication] Fix test failure.<commit_after>#include <array>
#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "OsMock.hpp"
#include "mission/sail.hpp"
#include "mock/HasStateMock.hpp"
#include "mock/comm.hpp"
#include "obc/telecommands/sail.hpp"
#include "os/os.hpp"

using telecommunication::downlink::DownlinkAPID;
using testing::ElementsAre;
using testing::Eq;
using testing::Return;
using testing::ReturnRef;
using testing::_;

namespace
{
    class StopSailDeploymentTelecommandTest : public testing::Test
    {
      protected:
        StopSailDeploymentTelecommandTest();
        testing::NiceMock<HasStateMock<SystemState>> stateContainer;
        testing::NiceMock<TransmitterMock> transmitter;

        testing::NiceMock<OSMock> os;
        OSReset guard;

        obc::telecommands::StopSailDeployment telecommand{stateContainer};
    };

    StopSailDeploymentTelecommandTest::StopSailDeploymentTelecommandTest()
    {
        this->guard = InstallProxy(&os);
        ON_CALL(os, TakeSemaphore(_, _)).WillByDefault(Return(OSResult::Success));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldDisableDeploymentBeforeItBegins)
    {
        SystemState state;
        auto& persistentState = state.PersistentState;
        persistentState.Set(state::SailState(state::SailOpeningState::Waiting));
        EXPECT_CALL(stateContainer, MockGetState()).WillOnce(ReturnRef(state));

        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0x22, 0))));

        std::array<std::uint8_t, 1> frame{0x22};

        telecommand.Handle(this->transmitter, frame);

        state::SailState sailState;
        persistentState.Get(sailState);
        ASSERT_THAT(sailState.CurrentState(), Eq(state::SailOpeningState::OpeningStopped));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldStopOpening)
    {
        SystemState state;
        auto& persistentState = state.PersistentState;
        persistentState.Set(state::SailState(state::SailOpeningState::Opening));
        EXPECT_CALL(stateContainer, MockGetState()).WillOnce(ReturnRef(state));

        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0x22, 0))));

        std::array<std::uint8_t, 1> frame{0x22};

        telecommand.Handle(this->transmitter, frame);

        state::SailState sailState;
        persistentState.Get(sailState);
        ASSERT_THAT(sailState.CurrentState(), Eq(state::SailOpeningState::OpeningStopped));
    }

    TEST_F(StopSailDeploymentTelecommandTest, ShouldRespondWithErrorOnInvalidFrame)
    {
        SystemState state;
        ON_CALL(stateContainer, MockGetState()).WillByDefault(ReturnRef(state));
        EXPECT_CALL(this->transmitter, SendFrame(IsDownlinkFrame(Eq(DownlinkAPID::DisableSailDeployment), Eq(0U), ElementsAre(0, 255))));

        telecommand.Handle(this->transmitter, gsl::span<const uint8_t>());
    }
}
<|endoftext|>