<commit_before>/** @copyright
 * Copyright (c) 2017 Stuart W Baker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file MCP2515Can.cxx
 * This file implements the CAN driver for the MCP2515 CAN Controller.
 *
 * @author Stuart W. Baker
 * @date 3 January 2017
 */

#include "MCP2515Can.hxx"

#include <fcntl.h>

const MCP2515Can::MCP2515Baud MCP2515Can::baudTable[] =
{
    /* 20 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 4) / 20 MHz = 400 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 20 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 8 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 8 TQ) / 20 TQ = 80%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 400 nsec = 1.2 usec
     */
    {20000000, 125000, {(4 - 1), (2 - 1)},
                       {(7 - 1), (8 - 1), 0, 1},
                       {(4 - 1), 0, 0}},
    /* 8 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 2) / 8 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 4 TQ
     * PS1 = 8 TQ
     * PS2 = 3 TQ
     * sample time = (1 TQ + 4 TQ + 8 TQ) / 16 TQ = 81.25%
     * SJW = PS2 - 1 = 3 - 1 = 2
     * SJW = 2 * 500 nsec = 1 usec
     */
    {8000000, 125000, {(2 - 1), (2 - 1)},
                      {(4 - 1), (8 - 1), 0, 1},
                      {(3 - 1), 0, 0}}
};

/*
 * init()
 */
void MCP2515Can::init(const char *spi_name, uint32_t freq, uint32_t baud)
{
    spi = ::open(spi_name, O_RDWR);
    HASSERT(spi >= 0);

    /* reset device */
    reset();

    /* wait until device is in configuration mode */
    while ((register_read(CANSTAT) & 0xE0) != 0x80);

    /* find valid timing settings for the requested frequency and buad rates */
    for (size_t i = 0; i < (sizeof(baudTable) / sizeof(baudTable[0])); ++i)
    {
        if (baudTable[i].freq == freq && baudTable[i].baud == baud)
        {
            register_write(CNF1, baudTable[i].cnf1.data);
            register_write(CNF2, baudTable[i].cnf2.data);
            register_write(CNF3, baudTable[i].cnf3.data);

            /* setup RX Buf 0 and 1 to receive any message */
            register_write(RXB0CTRL, 0x60);
            register_write(RXB1CTRL, 0x60);

            /* enable error and receive interrupts */
            register_write(CANINTE, MERR | ERRI | RX1I | RX0I);

            /* put the device into normal operation mode */
            register_write(CANCTRL, 0x00);

            /* wait until device is in normal mode */
            while ((register_read(CANSTAT) & 0xE0) != 0x00);

            return;
        }
    }

    /* unsupported frequency */
    HASSERT(0);
}

/*
 * enable()
 */
void MCP2515Can::enable()
{
    /* there is a mutex lock above us, so the following sequence is atomic */
    if (!is_created())
    {
        /* start the thread */
        /** @todo make this the highest possible thread priority */
        start(name, 0, 1024);
    }

    /* reset device */
    //reset();

    interrupt_enable();
}

/*
 * disable()
 */
void MCP2515Can::disable()
{
    interrupt_disable();

    register_write(TXB0CTRL, 0x00);
    register_write(TXB1CTRL, 0x00);
    portENTER_CRITICAL();
    /* flush out any transmit data in the pipleline */
    txBuf->flush();
    txPending = 0;
    portEXIT_CRITICAL();
}

/*
 * tx_msg_locked()
 */
void MCP2515Can::tx_msg_locked()
{
    /* the node lock_ will be locked by the caller */
    if (txPending < 3)
    {
        struct can_frame *can_frame;

        /* find an empty buffer */
        int index = 0;
        if (txPending & 0x1)
        {
            /* buffer 0 already in use */
            index = 1;
        }

        portENTER_CRITICAL();
        if (txBuf->data_read_pointer(&can_frame))
        {
            Buffer tx_buf(can_frame);
            txBuf->consume(1);
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending |= (0x1 << index);

            /* bump up priority of the other buffer so it will transmit first
             * if it is pending
             */
            bit_modify(index == 0 ? TXB1CTRL : TXB0CTRL, 0x01, 0x03);
            /* load the tranmsit buffer */
            buffer_write(index, tx_buf.get_payload());
            /* request to send at lowest priority */
            bit_modify(index == 0 ? TXB0CTRL : TXB1CTRL, 0x08, 0x0B);
            /* enable transmit interrupt */
            bit_modify(CANINTE, TX0I << index, TX0I << index);
        }
        else
        {
            portEXIT_CRITICAL();
        }
    }
}

/*
 * rx_msg()
 */
void MCP2515Can::rx_msg(int index)
{
    Buffer rx_buf;
    buffer_read(index, rx_buf.get_payload());
    struct can_frame *can_frame;

    portENTER_CRITICAL();
    if (rxBuf->data_write_pointer(&can_frame))
    {
        rx_buf.build_struct_can_frame(can_frame);
        rxBuf->advance(1);
        ++numReceivedPackets_;
        rxBuf->signal_condition();
    }
    else
    {
        /* receive overrun occured */
        ++overrunCount;
    }
    portEXIT_CRITICAL();
}

/*
 * entry()
 */
void *MCP2515Can::entry()
{
    for ( ; /* forever */ ; )
    {
#if MCP2515_DEBUG
        int result = sem.timedwait(SEC_TO_NSEC(1));
        lock_.lock();

        if (result != 0)
        {
            spi_ioc_transfer xfer[2];
            memset(xfer, 0, sizeof(xfer));
            uint8_t wr_data[2] = {READ, 0};
            xfer[0].tx_buf = (unsigned long)wr_data;
            xfer[0].len = sizeof(wr_data);
            xfer[1].rx_buf = (unsigned long)regs;
            xfer[1].len = sizeof(regs);
            xfer[1].cs_change = 1;
            ::ioctl(spi, SPI_IOC_MESSAGE(2), xfer);
            lock_.unlock();
            continue;
        }
#else
        sem.wait();
        lock_.lock();
#endif
        /* read status flags */
        uint8_t canintf = register_read(CANINTF);

        if (canintf & MERR)
        {
            /* message error interrupt active */
        }
        if (canintf & ERRI)
        {
            /* error interrupt active */
            register_write(TXB0CTRL, 0x00);
            register_write(TXB1CTRL, 0x00);

            portENTER_CRITICAL();
            ++softErrorCount;
            /* flush out any transmit data in the pipleline */
            txBuf->flush();
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending = 0;
        }
        if (canintf & RX0I)
        {
            /* receive interrupt active */
            rx_msg(0);
        }

        if (txPending)
        {
            /* transmit interrupt active and transmission complete */
            if (canintf & TX0I)
            {
                txPending &= ~0x1;
                bit_modify(CANINTE, 0, TX0I);
                bit_modify(CANINTF, 0, TX0I);
                ++numTransmittedPackets_;
            }
            if (canintf & TX1I)
            {
                txPending &= ~0x2;
                bit_modify(CANINTE, 0, TX1I);
                bit_modify(CANINTF, 0, TX1I);
                ++numTransmittedPackets_;
            }

            tx_msg_locked();
        }

        /* Refresh status flags just in case RX1 buffer became active
         * before we could finish reading out RX0 buffer.  This ussually
         * won't happen because we should be able to respond to incoming
         * messages fast enough to only use RX0 buffer.
         */
        canintf = register_read(CANINTF);
        if (canintf & RX1I)
        {
            /* receive interrupt active */
            rx_msg(1);
        }
        lock_.unlock();

        interrupt_enable();
    }

    return NULL;
}

/*
 * interrupt_handler()
 */
void MCP2515Can::interrupt_handler()
{
    int woken = false;
    interrupt_disable();
    sem.post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}
<commit_msg>Update MCP2515 driver to configure the SPI bus settings.<commit_after>/** @copyright
 * Copyright (c) 2017 Stuart W Baker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file MCP2515Can.cxx
 * This file implements the CAN driver for the MCP2515 CAN Controller.
 *
 * @author Stuart W. Baker
 * @date 3 January 2017
 */

#include "MCP2515Can.hxx"

#include <fcntl.h>

const MCP2515Can::MCP2515Baud MCP2515Can::baudTable[] =
{
    /* 20 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 4) / 20 MHz = 400 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 20 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 7 TQ
     * PS1 = 8 TQ
     * PS2 = 4 TQ
     * sample time = (1 TQ + 7 TQ + 8 TQ) / 20 TQ = 80%
     * SJW = PS2 - 1 = 4 - 1 = 3
     * SJW = 3 * 400 nsec = 1.2 usec
     */
    {20000000, 125000, {(4 - 1), (2 - 1)},
                       {(7 - 1), (8 - 1), 0, 1},
                       {(4 - 1), 0, 0}},
    /* 8 MHz clock source
     * TQ = (2 * BRP) / freq = (2 * 2) / 8 MHz = 500 nsec
     * Baud = 125 kHz
     * bit time = 1 / 125 kHz = 8 usec = 16 TQ
     * SyncSeg = 1 TQ
     * PropSeg = 4 TQ
     * PS1 = 8 TQ
     * PS2 = 3 TQ
     * sample time = (1 TQ + 4 TQ + 8 TQ) / 16 TQ = 81.25%
     * SJW = PS2 - 1 = 3 - 1 = 2
     * SJW = 2 * 500 nsec = 1 usec
     */
    {8000000, 125000, {(2 - 1), (2 - 1)},
                      {(4 - 1), (8 - 1), 0, 1},
                      {(3 - 1), 0, 0}}
};

/*
 * init()
 */
void MCP2515Can::init(const char *spi_name, uint32_t freq, uint32_t baud)
{
    spi = ::open(spi_name, O_RDWR);
    HASSERT(spi >= 0);

    /* configure SPI bus settings */
    uint8_t spi_mode = SPI_MODE_0;
    uint8_t spi_bpw = 8;
    uint32_t spi_max_speed_hz = freq / 4;
    ::ioctl(spi, SPI_IOC_WR_MODE, &spi_mode);
    ::ioctl(spi, SPI_IOC_WR_BITS_PER_WORD, &spi_bpw);
    ::ioctl(spi, SPI_IOC_WR_MAX_SPEED_HZ, &spi_max_speed_hz);

    /* reset device */
    reset();

    /* wait until device is in configuration mode */
    while ((register_read(CANSTAT) & 0xE0) != 0x80);

    /* find valid timing settings for the requested frequency and buad rates */
    for (size_t i = 0; i < (sizeof(baudTable) / sizeof(baudTable[0])); ++i)
    {
        if (baudTable[i].freq == freq && baudTable[i].baud == baud)
        {
            register_write(CNF1, baudTable[i].cnf1.data);
            register_write(CNF2, baudTable[i].cnf2.data);
            register_write(CNF3, baudTable[i].cnf3.data);

            /* setup RX Buf 0 and 1 to receive any message */
            register_write(RXB0CTRL, 0x60);
            register_write(RXB1CTRL, 0x60);

            /* enable error and receive interrupts */
            register_write(CANINTE, MERR | ERRI | RX1I | RX0I);

            /* put the device into normal operation mode */
            register_write(CANCTRL, 0x00);

            /* wait until device is in normal mode */
            while ((register_read(CANSTAT) & 0xE0) != 0x00);

            return;
        }
    }

    /* unsupported frequency */
    HASSERT(0);
}

