<commit_before>#include "task_synthetic_shapes.h"
#include "libnanocv/loss.h"
#include "libnanocv/util/math.hpp"
#include "libnanocv/util/random.hpp"

namespace ncv
{
        synthetic_shapes_task_t::synthetic_shapes_task_t(const string_t& configuration)
                :       task_t(configuration),
                        m_rows(math::clamp(text::from_params<size_t>(configuration, "rows", 32), 16, 32)),
                        m_cols(math::clamp(text::from_params<size_t>(configuration, "cols", 32), 16, 32)),
                        m_outputs(math::clamp(text::from_params<size_t>(configuration, "dims", 4), 2, 16)),
                        m_folds(1),
                        m_color(text::from_params<color_mode>(configuration, "color", color_mode::rgba)),
                        m_size(math::clamp(text::from_params<size_t>(configuration, "size", 1024), 256, 16 * 1024))
        {
        }

        namespace
        {
                rgba_t make_transparent_color()
                {
                        return 0;
                }

                rgba_t make_light_color()
                {
                        random_t<rgba_t> rng_red(175, 255);
                        random_t<rgba_t> rng_green(175, 255);
                        random_t<rgba_t> rng_blue(175, 255);

                        return color::make_rgba(rng_red(), rng_green(), rng_blue());
                }

                rect_t make_rect(coord_t rows, coord_t cols)
                {
                        random_t<coord_t> rng(2, std::min(rows / 4, cols / 4));

                        const coord_t dx = rng();
                        const coord_t dy = rng();
                        const coord_t dw = rng();
                        const coord_t dh = rng();

                        return rect_t(dx, dy, cols - dx - dw, rows - dy - dh);
                }

                rect_t make_interior_rect(coord_t x, coord_t y, coord_t w, coord_t h)
                {
                        random_t<coord_t> rng(2, std::min(w / 4, h / 4));

                        const coord_t dx = rng();
                        const coord_t dy = rng();
                        const coord_t dw = rng();
                        const coord_t dh = rng();

                        return rect_t(x + dx, y + dy, w - dx - dw, h - dy - dh);
                }

                rect_t make_interior_rect(const rect_t& rect)
                {
                        return make_interior_rect(rect.left(), rect.top(), rect.width(), rect.height());
                }

                image_t make_filled_rect(coord_t rows, coord_t cols)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill(rect, make_light_color());

                        return image;
                }

                image_t make_hollow_rect(coord_t rows, coord_t cols)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill(rect, make_light_color());
                        image.fill(make_interior_rect(rect), make_transparent_color());

                        return image;
                }

                image_t make_filled_ellipse(coord_t rows, coord_t cols)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill_ellipse(rect, make_light_color());

                        return image;
                }

                image_t make_hollow_ellipse(coord_t rows, coord_t cols)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill_ellipse(rect, make_light_color());
                        image.fill_ellipse(make_interior_rect(rect), make_transparent_color());

                        return image;
                }
        }

        bool synthetic_shapes_task_t::load(const string_t &)
        {
                random_t<size_t> rng_protocol(1, 10);
                random_t<size_t> rng_output(1, osize());

                random_t<scalar_t> rng_gauss(scalar_t(1), math::cast<scalar_t>(icols() + irows()) / scalar_t(8));

                const coord_t rows = static_cast<coord_t>(irows());
                const coord_t cols = static_cast<coord_t>(icols());

                clear_memory(0);

                for (size_t f = 0; f < fsize(); f ++)
                {
                        for (size_t i = 0; i < m_size; i ++)
                        {
                                // random protocol: train vs. test
                                const protocol p = (rng_protocol() < 9) ? protocol::train : protocol::test;

                                // random output class: #dots
                                const size_t o = rng_output();

                                // generate random image background
                                image_t image(irows(), icols(), color());
                                image.fill(make_light_color());
                                image.random_noise(color_channel::rgba, -155.0, 55.0, rng_gauss());

                                // generate random shapes
                                image_t shape;

                                switch (o)
                                {
                                case 1:         shape = make_filled_rect(rows, cols); break;
                                case 2:         shape = make_hollow_rect(rows, cols); break;
                                case 3:         shape = make_filled_ellipse(rows, cols); break;
                                case 4:         shape = make_hollow_ellipse(rows, cols); break;
                                default:        break;
                                }

                                shape.random_noise(color_channel::rgba, -10.0, +10.0, rng_gauss() * 0.5);
                                image.alpha_blend(shape.rgba());

                                add_image(image);

                                // generate sample
                                sample_t sample(n_images() - 1, sample_region(0, 0));
                                switch (o)
                                {
                                case 1:         sample.m_label = "filled_rectangle"; break;
                                case 2:         sample.m_label = "hollow_rectangle"; break;
                                case 3:         sample.m_label = "filled_ellipse"; break;
                                case 4:         sample.m_label = "hollow_ellipse"; break;
                                default:        sample.m_label = "unkown"; break;
                                }

                                sample.m_target = ncv::class_target(o - 1, osize());
                                sample.m_fold = {f, p};
                                add_sample(sample);
                        }
                }

                return true;
        }
}
<commit_msg>synthetic samples with dark or light background<commit_after>#include "task_synthetic_shapes.h"
#include "libnanocv/loss.h"
#include "libnanocv/util/math.hpp"
#include "libnanocv/util/random.hpp"

namespace ncv
{
        synthetic_shapes_task_t::synthetic_shapes_task_t(const string_t& configuration)
                :       task_t(configuration),
                        m_rows(math::clamp(text::from_params<size_t>(configuration, "rows", 32), 16, 32)),
                        m_cols(math::clamp(text::from_params<size_t>(configuration, "cols", 32), 16, 32)),
                        m_outputs(math::clamp(text::from_params<size_t>(configuration, "dims", 4), 2, 16)),
                        m_folds(1),
                        m_color(text::from_params<color_mode>(configuration, "color", color_mode::rgba)),
                        m_size(math::clamp(text::from_params<size_t>(configuration, "size", 1024), 256, 16 * 1024))
        {
        }

        namespace
        {
                rgba_t make_transparent_color()
                {
                        return 0;
                }

                rgba_t make_light_color()
                {
                        random_t<rgba_t> rng_red(175, 255);
                        random_t<rgba_t> rng_green(175, 255);
                        random_t<rgba_t> rng_blue(175, 255);

                        return color::make_rgba(rng_red(), rng_green(), rng_blue());
                }

                rgba_t make_dark_color()
                {
                        random_t<rgba_t> rng_red(0, 125);
                        random_t<rgba_t> rng_green(0, 125);
                        random_t<rgba_t> rng_blue(0, 125);

                        return color::make_rgba(rng_red(), rng_green(), rng_blue());
                }

                rect_t make_rect(coord_t rows, coord_t cols)
                {
                        random_t<coord_t> rng(2, std::min(rows / 4, cols / 4));

                        const coord_t dx = rng();
                        const coord_t dy = rng();
                        const coord_t dw = rng();
                        const coord_t dh = rng();

                        return rect_t(dx, dy, cols - dx - dw, rows - dy - dh);
                }

                rect_t make_interior_rect(coord_t x, coord_t y, coord_t w, coord_t h)
                {
                        random_t<coord_t> rng(2, std::min(w / 4, h / 4));

                        const coord_t dx = rng();
                        const coord_t dy = rng();
                        const coord_t dw = rng();
                        const coord_t dh = rng();

                        return rect_t(x + dx, y + dy, w - dx - dw, h - dy - dh);
                }

                rect_t make_interior_rect(const rect_t& rect)
                {
                        return make_interior_rect(rect.left(), rect.top(), rect.width(), rect.height());
                }

                image_t make_filled_rect(coord_t rows, coord_t cols, rgba_t fill_color)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill(rect, fill_color);

                        return image;
                }

                image_t make_hollow_rect(coord_t rows, coord_t cols, rgba_t fill_color)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill(rect, fill_color);
                        image.fill(make_interior_rect(rect), make_transparent_color());

                        return image;
                }

                image_t make_filled_circle(coord_t rows, coord_t cols, rgba_t fill_color)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill_circle(rect, fill_color);

                        return image;
                }

                image_t make_hollow_circle(coord_t rows, coord_t cols, rgba_t fill_color)
                {
                        const rect_t rect = make_rect(rows, cols);

                        image_t image(rows, cols, color_mode::rgba);

                        image.fill(make_transparent_color());
                        image.fill_circle(rect, fill_color);
                        image.fill_circle(make_interior_rect(rect), make_transparent_color());

                        return image;
                }
        }

        bool synthetic_shapes_task_t::load(const string_t &)
        {
                random_t<size_t> rng_protocol(1, 10);
                random_t<size_t> rng_output(1, osize());

                random_t<scalar_t> rng_gauss(scalar_t(1), math::cast<scalar_t>(icols() + irows()) / scalar_t(8));

                const coord_t rows = static_cast<coord_t>(irows());
                const coord_t cols = static_cast<coord_t>(icols());

                clear_memory(0);

                for (size_t f = 0; f < fsize(); f ++)
                {
                        for (size_t i = 0; i < m_size; i ++)
                        {
                                // random protocol: train vs. test
                                const protocol p = (rng_protocol() < 9) ? protocol::train : protocol::test;

                                // random output class: #dots
                                const size_t o = rng_output();

                                const bool is_dark_background = (rng_protocol() % 2) == 0;
                                const rgba_t back_color = is_dark_background ? make_dark_color() : make_light_color();
                                const rgba_t shape_color = is_dark_background ? make_light_color() : make_dark_color();

                                // generate random image background
                                image_t image(irows(), icols(), color());
                                image.fill(back_color);
                                image.random_noise(color_channel::rgba, -25.0, +25.0, rng_gauss());

                                // generate random shapes
                                image_t shape;

                                switch (o)
                                {
                                case 1:         shape = make_filled_rect(rows, cols, shape_color); break;
                                case 2:         shape = make_hollow_rect(rows, cols, shape_color); break;
                                case 3:         shape = make_filled_circle(rows, cols, shape_color); break;
                                case 4:         shape = make_hollow_circle(rows, cols, shape_color); break;
                                default:        break;
                                }

                                shape.random_noise(color_channel::rgba, -25.0, +25.0, rng_gauss() * 0.5);
                                image.alpha_blend(shape.rgba());

                                add_image(image);

                                // generate sample
                                sample_t sample(n_images() - 1, sample_region(0, 0));
                                switch (o)
                                {
                                case 1:         sample.m_label = "filled_rectangle"; break;
                                case 2:         sample.m_label = "hollow_rectangle"; break;
                                case 3:         sample.m_label = "filled_circle"; break;
                                case 4:         sample.m_label = "hollow_circle"; break;
                                default:        sample.m_label = "unkown"; break;
                                }

                                sample.m_target = ncv::class_target(o - 1, osize());
                                sample.m_fold = {f, p};
                                add_sample(sample);
                        }
                }

                return true;
        }
}
<|endoftext|>