/*
 * enable()
 */
void MCP2515Can::enable()
{
    /* there is a mutex lock above us, so the following sequence is atomic */
    if (!is_created())
    {
        /* start the thread */
        /** @todo make this the highest possible thread priority */
        start(name, 0, 1024);
    }

    /* reset device */
    //reset();

    interrupt_enable();
}

/*
 * disable()
 */
void MCP2515Can::disable()
{
    interrupt_disable();

    register_write(TXB0CTRL, 0x00);
    register_write(TXB1CTRL, 0x00);
    portENTER_CRITICAL();
    /* flush out any transmit data in the pipleline */
    txBuf->flush();
    txPending = 0;
    portEXIT_CRITICAL();
}

/*
 * tx_msg_locked()
 */
void MCP2515Can::tx_msg_locked()
{
    /* the node lock_ will be locked by the caller */
    if (txPending < 3)
    {
        struct can_frame *can_frame;

        /* find an empty buffer */
        int index = 0;
        if (txPending & 0x1)
        {
            /* buffer 0 already in use */
            index = 1;
        }

        portENTER_CRITICAL();
        if (txBuf->data_read_pointer(&can_frame))
        {
            Buffer tx_buf(can_frame);
            txBuf->consume(1);
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending |= (0x1 << index);

            /* bump up priority of the other buffer so it will transmit first
             * if it is pending
             */
            bit_modify(index == 0 ? TXB1CTRL : TXB0CTRL, 0x01, 0x03);
            /* load the tranmsit buffer */
            buffer_write(index, tx_buf.get_payload());
            /* request to send at lowest priority */
            bit_modify(index == 0 ? TXB0CTRL : TXB1CTRL, 0x08, 0x0B);
            /* enable transmit interrupt */
            bit_modify(CANINTE, TX0I << index, TX0I << index);
        }
        else
        {
            portEXIT_CRITICAL();
        }
    }
}

/*
 * rx_msg()
 */
void MCP2515Can::rx_msg(int index)
{
    Buffer rx_buf;
    buffer_read(index, rx_buf.get_payload());
    struct can_frame *can_frame;

    portENTER_CRITICAL();
    if (rxBuf->data_write_pointer(&can_frame))
    {
        rx_buf.build_struct_can_frame(can_frame);
        rxBuf->advance(1);
        ++numReceivedPackets_;
        rxBuf->signal_condition();
    }
    else
    {
        /* receive overrun occured */
        ++overrunCount;
    }
    portEXIT_CRITICAL();
}

/*
 * entry()
 */
void *MCP2515Can::entry()
{
    for ( ; /* forever */ ; )
    {
#if MCP2515_DEBUG
        int result = sem.timedwait(SEC_TO_NSEC(1));
        lock_.lock();

        if (result != 0)
        {
            spi_ioc_transfer xfer[2];
            memset(xfer, 0, sizeof(xfer));
            uint8_t wr_data[2] = {READ, 0};
            xfer[0].tx_buf = (unsigned long)wr_data;
            xfer[0].len = sizeof(wr_data);
            xfer[1].rx_buf = (unsigned long)regs;
            xfer[1].len = sizeof(regs);
            xfer[1].cs_change = 1;
            ::ioctl(spi, SPI_IOC_MESSAGE(2), xfer);
            lock_.unlock();
            continue;
        }
#else
        sem.wait();
        lock_.lock();
#endif
        /* read status flags */
        uint8_t canintf = register_read(CANINTF);

        if (canintf & MERR)
        {
            /* message error interrupt active */
        }
        if (canintf & ERRI)
        {
            /* error interrupt active */
            register_write(TXB0CTRL, 0x00);
            register_write(TXB1CTRL, 0x00);

            portENTER_CRITICAL();
            ++softErrorCount;
            /* flush out any transmit data in the pipleline */
            txBuf->flush();
            txBuf->signal_condition();
            portEXIT_CRITICAL();

            txPending = 0;
        }
        if (canintf & RX0I)
        {
            /* receive interrupt active */
            rx_msg(0);
        }

        if (txPending)
        {
            /* transmit interrupt active and transmission complete */
            if (canintf & TX0I)
            {
                txPending &= ~0x1;
                bit_modify(CANINTE, 0, TX0I);
                bit_modify(CANINTF, 0, TX0I);
                ++numTransmittedPackets_;
            }
            if (canintf & TX1I)
            {
                txPending &= ~0x2;
                bit_modify(CANINTE, 0, TX1I);
                bit_modify(CANINTF, 0, TX1I);
                ++numTransmittedPackets_;
            }

            tx_msg_locked();
        }

        /* Refresh status flags just in case RX1 buffer became active
         * before we could finish reading out RX0 buffer.  This ussually
         * won't happen because we should be able to respond to incoming
         * messages fast enough to only use RX0 buffer.
         */
        canintf = register_read(CANINTF);
        if (canintf & RX1I)
        {
            /* receive interrupt active */
            rx_msg(1);
        }
        lock_.unlock();

        interrupt_enable();
    }

    return NULL;
}

/*
 * interrupt_handler()
 */
void MCP2515Can::interrupt_handler()
{
    int woken = false;
    interrupt_disable();
    sem.post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}
<|endoftext|>


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

#include "common/shared_types.h"
#include "types/typeops.h"

#include "functions/function.h"
#include "functions/function_impl.h"

#include "functions/func_parse_fragment.h"

#include "system/globalenv.h"

#include "compiler/expression/expr_consts.h"

namespace zorba
{

/*******************************************************************************

********************************************************************************/
void populate_context_parse_fragment_impl(static_context* sctx)
{
  xqtref_t lParseOptType =
    GENV_TYPESYSTEM.create_node_type(
        store::StoreConsts::elementNode,
        createQName("http://www.zorba-xquery.com/modules/xml-options","","options"),
        NULL,
        TypeConstants::QUANT_ONE,
        false,
        false
      );

  {
    DECL_WITH_KIND(sctx, fn_zorba_xml_parse,
        (createQName("http://www.zorba-xquery.com/modules/xml","","parse"),
        GENV_TYPESYSTEM.STRING_TYPE_QUESTION,
        lParseOptType,
        GENV_TYPESYSTEM.ANY_NODE_TYPE_STAR),
        FunctionConsts::FN_ZORBA_XML_PARSE_2);
  }
}

}

/* vim:set et sw=2 ts=2: */
<commit_msg>Fixed the quantity of the options parameter from ONE to QUESTION.<commit_after>/*
 * Copyright 2006-2008 The FLWOR Foundation.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "stdafx.h"

#include "common/shared_types.h"
#include "types/typeops.h"

#include "functions/function.h"
#include "functions/function_impl.h"

#include "functions/func_parse_fragment.h"

#include "system/globalenv.h"

#include "compiler/expression/expr_consts.h"

namespace zorba
{

/*******************************************************************************

********************************************************************************/
void populate_context_parse_fragment_impl(static_context* sctx)
{
  xqtref_t lParseOptType =
    GENV_TYPESYSTEM.create_node_type(
        store::StoreConsts::elementNode,
        createQName("http://www.zorba-xquery.com/modules/xml-options","","options"),
        NULL,
        TypeConstants::QUANT_QUESTION,
        false,
        false
      );

  {
    DECL_WITH_KIND(sctx, fn_zorba_xml_parse,
        (createQName("http://www.zorba-xquery.com/modules/xml","","parse"),
        GENV_TYPESYSTEM.STRING_TYPE_QUESTION,
        lParseOptType,
        GENV_TYPESYSTEM.ANY_NODE_TYPE_STAR),
        FunctionConsts::FN_ZORBA_XML_PARSE_2);
  }
}

}

/* vim:set et sw=2 ts=2: */
<|endoftext|>


<commit_before>// Copyright 2017 Massachusetts Institute of Technology

#include <csignal>
#include <memory>
#include <utility>
#include <algorithm>

#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/OccupancyGrid.h>

#include <fla_utils/param_utils.h>

#include "global_mapper_ros/global_mapper_ros.h"
#include "global_mapper/global_mapper.h"
#include "global_mapper/params.h"