<commit_before>#include "follow_me_impl.h"
#include "px4_custom_mode.h"

namespace dronecore {

using namespace std::placeholders; // for `_1`

FollowMeImpl::FollowMeImpl() :
    PluginImplBase()
{
    // (Lat, Lon, Alt) => double, (vx, vy, vz) => float
    _last_location =  _target_location = \
                                         FollowMe::TargetLocation { double(NAN), double(NAN), double(NAN),
                                                                    NAN, NAN, NAN };
}

FollowMeImpl::~FollowMeImpl() {}

void FollowMeImpl::init()
{
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&FollowMeImpl::process_heartbeat, this, _1), static_cast<void *>(this));
    set_default_config();
}

void FollowMeImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

void FollowMeImpl::enable() {}

void FollowMeImpl::disable()
{
    stop_sending_target_location();
}

const FollowMe::Config &FollowMeImpl::get_config() const
{
    return _config;
}

FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config &config)
{
    // Valdidate configuration
    if (!is_config_ok(config)) {
        LogErr() << "set_config() failed. Last configuration is preserved.";
        return FollowMe::Result::SET_CONFIG_FAILED;
    }

    auto height = config.min_height_m;
    auto distance = config.follow_distance_m;
    int32_t direction = static_cast<int32_t>(config.follow_direction);
    auto responsiveness = config.responsiveness;

    // Send configuration to Vehicle
    _parent->set_param_float_async("NAV_MIN_FT_HT", height,
                                   std::bind(&FollowMeImpl::receive_param_min_height,
                                             this, _1, height));
    _parent->set_param_float_async("NAV_FT_DST", distance,
                                   std::bind(&FollowMeImpl::receive_param_follow_distance,
                                             this, _1, distance));
    _parent->set_param_int_async("NAV_FT_FS", direction,
                                 std::bind(&FollowMeImpl::receive_param_follow_direction,
                                           this, _1, direction));
    _parent->set_param_float_async("NAV_FT_RS", responsiveness,
                                   std::bind(&FollowMeImpl::receive_param_responsiveness,
                                             this, _1, responsiveness));

    // FIXME: We've sent valid configuration to Vehicle.
    // But that doesn't mean configuration is applied, untill we receive confirmation.
    // For now we're hoping that it is applied successfully.
    return FollowMe::Result::SUCCESS;
}

void FollowMeImpl::set_target_location(const FollowMe::TargetLocation &location)
{
    _mutex.lock();
    _target_location = location;
    // We're interested only in lat, long.
    _estimatation_capabilities |= (1 << static_cast<int>(EstimationCapabilites::POS));

    if (_mode != Mode::ACTIVE) {
        _mutex.unlock();
        return;
    }
    // If set already, reschedule it.
    if (_target_location_cookie) {
        _parent->reset_call_every(_target_location_cookie);
        _target_location_cookie = nullptr;
    } else {
        // Regiter now for sending in the next cycle.
        _parent->add_call_every([this]() { send_target_location(); },
        SENDER_RATE,
        &_target_location_cookie);
    }
    _mutex.unlock();

    // Send it immediately for now.
    send_target_location();
}

const FollowMe::TargetLocation &FollowMeImpl::get_last_location() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _last_location;
}

bool FollowMeImpl::is_active() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _mode == Mode::ACTIVE;
}

FollowMe::Result FollowMeImpl::start()
{
    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;

    FollowMe::Result result = to_follow_me_result(
                                  _parent->send_command_with_ack(
                                      MAV_CMD_DO_SET_MODE,
                                      MavlinkCommands::Params {float(mode),
                                                               float(custom_mode),
                                                               float(custom_sub_mode),
                                                               NAN, NAN, NAN, NAN},
                                      MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));

    if (result == FollowMe::Result::SUCCESS) {
        // If location was set before, lets send it to vehicle
        std::lock_guard<std::mutex> lock(
            _mutex); // locking is not necessary here but lets do it for integrity
        if (is_target_location_set()) {
            _parent->add_call_every([this]() { send_target_location(); },
            SENDER_RATE,
            &_target_location_cookie);
        }
    }
    return result;
}

FollowMe::Result FollowMeImpl::stop()
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode == Mode::ACTIVE) {
            stop_sending_target_location();
        }
    }
    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

    return to_follow_me_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

// Applies default FollowMe configuration to the device
void FollowMeImpl::set_default_config()
{
    LogInfo() << "Applying default FollowMe configuration FollowMe to the device...";
    FollowMe::Config default_config {};

    auto height = default_config.min_height_m;
    auto distance = default_config.follow_distance_m;
    int32_t direction = static_cast<int32_t>(default_config.follow_direction);
    auto responsiveness = default_config.responsiveness;

    // Send configuration to Vehicle
    _parent->set_param_float_async("NAV_MIN_FT_HT", height,
                                   std::bind(&FollowMeImpl::receive_param_min_height,
                                             this, _1, height));
    _parent->set_param_float_async("NAV_FT_DST", distance,
                                   std::bind(&FollowMeImpl::receive_param_follow_distance,
                                             this, _1, distance));
    _parent->set_param_int_async("NAV_FT_FS", direction,
                                 std::bind(&FollowMeImpl::receive_param_follow_direction,
                                           this, _1, direction));
    _parent->set_param_float_async("NAV_FT_RS", responsiveness,
                                   std::bind(&FollowMeImpl::receive_param_responsiveness,
                                             this, _1, responsiveness));
}

bool FollowMeImpl::is_config_ok(const FollowMe::Config &config) const
{
    auto config_ok = false;

    if (config.min_height_m < FollowMe::Config::MIN_HEIGHT_M) {
        LogErr() << "Err: Min height must be atleast 8.0 meters";
    } else if (config.follow_distance_m < FollowMe::Config::MIN_FOLLOW_DIST_M) {
        LogErr() << "Err: Min Follow distance must be atleast 1.0 meter";
    } else if (config.follow_direction < FollowMe::Config::FollowDirection::FRONT_RIGHT ||
               config.follow_direction > FollowMe::Config::FollowDirection::NONE) {
        LogErr() << "Err: Invalid Follow direction";
    } else if (config.responsiveness < FollowMe::Config::MIN_RESPONSIVENESS ||
               config.responsiveness > FollowMe::Config::MAX_RESPONSIVENESS) {
        LogErr() << "Err: Responsiveness must be in range (0.0 to 1.0)";
    } else { // Config is OK
        config_ok = true;
    }

    return config_ok;
}

void FollowMeImpl::receive_param_min_height(bool success, float min_height_m)
{
    if (success) {
        _config.min_height_m = min_height_m;
    } else {
        LogErr() << "Failed to set NAV_MIN_FT_HT: " << min_height_m << "m";
    }
}

void FollowMeImpl::receive_param_follow_distance(bool success, float follow_distance_m)
{
    if (success) {
        _config.follow_distance_m = follow_distance_m;
    } else {
        LogErr() << "Failed to set NAV_FT_DST: " << follow_distance_m << "m";
    }
}

void FollowMeImpl::receive_param_follow_direction(bool success, int32_t direction)
{
    auto new_direction = FollowMe::Config::FollowDirection::NONE;
    switch (direction) {
        case 0: new_direction = FollowMe::Config::FollowDirection::FRONT_RIGHT; break;
        case 1: new_direction = FollowMe::Config::FollowDirection::BEHIND; break;
        case 2: new_direction = FollowMe::Config::FollowDirection::FRONT; break;
        case 3: new_direction = FollowMe::Config::FollowDirection::FRONT_LEFT; break;
        default: break;
    }
    if (success) {
        if (new_direction != FollowMe::Config::FollowDirection::NONE) {
            _config.follow_direction = new_direction;
        }
    } else {
        LogErr() << "Failed to set NAV_FT_FS: " <<  FollowMe::Config::to_str(new_direction);
    }
}

void FollowMeImpl::receive_param_responsiveness(bool success, float responsiveness)
{
    if (success) {
        _config.responsiveness = responsiveness;
    } else {
        LogErr() << "Failed to set NAV_FT_RS: " << responsiveness;
    }
}

FollowMe::Result
FollowMeImpl::to_follow_me_result(MavlinkCommands::Result result) const
{
    switch (result) {
        case MavlinkCommands::Result::SUCCESS:
            return FollowMe::Result::SUCCESS;
        case MavlinkCommands::Result::NO_DEVICE:
            return FollowMe::Result::NO_DEVICE;
        case MavlinkCommands::Result::CONNECTION_ERROR:
            return FollowMe::Result::CONNECTION_ERROR;
        case MavlinkCommands::Result::BUSY:
            return FollowMe::Result::BUSY;
        case MavlinkCommands::Result::COMMAND_DENIED:
            return FollowMe::Result::COMMAND_DENIED;
        case MavlinkCommands::Result::TIMEOUT:
            return FollowMe::Result::TIMEOUT;
        default:
            return FollowMe::Result::UNKNOWN;
    }
}

bool FollowMeImpl::is_target_location_set() const
{
    // If the target's latitude is NAN, we assume that location is not set.
    // We assume that mutex was acquired by the caller
    return std::isfinite(_target_location.latitude_deg);
}

void FollowMeImpl::send_target_location()
{
    // Don't send if we're not in FollowMe mode.
    if (!is_active()) {
        return;
    }

    dl_time_t now = _time.steady_time();
    // needed by http://mavlink.org/messages/common#FOLLOW_TARGET
    uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds

    _mutex.lock();
    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
               <<
               " Alt: " << _target_location.absolute_altitude_m;
    const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
    const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
    const float alt = static_cast<float>(_target_location.absolute_altitude_m);
    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
    _mutex.unlock();

    const float pos_std_dev[] = { NAN, NAN, NAN };
    const float vel[] = { NAN, NAN, NAN };
    const float accel_unknown[] = { NAN, NAN, NAN };
    const float attitude_q_unknown[] = { 1.f, NAN, NAN, NAN };
    const float rates_unknown[] = { NAN, NAN, NAN };
    uint64_t custom_state = 0;

    mavlink_message_t msg {};
    mavlink_msg_follow_target_pack(_parent->get_own_system_id(),
                                   _parent->get_own_component_id(),
                                   &msg,
                                   elapsed_msec,
                                   _estimatation_capabilities,
                                   lat_int,
                                   lon_int,
                                   alt,
                                   vel,
                                   accel_unknown,
                                   attitude_q_unknown,
                                   rates_unknown,
                                   pos_std_dev,
                                   custom_state);

    if (!_parent->send_message(msg)) {
        LogErr() << "send_target_location() failed..";
    } else {
        std::lock_guard<std::mutex> lock(_mutex);
        _last_location = _target_location;
    }
}

void FollowMeImpl::stop_sending_target_location()
{
    // We assume that mutex was acquired by the caller
    if (_target_location_cookie) {
        _parent->remove_call_every(_target_location_cookie);
        _target_location_cookie = nullptr;
    }
    _mode = Mode::NOT_ACTIVE;
}

void FollowMeImpl::process_heartbeat(const mavlink_message_t &message)
{
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(&message, &heartbeat);

    bool follow_me_active = false; // tells whether we're in FollowMe mode right now
    if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {

        px4::px4_custom_mode px4_custom_mode;
        px4_custom_mode.data = heartbeat.custom_mode;

        if (px4_custom_mode.main_mode == px4::PX4_CUSTOM_MAIN_MODE_AUTO &&
            px4_custom_mode.sub_mode == px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET) {
            follow_me_active = true; // we're in FollowMe mode
        }
    }

    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (!follow_me_active && _mode == Mode::ACTIVE) {
            // We're NOT in FollowMe mode anymore.
            // Lets stop sending target location updates
            stop_sending_target_location();
        } else if (follow_me_active && _mode == Mode::NOT_ACTIVE) {
            // We're in FollowMe mode now
            _mode = Mode::ACTIVE;
            _mutex.unlock(); // we must unlock to avoid deadlock in send_target_location()
            return;
        }
    }
}

} // namespace dronecore
<commit_msg>Simplify Follow Me example<commit_after>#include "follow_me_impl.h"
#include "px4_custom_mode.h"

namespace dronecore {

using namespace std::placeholders; // for `_1`

FollowMeImpl::FollowMeImpl() :
    PluginImplBase()
{
    // (Lat, Lon, Alt) => double, (vx, vy, vz) => float
    _last_location =  _target_location = \
                                         FollowMe::TargetLocation { double(NAN), double(NAN), double(NAN),
                                                                    NAN, NAN, NAN };
}

FollowMeImpl::~FollowMeImpl() {}

void FollowMeImpl::init()
{
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&FollowMeImpl::process_heartbeat, this, _1), static_cast<void *>(this));
    set_default_config();
}

void FollowMeImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

void FollowMeImpl::enable() {}

void FollowMeImpl::disable()
{
    stop_sending_target_location();
}

const FollowMe::Config &FollowMeImpl::get_config() const
{
    return _config;
}

FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config &config)
{
    // Valdidate configuration
    if (!is_config_ok(config)) {
        LogErr() << "set_config() failed. Last configuration is preserved.";
        return FollowMe::Result::SET_CONFIG_FAILED;
    }

    auto height = config.min_height_m;
    auto distance = config.follow_distance_m;
    int32_t direction = static_cast<int32_t>(config.follow_direction);
    auto responsiveness = config.responsiveness;

    // Send configuration to Vehicle
    _parent->set_param_float_async("NAV_MIN_FT_HT", height,
                                   std::bind(&FollowMeImpl::receive_param_min_height,
                                             this, _1, height));
    _parent->set_param_float_async("NAV_FT_DST", distance,
                                   std::bind(&FollowMeImpl::receive_param_follow_distance,
                                             this, _1, distance));
    _parent->set_param_int_async("NAV_FT_FS", direction,
                                 std::bind(&FollowMeImpl::receive_param_follow_direction,
                                           this, _1, direction));
    _parent->set_param_float_async("NAV_FT_RS", responsiveness,
                                   std::bind(&FollowMeImpl::receive_param_responsiveness,
                                             this, _1, responsiveness));

    // FIXME: We've sent valid configuration to Vehicle.
    // But that doesn't mean configuration is applied, untill we receive confirmation.
    // For now we're hoping that it is applied successfully.
    return FollowMe::Result::SUCCESS;
}

void FollowMeImpl::set_target_location(const FollowMe::TargetLocation &location)
{
    _mutex.lock();
    _target_location = location;
    // We're interested only in lat, long.
    _estimatation_capabilities |= (1 << static_cast<int>(EstimationCapabilites::POS));

    if (_mode != Mode::ACTIVE) {
        _mutex.unlock();
        return;
    }
    // If set already, reschedule it.
    if (_target_location_cookie) {
        _parent->reset_call_every(_target_location_cookie);
        _target_location_cookie = nullptr;
    } else {
        // Regiter now for sending in the next cycle.
        _parent->add_call_every([this]() { send_target_location(); },
        SENDER_RATE,
        &_target_location_cookie);
    }
    _mutex.unlock();

    // Send it immediately for now.
    send_target_location();
}

const FollowMe::TargetLocation &FollowMeImpl::get_last_location() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _last_location;
}

bool FollowMeImpl::is_active() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _mode == Mode::ACTIVE;
}

FollowMe::Result FollowMeImpl::start()
{
    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;

    FollowMe::Result result = to_follow_me_result(
                                  _parent->send_command_with_ack(
                                      MAV_CMD_DO_SET_MODE,
                                      MavlinkCommands::Params {float(mode),
                                                               float(custom_mode),
                                                               float(custom_sub_mode),
                                                               NAN, NAN, NAN, NAN},
                                      MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));

    if (result == FollowMe::Result::SUCCESS) {
        // If location was set before, lets send it to vehicle
        std::lock_guard<std::mutex> lock(
            _mutex); // locking is not necessary here but lets do it for integrity
        if (is_target_location_set()) {
            _parent->add_call_every([this]() { send_target_location(); },
            SENDER_RATE,
            &_target_location_cookie);
        }
    }
    return result;
}

FollowMe::Result FollowMeImpl::stop()
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode == Mode::ACTIVE) {
            stop_sending_target_location();
        }
    }
    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

    return to_follow_me_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

// Applies default FollowMe configuration to the device
void FollowMeImpl::set_default_config()
{
    LogInfo() << "Applying default FollowMe configuration FollowMe to the device...";
    FollowMe::Config default_config {};

    auto height = default_config.min_height_m;
    auto distance = default_config.follow_distance_m;
    int32_t direction = static_cast<int32_t>(default_config.follow_direction);
    auto responsiveness = default_config.responsiveness;

    // Send configuration to Vehicle
    _parent->set_param_float_async("NAV_MIN_FT_HT", height,
                                   std::bind(&FollowMeImpl::receive_param_min_height,
                                             this, _1, height));
    _parent->set_param_float_async("NAV_FT_DST", distance,
                                   std::bind(&FollowMeImpl::receive_param_follow_distance,
                                             this, _1, distance));
    _parent->set_param_int_async("NAV_FT_FS", direction,
                                 std::bind(&FollowMeImpl::receive_param_follow_direction,
                                           this, _1, direction));
    _parent->set_param_float_async("NAV_FT_RS", responsiveness,
                                   std::bind(&FollowMeImpl::receive_param_responsiveness,
                                             this, _1, responsiveness));
}

bool FollowMeImpl::is_config_ok(const FollowMe::Config &config) const
{
    auto config_ok = false;

    if (config.min_height_m < FollowMe::Config::MIN_HEIGHT_M) {
        LogErr() << "Err: Min height must be atleast 8.0 meters";
    } else if (config.follow_distance_m < FollowMe::Config::MIN_FOLLOW_DIST_M) {
        LogErr() << "Err: Min Follow distance must be atleast 1.0 meter";
    } else if (config.follow_direction < FollowMe::Config::FollowDirection::FRONT_RIGHT ||
               config.follow_direction > FollowMe::Config::FollowDirection::NONE) {
        LogErr() << "Err: Invalid Follow direction";
    } else if (config.responsiveness < FollowMe::Config::MIN_RESPONSIVENESS ||
               config.responsiveness > FollowMe::Config::MAX_RESPONSIVENESS) {
        LogErr() << "Err: Responsiveness must be in range (0.0 to 1.0)";
    } else { // Config is OK
        config_ok = true;
    }

    return config_ok;
}

void FollowMeImpl::receive_param_min_height(bool success, float min_height_m)
{
    if (success) {
        _config.min_height_m = min_height_m;
    } else {
        LogErr() << "Failed to set NAV_MIN_FT_HT: " << min_height_m << "m";
    }
}

void FollowMeImpl::receive_param_follow_distance(bool success, float follow_distance_m)
{
    if (success) {
        _config.follow_distance_m = follow_distance_m;
    } else {
        LogErr() << "Failed to set NAV_FT_DST: " << follow_distance_m << "m";
    }
}

void FollowMeImpl::receive_param_follow_direction(bool success, int32_t direction)
{
    auto new_direction = FollowMe::Config::FollowDirection::NONE;
    switch (direction) {
        case 0: new_direction = FollowMe::Config::FollowDirection::FRONT_RIGHT; break;
        case 1: new_direction = FollowMe::Config::FollowDirection::BEHIND; break;
        case 2: new_direction = FollowMe::Config::FollowDirection::FRONT; break;
        case 3: new_direction = FollowMe::Config::FollowDirection::FRONT_LEFT; break;
        default: break;
    }
    if (success) {
        if (new_direction != FollowMe::Config::FollowDirection::NONE) {
            _config.follow_direction = new_direction;
        }
    } else {
        LogErr() << "Failed to set NAV_FT_FS: " <<  FollowMe::Config::to_str(new_direction);
    }
}