namespace global_mapper {

GlobalMapperRos::GlobalMapperRos(volatile std::sig_atomic_t* stop_signal_ptr)
  : stop_signal_ptr_(stop_signal_ptr),
    publish_voxel_map_(false),
    nh_(),
    pnh_("~"),
    tf_listener_(tf_buffer_) {
  tf_buffer_.setUsingDedicatedThread(true);

  GetParams();
  InitSubscribers();
  InitPublishers();
}

void GlobalMapperRos::GetParams() {
  fla_utils::SafeGetParam(pnh_, "map_frame", params_.map_frame);

  // pixel map params
  std::vector<double> pixel_xy_min(2, 0.0);
  fla_utils::SafeGetParam(pnh_, "pixel_map/xy_min", pixel_xy_min);
  std::copy(pixel_xy_min.begin(), pixel_xy_min.end(), params_.pixel_xy_min);

  std::vector<double> pixel_xy_max(2, 0.0);
  fla_utils::SafeGetParam(pnh_, "pixel_map/xy_max", pixel_xy_max);
  std::copy(pixel_xy_max.begin(), pixel_xy_max.end(), params_.pixel_xy_max);

  fla_utils::SafeGetParam(pnh_, "pixel_map/resolution", params_.pixel_resolution);
  fla_utils::SafeGetParam(pnh_, "pixel_map/init_value", params_.pixel_init_value);
  fla_utils::SafeGetParam(pnh_, "pixel_map/bound_min", params_.pixel_bound_min);
  fla_utils::SafeGetParam(pnh_, "pixel_map/bound_max", params_.pixel_bound_max);
  fla_utils::SafeGetParam(pnh_, "pixel_map/min_z_abs", params_.pixel_min_z_abs);
  fla_utils::SafeGetParam(pnh_, "pixel_map/max_z_abs", params_.pixel_max_z_abs);
  fla_utils::SafeGetParam(pnh_, "pixel_map/use_rel_flatten", params_.pixel_use_rel_flatten);
  fla_utils::SafeGetParam(pnh_, "pixel_map/min_z_rel", params_.pixel_min_z_rel);
  fla_utils::SafeGetParam(pnh_, "pixel_map/max_z_rel", params_.pixel_max_z_rel);
  fla_utils::SafeGetParam(pnh_, "pixel_map/publish_map", publish_pixel_map_);

  // voxel map params
  std::vector<double> voxel_xyz_min(3, 0.0);
  fla_utils::SafeGetParam(pnh_, "voxel_map/xyz_min", voxel_xyz_min);
  std::copy(voxel_xyz_min.begin(), voxel_xyz_min.end(), params_.voxel_xyz_min);

  std::vector<double> voxel_xyz_max(3, 0.0);
  fla_utils::SafeGetParam(pnh_, "voxel_map/xyz_max", voxel_xyz_max);
  std::copy(voxel_xyz_max.begin(), voxel_xyz_max.end(), params_.voxel_xyz_max);

  fla_utils::SafeGetParam(pnh_, "voxel_map/resolution", params_.voxel_resolution);

  fla_utils::SafeGetParam(pnh_, "voxel_map/init_value", params_.voxel_init_value);
  fla_utils::SafeGetParam(pnh_, "voxel_map/bound_min", params_.voxel_bound_min);
  fla_utils::SafeGetParam(pnh_, "voxel_map/bound_max", params_.voxel_bound_max);
  fla_utils::SafeGetParam(pnh_, "voxel_map/min_z_abs", params_.voxel_min_z_abs);
  fla_utils::SafeGetParam(pnh_, "voxel_map/max_z_abs", params_.voxel_max_z_abs);
  fla_utils::SafeGetParam(pnh_, "voxel_map/use_rel_cropping", params_.voxel_use_rel_cropping);
  fla_utils::SafeGetParam(pnh_, "voxel_map/min_z_rel", params_.voxel_min_z_rel);
  fla_utils::SafeGetParam(pnh_, "voxel_map/max_z_rel", params_.voxel_max_z_rel);
  fla_utils::SafeGetParam(pnh_, "voxel_map/hit_inc", params_.voxel_hit_inc);
  fla_utils::SafeGetParam(pnh_, "voxel_map/miss_inc", params_.voxel_miss_inc);
  fla_utils::SafeGetParam(pnh_, "voxel_map/publish_map", publish_voxel_map_);
}

void GlobalMapperRos::InitSubscribers() {
  point_cloud_sub_ = pnh_.subscribe("cloud_topic", 10, &GlobalMapperRos::PointCloudCallback, this);
}

void GlobalMapperRos::InitPublishers() {
  pixel_map_pub_ = pnh_.advertise<nav_msgs::OccupancyGrid>("pixel_map_topic", 1);
  voxel_map_pub_ = pnh_.advertise<visualization_msgs::MarkerArray>("voxel_map_topic", 1);
  map_pub_timer_ = nh_.createTimer(ros::Duration(1.0), &GlobalMapperRos::PublishMap, this);
}

/*
  Return a RGB colour value given a scalar v in the range [vmin,vmax]
  In this case each colour component ranges from 0 (no contribution) to
  1 (fully saturated). The colour is clipped at the end of the scales if v is outside
  the range [vmin,vmax]
*/
void GlobalMapperRos::GrayscaleToRGBJet(double v, double vmin, double vmax, std::vector<double>* rgb) {
  double dv;

  if (v < vmin)
    v = vmin;
  if (v > vmax)
    v = vmax;
  dv = vmax - vmin;

  if (v < (vmin + 0.25 * dv)) {
    rgb->at(0) = 0;
    rgb->at(1) = 4 * (v - vmin) / dv;
  } else if (v < (vmin + 0.5 * dv)) {
    rgb->at(0) = 0;
    rgb->at(2) = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
  } else if (v < (vmin + 0.75 * dv)) {
    rgb->at(0) = 4 * (v - vmin - 0.5 * dv) / dv;
    rgb->at(2) = 0;
  } else {
    rgb->at(1) = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
    rgb->at(2) = 0;
  }
}

void GlobalMapperRos::PopulateVoxelMapMsg(visualization_msgs::MarkerArray* marker_array) {
  // check for bad input
  if (marker_array == nullptr) {
    return;
  }

  geometry_msgs::TransformStamped transform_stamped;
  Eigen::Vector3d transform;
  try {
    transform_stamped = tf_buffer_.lookupTransform("world", "body",
                                                   ros::Time(0), ros::Duration(1.0));

    transform(0) = transform_stamped.transform.translation.x;
    transform(1) = transform_stamped.transform.translation.y;
    transform(2) = transform_stamped.transform.translation.z;
  } catch (tf2::TransformException &ex) {
    ROS_WARN("[world_database_master_ros] OnGetTransform failed with %s", ex.what());

    transform(0) = std::numeric_limits<double>::quiet_NaN();
    transform(1) = std::numeric_limits<double>::quiet_NaN();
    transform(2) = std::numeric_limits<double>::quiet_NaN();
  }

  // get occuppied voxels
  double xyz[3] = {0.0};
  Eigen::Map<Eigen::Vector3d> xyz_map(&xyz[0]);
  std::vector<int> occ_inds;
  for (int i = 0; i < global_mapper_ptr_->voxel_map_ptr_->num_cells; ++i) {
    global_mapper_ptr_->voxel_map_ptr_->indToLoc(i, xyz);

    if (global_mapper_ptr_->voxel_map_ptr_->readValue(xyz) > 0.6) {
      if((xyz_map - transform).norm() < 20.0) {
        occ_inds.push_back(i);
      }
    }
  }

  marker_array->markers.resize(occ_inds.size());
  std::vector<double> rgb(3, 1.0);
  for (int i = 0; i < occ_inds.size(); ++i) {
    visualization_msgs::Marker& marker = marker_array->markers[i];

    // create voxel marker
    marker.header.frame_id = params_.map_frame;
    marker.header.stamp = ros::Time::now();
    marker.id = i;
    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = visualization_msgs::Marker::ADD;

    // orientation
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // scale
    marker.scale.x = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[0];
    marker.scale.y = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[1];
    marker.scale.z = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[2];

    // position
    global_mapper_ptr_->voxel_map_ptr_->indToLoc(occ_inds[i], xyz);
    marker.pose.position.x = xyz[0];
    marker.pose.position.y = xyz[1];
    marker.pose.position.z = xyz[2];

    // color
    GrayscaleToRGBJet(marker.pose.position.z,
                      global_mapper_ptr_->voxel_map_ptr_->xyz0[2],
                      global_mapper_ptr_->voxel_map_ptr_->xyz1[2],
                      &rgb);
    marker.color.r = static_cast<float>(rgb[0]);
    marker.color.g = static_cast<float>(rgb[1]);
    marker.color.b = static_cast<float>(rgb[2]);
    marker.color.a = 1.0f;
  }
}

void GlobalMapperRos::PopulatePixelMapMsg(nav_msgs::OccupancyGrid* occupancy_grid) {
  // check for bad input
  if (occupancy_grid == nullptr) {
    return;
  }

  // header
  occupancy_grid->header.frame_id = params_.map_frame;
  occupancy_grid->header.stamp = ros::Time::now();

  // metadata
  occupancy_grid->info.resolution = global_mapper_ptr_->pixel_map_ptr_->metersPerPixel;
  occupancy_grid->info.width = global_mapper_ptr_->pixel_map_ptr_->dimensions[0];
  occupancy_grid->info.height = global_mapper_ptr_->pixel_map_ptr_->dimensions[1];
  occupancy_grid->info.origin.position.x = 0.0;
  occupancy_grid->info.origin.position.y = 0.0;
  occupancy_grid->info.origin.position.z = 0.0;

  // data
  float occ_value = 0.0;
  double xy[2] = {0.0};
  occupancy_grid->data.resize(global_mapper_ptr_->pixel_map_ptr_->num_cells);
  for (int i = 0; i < global_mapper_ptr_->pixel_map_ptr_->num_cells; ++i) {
    // get xy location
    global_mapper_ptr_->pixel_map_ptr_->indToLoc(i, xy);

    // get pixel value and scale
    occ_value = global_mapper_ptr_->pixel_map_ptr_->readValue(xy);
    occ_value *= 100.0;
    occupancy_grid->data[i] = static_cast<uint8_t>(occ_value + 0.5);
  }
}

void GlobalMapperRos::PublishMap(const ros::TimerEvent& event) {
  // lock
  std::lock_guard<std::mutex> map_lock(global_mapper_ptr_->map_mutex());

  // voxel map
  if (publish_voxel_map_) {
    visualization_msgs::MarkerArray marker_array;
    PopulateVoxelMapMsg(&marker_array);
    voxel_map_pub_.publish(marker_array);
  }

  // pixel_map
  if (publish_pixel_map_) {
    nav_msgs::OccupancyGrid occupancy_grid;
    PopulatePixelMapMsg(&occupancy_grid);
    pixel_map_pub_.publish(occupancy_grid);
  }
}

void GlobalMapperRos::PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_ptr) {
  // get transform
  const std::string target_frame = params_.map_frame;
  geometry_msgs::TransformStamped transform_stamped;
  // try to get correct transform
  try {
    transform_stamped = tf_buffer_.lookupTransform(target_frame, cloud_ptr->header.frame_id,
                                                   ros::Time(cloud_ptr->header.stamp),
                                                   ros::Duration(0.02));
  } catch (tf2::TransformException &ex) {
    ROS_WARN("%s", ex.what());
    return;
  }

  sensor_msgs::PointCloud2 cloud_out;
  tf2::doTransform(*cloud_ptr, cloud_out, transform_stamped);

  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(cloud_out, cloud);

  // // push to mapper
  global_mapper_ptr_->PushPointCloud(cloud.makeShared());
}

void GlobalMapperRos::Run() {
  // start mapping thread
  global_mapper_ptr_ = std::unique_ptr<GlobalMapper>(new GlobalMapper(stop_signal_ptr_, params_));
  global_mapper_ptr_->Run();

  // handle ros callbacks
  ros::spin();
}

}  // namespace global_mapper
<commit_msg>Cleanup voxel visualization code.<commit_after>// Copyright 2017 Massachusetts Institute of Technology

#include <csignal>
#include <memory>
#include <utility>
#include <algorithm>

#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/OccupancyGrid.h>

#include <fla_utils/param_utils.h>

#include "global_mapper_ros/global_mapper_ros.h"
#include "global_mapper/global_mapper.h"
#include "global_mapper/params.h"

namespace global_mapper {

GlobalMapperRos::GlobalMapperRos(volatile std::sig_atomic_t* stop_signal_ptr)
  : stop_signal_ptr_(stop_signal_ptr),
    publish_voxel_map_(false),
    nh_(),
    pnh_("~"),
    tf_listener_(tf_buffer_) {
  tf_buffer_.setUsingDedicatedThread(true);

  GetParams();
  InitSubscribers();
  InitPublishers();
}

void GlobalMapperRos::GetParams() {
  fla_utils::SafeGetParam(pnh_, "map_frame", params_.map_frame);

  // pixel map params
  std::vector<double> pixel_xy_min(2, 0.0);
  fla_utils::SafeGetParam(pnh_, "pixel_map/xy_min", pixel_xy_min);
  std::copy(pixel_xy_min.begin(), pixel_xy_min.end(), params_.pixel_xy_min);

  std::vector<double> pixel_xy_max(2, 0.0);
  fla_utils::SafeGetParam(pnh_, "pixel_map/xy_max", pixel_xy_max);
  std::copy(pixel_xy_max.begin(), pixel_xy_max.end(), params_.pixel_xy_max);

  fla_utils::SafeGetParam(pnh_, "pixel_map/resolution", params_.pixel_resolution);
  fla_utils::SafeGetParam(pnh_, "pixel_map/init_value", params_.pixel_init_value);
  fla_utils::SafeGetParam(pnh_, "pixel_map/bound_min", params_.pixel_bound_min);
  fla_utils::SafeGetParam(pnh_, "pixel_map/bound_max", params_.pixel_bound_max);
  fla_utils::SafeGetParam(pnh_, "pixel_map/min_z_abs", params_.pixel_min_z_abs);
  fla_utils::SafeGetParam(pnh_, "pixel_map/max_z_abs", params_.pixel_max_z_abs);
  fla_utils::SafeGetParam(pnh_, "pixel_map/use_rel_flatten", params_.pixel_use_rel_flatten);
  fla_utils::SafeGetParam(pnh_, "pixel_map/min_z_rel", params_.pixel_min_z_rel);
  fla_utils::SafeGetParam(pnh_, "pixel_map/max_z_rel", params_.pixel_max_z_rel);
  fla_utils::SafeGetParam(pnh_, "pixel_map/publish_map", publish_pixel_map_);

  // voxel map params
  std::vector<double> voxel_xyz_min(3, 0.0);
  fla_utils::SafeGetParam(pnh_, "voxel_map/xyz_min", voxel_xyz_min);
  std::copy(voxel_xyz_min.begin(), voxel_xyz_min.end(), params_.voxel_xyz_min);

  std::vector<double> voxel_xyz_max(3, 0.0);
  fla_utils::SafeGetParam(pnh_, "voxel_map/xyz_max", voxel_xyz_max);
  std::copy(voxel_xyz_max.begin(), voxel_xyz_max.end(), params_.voxel_xyz_max);

  fla_utils::SafeGetParam(pnh_, "voxel_map/resolution", params_.voxel_resolution);

  fla_utils::SafeGetParam(pnh_, "voxel_map/init_value", params_.voxel_init_value);
  fla_utils::SafeGetParam(pnh_, "voxel_map/bound_min", params_.voxel_bound_min);
  fla_utils::SafeGetParam(pnh_, "voxel_map/bound_max", params_.voxel_bound_max);
  fla_utils::SafeGetParam(pnh_, "voxel_map/min_z_abs", params_.voxel_min_z_abs);
  fla_utils::SafeGetParam(pnh_, "voxel_map/max_z_abs", params_.voxel_max_z_abs);
  fla_utils::SafeGetParam(pnh_, "voxel_map/use_rel_cropping", params_.voxel_use_rel_cropping);
  fla_utils::SafeGetParam(pnh_, "voxel_map/min_z_rel", params_.voxel_min_z_rel);
  fla_utils::SafeGetParam(pnh_, "voxel_map/max_z_rel", params_.voxel_max_z_rel);
  fla_utils::SafeGetParam(pnh_, "voxel_map/hit_inc", params_.voxel_hit_inc);
  fla_utils::SafeGetParam(pnh_, "voxel_map/miss_inc", params_.voxel_miss_inc);
  fla_utils::SafeGetParam(pnh_, "voxel_map/publish_map", publish_voxel_map_);
}

void GlobalMapperRos::InitSubscribers() {
  point_cloud_sub_ = pnh_.subscribe("cloud_topic", 10, &GlobalMapperRos::PointCloudCallback, this);
}

void GlobalMapperRos::InitPublishers() {
  pixel_map_pub_ = pnh_.advertise<nav_msgs::OccupancyGrid>("pixel_map_topic", 1);
  voxel_map_pub_ = pnh_.advertise<visualization_msgs::MarkerArray>("voxel_map_topic", 1);
  map_pub_timer_ = nh_.createTimer(ros::Duration(0.2), &GlobalMapperRos::PublishMap, this);
}

/*
  Return a RGB colour value given a scalar v in the range [vmin,vmax]
  In this case each colour component ranges from 0 (no contribution) to
  1 (fully saturated). The colour is clipped at the end of the scales if v is outside
  the range [vmin,vmax]
*/
void GlobalMapperRos::GrayscaleToRGBJet(double v, double vmin, double vmax, std::vector<double>* rgb) {
  double dv;

  if (v < vmin)
    v = vmin;
  if (v > vmax)
    v = vmax;
  dv = vmax - vmin;

  if (v < (vmin + 0.25 * dv)) {
    rgb->at(0) = 0;
    rgb->at(1) = 4 * (v - vmin) / dv;
  } else if (v < (vmin + 0.5 * dv)) {
    rgb->at(0) = 0;
    rgb->at(2) = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
  } else if (v < (vmin + 0.75 * dv)) {
    rgb->at(0) = 4 * (v - vmin - 0.5 * dv) / dv;
    rgb->at(2) = 0;
  } else {
    rgb->at(1) = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
    rgb->at(2) = 0;
  }
}

void GlobalMapperRos::PopulateVoxelMapMsg(visualization_msgs::MarkerArray* marker_array) {
  // check for bad input
  if (marker_array == nullptr) {
    return;
  }

  geometry_msgs::TransformStamped transform_stamped;
  Eigen::Vector3d transform;
  try {
    transform_stamped = tf_buffer_.lookupTransform("world", "body",
                                                   ros::Time(0), ros::Duration(1.0));
    transform(0) = transform_stamped.transform.translation.x;
    transform(1) = transform_stamped.transform.translation.y;
    transform(2) = transform_stamped.transform.translation.z;
  } catch (tf2::TransformException &ex) {
    ROS_WARN("[world_database_master_ros] OnGetTransform failed with %s", ex.what());

    transform(0) = std::numeric_limits<double>::quiet_NaN();
    transform(1) = std::numeric_limits<double>::quiet_NaN();
    transform(2) = std::numeric_limits<double>::quiet_NaN();
  }

  // get occuppied voxels
  double xyz[3] = {0.0};
  Eigen::Map<Eigen::Vector3d> xyz_map(&xyz[0]);
  std::vector<int> occ_inds;

  int min_ixyz[3];
  int max_ixyz[3];

  const double subgrid_half_size[3] = {10,10,3};

  const double min_xyz[3] = {transform[0] - subgrid_half_size[0],
                       transform[1] - subgrid_half_size[1],
                       transform[2] - subgrid_half_size[2]};

  const double max_xyz[3] = {transform[0] + subgrid_half_size[0],
                       transform[1] + subgrid_half_size[1],
                       transform[2] + subgrid_half_size[2]};

  global_mapper_ptr_->voxel_map_ptr_->worldToTable(min_xyz, min_ixyz);
  global_mapper_ptr_->voxel_map_ptr_->worldToTable(max_xyz, max_ixyz);

  int voxel_index = 0;
  for (int x = min_ixyz[0]; x < max_ixyz[0]; x++) {
    for (int y = min_ixyz[1]; y < max_ixyz[1]; y++) {
      for (int z = min_ixyz[2]; z < max_ixyz[2]; z++) {
        int ixyz[3] = {x, y, z};
        if(global_mapper_ptr_->voxel_map_ptr_->readValue(ixyz) > 0.6) {
          voxel_index = global_mapper_ptr_->voxel_map_ptr_->getInd(ixyz);
          occ_inds.push_back(voxel_index);
        }
      }
    }
  }

  marker_array->markers.resize(occ_inds.size());
  std::vector<double> rgb(3, 1.0);
  for (int i = 0; i < occ_inds.size(); ++i) {
    visualization_msgs::Marker& marker = marker_array->markers[i];

    // create voxel marker
    marker.header.frame_id = params_.map_frame;
    marker.header.stamp = ros::Time::now();
    marker.id = i;
    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = visualization_msgs::Marker::ADD;

    // orientation
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // scale
    marker.scale.x = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[0];
    marker.scale.y = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[1];
    marker.scale.z = global_mapper_ptr_->voxel_map_ptr_->metersPerPixel[2];

    // position
    global_mapper_ptr_->voxel_map_ptr_->indToLoc(occ_inds[i], xyz);
    marker.pose.position.x = xyz[0];
    marker.pose.position.y = xyz[1];
    marker.pose.position.z = xyz[2];

    // color
    GrayscaleToRGBJet(marker.pose.position.z,
                      global_mapper_ptr_->voxel_map_ptr_->xyz0[2],
                      global_mapper_ptr_->voxel_map_ptr_->xyz1[2],
                      &rgb);
    marker.color.r = static_cast<float>(rgb[0]);
    marker.color.g = static_cast<float>(rgb[1]);
    marker.color.b = static_cast<float>(rgb[2]);
    marker.color.a = 1.0f;
  }
}

void GlobalMapperRos::PopulatePixelMapMsg(nav_msgs::OccupancyGrid* occupancy_grid) {
  // check for bad input
  if (occupancy_grid == nullptr) {
    return;
  }

  // header
  occupancy_grid->header.frame_id = params_.map_frame;
  occupancy_grid->header.stamp = ros::Time::now();

  // metadata
  occupancy_grid->info.resolution = global_mapper_ptr_->pixel_map_ptr_->metersPerPixel;
  occupancy_grid->info.width = global_mapper_ptr_->pixel_map_ptr_->dimensions[0];
  occupancy_grid->info.height = global_mapper_ptr_->pixel_map_ptr_->dimensions[1];
  occupancy_grid->info.origin.position.x = 0.0;
  occupancy_grid->info.origin.position.y = 0.0;
  occupancy_grid->info.origin.position.z = 0.0;

  // data
  float occ_value = 0.0;
  double xy[2] = {0.0};
  occupancy_grid->data.resize(global_mapper_ptr_->pixel_map_ptr_->num_cells);
  for (int i = 0; i < global_mapper_ptr_->pixel_map_ptr_->num_cells; ++i) {
    // get xy location
    global_mapper_ptr_->pixel_map_ptr_->indToLoc(i, xy);

    // get pixel value and scale
    occ_value = global_mapper_ptr_->pixel_map_ptr_->readValue(xy);
    occ_value *= 100.0;
    occupancy_grid->data[i] = static_cast<uint8_t>(occ_value + 0.5);
  }
}

void GlobalMapperRos::PublishMap(const ros::TimerEvent& event) {
  // lock
  // std::lock_guard<std::mutex> map_lock(global_mapper_ptr_->map_mutex());

  // voxel map
  if (publish_voxel_map_) {
    double start_time = ros::Time::now().toSec();
    visualization_msgs::MarkerArray marker_array;
    PopulateVoxelMapMsg(&marker_array);
    std::cout << "(global_mapper) PopulateVoxelMapMsg took " << ros::Time::now().toSec() - start_time << " seconds" << std::endl;
    voxel_map_pub_.publish(marker_array);
  }

  // pixel_map
  if (publish_pixel_map_) {
    nav_msgs::OccupancyGrid occupancy_grid;
    PopulatePixelMapMsg(&occupancy_grid);
    pixel_map_pub_.publish(occupancy_grid);
  }
}

void GlobalMapperRos::PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_ptr) {
  // get transform
  const std::string target_frame = params_.map_frame;
  geometry_msgs::TransformStamped transform_stamped;
  // try to get correct transform
  try {
    transform_stamped = tf_buffer_.lookupTransform(target_frame, cloud_ptr->header.frame_id,
                                                   ros::Time(cloud_ptr->header.stamp),
                                                   ros::Duration(0.02));
  } catch (tf2::TransformException &ex) {
    ROS_WARN("%s", ex.what());
    return;
  }

  sensor_msgs::PointCloud2 cloud_out;
  tf2::doTransform(*cloud_ptr, cloud_out, transform_stamped);

  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(cloud_out, cloud);

  // push to mapper
  global_mapper_ptr_->PushPointCloud(cloud.makeShared());
}