void FollowMeImpl::receive_param_responsiveness(bool success, float responsiveness)
{
    if (success) {
        _config.responsiveness = responsiveness;
    } else {
        LogErr() << "Failed to set NAV_FT_RS: " << responsiveness;
    }
}

FollowMe::Result
FollowMeImpl::to_follow_me_result(MavlinkCommands::Result result) const
{
    switch (result) {
        case MavlinkCommands::Result::SUCCESS:
            return FollowMe::Result::SUCCESS;
        case MavlinkCommands::Result::NO_DEVICE:
            return FollowMe::Result::NO_DEVICE;
        case MavlinkCommands::Result::CONNECTION_ERROR:
            return FollowMe::Result::CONNECTION_ERROR;
        case MavlinkCommands::Result::BUSY:
            return FollowMe::Result::BUSY;
        case MavlinkCommands::Result::COMMAND_DENIED:
            return FollowMe::Result::COMMAND_DENIED;
        case MavlinkCommands::Result::TIMEOUT:
            return FollowMe::Result::TIMEOUT;
        default:
            return FollowMe::Result::UNKNOWN;
    }
}

bool FollowMeImpl::is_target_location_set() const
{
    // If the target's latitude is NAN, we assume that location is not set.
    // We assume that mutex was acquired by the caller
    return std::isfinite(_target_location.latitude_deg);
}

void FollowMeImpl::send_target_location()
{
    // Don't send if we're not in FollowMe mode.
    if (!is_active()) {
        return;
    }

    dl_time_t now = _time.steady_time();
    // needed by http://mavlink.org/messages/common#FOLLOW_TARGET
    uint64_t elapsed_msec = static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds

    _mutex.lock();
//    LogDebug() << "Lat: " << _target_location.latitude_deg << " Lon: " << _target_location.longitude_deg
//               <<
//               " Alt: " << _target_location.absolute_altitude_m;
    const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
    const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
    const float alt = static_cast<float>(_target_location.absolute_altitude_m);
//    LogDebug() << "Lat int: " << lat_int << " Lon int: " << lon_int << " Alt: " << alt;
    _mutex.unlock();

    const float pos_std_dev[] = { NAN, NAN, NAN };
    const float vel[] = { NAN, NAN, NAN };
    const float accel_unknown[] = { NAN, NAN, NAN };
    const float attitude_q_unknown[] = { 1.f, NAN, NAN, NAN };
    const float rates_unknown[] = { NAN, NAN, NAN };
    uint64_t custom_state = 0;

    mavlink_message_t msg {};
    mavlink_msg_follow_target_pack(_parent->get_own_system_id(),
                                   _parent->get_own_component_id(),
                                   &msg,
                                   elapsed_msec,
                                   _estimatation_capabilities,
                                   lat_int,
                                   lon_int,
                                   alt,
                                   vel,
                                   accel_unknown,
                                   attitude_q_unknown,
                                   rates_unknown,
                                   pos_std_dev,
                                   custom_state);

    if (!_parent->send_message(msg)) {
        LogErr() << "send_target_location() failed..";
    } else {
        std::lock_guard<std::mutex> lock(_mutex);
        _last_location = _target_location;
    }
}

void FollowMeImpl::stop_sending_target_location()
{
    // We assume that mutex was acquired by the caller
    if (_target_location_cookie) {
        _parent->remove_call_every(_target_location_cookie);
        _target_location_cookie = nullptr;
    }
    _mode = Mode::NOT_ACTIVE;
}

void FollowMeImpl::process_heartbeat(const mavlink_message_t &message)
{
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(&message, &heartbeat);

    bool follow_me_active = false; // tells whether we're in FollowMe mode right now
    if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {

        px4::px4_custom_mode px4_custom_mode;
        px4_custom_mode.data = heartbeat.custom_mode;

        if (px4_custom_mode.main_mode == px4::PX4_CUSTOM_MAIN_MODE_AUTO &&
            px4_custom_mode.sub_mode == px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET) {
            follow_me_active = true; // we're in FollowMe mode
        }
    }

    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (!follow_me_active && _mode == Mode::ACTIVE) {
            // We're NOT in FollowMe mode anymore.
            // Lets stop sending target location updates
            stop_sending_target_location();
        } else if (follow_me_active && _mode == Mode::NOT_ACTIVE) {
            // We're in FollowMe mode now
            _mode = Mode::ACTIVE;
            _mutex.unlock(); // we must unlock to avoid deadlock in send_target_location()
            return;
        }
    }
}

} // namespace dronecore
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2007-2010 Digital Bazaar, Inc. All rights reserved.
 */
#define __STDC_CONSTANT_MACROS

#include "monarch/net/SocketTools.h"

#include "monarch/net/WindowsSupport.h"
#include "monarch/rt/Thread.h"
#include "monarch/rt/System.h"

#include <limits.h>
#ifndef WIN32
#include <poll.h>
#endif

using namespace monarch::net;
using namespace monarch::rt;

#ifndef HOST_NAME_MAX
#define HOST_NAME_MAX 255
#endif

int SocketTools::poll(bool read, int fd, int64_t timeout)
{
#if defined(WIN32)
   return select(read, fd, timeout);
#else
   int rval = 0;

   // create poll set
   struct pollfd fds;
   fds.fd = fd;
   fds.events = read ? POLLIN : POLLOUT;

   // set 20 millisecond interrupt check timeout (currently necessary to
   // enable thread interruptions)
   int intck = INT32_C(20);

   // keep selecting (polling) until timeout is reached
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   int to;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to = 0;
   }
   else
   {
      // create intck millisecond timeout
      to = (remaining < intck ? (int)remaining : intck);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptor to be updated
      rval = ::poll(&fds, 1, to);

      // if no data in or out, check for error events
      if(rval > 0 && !(fds.revents & POLLIN) && !(fds.revents & POLLOUT))
      {
         // remote side hung up
         if(fds.revents & POLLHUP)
         {
            rval = -1;
            errno = EPIPE;
         }
         // file descriptor not open
         else if(fds.revents & POLLNVAL)
         {
            rval = -1;
            errno = EBADF;
         }
         // error
         else if(fds.revents & POLLERR)
         {
            // some kind of IO error
            rval = -1;
            errno = EIO;
         }
      }
      else if(rval == -1 && errno == EINTR)
      {
         // no error, syscall interrupted
         rval = 0;
      }

      // decrement timeout
      if(rval == 0 && timeout != 0)
      {
         // decrement remaining time
         end = System::getCurrentMilliseconds();
         remaining -= (end - start);
         start = end;
         to = (remaining < intck ? (int)remaining : intck);
      }
   }

   if(rval > 0)
   {
      if(t->isInterrupted())
      {
         rval = -1;
         errno = EINTR;

         // set interrupted exception
         ExceptionRef e = t->createInterruptedException();
         Exception::set(e);
      }
   }

   return rval;
#endif
}

#ifdef WIN32
int SocketTools::select(bool read, unsigned int fd, int64_t timeout)
#else
int SocketTools::select(bool read, int fd, int64_t timeout)
#endif
{
   int rval = 0;

   // create a file descriptor set to read on
   fd_set rfds;
   FD_ZERO(&rfds);

   // create a file descriptor set to write on
   fd_set wfds;
   FD_ZERO(&wfds);

   // create a file descriptor set to check for exceptions on
   fd_set exfds;
   FD_ZERO(&exfds);

   // add file descriptor to sets
   FD_SET(fd, &rfds);
   FD_SET(fd, &wfds);
   FD_SET(fd, &exfds);

   // "n" parameter is the highest numbered descriptor plus 1
#ifdef WIN32
   unsigned int n = fd + 1;
#else
   int n = fd + 1;
#endif

   // set 20 millisecond interrupt check timeout (necessary because
   // windows lacks SIGNAL support to do interruptions properly)
   int64_t intck = INT64_C(20);

   // keep selecting (polling) until timeout is reached, if timeout is
   // indefinite (0), then set remaining to intck and never decrement it
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   struct timeval to;
   to.tv_sec = 0;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to.tv_usec = 0;
   }
   else
   {
      // create intck millisecond timeout (1 millisecond is 1000 microseconds)
      to.tv_usec = (remaining < intck ? remaining : intck) * INT64_C(1000);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptors to be updated
      if(read)
      {
         // wait for readability
         rval = ::select(n, &rfds, NULL, &exfds, &to);
      }
      else
      {
         // wait for readability and writability
         //
         // Note: We must test to see if the pipe is broken by
         // testing for readability -- as it will occur if the
         // connection closes due to TCP sending an RST to the
         // socket ... and this will cause recv() to return 0
         rval = ::select(n, &rfds, &wfds, &exfds, &to);
         if(rval > 0)
         {
            if(FD_ISSET(fd, &rfds))
            {
               // readability flag switched, check to see if the connection has
               // been shutdown, by seeing if recv() will return 0 (do a peek
               // so as not to disturb real data)
               char buf;
               int flags = MSG_PEEK;
#ifdef MSG_DONTWAIT
               flags |= MSG_DONTWAIT;
#endif
               rval = SOCKET_MACRO_recv(fd, &buf, 1, flags);
               if(rval == 0)
               {
                  // connection closed
                  rval = -1;
                  errno = EPIPE;
               }
            }
            if(rval > 0)
            {
               // connection not closed, see if write was detected
               rval = FD_ISSET(fd, &wfds) ? 1 : 0;
            }
         }
      }

      if(rval < 0 && (errno == 0 || errno == EINPROGRESS || errno == EINTR))
      {
         // no error, just timed out, operation in progress,
         // or syscall interrupted
         rval = 0;

         // Note: handling EINTR could be changed to interrupt the current
         // thread if appropriate
      }

      // select() implementation may alter sets or timeout, so reset them
      // if calling select() again
      if(rval == 0)
      {
         // clear sets and re-add file descriptor
         FD_ZERO(&rfds);
         FD_ZERO(&wfds);
         FD_ZERO(&exfds);
         FD_SET(fd, &rfds);
         FD_SET(fd, &wfds);
         FD_SET(fd, &exfds);

         // reset timeout
         to.tv_sec = 0;
         if(timeout < 0)
         {
            // instant polling
            to.tv_usec = 0;
         }
         else if(timeout == 0)
         {
            // indefinite, do not decrement remaining
            to.tv_usec = intck * INT64_C(1000);
         }
         else
         {
            // decrement remaining time
            end = System::getCurrentMilliseconds();
            remaining -= (end - start);
            start = end;
            to.tv_usec =
               (remaining < intck ? remaining : intck) * INT64_C(1000);
         }
      }
   }

   if(rval > 0)
   {
      if(t->isInterrupted())
      {
         rval = -1;
         errno = EINTR;

         // set interrupted exception
         ExceptionRef e = t->createInterruptedException();
         Exception::set(e);
      }
      else if(FD_ISSET(fd, &exfds) != 0)
      {
         // exception occurred with file descriptor, consider pipe broken,
         // cannot mark it as a bad file descriptor here or else it won't
         // be closed
         rval = -1;
         errno = EPIPE;
      }
   }

   return rval;
}