void GlobalMapperRos::Run() {
  // start mapping thread
  global_mapper_ptr_ = std::unique_ptr<GlobalMapper>(new GlobalMapper(stop_signal_ptr_, params_));
  global_mapper_ptr_->Run();

  // handle ros callbacks
  ros::spin();
}

}  // namespace global_mapper
<|endoftext|>


<commit_before>// Print each number in the range specified by two integers.

#include <iostream>

using std::cout;
using std::cin;

void print_range(int lo, int hi)
{
    if (lo > hi)
        print_range(hi, lo);
    for (int i = lo; i != hi; ++i)
        cout << i << " ";
}

int main()
{
    int low = 0, high = 0;
    cout << "please input two integers:\n";
    cin >> low >> high;
    print_range(low, high);
    return 0;
}
<commit_msg>Fixed #438<commit_after>// Print each number in the range specified by two integers.

#include <iostream>

using std::cout;
using std::cin;

void print_range(int lo, int hi)
{
    if (lo > hi)
    {
        print_range(hi, lo);
        return;
    }
    for (int i = lo; i != hi; ++i)
        cout << i << " ";
}

int main()
{
    int low = 0, high = 0;
    cout << "please input two integers:\n";
    cin >> low >> high;
    print_range(low, high);
    return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include "include/Sales_item.h"

int main()
{
  Sales_item currItem, valItem;
  if (std::cin >> currItem)
  {
    int cnt = 1;
    while (std::cin >> valItem)
    {
      if (valItem.isbn() == currItem.isbn())
        ++cnt;
      else
      {
        std::cout << currItem << " occurs "
                  << cnt << " times " << std::endl;
        currItem = valItem;
        cnt = 1;
      }
    }

    std::cout << currItem << " occurs "
              << cnt << " times " << std::endl;
  }
  return 0;
}

<commit_msg>Update ex1_23.cpp<commit_after>#include <iostream>
#include "include/Sales_item.h"

int main()
{
    Sales_item currItem, valItem;
    if (std::cin >> currItem)
    {
        int cnt = 1;
        while (std::cin >> valItem)
        {
            if (valItem.isbn() == currItem.isbn())
            {
                ++cnt;
            }
            else
            {
                std::cout << currItem << " occurs " << cnt << " times " << std::endl;
                currItem = valItem;
                cnt = 1;
            }
        }
        std::cout << currItem << " occurs "<< cnt << " times " << std::endl;
    }
    return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) 2010 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "chrome/browser/bug_report_util.h"

#include "app/l10n_util.h"
#include "base/file_version_info.h"
#include "base/utf_string_conversions.h"
#include "chrome/app/chrome_version_info.h"
#include "chrome/browser/browser_process_impl.h"
#include "chrome/browser/profile.h"
#include "chrome/browser/safe_browsing/safe_browsing_util.h"
#include "chrome/browser/tab_contents/tab_contents.h"
#include "chrome/common/net/url_fetcher.h"
#include "googleurl/src/gurl.h"
#include "grit/locale_settings.h"
#include "net/url_request/url_request_status.h"
#include "unicode/locid.h"

#include <string>

namespace {

const int kBugReportVersion = 1;

const char kReportPhishingUrl[] =
    "http://www.google.com/safebrowsing/report_phish/";

// URL to post bug reports to.
const char* const kBugReportPostUrl =
    "https://feedback2-test.corp.google.com/tools/feedback/chrome/__submit";

const char* const kProtBufMimeType = "application/x-protobuf";
const char* const kPngMimeType = "image/png";

// Tags we use in product specific data
const char* const kPageTitleTag = "PAGE TITLE";
const char* const kProblemTypeTag = "PROBLEM TYPE";
const char* const kChromeVersionTag = "CHROME VERSION";
const char* const kOsVersionTag = "OS VERSION";

}  // namespace

// Simple URLFetcher::Delegate to clean up URLFetcher on completion.
class BugReportUtil::PostCleanup : public URLFetcher::Delegate {
 public:
  PostCleanup();

  // Overridden from URLFetcher::Delegate.
  virtual void OnURLFetchComplete(const URLFetcher* source,
                                  const GURL& url,
                                  const URLRequestStatus& status,
                                  int response_code,
                                  const ResponseCookies& cookies,
                                  const std::string& data);
 private:
  DISALLOW_COPY_AND_ASSIGN(PostCleanup);
};

BugReportUtil::PostCleanup::PostCleanup() {
}

void BugReportUtil::PostCleanup::OnURLFetchComplete(
    const URLFetcher* source,
    const GURL& url,
    const URLRequestStatus& status,
    int response_code,
    const ResponseCookies& cookies,
    const std::string& data) {
  // if not 204, something went wrong
  if (response_code != 204)
    LOG(WARNING) << "Submission to feedback server failed. Response code: " <<
                 response_code << std::endl;
  // Delete the URLFetcher.
  delete source;
  // And then delete ourselves.
  delete this;
}

// static
void BugReportUtil::SetOSVersion(std::string *os_version) {
#if defined(OS_WIN)
  OSVERSIONINFO osvi;
  ZeroMemory(&osvi, sizeof(OSVERSIONINFO));
  osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);

  if (GetVersionEx(&osvi)) {
    *os_version = StringPrintf("%d.%d.%d %S",
      osvi.dwMajorVersion,
      osvi.dwMinorVersion,
      osvi.dwBuildNumber,
      osvi.szCSDVersion);
  } else {
    *os_version = "unknown";
  }
#elif defined(OS_MACOSX)
  int32 major;
  int32 minor;
  int32 bugFix;
  base::SysInfo::OperatingSystemVersionNumbers(&major, &minor, &bugFix);
  *os_version = StringPrintf("%d.%d.%d", major, minor, bugFix);
#else
  *os_version = "unknown";
#endif
}

// static
void BugReportUtil::AddFeedbackData(
    userfeedback::ExternalExtensionSubmit* feedback_data,
    const std::string& key, const std::string& value) {
  // Create log_value object and add it to the web_data object
  userfeedback::ProductSpecificData log_value;
  log_value.set_key(key);
  log_value.set_value(value);
  userfeedback::WebData* web_data = feedback_data->mutable_web_data();
  *(web_data->add_product_specific_data()) = log_value;
}

// static
void BugReportUtil::SendReport(Profile* profile,
    const std::string& page_title_text,
    int problem_type,
    const std::string& page_url_text,
    const std::string& user_email_text,
    const std::string& description,
    const char* png_data,
    int png_data_length,
    int png_width,
#if defined(OS_CHROMEOS)
    int png_height,
    const chromeos::LogDictionaryType* const sys_info) {
#else
    int png_height) {
#endif
  GURL post_url(kBugReportPostUrl);

  // Create google feedback protocol buffer objects
  userfeedback::ExternalExtensionSubmit feedback_data;
  // type id set to 0, unused field but needs to be initialized to 0
  feedback_data.set_type_id(0);

  userfeedback::CommonData* common_data = feedback_data.mutable_common_data();
  userfeedback::WebData* web_data = feedback_data.mutable_web_data();

  // Set GAIA id to 0. We're not using gaia id's for recording
  // use feedback - we're using the e-mail field, allows users to
  // submit feedback from incognito mode and specify any mail id
  // they wish
  common_data->set_gaia_id(0);

  // Add the page title.
  AddFeedbackData(&feedback_data, std::string(kPageTitleTag),
                  page_title_text);

  AddFeedbackData(&feedback_data, std::string(kProblemTypeTag),
                  StringPrintf("%d\r\n", problem_type));

  // Add the user e-mail to the feedback object
  common_data->set_user_email(user_email_text);

  // Add the description to the feedback object
  common_data->set_description(description);

  // Add the language
  std::string chrome_locale = g_browser_process->GetApplicationLocale();
  common_data->set_source_descripton_language(chrome_locale);

  // Set the url
  web_data->set_url(page_url_text);

  // Add the Chrome version
  std::string chrome_version;
  scoped_ptr<FileVersionInfo> version_info(
      chrome_app::GetChromeVersionInfo());
  if (version_info.get()) {
    chrome_version = WideToUTF8(version_info->product_name()) + " - " +
        WideToUTF8(version_info->file_version()) +
        " (" + WideToUTF8(version_info->last_change()) + ")";
  }

  if (!chrome_version.empty())
    AddFeedbackData(&feedback_data, std::string(kChromeVersionTag),
                    chrome_version);

  // Add OS version (eg, for WinXP SP2: "5.1.2600 Service Pack 2").
  std::string os_version = "";
  SetOSVersion(&os_version);
  AddFeedbackData(&feedback_data, std::string(kOsVersionTag), os_version);

#if defined(OS_CHROMEOS)
  for (chromeos::LogDictionaryType::const_iterator i = sys_info->begin();
       i != sys_info->end(); ++i)
    AddFeedbackData(&feedback_data, i->first, i->second);
#endif

  // Include the page image if we have one.
  if (png_data) {
    userfeedback::PostedScreenshot screenshot;
    screenshot.set_mime_type(kPngMimeType);
    // Set the dimensions of the screenshot
    userfeedback::Dimensions dimensions;
    dimensions.set_width(static_cast<float>(png_width));
    dimensions.set_height(static_cast<float>(png_height));
    *(screenshot.mutable_dimensions()) = dimensions;
    screenshot.set_binary_content(std::string(png_data, png_data_length));

    // Set the screenshot object in feedback
    *(feedback_data.mutable_screenshot()) = screenshot;
  }

  // We have the body of our POST, so send it off to the server.
  URLFetcher* fetcher = new URLFetcher(post_url, URLFetcher::POST,
                                       new BugReportUtil::PostCleanup);
  fetcher->set_request_context(profile->GetRequestContext());

  std::string post_body;
  feedback_data.SerializeToString(&post_body);
  fetcher->set_upload_data(std::string(kProtBufMimeType), post_body);
  fetcher->Start();
}

// static
void BugReportUtil::ReportPhishing(TabContents* currentTab,
                                   const std::string& phishing_url) {
  currentTab->controller().LoadURL(
      safe_browsing_util::GeneratePhishingReportUrl(
          kReportPhishingUrl, phishing_url),
      GURL(),
      PageTransition::LINK);
}
<commit_msg>Flipped feedback URL to production servers.<commit_after>// Copyright (c) 2010 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "chrome/browser/bug_report_util.h"

#include "app/l10n_util.h"
#include "base/file_version_info.h"
#include "base/utf_string_conversions.h"
#include "chrome/app/chrome_version_info.h"
#include "chrome/browser/browser_process_impl.h"
#include "chrome/browser/profile.h"
#include "chrome/browser/safe_browsing/safe_browsing_util.h"
#include "chrome/browser/tab_contents/tab_contents.h"
#include "chrome/common/net/url_fetcher.h"
#include "googleurl/src/gurl.h"
#include "grit/locale_settings.h"
#include "net/url_request/url_request_status.h"
#include "unicode/locid.h"

#include <string>

namespace {

const int kBugReportVersion = 1;

const char kReportPhishingUrl[] =
    "http://www.google.com/safebrowsing/report_phish/";

// URL to post bug reports to.
const char* const kBugReportPostUrl =
    "https://www.google.com/tools/feedback/chrome/__submit";

const char* const kProtBufMimeType = "application/x-protobuf";
const char* const kPngMimeType = "image/png";

// Tags we use in product specific data
const char* const kPageTitleTag = "PAGE TITLE";
const char* const kProblemTypeTag = "PROBLEM TYPE";
const char* const kChromeVersionTag = "CHROME VERSION";
const char* const kOsVersionTag = "OS VERSION";

}  // namespace

// Simple URLFetcher::Delegate to clean up URLFetcher on completion.
class BugReportUtil::PostCleanup : public URLFetcher::Delegate {
 public:
  PostCleanup();

  // Overridden from URLFetcher::Delegate.
  virtual void OnURLFetchComplete(const URLFetcher* source,
                                  const GURL& url,
                                  const URLRequestStatus& status,
                                  int response_code,
                                  const ResponseCookies& cookies,
                                  const std::string& data);
 private:
  DISALLOW_COPY_AND_ASSIGN(PostCleanup);
};

BugReportUtil::PostCleanup::PostCleanup() {
}

void BugReportUtil::PostCleanup::OnURLFetchComplete(
    const URLFetcher* source,
    const GURL& url,
    const URLRequestStatus& status,
    int response_code,
    const ResponseCookies& cookies,
    const std::string& data) {
  // if not 204, something went wrong
  if (response_code != 204)
    LOG(WARNING) << "Submission to feedback server failed. Response code: " <<
                 response_code << std::endl;
  // Delete the URLFetcher.
  delete source;
  // And then delete ourselves.
  delete this;
}

// static
void BugReportUtil::SetOSVersion(std::string *os_version) {
#if defined(OS_WIN)
  OSVERSIONINFO osvi;
  ZeroMemory(&osvi, sizeof(OSVERSIONINFO));
  osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);

  if (GetVersionEx(&osvi)) {
    *os_version = StringPrintf("%d.%d.%d %S",
      osvi.dwMajorVersion,
      osvi.dwMinorVersion,
      osvi.dwBuildNumber,
      osvi.szCSDVersion);
  } else {
    *os_version = "unknown";
  }
#elif defined(OS_MACOSX)
  int32 major;
  int32 minor;
  int32 bugFix;
  base::SysInfo::OperatingSystemVersionNumbers(&major, &minor, &bugFix);
  *os_version = StringPrintf("%d.%d.%d", major, minor, bugFix);
#else
  *os_version = "unknown";
#endif
}

// static
void BugReportUtil::AddFeedbackData(
    userfeedback::ExternalExtensionSubmit* feedback_data,
    const std::string& key, const std::string& value) {
  // Create log_value object and add it to the web_data object
  userfeedback::ProductSpecificData log_value;
  log_value.set_key(key);
  log_value.set_value(value);
  userfeedback::WebData* web_data = feedback_data->mutable_web_data();
  *(web_data->add_product_specific_data()) = log_value;
}

// static
void BugReportUtil::SendReport(Profile* profile,
    const std::string& page_title_text,
    int problem_type,
    const std::string& page_url_text,
    const std::string& user_email_text,
    const std::string& description,
    const char* png_data,
    int png_data_length,
    int png_width,
#if defined(OS_CHROMEOS)
    int png_height,
    const chromeos::LogDictionaryType* const sys_info) {
#else
    int png_height) {
#endif
  GURL post_url(kBugReportPostUrl);

  // Create google feedback protocol buffer objects
  userfeedback::ExternalExtensionSubmit feedback_data;
  // type id set to 0, unused field but needs to be initialized to 0
  feedback_data.set_type_id(0);

  userfeedback::CommonData* common_data = feedback_data.mutable_common_data();
  userfeedback::WebData* web_data = feedback_data.mutable_web_data();

  // Set GAIA id to 0. We're not using gaia id's for recording
  // use feedback - we're using the e-mail field, allows users to
  // submit feedback from incognito mode and specify any mail id
  // they wish
  common_data->set_gaia_id(0);

  // Add the page title.
  AddFeedbackData(&feedback_data, std::string(kPageTitleTag),
                  page_title_text);

  AddFeedbackData(&feedback_data, std::string(kProblemTypeTag),
                  StringPrintf("%d\r\n", problem_type));

  // Add the user e-mail to the feedback object
  common_data->set_user_email(user_email_text);

  // Add the description to the feedback object
  common_data->set_description(description);

  // Add the language
  std::string chrome_locale = g_browser_process->GetApplicationLocale();
  common_data->set_source_descripton_language(chrome_locale);

  // Set the url
  web_data->set_url(page_url_text);

  // Add the Chrome version
  std::string chrome_version;
  scoped_ptr<FileVersionInfo> version_info(
      chrome_app::GetChromeVersionInfo());
  if (version_info.get()) {
    chrome_version = WideToUTF8(version_info->product_name()) + " - " +
        WideToUTF8(version_info->file_version()) +
        " (" + WideToUTF8(version_info->last_change()) + ")";
  }

  if (!chrome_version.empty())
    AddFeedbackData(&feedback_data, std::string(kChromeVersionTag),
                    chrome_version);

  // Add OS version (eg, for WinXP SP2: "5.1.2600 Service Pack 2").
  std::string os_version = "";
  SetOSVersion(&os_version);
  AddFeedbackData(&feedback_data, std::string(kOsVersionTag), os_version);

#if defined(OS_CHROMEOS)
  for (chromeos::LogDictionaryType::const_iterator i = sys_info->begin();
       i != sys_info->end(); ++i)
    AddFeedbackData(&feedback_data, i->first, i->second);
#endif

  // Include the page image if we have one.
  if (png_data) {
    userfeedback::PostedScreenshot screenshot;
    screenshot.set_mime_type(kPngMimeType);
    // Set the dimensions of the screenshot
    userfeedback::Dimensions dimensions;
    dimensions.set_width(static_cast<float>(png_width));
    dimensions.set_height(static_cast<float>(png_height));
    *(screenshot.mutable_dimensions()) = dimensions;
    screenshot.set_binary_content(std::string(png_data, png_data_length));

    // Set the screenshot object in feedback
    *(feedback_data.mutable_screenshot()) = screenshot;
  }

  // We have the body of our POST, so send it off to the server.
  URLFetcher* fetcher = new URLFetcher(post_url, URLFetcher::POST,
                                       new BugReportUtil::PostCleanup);
  fetcher->set_request_context(profile->GetRequestContext());

  std::string post_body;
  feedback_data.SerializeToString(&post_body);
  fetcher->set_upload_data(std::string(kProtBufMimeType), post_body);
  fetcher->Start();
}

// static
void BugReportUtil::ReportPhishing(TabContents* currentTab,
                                   const std::string& phishing_url) {
  currentTab->controller().LoadURL(
      safe_browsing_util::GeneratePhishingReportUrl(
          kReportPhishingUrl, phishing_url),
      GURL(),
      PageTransition::LINK);
}
<|endoftext|>


<commit_before>#include "selfdrive/ui/replay/camera.h"

#include <cassert>
#include <iostream>

const int YUV_BUF_COUNT = 50;

CameraServer::CameraServer() {
  camera_thread_ = std::thread(&CameraServer::thread, this);
}

CameraServer::~CameraServer() {
  queue_.push({});
  camera_thread_.join();
  vipc_server_.reset(nullptr);
}

void CameraServer::startVipcServer() {
  std::cout << (vipc_server_ ? "start" : "restart") << " vipc server" << std::endl;
  vipc_server_.reset(new VisionIpcServer("camerad"));
  for (auto &cam : cameras_) {
    if (cam.width > 0 && cam.height > 0) {
      vipc_server_->create_buffers(cam.rgb_type, UI_BUF_COUNT, true, cam.width, cam.height);
      vipc_server_->create_buffers(cam.yuv_type, YUV_BUF_COUNT, false, cam.width, cam.height);
    }
  }
  vipc_server_->start_listener();
}

void CameraServer::thread() {
  while (true) {
    const auto [type, fr, eidx] = queue_.pop();
    if (!fr) break;

    auto &cam = cameras_[type];
    // start|restart the vipc server if frame size changed
    if (cam.width != fr->width || cam.height != fr->height) {
      cam.width = fr->width;
      cam.height = fr->height;
      std::cout << "camera[" << type << "] frame size " << cam.width << "x" << cam.height << std::endl;
      startVipcServer();
    }

    // send frame
    if (auto dat = fr->get(eidx.getSegmentId())) {
      auto [rgb_dat, yuv_dat] = *dat;
      VisionIpcBufExtra extra = {
          .frame_id = eidx.getFrameId(),
          .timestamp_sof = eidx.getTimestampSof(),
          .timestamp_eof = eidx.getTimestampEof(),
      };

      VisionBuf *rgb_buf = vipc_server_->get_buffer(cam.rgb_type);
      memcpy(rgb_buf->addr, rgb_dat, fr->getRGBSize());
      VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.yuv_type);
      memcpy(yuv_buf->addr, yuv_dat, fr->getYUVSize());

      vipc_server_->send(rgb_buf, &extra, false);
      vipc_server_->send(yuv_buf, &extra, false);
    } else {
      std::cout << "camera[" << type << "] failed to get frame:" << eidx.getSegmentId() << std::endl;
    }
  }
}
<commit_msg>replay/camera: fix incorrect console output (#22491)<commit_after>#include "selfdrive/ui/replay/camera.h"

#include <cassert>
#include <iostream>

const int YUV_BUF_COUNT = 50;

CameraServer::CameraServer() {
  camera_thread_ = std::thread(&CameraServer::thread, this);
}

CameraServer::~CameraServer() {
  queue_.push({});
  camera_thread_.join();
  vipc_server_.reset(nullptr);
}

void CameraServer::startVipcServer() {
  std::cout << (vipc_server_ ? "restart" : "start") << " vipc server" << std::endl;
  vipc_server_.reset(new VisionIpcServer("camerad"));
  for (auto &cam : cameras_) {
    if (cam.width > 0 && cam.height > 0) {
      vipc_server_->create_buffers(cam.rgb_type, UI_BUF_COUNT, true, cam.width, cam.height);
      vipc_server_->create_buffers(cam.yuv_type, YUV_BUF_COUNT, false, cam.width, cam.height);
    }
  }
  vipc_server_->start_listener();
}

void CameraServer::thread() {
  while (true) {
    const auto [type, fr, eidx] = queue_.pop();
    if (!fr) break;

    auto &cam = cameras_[type];
    // start|restart the vipc server if frame size changed
    if (cam.width != fr->width || cam.height != fr->height) {
      cam.width = fr->width;
      cam.height = fr->height;
      std::cout << "camera[" << type << "] frame size " << cam.width << "x" << cam.height << std::endl;
      startVipcServer();
    }

    // send frame
    if (auto dat = fr->get(eidx.getSegmentId())) {
      auto [rgb_dat, yuv_dat] = *dat;
      VisionIpcBufExtra extra = {
          .frame_id = eidx.getFrameId(),
          .timestamp_sof = eidx.getTimestampSof(),
          .timestamp_eof = eidx.getTimestampEof(),
      };

      VisionBuf *rgb_buf = vipc_server_->get_buffer(cam.rgb_type);
      memcpy(rgb_buf->addr, rgb_dat, fr->getRGBSize());
      VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.yuv_type);
      memcpy(yuv_buf->addr, yuv_dat, fr->getYUVSize());

      vipc_server_->send(rgb_buf, &extra, false);
      vipc_server_->send(yuv_buf, &extra, false);
    } else {
      std::cout << "camera[" << type << "] failed to get frame:" << eidx.getSegmentId() << std::endl;
    }
  }
}
<|endoftext|>