int SocketTools::select(
   int nfds, fd_set* readfds, fd_set* writefds, fd_set* exceptfds,
   int64_t timeout, const sigset_t* sigmask)
{
   int rval = 0;

// Note: disabled due to a lack of support in windows
//   // create timeout
//   struct timeval* tv = NULL;
//   struct timeval to;
//   if(timeout > 0)
//   {
//      // set timeout (1 millisecond is 1000 microseconds)
//      to.tv_sec = timeout / INT64_C(1000);
//      to.tv_usec = (timeout % INT64_C(1000)) * INT64_C(1000);
//      tv = &to;
//   }
//
//   // FIXME: signals supposedly don't make select() return in windows
//   // this needs to be tested and potentially remedied somehow
//
//   // FIXME: furthermore, even if we block SIGINT (interruption signal) up
//   // until we reach the select call -- and then unblock right before it
//   // the signal could still sneak in right before select() is called and
//   // control is transferred to the kernel, and therefore we'd handle the
//   // SIGINT before the select() call and select() wouldn't get interrupted
//   // (there is pselect() for doing that unblocking atomically, but
//   // it's UNIX only) -- this may be solved by writing to another file
//   // descriptor when we receive SIGINTs and checking that file descriptor
//   // as well as the one we are waiting on -- but this might not be a
//   // viable solution for windows
//
//   // block SIGINTs
//   blockSignal(SIGINT);
//
//   Thread* t = Thread::currentThread();
//   if(!t->isInterrupted())
//   {
//      // FIXME: pselect() required here to do atomic unblocking & selecting
//
//      // wait for file descriptors to be updated
//      unblockSignal(SIGINT);
//      rval = ::select(nfds, readfds, writefds, exceptfds, timeout);
//      if(rval < 0)
//      {
//         if(errno == EINTR)
//         {
//            // interrupt thread
//            t->interrupt();
//         }
//      }
//   }
//   else
//   {
//      rval = -1;
//      errno = EINTR;
//   }

   // clone file descriptor sets
   fd_set readfds2;
   fd_set writefds2;
   fd_set exceptfds2;

   if(readfds != NULL)
   {
      readfds2 = *readfds;
   }

   if(writefds != NULL)
   {
      writefds2 = *writefds;
   }

   if(exceptfds != NULL)
   {
      exceptfds2 = *exceptfds;
   }

   // set 20 millisecond interrupt check timeout (necessary because
   // windows lacks SIGNAL support to do interruptions properly)
   int64_t intck = INT64_C(20);

   // keep selecting (polling) until timeout is reached, if timeout is
   // indefinite (0), then set remaining to intck and never decrement it
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   struct timeval to;
   to.tv_sec = 0;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to.tv_usec = 0;
   }
   else
   {
      // create 20 millisecond timeout (1 millisecond is 1000 microseconds)
      to.tv_usec = (remaining < intck ? remaining : intck) * INT64_C(1000);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptors to be updated
      rval = ::select(nfds, readfds, writefds, exceptfds, &to);

      if(rval < 0 && (errno == 0 || errno == EINPROGRESS || errno == EINTR))
      {
         // no error, just timed out, operation in progress,
         // or syscall interrupted
         rval = 0;

         // Note: handling EINTR could be changed to interrupt the current
         // thread if appropriate
      }

      // select() implementation may alter sets or timeout, so reset them
      // if calling select() again
      if(rval == 0)
      {
         // reset file descriptor sets
         if(readfds != NULL)
         {
            *readfds = readfds2;
         }

         if(writefds != NULL)
         {
            *writefds = writefds2;
         }

         if(exceptfds != NULL)
         {
            *exceptfds = exceptfds2;
         }

         // reset timeout
         to.tv_sec = 0;
         if(timeout < 0)
         {
            // instant polling
            to.tv_usec = 0;
         }
         else if(timeout == 0)
         {
            // indefinite, do not decrement remaining
            to.tv_usec = intck * INT64_C(1000);
         }
         else
         {
            // decrement remaining time
            end = System::getCurrentMilliseconds();
            remaining -= (end - start);
            start = end;
            to.tv_usec =
               (remaining < intck ? remaining : intck) * INT64_C(1000);
         }
      }
   }

   if(t->isInterrupted())
   {
      rval = -1;
      errno = EINTR;

      // set interrupted exception
      ExceptionRef e = t->createInterruptedException();
      Exception::set(e);
   }

   return rval;
}

std::string SocketTools::getHostname()
{
   char tmp[HOST_NAME_MAX + 1];
   memset(tmp, 0, HOST_NAME_MAX + 1);
   gethostname(tmp, HOST_NAME_MAX);
   return tmp;
}
<commit_msg>Removed unnecessary extra comparison.<commit_after>/*
 * Copyright (c) 2007-2010 Digital Bazaar, Inc. All rights reserved.
 */
#define __STDC_CONSTANT_MACROS

#include "monarch/net/SocketTools.h"

#include "monarch/net/WindowsSupport.h"
#include "monarch/rt/Thread.h"
#include "monarch/rt/System.h"

#include <limits.h>
#ifndef WIN32
#include <poll.h>
#endif

using namespace monarch::net;
using namespace monarch::rt;

#ifndef HOST_NAME_MAX
#define HOST_NAME_MAX 255
#endif

int SocketTools::poll(bool read, int fd, int64_t timeout)
{
#if defined(WIN32)
   return select(read, fd, timeout);
#else
   int rval = 0;

   // create poll set
   struct pollfd fds;
   fds.fd = fd;
   fds.events = read ? POLLIN : POLLOUT;

   // set 20 millisecond interrupt check timeout (currently necessary to
   // enable thread interruptions)
   int intck = INT32_C(20);

   // keep selecting (polling) until timeout is reached
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   int to;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to = 0;
   }
   else
   {
      // create intck millisecond timeout
      to = (remaining < intck ? (int)remaining : intck);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptor to be updated
      rval = ::poll(&fds, 1, to);

      // if no data in or out, check for error events
      if(rval > 0 && !(fds.revents & fds.events))
      {
         // remote side hung up
         if(fds.revents & POLLHUP)
         {
            rval = -1;
            errno = EPIPE;
         }
         // file descriptor not open
         else if(fds.revents & POLLNVAL)
         {
            rval = -1;
            errno = EBADF;
         }
         // error
         else if(fds.revents & POLLERR)
         {
            // some kind of IO error
            rval = -1;
            errno = EIO;
         }
      }
      else if(rval == -1 && errno == EINTR)
      {
         // no error, syscall interrupted
         rval = 0;
      }

      // decrement timeout
      if(rval == 0 && timeout != 0)
      {
         // decrement remaining time
         end = System::getCurrentMilliseconds();
         remaining -= (end - start);
         start = end;
         to = (remaining < intck ? (int)remaining : intck);
      }
   }

   if(rval > 0)
   {
      if(t->isInterrupted())
      {
         rval = -1;
         errno = EINTR;

         // set interrupted exception
         ExceptionRef e = t->createInterruptedException();
         Exception::set(e);
      }
   }

   return rval;
#endif
}

#ifdef WIN32
int SocketTools::select(bool read, unsigned int fd, int64_t timeout)
#else
int SocketTools::select(bool read, int fd, int64_t timeout)
#endif
{
   int rval = 0;

   // create a file descriptor set to read on
   fd_set rfds;
   FD_ZERO(&rfds);

   // create a file descriptor set to write on
   fd_set wfds;
   FD_ZERO(&wfds);

   // create a file descriptor set to check for exceptions on
   fd_set exfds;
   FD_ZERO(&exfds);

   // add file descriptor to sets
   FD_SET(fd, &rfds);
   FD_SET(fd, &wfds);
   FD_SET(fd, &exfds);

   // "n" parameter is the highest numbered descriptor plus 1
#ifdef WIN32
   unsigned int n = fd + 1;
#else
   int n = fd + 1;
#endif

   // set 20 millisecond interrupt check timeout (necessary because
   // windows lacks SIGNAL support to do interruptions properly)
   int64_t intck = INT64_C(20);

   // keep selecting (polling) until timeout is reached, if timeout is
   // indefinite (0), then set remaining to intck and never decrement it
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   struct timeval to;
   to.tv_sec = 0;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to.tv_usec = 0;
   }
   else
   {
      // create intck millisecond timeout (1 millisecond is 1000 microseconds)
      to.tv_usec = (remaining < intck ? remaining : intck) * INT64_C(1000);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptors to be updated
      if(read)
      {
         // wait for readability
         rval = ::select(n, &rfds, NULL, &exfds, &to);
      }
      else
      {
         // wait for readability and writability
         //
         // Note: We must test to see if the pipe is broken by
         // testing for readability -- as it will occur if the
         // connection closes due to TCP sending an RST to the
         // socket ... and this will cause recv() to return 0
         rval = ::select(n, &rfds, &wfds, &exfds, &to);
         if(rval > 0)
         {
            if(FD_ISSET(fd, &rfds))
            {
               // readability flag switched, check to see if the connection has
               // been shutdown, by seeing if recv() will return 0 (do a peek
               // so as not to disturb real data)
               char buf;
               int flags = MSG_PEEK;
#ifdef MSG_DONTWAIT
               flags |= MSG_DONTWAIT;
#endif
               rval = SOCKET_MACRO_recv(fd, &buf, 1, flags);
               if(rval == 0)
               {
                  // connection closed
                  rval = -1;
                  errno = EPIPE;
               }
            }
            if(rval > 0)
            {
               // connection not closed, see if write was detected
               rval = FD_ISSET(fd, &wfds) ? 1 : 0;
            }
         }
      }

      if(rval < 0 && (errno == 0 || errno == EINPROGRESS || errno == EINTR))
      {
         // no error, just timed out, operation in progress,
         // or syscall interrupted
         rval = 0;

         // Note: handling EINTR could be changed to interrupt the current
         // thread if appropriate
      }

      // select() implementation may alter sets or timeout, so reset them
      // if calling select() again
      if(rval == 0)
      {
         // clear sets and re-add file descriptor
         FD_ZERO(&rfds);
         FD_ZERO(&wfds);
         FD_ZERO(&exfds);
         FD_SET(fd, &rfds);
         FD_SET(fd, &wfds);
         FD_SET(fd, &exfds);

         // reset timeout
         to.tv_sec = 0;
         if(timeout < 0)
         {
            // instant polling
            to.tv_usec = 0;
         }
         else if(timeout == 0)
         {
            // indefinite, do not decrement remaining
            to.tv_usec = intck * INT64_C(1000);
         }
         else
         {
            // decrement remaining time
            end = System::getCurrentMilliseconds();
            remaining -= (end - start);
            start = end;
            to.tv_usec =
               (remaining < intck ? remaining : intck) * INT64_C(1000);
         }
      }
   }

   if(rval > 0)
   {
      if(t->isInterrupted())
      {
         rval = -1;
         errno = EINTR;

         // set interrupted exception
         ExceptionRef e = t->createInterruptedException();
         Exception::set(e);
      }
      else if(FD_ISSET(fd, &exfds) != 0)
      {
         // exception occurred with file descriptor, consider pipe broken,
         // cannot mark it as a bad file descriptor here or else it won't
         // be closed
         rval = -1;
         errno = EPIPE;
      }
   }

   return rval;
}