<commit_before>#pragma once

#include "beast.hpp"
#include "server_core.hpp"

namespace msrv {

class BeastConnectionContext;

class BeastListener : public std::enable_shared_from_this<BeastListener>
{
public:
    BeastListener(
        asio::io_context* ioContext,
        BeastConnectionContext* connectionContext,
        const asio::ip::tcp::endpoint& endpoint);

    ~BeastListener();

    void run();

private:
    void accept();
    void handleAccept(const boost::system::error_code& error);

    asio::io_context* ioContext_;
    BeastConnectionContext* connectionContext_;

    asio::ip::tcp::acceptor acceptor_;
    asio::ip::tcp::socket peerSocket_;
    asio::ip::tcp::endpoint peerEndpoint_;
};

}
<commit_msg>fix compilation with clang<commit_after>#pragma once

#include "beast.hpp"
#include "server_core.hpp"

namespace msrv {

struct BeastConnectionContext;

class BeastListener : public std::enable_shared_from_this<BeastListener>
{
public:
    BeastListener(
        asio::io_context* ioContext,
        BeastConnectionContext* connectionContext,
        const asio::ip::tcp::endpoint& endpoint);

    ~BeastListener();

    void run();

private:
    void accept();
    void handleAccept(const boost::system::error_code& error);

    asio::io_context* ioContext_;
    BeastConnectionContext* connectionContext_;

    asio::ip::tcp::acceptor acceptor_;
    asio::ip::tcp::socket peerSocket_;
    asio::ip::tcp::endpoint peerEndpoint_;
};

}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: brokenpackageint.hxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-07 16:46:10 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 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
 *
 ************************************************************************/

#ifndef _SAL_CONFIG_H_
#include "sal/config.h"
#endif

#ifndef INCLUDED_SFX2_DLLAPI_H
#include "sfx2/dllapi.h"
#endif

#ifndef _COM_SUN_STAR_DOCUMENT_BROKENPACKAGEREQUEST_HPP_
#include <com/sun/star/document/BrokenPackageRequest.hpp>
#endif
#ifndef _COM_SUN_STAR_TASK_XINTERACTIONAPPROVE_HPP_
#include <com/sun/star/task/XInteractionApprove.hpp>
#endif
#ifndef _COM_SUN_STAR_TASK_XINTERACTIONDISAPPROVE_HPP_
#include <com/sun/star/task/XInteractionDisapprove.hpp>
#endif

#ifndef __FRAMEWORK_DISPATCH_INTERACTION_HXX_
#include <framework/interaction.hxx>
#endif

using namespace ::framework;
typedef ContinuationBase< ::com::sun::star::task::XInteractionApprove > SfxContinuationApprove;
typedef ContinuationBase< ::com::sun::star::task::XInteractionDisapprove > SfxContinuationDisapprove;

class SFX2_DLLPUBLIC RequestPackageReparation : public ::cppu::WeakImplHelper1< ::com::sun::star::task::XInteractionRequest >
{
    ::com::sun::star::uno::Any m_aRequest;

    ::com::sun::star::uno::Sequence<
                    ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
                > m_lContinuations;

    SfxContinuationApprove* m_pApprove;
    SfxContinuationDisapprove*  m_pDisapprove;

public:
    RequestPackageReparation( ::rtl::OUString aName );

    sal_Bool    isApproved() { return m_pApprove->isSelected(); }

    virtual ::com::sun::star::uno::Any SAL_CALL getRequest()
        throw( ::com::sun::star::uno::RuntimeException );

    virtual ::com::sun::star::uno::Sequence<
                ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
            > SAL_CALL getContinuations()
        throw( ::com::sun::star::uno::RuntimeException );
};

class SFX2_DLLPUBLIC NotifyBrokenPackage : public ::cppu::WeakImplHelper1< ::com::sun::star::task::XInteractionRequest >
{
    ::com::sun::star::uno::Any m_aRequest;

    ::com::sun::star::uno::Sequence<
                    ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
                > m_lContinuations;

    ContinuationAbort*  m_pAbort;

public:
    NotifyBrokenPackage( ::rtl::OUString aName );

    sal_Bool    isAborted() { return m_pAbort->isSelected(); }

    virtual ::com::sun::star::uno::Any SAL_CALL getRequest()
        throw( ::com::sun::star::uno::RuntimeException );

    virtual ::com::sun::star::uno::Sequence<
                ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
            > SAL_CALL getContinuations()
        throw( ::com::sun::star::uno::RuntimeException );
};

<commit_msg>INTEGRATION: CWS changefileheader (1.4.732); FILE MERGED 2008/04/01 15:38:14 thb 1.4.732.3: #i85898# Stripping all external header guards 2008/04/01 12:40:32 thb 1.4.732.2: #i85898# Stripping all external header guards 2008/03/31 13:37:53 rt 1.4.732.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: brokenpackageint.hxx,v $
 * $Revision: 1.5 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#include "sal/config.h"
#include "sfx2/dllapi.h"
#include <com/sun/star/document/BrokenPackageRequest.hpp>
#include <com/sun/star/task/XInteractionApprove.hpp>
#include <com/sun/star/task/XInteractionDisapprove.hpp>
#include <framework/interaction.hxx>

using namespace ::framework;
typedef ContinuationBase< ::com::sun::star::task::XInteractionApprove > SfxContinuationApprove;
typedef ContinuationBase< ::com::sun::star::task::XInteractionDisapprove > SfxContinuationDisapprove;

class SFX2_DLLPUBLIC RequestPackageReparation : public ::cppu::WeakImplHelper1< ::com::sun::star::task::XInteractionRequest >
{
    ::com::sun::star::uno::Any m_aRequest;

    ::com::sun::star::uno::Sequence<
                    ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
                > m_lContinuations;

    SfxContinuationApprove* m_pApprove;
    SfxContinuationDisapprove*  m_pDisapprove;

public:
    RequestPackageReparation( ::rtl::OUString aName );

    sal_Bool    isApproved() { return m_pApprove->isSelected(); }

    virtual ::com::sun::star::uno::Any SAL_CALL getRequest()
        throw( ::com::sun::star::uno::RuntimeException );

    virtual ::com::sun::star::uno::Sequence<
                ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
            > SAL_CALL getContinuations()
        throw( ::com::sun::star::uno::RuntimeException );
};

class SFX2_DLLPUBLIC NotifyBrokenPackage : public ::cppu::WeakImplHelper1< ::com::sun::star::task::XInteractionRequest >
{
    ::com::sun::star::uno::Any m_aRequest;

    ::com::sun::star::uno::Sequence<
                    ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
                > m_lContinuations;

    ContinuationAbort*  m_pAbort;

public:
    NotifyBrokenPackage( ::rtl::OUString aName );

    sal_Bool    isAborted() { return m_pAbort->isSelected(); }

    virtual ::com::sun::star::uno::Any SAL_CALL getRequest()
        throw( ::com::sun::star::uno::RuntimeException );

    virtual ::com::sun::star::uno::Sequence<
                ::com::sun::star::uno::Reference< ::com::sun::star::task::XInteractionContinuation >
            > SAL_CALL getContinuations()
        throw( ::com::sun::star::uno::RuntimeException );
};

<|endoftext|>


<commit_before>//
//  Set.h
//  Set
//
//  Created by Antonio Seprano on 10/08/14.
//  Copyright (c) 2014 Antonio Seprano. All rights reserved.
//

#include <vector>
#include <functional>
#include <utility>
#include <algorithm>

#ifndef _SET_HPP
#define _SET_HPP

template<class _T, class _Allocator = std::allocator<_T> >
class Set {
public:
	typedef _T				  value_type;
	typedef _Allocator	allocator_type;

private:
	typedef Set<value_type,allocator_type> _SET;

public:
	typedef typename std::vector<value_type,allocator_type>::iterator				iterator;
	typedef typename std::vector<value_type,allocator_type>::const_iterator	const_iterator;

	// Empty set
	Set() {};

	// Set with initializer list
	Set(const std::initializer_list<value_type>& l) {
		_set.reserve(l.size());

		for (const auto& value : l) {
			insert(value);
		}
	}

	Set(std::initializer_list<value_type>&& l) {
		_set.reserve(l.size());

		for (auto& value : l) {
			insert(std::move(value));
		}
	}

	// Copy constructor
	Set(const _SET& s) {
		_set = s._set;
	}

	// Move constructor
	Set(_SET&& s) {
		_set = std::move(s._set);
	}

	// Copy assignment
	_SET& operator=(const _SET& s) noexcept {
		_set = s._set;
		return *this;
	}

	// Move assignment
	_SET& operator=(_SET&& s) noexcept {
		_set = std::move(s._set);
		return *this;
	}

	iterator begin() noexcept {
		return _set.begin();
	}

	const_iterator begin() const noexcept {
		return _set.begin();
	}

	iterator end() noexcept {
		return _set.end();
	};

	const_iterator end() const noexcept {
		return _set.end();
	}

	size_t size() const noexcept {
		return _set.size();
	}

	bool empty() const noexcept {
		return _set.empty();
	}

	void clear() noexcept {
		_set.clear();
	}

	iterator find(const value_type& value) noexcept {
		for (auto it = _set.begin(); it != _set.end(); it++) {
			if (*it == value) return it;
		}

		return _set.end();
	}

	const_iterator find(const value_type& value) const noexcept {
		return (const_iterator)find(value);
	}

	size_t count(const value_type& value) const noexcept {
		size_t ret{};

		for (const auto& v : _set) {
			if (v == value) {
				ret++;
			}
		}

		return ret;
	}

	bool operator==(const _SET& s) const noexcept {
		return this->contains(s, false) && s.contains(*this, false);
	}

	bool operator!=(const _SET& s) const noexcept {
		return !(*this == s);
	};

	bool operator<(const _SET& s) const noexcept {
		if (size() != s.size()) {
			return size() < s.size();
		}

		auto it1=begin(), it2=s.begin();

		while (it1 != end() && it2 != s.end()) {
			if (*it1 < *it2) {
				return true;
			}
			else if (*it1 > *it2) {
				return false;
			}

			it1++;
			it2++;
		};

		return false;
	}

	// Insert new value
	// unique = true => values must be unique (no duplicate values allowed)
	_SET& insert(const value_type& value, bool unique = false) noexcept {
		if (!unique || find(value) == _set.end()) {
			auto it = begin();

			while (it != end() && *it < value) {
				it++;
			}

			_set.insert(it, value);
		}

		return *this;
	}

	_SET& insert(value_type&& value, bool unique = false) noexcept {
		if (!unique || find(value) == _set.end()) {
			auto it = begin();

			while (it != end() && *it < value) {
				it++;
			}

			_set.insert(it, std::move(value));
		}

		return *this;
	}