int SocketTools::select(
   int nfds, fd_set* readfds, fd_set* writefds, fd_set* exceptfds,
   int64_t timeout, const sigset_t* sigmask)
{
   int rval = 0;

// Note: disabled due to a lack of support in windows
//   // create timeout
//   struct timeval* tv = NULL;
//   struct timeval to;
//   if(timeout > 0)
//   {
//      // set timeout (1 millisecond is 1000 microseconds)
//      to.tv_sec = timeout / INT64_C(1000);
//      to.tv_usec = (timeout % INT64_C(1000)) * INT64_C(1000);
//      tv = &to;
//   }
//
//   // FIXME: signals supposedly don't make select() return in windows
//   // this needs to be tested and potentially remedied somehow
//
//   // FIXME: furthermore, even if we block SIGINT (interruption signal) up
//   // until we reach the select call -- and then unblock right before it
//   // the signal could still sneak in right before select() is called and
//   // control is transferred to the kernel, and therefore we'd handle the
//   // SIGINT before the select() call and select() wouldn't get interrupted
//   // (there is pselect() for doing that unblocking atomically, but
//   // it's UNIX only) -- this may be solved by writing to another file
//   // descriptor when we receive SIGINTs and checking that file descriptor
//   // as well as the one we are waiting on -- but this might not be a
//   // viable solution for windows
//
//   // block SIGINTs
//   blockSignal(SIGINT);
//
//   Thread* t = Thread::currentThread();
//   if(!t->isInterrupted())
//   {
//      // FIXME: pselect() required here to do atomic unblocking & selecting
//
//      // wait for file descriptors to be updated
//      unblockSignal(SIGINT);
//      rval = ::select(nfds, readfds, writefds, exceptfds, timeout);
//      if(rval < 0)
//      {
//         if(errno == EINTR)
//         {
//            // interrupt thread
//            t->interrupt();
//         }
//      }
//   }
//   else
//   {
//      rval = -1;
//      errno = EINTR;
//   }

   // clone file descriptor sets
   fd_set readfds2;
   fd_set writefds2;
   fd_set exceptfds2;

   if(readfds != NULL)
   {
      readfds2 = *readfds;
   }

   if(writefds != NULL)
   {
      writefds2 = *writefds;
   }

   if(exceptfds != NULL)
   {
      exceptfds2 = *exceptfds;
   }

   // set 20 millisecond interrupt check timeout (necessary because
   // windows lacks SIGNAL support to do interruptions properly)
   int64_t intck = INT64_C(20);

   // keep selecting (polling) until timeout is reached, if timeout is
   // indefinite (0), then set remaining to intck and never decrement it
   int64_t remaining = (timeout <= 0) ? intck : timeout;

   struct timeval to;
   to.tv_sec = 0;
   if(timeout < 0)
   {
      // create instant timeout (polling)
      to.tv_usec = 0;
   }
   else
   {
      // create 20 millisecond timeout (1 millisecond is 1000 microseconds)
      to.tv_usec = (remaining < intck ? remaining : intck) * INT64_C(1000);
   }

   uint64_t start = System::getCurrentMilliseconds();
   uint64_t end;

   Thread* t = Thread::currentThread();
   while(remaining > 0 && rval == 0 && !t->isInterrupted())
   {
      // wait for file descriptors to be updated
      rval = ::select(nfds, readfds, writefds, exceptfds, &to);

      if(rval < 0 && (errno == 0 || errno == EINPROGRESS || errno == EINTR))
      {
         // no error, just timed out, operation in progress,
         // or syscall interrupted
         rval = 0;

         // Note: handling EINTR could be changed to interrupt the current
         // thread if appropriate
      }

      // select() implementation may alter sets or timeout, so reset them
      // if calling select() again
      if(rval == 0)
      {
         // reset file descriptor sets
         if(readfds != NULL)
         {
            *readfds = readfds2;
         }

         if(writefds != NULL)
         {
            *writefds = writefds2;
         }

         if(exceptfds != NULL)
         {
            *exceptfds = exceptfds2;
         }

         // reset timeout
         to.tv_sec = 0;
         if(timeout < 0)
         {
            // instant polling
            to.tv_usec = 0;
         }
         else if(timeout == 0)
         {
            // indefinite, do not decrement remaining
            to.tv_usec = intck * INT64_C(1000);
         }
         else
         {
            // decrement remaining time
            end = System::getCurrentMilliseconds();
            remaining -= (end - start);
            start = end;
            to.tv_usec =
               (remaining < intck ? remaining : intck) * INT64_C(1000);
         }
      }
   }

   if(t->isInterrupted())
   {
      rval = -1;
      errno = EINTR;

      // set interrupted exception
      ExceptionRef e = t->createInterruptedException();
      Exception::set(e);
   }

   return rval;
}

std::string SocketTools::getHostname()
{
   char tmp[HOST_NAME_MAX + 1];
   memset(tmp, 0, HOST_NAME_MAX + 1);
   gethostname(tmp, HOST_NAME_MAX);
   return tmp;
}
<|endoftext|>


<commit_before>#include "util/etcd_delete.h"

#include <functional>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <mutex>

using std::move;
using std::mutex;
using std::pair;
using std::placeholders::_1;
using std::string;
using std::unique_lock;
using std::vector;
using util::Status;
using util::Task;
using util::TaskHold;

DEFINE_int32(etcd_delete_concurrency, 4,
             "number of etcd keys to delete at a time");

namespace cert_trans {

namespace {

class DeleteState {
 public:
  DeleteState(EtcdClient* client, vector<pair<string, int64_t>>&& keys,
              Task* task)
      : client_(CHECK_NOTNULL(client)),
        task_(CHECK_NOTNULL(task)),
        outstanding_(0),
        keys_(move(keys)),
        it_(keys_.begin()) {
    CHECK_GT(FLAGS_etcd_delete_concurrency, 0);

    if (it_ == keys_.end()) {
      // Nothing to do!
      task_->Return();
    } else {
      StartNextRequest(unique_lock<mutex>(mutex_));
    }
  }

  ~DeleteState() {
    CHECK_EQ(outstanding_, 0);
  }

 private:
  void RequestDone(Task* child_task);
  void StartNextRequest(unique_lock<mutex>&& lock);

  EtcdClient* const client_;
  Task* const task_;
  mutex mutex_;
  int outstanding_;
  const vector<pair<string, int64_t>> keys_;
  vector<pair<string, int64_t>>::const_iterator it_;
};

void DeleteState::RequestDone(Task* child_task) {
  // If a child task has an error, return that error, and do not start
  // any more requests.
  if (!child_task->status().ok()) {
    task_->Return(child_task->status());
    return;
  }

  unique_lock<mutex> lock(mutex_);
  --outstanding_;

  if (it_ != keys_.end()) {
    StartNextRequest(move(lock));
  } else {
    if (outstanding_ < 1) {
      // No more keys to get, and this was the last one to complete.
      lock.unlock();
      task_->Return();
    }
  }
}

void DeleteState::StartNextRequest(unique_lock<mutex>&& lock) {
  CHECK(lock.owns_lock());

  if (task_->CancelRequested()) {
    // In case the task uses an inline executor.
    lock.unlock();
    task_->Return(Status::CANCELLED);
    return;
  }

  while (outstanding_ < FLAGS_etcd_delete_concurrency && it_ != keys_.end()) {
    CHECK(lock.owns_lock());
    const pair<string, int64_t>& key(*it_);
    ++it_;
    ++outstanding_;

    // In case the task uses an inline executor.
    lock.unlock();

    client_->Delete(key.first, key.second,
                    task_->AddChild(
                        bind(&DeleteState::RequestDone, this, _1)));

    // We must be holding the lock to evaluate the loop condition.
    lock.lock();
  }
}

}  // namespace

void EtcdDeleteKeys(EtcdClient* client, vector<pair<string, int64_t>>&& keys,
                    Task* task) {
  TaskHold hold(task);
  DeleteState* const state(new DeleteState(client, move(keys), task));
  task->DeleteWhenDone(state);
}

}  // namespace cert_trans
<commit_msg>Always decrement outstanding_, even if there was an error.<commit_after>#include "util/etcd_delete.h"

#include <functional>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <mutex>

using std::move;
using std::mutex;
using std::pair;
using std::placeholders::_1;
using std::string;
using std::unique_lock;
using std::vector;
using util::Status;
using util::Task;
using util::TaskHold;

DEFINE_int32(etcd_delete_concurrency, 4,
             "number of etcd keys to delete at a time");