	_SET operator+(const value_type& value) const noexcept {
		return _SET{*this}+_SET{value};
	}

	_SET operator+(value_type&& value) const noexcept {
		return _SET{*this}+_SET{std::move(value)};
	}

	_SET operator+(const _SET& s) const noexcept {
		_SET ret{*this};

		for (const auto& value : s) {
			ret.insert(value);
		}

		return ret;
	}

	_SET operator+(_SET&& s) const noexcept {
		_SET ret{*this};

		for (auto& value : s) {
			ret.insert(std::move(value));
		}

		return ret;
	}

	_SET& operator+=(const value_type& value) noexcept {
		insert(value);
		return *this;
	}

	_SET& operator+=(value_type&& value) noexcept {
		insert(std::move(value));
		return *this;
	}

	_SET& operator+=(const _SET& s) noexcept {
		for (const auto& value : s) {
			insert(value);
		}

		return *this;
	}

	_SET& operator+=(_SET&& s) noexcept {
		for (auto& value : s) {
			insert(std::move(value));
		}

		return *this;
	}

	_SET operator-(const value_type& value) const noexcept {
		_SET ret{*this};
		ret.erase(value);
		return ret;
	}

	_SET operator-(const _SET& s) const noexcept {
		_SET ret{*this};
		for (const auto& v : s) {
			ret.erase(v);
		}
		return ret;
	}

	_SET& operator-=(const value_type& value) noexcept {
		_set.erase(value);
		return *this;
	}

	_SET& operator-=(const _SET& s) noexcept {
		for (const auto& value : s) {
			_set.erase(value);
		}
		return *this;
	}

	Set<_SET> operator*(const _SET& other) noexcept {
		Set<_SET> ret;

		if (!this->empty() && !other.empty()) {
			for (const auto& v1 : _set) {
				for (const auto& v2 : other) {
					ret.insert(_SET{v1,v2});
				}
			}
		}

		return ret;
	}

	_SET intersection(const _SET& s) noexcept {
		_SET s1, s2;

		if (s.size() <= size()) {
			s1 = s;
			s2 = *this;
		}
		else {
			s1 = *this;
			s2 = s;
		}

		for (const auto &v : s1) {
			if (!s2.contains(v)) s1.erase(v);
		}

		return s1;
	}

	bool contains(const value_type& value) const noexcept {
		return contains(_SET{value});
	}

	bool contains(const _SET& s, bool strict = false) const noexcept {
		for (const auto& value : s) {
			bool found{false};

			for (const auto& v : _set) {
				if (v == value) {
					found = true;
					break;
				}
			}

			if (!found) {
				return false;
			}
		}

		return strict ? (s.size() != size()) : true;
	}

	template<class InputIterator>
	void insert(InputIterator first, InputIterator last) {
		for (auto it = first; it != last; it++) {
			_set.insert(*it);
		}
	};

	iterator erase(const_iterator position) {
		return _set.erase(position);
	}

	iterator erase(const_iterator first, const_iterator last) {
		return _set.erase(first,last);
	}

	size_t erase(const value_type& value, bool all = false) {
		size_t ret{};

		if (!all) {
			auto it = find(value);

			if (it != end()) {
				erase(it);
				ret++;
			}

			return ret;
		}

		for (auto it = _set.begin(); it != _set.end(); it++) {
			if (*it == value) {
				_set.erase(it);
				++ret;
			}
		}

		return ret;
	}

	_SET& unique() noexcept {
		for (auto i = begin(); i != end(); i++) {
			auto j = i+1;

			while (j != end()) {
				if (*i == *j) {
					_set.erase(j);
				}
				else {
					j++;
				}
			}
		}

		return *this;
	}

private:
	std::vector<value_type,allocator_type> _set;
};

#endif /* defined(_SET_HPP) */
<commit_msg>[add] Changed Set::intersection() to Set::intersectionWith()<commit_after>//
//  Set.h
//  Set
//
//  Created by Antonio Seprano on 10/08/14.
//  Copyright (c) 2014 Antonio Seprano. All rights reserved.
//

#include <vector>
#include <functional>
#include <utility>
#include <algorithm>

#ifndef _SET_HPP
#define _SET_HPP

template<class _T, class _Compare = std::less<_T>, class _Allocator = std::allocator<_T>>
class Set {
public:
	typedef _T				  value_type;
	typedef _Allocator	allocator_type;
	typedef _Compare    value_compare;

private:
	typedef Set<value_type,value_compare,allocator_type> _SET;

public:
	typedef typename std::vector<value_type,allocator_type>::iterator				iterator;
	typedef typename std::vector<value_type,allocator_type>::const_iterator	const_iterator;

	// Empty set
	Set() {};

	// Set with initializer list
	Set(const std::initializer_list<value_type>& l) {
		_set.reserve(l.size());

		for (const auto& value : l) {
			insert(value);
		}
	}

	Set(std::initializer_list<value_type>&& l) {
		_set.reserve(l.size());

		for (auto& value : l) {
			insert(std::move(value));
		}
	}

	// Copy constructor
	Set(const _SET& s) {
		_set = s._set;
	}

	// Move constructor
	Set(_SET&& s) {
		_set = std::move(s._set);
	}

	// Copy assignment
	_SET& operator=(const _SET& s) noexcept {
		_set = s._set;
		return *this;
	}

	// Move assignment
	_SET& operator=(_SET&& s) noexcept {
		_set = std::move(s._set);
		return *this;
	}

	iterator begin() noexcept {
		return _set.begin();
	}

	const_iterator begin() const noexcept {
		return _set.begin();
	}

	iterator end() noexcept {
		return _set.end();
	};

	const_iterator end() const noexcept {
		return _set.end();
	}

	size_t size() const noexcept {
		return _set.size();
	}

	bool empty() const noexcept {
		return _set.empty();
	}

	void clear() noexcept {
		_set.clear();
	}

	iterator find(const value_type& value) noexcept {
		for (auto it = _set.begin(); it != _set.end(); it++) {
			if (*it == value) return it;
		}

		return _set.end();
	}

	const_iterator find(const value_type& value) const noexcept {
		return (const_iterator)find(value);
	}

	size_t count(const value_type& value) const noexcept {
		size_t ret{};

		for (const auto& v : _set) {
			if (v == value) {
				ret++;
			}
		}

		return ret;
	}

	bool operator==(const _SET& s) const noexcept {
		return this->contains(s, false) && s.contains(*this, false);
	}

	bool operator!=(const _SET& s) const noexcept {
		return !(*this == s);
	};

	bool operator<(const _SET& s) const noexcept {
		if (size() != s.size()) {
			return size() < s.size();
		}

		auto it1=begin(), it2=s.begin();

		while (it1 != end() && it2 != s.end()) {
			if (*it1 < *it2) {
				return true;
			}
			else if (*it1 > *it2) {
				return false;
			}

			it1++;
			it2++;
		};

		return false;
	}

	// Insert new value
	// unique = true => values must be unique (no duplicate values allowed)
	_SET& insert(const value_type& value, bool unique = false) noexcept {
		if (!unique || this->find(value) == _set.end()) {
			auto it = begin();

			while (it != end() && value_compare()(*it, value)) {
				it++;
			}

			_set.insert(it, value);
		}

		return *this;
	}

	_SET& insert(value_type&& value, bool unique = false) noexcept {
		if (!unique || find(value) == _set.end()) {
			auto it = begin();

			while (it != end() && value_compare()(*it, value)) {
				it++;
			}

			_set.insert(it, std::move(value));
		}

		return *this;
	}

	_SET operator+(const value_type& value) const noexcept {
		return _SET{*this}+_SET{value};
	}

	_SET operator+(value_type&& value) const noexcept {
		return _SET{*this}+_SET{std::move(value)};
	}

	_SET operator+(const _SET& s) const noexcept {
		_SET ret{*this};

		for (const auto& value : s) {
			ret.insert(value);
		}

		return ret;
	}

	_SET operator+(_SET&& s) const noexcept {
		_SET ret{*this};

		for (auto& value : s) {
			ret.insert(std::move(value));
		}

		return ret;
	}

	_SET& operator+=(const value_type& value) noexcept {
		insert(value);
		return *this;
	}

	_SET& operator+=(value_type&& value) noexcept {
		insert(std::move(value));
		return *this;
	}

	_SET& operator+=(const _SET& s) noexcept {
		for (const auto& value : s) {
			insert(value);
		}

		return *this;
	}

	_SET& operator+=(_SET&& s) noexcept {
		for (auto& value : s) {
			insert(std::move(value));
		}

		return *this;
	}

	_SET operator-(const value_type& value) const noexcept {
		_SET ret{*this};
		ret.erase(value);
		return ret;
	}

	_SET operator-(const _SET& s) const noexcept {
		_SET ret{*this};
		for (const auto& v : s) {
			ret.erase(v);
		}
		return ret;
	}

	_SET& operator-=(const value_type& value) noexcept {
		_set.erase(value);
		return *this;
	}

	_SET& operator-=(const _SET& s) noexcept {
		for (const auto& value : s) {
			_set.erase(value);
		}
		return *this;
	}

	Set<_SET> operator*(const _SET& other) noexcept {
		Set<_SET> ret;

		if (!this->empty() && !other.empty()) {
			for (const auto& v1 : _set) {
				for (const auto& v2 : other) {
					ret.insert(_SET{v1,v2});
				}
			}
		}

		return ret;
	}

	_SET intersectionWith(const _SET& s) noexcept {
		_SET s1, s2;

		if (s.size() <= size()) {
			s1 = s;
			s2 = *this;
		}
		else {
			s1 = *this;
			s2 = s;
		}

		for (const auto &v : s1) {
			if (!s2.contains(v)) s1.erase(v);
		}

		return s1;
	}

	bool contains(const value_type& value) const noexcept {
		return contains(_SET{value});
	}

	bool contains(const _SET& s, bool strict = false) const noexcept {
		for (const auto& value : s) {
			bool found{false};

			for (const auto& v : _set) {
				if (v == value) {
					found = true;
					break;
				}
			}

			if (!found) {
				return false;
			}
		}

		return strict ? (s.size() != size()) : true;
	}

	template<class InputIterator>
	void insert(InputIterator first, InputIterator last) {
		for (auto it = first; it != last; it++) {
			_set.insert(*it);
		}
	};

	iterator erase(const_iterator position) {
		return _set.erase(position);
	}

	iterator erase(const_iterator first, const_iterator last) {
		return _set.erase(first,last);
	}

	size_t erase(const value_type& value, bool all = false) {
		size_t ret{};

		if (!all) {
			auto it = find(value);

			if (it != end()) {
				erase(it);
				ret++;
			}

			return ret;
		}

		for (auto it = _set.begin(); it != _set.end(); it++) {
			if (*it == value) {
				_set.erase(it);
				++ret;
			}
		}

		return ret;
	}

	_SET& unique() noexcept {
		for (auto i = begin(); i != end(); i++) {
			auto j = i+1;

			while (j != end()) {
				if (*i == *j) {
					_set.erase(j);
				}
				else {
					j++;
				}
			}
		}

		return *this;
	}

private:
	std::vector<value_type,allocator_type> _set;
};

#endif /* defined(_SET_HPP) */
<|endoftext|>