namespace cert_trans {

namespace {

class DeleteState {
 public:
  DeleteState(EtcdClient* client, vector<pair<string, int64_t>>&& keys,
              Task* task)
      : client_(CHECK_NOTNULL(client)),
        task_(CHECK_NOTNULL(task)),
        outstanding_(0),
        keys_(move(keys)),
        it_(keys_.begin()) {
    CHECK_GT(FLAGS_etcd_delete_concurrency, 0);

    if (it_ == keys_.end()) {
      // Nothing to do!
      task_->Return();
    } else {
      StartNextRequest(unique_lock<mutex>(mutex_));
    }
  }

  ~DeleteState() {
    CHECK_EQ(outstanding_, 0);
  }

 private:
  void RequestDone(Task* child_task);
  void StartNextRequest(unique_lock<mutex>&& lock);

  EtcdClient* const client_;
  Task* const task_;
  mutex mutex_;
  int outstanding_;
  const vector<pair<string, int64_t>> keys_;
  vector<pair<string, int64_t>>::const_iterator it_;
};

void DeleteState::RequestDone(Task* child_task) {
  unique_lock<mutex> lock(mutex_);
  --outstanding_;

  // If a child task has an error, return that error, and do not start
  // any more requests.
  if (!child_task->status().ok()) {
    lock.unlock();
    task_->Return(child_task->status());
    return;
  }

  if (it_ != keys_.end()) {
    StartNextRequest(move(lock));
  } else {
    if (outstanding_ < 1) {
      // No more keys to get, and this was the last one to complete.
      lock.unlock();
      task_->Return();
    }
  }
}

void DeleteState::StartNextRequest(unique_lock<mutex>&& lock) {
  CHECK(lock.owns_lock());

  if (task_->CancelRequested()) {
    // In case the task uses an inline executor.
    lock.unlock();
    task_->Return(Status::CANCELLED);
    return;
  }

  while (outstanding_ < FLAGS_etcd_delete_concurrency && it_ != keys_.end()) {
    CHECK(lock.owns_lock());
    const pair<string, int64_t>& key(*it_);
    ++it_;
    ++outstanding_;

    // In case the task uses an inline executor.
    lock.unlock();

    client_->Delete(key.first, key.second,
                    task_->AddChild(
                        bind(&DeleteState::RequestDone, this, _1)));

    // We must be holding the lock to evaluate the loop condition.
    lock.lock();
  }
}

}  // namespace

void EtcdDeleteKeys(EtcdClient* client, vector<pair<string, int64_t>>&& keys,
                    Task* task) {
  TaskHold hold(task);
  DeleteState* const state(new DeleteState(client, move(keys), task));
  task->DeleteWhenDone(state);
}

}  // namespace cert_trans
<|endoftext|>


<commit_before><commit_msg>Delete ApeFileMaterialImpl.cpp<commit_after><|endoftext|>


<commit_before>#include "QXmppInformationRequestResult.h"
#include "QXmppConstants.h"
#include <QXmlStreamWriter>

QXmppInformationRequestResult::QXmppInformationRequestResult() : QXmppIq(QXmppIq::Result)
{
}

void QXmppInformationRequestResult::toXmlElementFromChild(QXmlStreamWriter *writer) const
{
    writer->writeStartElement("query");
    writer->writeAttribute("xmlns", ns_disco_info );
    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_disco_info );
    writer->writeEndElement();
    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_ibb );
    writer->writeEndElement();
    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_rpc);
    writer->writeEndElement();
    writer->writeStartElement("identity");
    writer->writeAttribute("category", "automation" );
    writer->writeAttribute("type", "rpc" );
    writer->writeEndElement();
    writer->writeEndElement();
}
<commit_msg>Add XEP-0199:XMPP Ping support http://xmpp.org/extensions/xep-0199.html#disco<commit_after>#include "QXmppInformationRequestResult.h"
#include "QXmppConstants.h"
#include <QXmlStreamWriter>

QXmppInformationRequestResult::QXmppInformationRequestResult() : QXmppIq(QXmppIq::Result)
{
}

void QXmppInformationRequestResult::toXmlElementFromChild(QXmlStreamWriter *writer) const
{
    writer->writeStartElement("query");
    writer->writeAttribute("xmlns", ns_disco_info );

    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_disco_info );
    writer->writeEndElement();

    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_ibb );
    writer->writeEndElement();

    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_rpc);
    writer->writeEndElement();
    writer->writeStartElement("identity");
    writer->writeAttribute("category", "automation" );
    writer->writeAttribute("type", "rpc" );
    writer->writeEndElement();

    writer->writeStartElement("feature");
    writer->writeAttribute("var", ns_ping);
    writer->writeEndElement();

    writer->writeEndElement();
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */

#pragma once

/**
 * UAVCAN version definition
 */
#define UAVCAN_VERSION_MAJOR    0
#define UAVCAN_VERSION_MINOR    1

/**
 * UAVCAN can be compiled in C++11 mode.
 * This macro allows to detect which version of the C++ standard is being used.
 *
 * UAVCAN_CPP_VERSION can be defined explicitly to override auto detection to force compilation in C++03 mode.
 * Valid values are listed below.
 *
 * Note that C++11 and newer requires C++ standard library, while C++03 does not. So, on platforms that
 * don't implement the standard C++ (e.g. NuttX) use of C++03 may be preferred.
 */
#define UAVCAN_CPP11    2011
#define UAVCAN_CPP03    2003

#ifndef UAVCAN_CPP_VERSION
# if __cplusplus > 201200
#  error Unsupported C++ standard
# elif (__cplusplus > 201100) || defined(__GXX_EXPERIMENTAL_CXX0X__)
#  define UAVCAN_CPP_VERSION    UAVCAN_CPP11
# else
#  define UAVCAN_CPP_VERSION    UAVCAN_CPP03
# endif
#endif

/**
 * UAVCAN can be explicitly told to ignore exceptions, or it can be detected automatically.
 * Autodetection is not expected to work with all compilers, so it's safer to define it explicitly.
 * If the autodetection fails, exceptions will be disabled by default.
 */
#ifndef UAVCAN_EXCEPTIONS
# if __EXCEPTIONS || _HAS_EXCEPTIONS
#  define UAVCAN_EXCEPTIONS  1
# else
#  define UAVCAN_EXCEPTIONS  0
# endif
#endif

/**
 * Struct layout control.
 * Set UAVCAN_PACK_STRUCTS=1 and define UAVCAN_PACKED_BEGIN and UAVCAN_PACKED_END to reduce memory usage.
 * THIS MAY BREAK THE CODE.
 */
#ifndef UAVCAN_PACK_STRUCTS
# define UAVCAN_PACK_STRUCTS    0
#endif
#ifndef UAVCAN_PACKED_BEGIN
# define UAVCAN_PACKED_BEGIN
#endif
#ifndef UAVCAN_PACKED_END
# define UAVCAN_PACKED_END
#endif

/**
 * Declaration visibility
 * http://gcc.gnu.org/wiki/Visibility
 */
#ifndef UAVCAN_EXPORT
# define UAVCAN_EXPORT
#endif

/**
 * Trade-off between ROM/RAM usage and functionality/determinism.
 * Note that this feature is not well tested and should be avoided.
 * Use code search for UAVCAN_TINY to find what functionality will be disabled.
 * This is particularly useful for embedded systems with less than 40kB of ROM.
 */
#ifndef UAVCAN_TINY
# define UAVCAN_TINY 0
#endif

/**
 * It might make sense to remove toString() methods for an embedded system.
 * If the autodetect fails, toString() will be disabled, so it's pretty safe by default.
 */
#ifndef UAVCAN_TOSTRING
// Objective is to make sure that we're NOT on a resource constrained platform
# if __linux__ || __linux || __APPLE__ || _WIN64 || _WIN32
#  define UAVCAN_TOSTRING 1
# else
#  define UAVCAN_TOSTRING 0
# endif
#endif

#if UAVCAN_TOSTRING
# include <string>
# include <cstdio>
#endif

/**
 * Some C++ implementations are half-broken because they don't implement the placement new operator.
 * If UAVCAN_IMPLEMENT_PLACEMENT_NEW is defined, libuavcan will implement its own operator new (std::size_t, void*)
 * and its delete() counterpart, instead of relying on the standard header <new>.
 */
#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW
# define UAVCAN_IMPLEMENT_PLACEMENT_NEW 0
#endif

/**
 * Run time checks.
 * Resolves to the standard assert() by default.
 * Disabled completely if UAVCAN_NO_ASSERTIONS is defined.
 */
#ifndef UAVCAN_ASSERT
# if UAVCAN_NO_ASSERTIONS
#  define UAVCAN_ASSERT(x)
# else
#  define UAVCAN_ASSERT(x) assert(x)
# endif
#endif

namespace uavcan
{

/**
 * Memory pool block size.
 *
 * The default of 64 bytes should be OK for any target arch up to AMD64 and any compiler.
 *
 * The library leverages compile-time checks to ensure that all types that are subject to dynamic allocation
 * fit this size, otherwise compilation fails.
 *
 * For platforms featuring small pointer width (16..32 bits), UAVCAN_MEM_POOL_BLOCK_SIZE can often be safely
 * reduced to 56 or even 48 bytes, which leads to lower memory footprint.
 *
 * Note that the pool block size shall be aligned at biggest alignment of the target platform (detected and
 * checked automatically at compile time).
 */
#if UAVCAN_MEM_POOL_BLOCK_SIZE
/// Explicitly specified by the user.
static const unsigned MemPoolBlockSize = UAVCAN_MEM_POOL_BLOCK_SIZE;
#elif defined(__BIGGEST_ALIGNMENT__) && (__BIGGEST_ALIGNMENT__ <= 8)
/// Convenient default for GCC-like compilers - if alignment allows, pool block size can be safely reduced.
static const unsigned MemPoolBlockSize = 56;
#else
/// Safe default that should be OK for any platform.
static const unsigned MemPoolBlockSize = 64;
#endif

#ifdef __BIGGEST_ALIGNMENT__
static const unsigned MemPoolAlignment = __BIGGEST_ALIGNMENT__;
#else
static const unsigned MemPoolAlignment = 16;
#endif

typedef char _alignment_check_for_MEM_POOL_BLOCK_SIZE[((MemPoolBlockSize & (MemPoolAlignment - 1)) == 0) ? 1 : -1];

/**
 * This class that allows to check at compile time whether type T can be allocated using the memory pool.
 * If the check fails, compilation fails.
 */
template <typename T>
struct UAVCAN_EXPORT IsDynamicallyAllocatable
{
    static void check()
    {
        char dummy[(sizeof(T) <= MemPoolBlockSize) ? 1 : -1];
        (void)dummy;
    }
};

}
<commit_msg>Clarified docs for UAVCAN_CPP_VERSION<commit_after>/*
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */

#pragma once

/**
 * UAVCAN version definition
 */
#define UAVCAN_VERSION_MAJOR    0
#define UAVCAN_VERSION_MINOR    1

/**
 * UAVCAN_CPP_VERSION - version of the C++ standard used during compilation.
 * This definition contains the integer year number after which the standard was named:
 *  - 2003 for C++03
 *  - 2011 for C++11
 *
 * This config automatically sets according to the actual C++ standard used by the compiler.
 *
 * In C++03 mode the library has almost zero dependency on the C++ standard library, which allows
 * to use it on platforms with a very limited C++ support. On the other hand, C++11 mode requires
 * many parts of the standard library (e.g. <functional>), thus the user might want to force older
 * standard than used by the compiler, in which case this symbol can be overridden manually via
 * compiler flags.
 */
#define UAVCAN_CPP11    2011
#define UAVCAN_CPP03    2003

#ifndef UAVCAN_CPP_VERSION
# if __cplusplus > 201200
#  error Unsupported C++ standard
# elif (__cplusplus > 201100) || defined(__GXX_EXPERIMENTAL_CXX0X__)
#  define UAVCAN_CPP_VERSION    UAVCAN_CPP11
# else
#  define UAVCAN_CPP_VERSION    UAVCAN_CPP03
# endif
#endif

/**
 * UAVCAN can be explicitly told to ignore exceptions, or it can be detected automatically.
 * Autodetection is not expected to work with all compilers, so it's safer to define it explicitly.
 * If the autodetection fails, exceptions will be disabled by default.
 */
#ifndef UAVCAN_EXCEPTIONS
# if __EXCEPTIONS || _HAS_EXCEPTIONS
#  define UAVCAN_EXCEPTIONS  1
# else
#  define UAVCAN_EXCEPTIONS  0
# endif
#endif

/**
 * Struct layout control.
 * Set UAVCAN_PACK_STRUCTS=1 and define UAVCAN_PACKED_BEGIN and UAVCAN_PACKED_END to reduce memory usage.
 * THIS MAY BREAK THE CODE.
 */
#ifndef UAVCAN_PACK_STRUCTS
# define UAVCAN_PACK_STRUCTS    0
#endif
#ifndef UAVCAN_PACKED_BEGIN
# define UAVCAN_PACKED_BEGIN
#endif
#ifndef UAVCAN_PACKED_END
# define UAVCAN_PACKED_END
#endif

/**
 * Declaration visibility
 * http://gcc.gnu.org/wiki/Visibility
 */
#ifndef UAVCAN_EXPORT
# define UAVCAN_EXPORT
#endif

/**
 * Trade-off between ROM/RAM usage and functionality/determinism.
 * Note that this feature is not well tested and should be avoided.
 * Use code search for UAVCAN_TINY to find what functionality will be disabled.
 * This is particularly useful for embedded systems with less than 40kB of ROM.
 */
#ifndef UAVCAN_TINY
# define UAVCAN_TINY 0
#endif

/**
 * It might make sense to remove toString() methods for an embedded system.
 * If the autodetect fails, toString() will be disabled, so it's pretty safe by default.
 */
#ifndef UAVCAN_TOSTRING
// Objective is to make sure that we're NOT on a resource constrained platform
# if __linux__ || __linux || __APPLE__ || _WIN64 || _WIN32
#  define UAVCAN_TOSTRING 1
# else
#  define UAVCAN_TOSTRING 0
# endif
#endif

#if UAVCAN_TOSTRING
# include <string>
# include <cstdio>
#endif

/**
 * Some C++ implementations are half-broken because they don't implement the placement new operator.
 * If UAVCAN_IMPLEMENT_PLACEMENT_NEW is defined, libuavcan will implement its own operator new (std::size_t, void*)
 * and its delete() counterpart, instead of relying on the standard header <new>.
 */
#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW
# define UAVCAN_IMPLEMENT_PLACEMENT_NEW 0
#endif

/**
 * Run time checks.
 * Resolves to the standard assert() by default.
 * Disabled completely if UAVCAN_NO_ASSERTIONS is defined.
 */
#ifndef UAVCAN_ASSERT
# if UAVCAN_NO_ASSERTIONS
#  define UAVCAN_ASSERT(x)
# else
#  define UAVCAN_ASSERT(x) assert(x)
# endif
#endif

namespace uavcan
{

/**
 * Memory pool block size.
 *
 * The default of 64 bytes should be OK for any target arch up to AMD64 and any compiler.
 *
 * The library leverages compile-time checks to ensure that all types that are subject to dynamic allocation
 * fit this size, otherwise compilation fails.
 *
 * For platforms featuring small pointer width (16..32 bits), UAVCAN_MEM_POOL_BLOCK_SIZE can often be safely
 * reduced to 56 or even 48 bytes, which leads to lower memory footprint.
 *
 * Note that the pool block size shall be aligned at biggest alignment of the target platform (detected and
 * checked automatically at compile time).
 */
#if UAVCAN_MEM_POOL_BLOCK_SIZE
/// Explicitly specified by the user.
static const unsigned MemPoolBlockSize = UAVCAN_MEM_POOL_BLOCK_SIZE;
#elif defined(__BIGGEST_ALIGNMENT__) && (__BIGGEST_ALIGNMENT__ <= 8)
/// Convenient default for GCC-like compilers - if alignment allows, pool block size can be safely reduced.
static const unsigned MemPoolBlockSize = 56;
#else
/// Safe default that should be OK for any platform.
static const unsigned MemPoolBlockSize = 64;
#endif

#ifdef __BIGGEST_ALIGNMENT__
static const unsigned MemPoolAlignment = __BIGGEST_ALIGNMENT__;
#else
static const unsigned MemPoolAlignment = 16;
#endif

typedef char _alignment_check_for_MEM_POOL_BLOCK_SIZE[((MemPoolBlockSize & (MemPoolAlignment - 1)) == 0) ? 1 : -1];

/**
 * This class that allows to check at compile time whether type T can be allocated using the memory pool.
 * If the check fails, compilation fails.
 */
template <typename T>
struct UAVCAN_EXPORT IsDynamicallyAllocatable
{
    static void check()
    {
        char dummy[(sizeof(T) <= MemPoolBlockSize) ? 1 : -1];
        (void)dummy;
    }
};

}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: pointer.hxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: jb $ $Date: 2002-03-28 08:47:03 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef CONFIGMGR_POINTER_HXX
#define CONFIGMGR_POINTER_HXX

#ifndef _SAL_TYPES_H_
#include <sal/types.h>
#endif

#ifndef INCLUDED_SHARABLE_BASETYPES_HXX
#include "types.hxx"
#endif

namespace configmgr
{
// -----------------------------------------------------------------------------
    namespace memory
    {
    // -------------------------------------------------------------------------
        class Accessor;
        class UpdateAccessor;
    // -------------------------------------------------------------------------
        /// class mediating read-only access to a memory::Segment
        class Pointer
        {
            friend class Accessor;
            friend class UpdateAccessor;

            typedef sharable::Address AddressType;

            AddressType m_value;

            struct Opaque_;
        public:
            typedef AddressType RawAddress;

            Pointer() : m_value(NULL) {}
            explicit Pointer(AddressType p) : m_value(p) {}

            RawAddress value() const { return m_value; }

            bool isNull()   const { return m_value == NULL; }
            bool is()       const { return m_value != NULL; }

            operator Opaque_ const * () const { return reinterpret_cast<Opaque_ const *>(m_value); }

            friend bool operator == (Pointer lhs, Pointer rhs)
            { return lhs.value() == rhs.value(); }

            friend bool operator != (Pointer lhs, Pointer rhs)
            { return lhs.value() != rhs.value(); }
        };

    // -------------------------------------------------------------------------
    }
// -----------------------------------------------------------------------------
} // namespace configmgr
// -----------------------------------------------------------------------------

#endif // CONFIGMGR_POINTER_HXX

<commit_msg>INTEGRATION: CWS ooo20031110 (1.2.86); FILE MERGED 2003/11/09 09:12:53 waratah 1.2.86.1: #i1858# switch from NULL to literal 0, gives warning in GCC as it is not a pointer variable<commit_after>/*************************************************************************
 *
 *  $RCSfile: pointer.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2003-12-01 16:05:48 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef CONFIGMGR_POINTER_HXX
#define CONFIGMGR_POINTER_HXX

#ifndef _SAL_TYPES_H_
#include <sal/types.h>
#endif

#ifndef INCLUDED_SHARABLE_BASETYPES_HXX
#include "types.hxx"
#endif

namespace configmgr
{
// -----------------------------------------------------------------------------
    namespace memory
    {
    // -------------------------------------------------------------------------
        class Accessor;
        class UpdateAccessor;
    // -------------------------------------------------------------------------
        /// class mediating read-only access to a memory::Segment
        class Pointer
        {
            friend class Accessor;
            friend class UpdateAccessor;

            typedef sharable::Address AddressType;

            AddressType m_value;

            struct Opaque_;
        public:
            typedef AddressType RawAddress;

            Pointer() : m_value(0) {}
            explicit Pointer(AddressType p) : m_value(p) {}

            RawAddress value() const { return m_value; }

            bool isNull()   const { return m_value == 0; }
            bool is()       const { return m_value != 0; }

            operator Opaque_ const * () const { return reinterpret_cast<Opaque_ const *>(m_value); }

            friend bool operator == (Pointer lhs, Pointer rhs)
            { return lhs.value() == rhs.value(); }

            friend bool operator != (Pointer lhs, Pointer rhs)
            { return lhs.value() != rhs.value(); }
        };

    // -------------------------------------------------------------------------
    }
// -----------------------------------------------------------------------------
} // namespace configmgr
// -----------------------------------------------------------------------------

#endif // CONFIGMGR_POINTER_HXX

<|endoftext|>