<commit_before>// Copyright (c) 2020 by Apex.AI Inc. All rights reserved.
//
// 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 "test.hpp"

#include "iceoryx_utils/concurrent/resizeable_lockfree_queue.hpp"
using namespace ::testing;

//*********************************************************
// Test the added functionaility of ResizeableLockFreeQueue
// to change the capacity (setCapacity).
// The remaining functionality is identical to LockFreeQueue
// and tested in test_lockfree_queue.cpp (as a typed test).
//*********************************************************

namespace
{
// use a non-POD type for testing (just a boxed version of int)
struct Integer
{
    Integer(uint64_t value = 0)
        : value(value)
    {
    }

    uint64_t value{0};

    // so that it behaves like an int for comparison purposes
    operator uint64_t() const
    {
        return value;
    }
};

template <typename T>
class ResizeableLockFreeQueueTest : public ::testing::Test
{
  protected:
    ResizeableLockFreeQueueTest()
    {
    }

    ~ResizeableLockFreeQueueTest()
    {
    }

    void SetUp()
    {
    }

    void TearDown()
    {
    }

    void fillQueue(int start = 0)
    {
        uint64_t element{start};
        for (uint64_t i = 0; i < queue.capacity(); ++i)
        {
            queue.tryPush(element);
            element++;
        }
    }

    using Queue = T;
    Queue queue;
};

template <size_t Capacity>
using IntegerQueue = iox::concurrent::ResizeableLockFreeQueue<Integer, Capacity>;

template <size_t Capacity>
using IntQueue = iox::concurrent::ResizeableLockFreeQueue<uint64_t, Capacity>;

typedef ::testing::Types<IntegerQueue<1>, IntegerQueue<10>, IntQueue<10>> TestQueues;

/// we require TYPED_TEST since we support gtest 1.8 for our safety targets
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
TYPED_TEST_CASE(ResizeableLockFreeQueueTest, TestQueues);
#pragma GCC diagnostic pop

TEST(ResizeableLockFreeQueueTest, maxCapacityIsConsistent)
{
    using Queue = IntegerQueue<37>;
    EXPECT_EQ(Queue::maxCapacity(), 37);
}

TYPED_TEST(ResizeableLockFreeQueueTest, initialCapacityIsMaximalbyDefault)
{
    using Queue = typename TestFixture::Queue;
    auto& q = this->queue;
    EXPECT_EQ(q.capacity(), q.maxCapacity());
    EXPECT_EQ(q.capacity(), Queue::maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithMaxCapacity)
{
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    typename TestFixture::Queue q(MAX_CAP);
    EXPECT_EQ(q.capacity(), q.maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithMoreThanMaxCapacitySaturatesAtMaxCapacity)
{
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    typename TestFixture::Queue q(MAX_CAP + 1);
    EXPECT_EQ(q.capacity(), q.maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithNoCapacity)
{
    typename TestFixture::Queue q(0);
    EXPECT_EQ(q.capacity(), 0);
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithHalfOfMaxCapacity)
{
    constexpr auto cap = TestFixture::Queue::MAX_CAPACITY / 2;
    typename TestFixture::Queue q(cap);
    EXPECT_EQ(q.capacity(), cap);
}

TYPED_TEST(ResizeableLockFreeQueueTest, decreaseCapacityToZeroOneByOne)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;

    auto i = MAX_CAP;
    while (i > 0)
    {
        EXPECT_TRUE(q.setCapacity(--i));
        ASSERT_EQ(q.capacity(), i);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, increaseToMaxCapacity)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_EQ(q.capacity(), 0);
    EXPECT_TRUE(q.setCapacity(MAX_CAP));
    EXPECT_EQ(q.capacity(), MAX_CAP);
}

TYPED_TEST(ResizeableLockFreeQueueTest, increaseToMaxCapacityOneByOne)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_EQ(q.capacity(), 0);

    for (uint64_t i = 0; i < MAX_CAP;)
    {
        EXPECT_TRUE(q.setCapacity(++i));
        ASSERT_EQ(q.capacity(), i);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToZero)
{
    auto& q = this->queue;
    EXPECT_TRUE(q.setCapacity(0));
    EXPECT_EQ(q.capacity(), 0);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToOne)
{
    auto& q = this->queue;
    EXPECT_TRUE(q.setCapacity(1));
    EXPECT_EQ(q.capacity(), 1);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToMaxCapacity)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_TRUE(q.setCapacity(MAX_CAP));
    EXPECT_EQ(q.capacity(), MAX_CAP);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToHalfOfMaxCapacityAndFillIt)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t newCap = MAX_CAP / 2;
    EXPECT_EQ(q.setCapacity(newCap), true);
    EXPECT_EQ(q.capacity(), newCap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), newCap);
    EXPECT_EQ(q.size(), newCap);
    EXPECT_EQ(element, newCap + 1);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityFromHalfOfMaxCapacityToMaxCapacity)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t cap = MAX_CAP / 2;
    EXPECT_EQ(q.setCapacity(cap), true);
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);
    EXPECT_EQ(element, cap + 1);

    EXPECT_EQ(q.setCapacity(MAX_CAP), true);
    EXPECT_EQ(q.capacity(), MAX_CAP);
    EXPECT_EQ(q.size(), cap);

    --element; // compensate the last failed tryPush
    while (q.tryPush(element++))
        ;

    // we want to find all elements we pushed
    for (element = 0; element < MAX_CAP; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityOfFullQueueToHalfOfMaxCapacity)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t cap = MAX_CAP / 2;

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;
    EXPECT_EQ(q.capacity(), MAX_CAP);
    EXPECT_EQ(q.size(), MAX_CAP);

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    if (cap == 0)
    {
        return;
    }
    // the least recent values are removed due to the capacity being decreased
    for (element = cap; element < MAX_CAP; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

// note this is one of the most general cases and necessary to test:
// decreasing the capacity starting with a partially filled queue and checking whether the last values
// remain (and the others are removed)
TYPED_TEST(ResizeableLockFreeQueueTest, DecreaseCapacityOfAPartiallyFilledQueue)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    iox::cxx::vector<int, MAX_CAP> removedElements;

    uint64_t cap = MAX_CAP / 2;

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    auto cap2 = cap + MAX_CAP / 4; // roughly 3 quarters of max (integer division)
    EXPECT_TRUE(q.setCapacity(cap2));

    EXPECT_EQ(q.capacity(), cap2);
    EXPECT_EQ(q.size(), cap);

    auto cap3 = cap2 - cap; // roughly a quarter of max

    EXPECT_TRUE(q.setCapacity(cap3, removedElements));
    EXPECT_EQ(q.capacity(), cap3);
    EXPECT_EQ(q.size(), cap3);

    // cap3 elements remain, the first cap - cap3 elements are removed

    // were the least recent elements removed?
    EXPECT_EQ(removedElements.size(), cap - cap3);
    element = 0;
    for (auto& removedElement : removedElements)
    {
        EXPECT_EQ(removedElement, element++);
    }

    // are the remaining elements correct? (i.e. we did not remove too many elements)
    for (element = cap - cap3; element < cap; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }

    // refill to verify the capacity can really be used

    element = 0;
    while (q.tryPush(element++))
        ;

    for (element = 0; element < cap3; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, DecreaseCapacityOfAPartiallyFilledQueueWithRemoveHandler)
{
    using element_t = typename TestFixture::Queue::element_t;
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;

    iox::cxx::vector<int, MAX_CAP> removedElements;
    auto removeHandler = [&](const element_t& value) { removedElements.push_back(std::move(value)); };

    uint64_t cap = MAX_CAP / 2;

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    auto cap2 = cap + MAX_CAP / 4; // roughly 3 quarters of max (integer division)
    EXPECT_TRUE(q.setCapacity(cap2));

    EXPECT_EQ(q.capacity(), cap2);
    EXPECT_EQ(q.size(), cap);

    auto cap3 = cap2 - cap; // roughly a quarter of max

    EXPECT_TRUE(q.setCapacity(cap3, removeHandler));
    EXPECT_EQ(q.capacity(), cap3);
    EXPECT_EQ(q.size(), cap3);

    // cap3 elements remain, the first cap - cap3 elements are removed

    // were the least recent elements removed?
    EXPECT_EQ(removedElements.size(), cap - cap3);
    element = 0;
    for (auto& removedElement : removedElements)
    {
        EXPECT_EQ(removedElement, element++);
    }

    // are the remaining elements correct? (i.e. we did not remove too many elements)
    for (element = cap - cap3; element < cap; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }

    // refill to verify the capacity can really be used

    element = 0;
    while (q.tryPush(element++))
        ;

    for (element = 0; element < cap3; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

} // namespace<commit_msg>iox-#216 fix narrowing conversion<commit_after>// Copyright (c) 2020 by Apex.AI Inc. All rights reserved.
//
// 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 "test.hpp"

#include "iceoryx_utils/concurrent/resizeable_lockfree_queue.hpp"
using namespace ::testing;

//*********************************************************
// Test the added functionaility of ResizeableLockFreeQueue
// to change the capacity (setCapacity).
// The remaining functionality is identical to LockFreeQueue
// and tested in test_lockfree_queue.cpp (as a typed test).
//*********************************************************

namespace
{
// use a non-POD type for testing (just a boxed version of int)
struct Integer
{
    Integer(uint64_t value = 0)
        : value(value)
    {
    }

    uint64_t value{0};

    // so that it behaves like an int for comparison purposes
    operator uint64_t() const
    {
        return value;
    }
};

template <typename T>
class ResizeableLockFreeQueueTest : public ::testing::Test
{
  protected:
    ResizeableLockFreeQueueTest()
    {
    }

    ~ResizeableLockFreeQueueTest()
    {
    }

    void SetUp()
    {
    }

    void TearDown()
    {
    }

    void fillQueue(uint64_t start = 0)
    {
        uint64_t element{start};
        for (uint64_t i = 0; i < queue.capacity(); ++i)
        {
            queue.tryPush(element);
            element++;
        }
    }

    using Queue = T;
    Queue queue;
};

template <size_t Capacity>
using IntegerQueue = iox::concurrent::ResizeableLockFreeQueue<Integer, Capacity>;

template <size_t Capacity>
using IntQueue = iox::concurrent::ResizeableLockFreeQueue<uint64_t, Capacity>;

typedef ::testing::Types<IntegerQueue<1>, IntegerQueue<10>, IntQueue<10>> TestQueues;

/// we require TYPED_TEST since we support gtest 1.8 for our safety targets
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
TYPED_TEST_CASE(ResizeableLockFreeQueueTest, TestQueues);
#pragma GCC diagnostic pop

TEST(ResizeableLockFreeQueueTest, maxCapacityIsConsistent)
{
    using Queue = IntegerQueue<37>;
    EXPECT_EQ(Queue::maxCapacity(), 37);
}

TYPED_TEST(ResizeableLockFreeQueueTest, initialCapacityIsMaximalbyDefault)
{
    using Queue = typename TestFixture::Queue;
    auto& q = this->queue;
    EXPECT_EQ(q.capacity(), q.maxCapacity());
    EXPECT_EQ(q.capacity(), Queue::maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithMaxCapacity)
{
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    typename TestFixture::Queue q(MAX_CAP);
    EXPECT_EQ(q.capacity(), q.maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithMoreThanMaxCapacitySaturatesAtMaxCapacity)
{
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    typename TestFixture::Queue q(MAX_CAP + 1);
    EXPECT_EQ(q.capacity(), q.maxCapacity());
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithNoCapacity)
{
    typename TestFixture::Queue q(0);
    EXPECT_EQ(q.capacity(), 0);
}

TYPED_TEST(ResizeableLockFreeQueueTest, constructWithHalfOfMaxCapacity)
{
    constexpr auto cap = TestFixture::Queue::MAX_CAPACITY / 2;
    typename TestFixture::Queue q(cap);
    EXPECT_EQ(q.capacity(), cap);
}

TYPED_TEST(ResizeableLockFreeQueueTest, decreaseCapacityToZeroOneByOne)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;

    auto i = MAX_CAP;
    while (i > 0)
    {
        EXPECT_TRUE(q.setCapacity(--i));
        ASSERT_EQ(q.capacity(), i);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, increaseToMaxCapacity)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_EQ(q.capacity(), 0);
    EXPECT_TRUE(q.setCapacity(MAX_CAP));
    EXPECT_EQ(q.capacity(), MAX_CAP);
}

TYPED_TEST(ResizeableLockFreeQueueTest, increaseToMaxCapacityOneByOne)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_EQ(q.capacity(), 0);

    for (uint64_t i = 0; i < MAX_CAP;)
    {
        EXPECT_TRUE(q.setCapacity(++i));
        ASSERT_EQ(q.capacity(), i);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToZero)
{
    auto& q = this->queue;
    EXPECT_TRUE(q.setCapacity(0));
    EXPECT_EQ(q.capacity(), 0);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToOne)
{
    auto& q = this->queue;
    EXPECT_TRUE(q.setCapacity(1));
    EXPECT_EQ(q.capacity(), 1);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToMaxCapacity)
{
    typename TestFixture::Queue q(0);
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    EXPECT_TRUE(q.setCapacity(MAX_CAP));
    EXPECT_EQ(q.capacity(), MAX_CAP);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityToHalfOfMaxCapacityAndFillIt)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t newCap = MAX_CAP / 2;
    EXPECT_EQ(q.setCapacity(newCap), true);
    EXPECT_EQ(q.capacity(), newCap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), newCap);
    EXPECT_EQ(q.size(), newCap);
    EXPECT_EQ(element, newCap + 1);
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityFromHalfOfMaxCapacityToMaxCapacity)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t cap = MAX_CAP / 2;
    EXPECT_EQ(q.setCapacity(cap), true);
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);
    EXPECT_EQ(element, cap + 1);

    EXPECT_EQ(q.setCapacity(MAX_CAP), true);
    EXPECT_EQ(q.capacity(), MAX_CAP);
    EXPECT_EQ(q.size(), cap);

    --element; // compensate the last failed tryPush
    while (q.tryPush(element++))
        ;

    // we want to find all elements we pushed
    for (element = 0; element < MAX_CAP; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, setCapacityOfFullQueueToHalfOfMaxCapacity)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    uint64_t cap = MAX_CAP / 2;

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;
    EXPECT_EQ(q.capacity(), MAX_CAP);
    EXPECT_EQ(q.size(), MAX_CAP);

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    if (cap == 0)
    {
        return;
    }
    // the least recent values are removed due to the capacity being decreased
    for (element = cap; element < MAX_CAP; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

// note this is one of the most general cases and necessary to test:
// decreasing the capacity starting with a partially filled queue and checking whether the last values
// remain (and the others are removed)
TYPED_TEST(ResizeableLockFreeQueueTest, DecreaseCapacityOfAPartiallyFilledQueue)
{
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;
    iox::cxx::vector<int, MAX_CAP> removedElements;

    uint64_t cap = MAX_CAP / 2;

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    auto cap2 = cap + MAX_CAP / 4; // roughly 3 quarters of max (integer division)
    EXPECT_TRUE(q.setCapacity(cap2));

    EXPECT_EQ(q.capacity(), cap2);
    EXPECT_EQ(q.size(), cap);

    auto cap3 = cap2 - cap; // roughly a quarter of max

    EXPECT_TRUE(q.setCapacity(cap3, removedElements));
    EXPECT_EQ(q.capacity(), cap3);
    EXPECT_EQ(q.size(), cap3);

    // cap3 elements remain, the first cap - cap3 elements are removed

    // were the least recent elements removed?
    EXPECT_EQ(removedElements.size(), cap - cap3);
    element = 0;
    for (auto& removedElement : removedElements)
    {
        EXPECT_EQ(removedElement, element++);
    }

    // are the remaining elements correct? (i.e. we did not remove too many elements)
    for (element = cap - cap3; element < cap; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }

    // refill to verify the capacity can really be used

    element = 0;
    while (q.tryPush(element++))
        ;

    for (element = 0; element < cap3; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

TYPED_TEST(ResizeableLockFreeQueueTest, DecreaseCapacityOfAPartiallyFilledQueueWithRemoveHandler)
{
    using element_t = typename TestFixture::Queue::element_t;
    auto& q = this->queue;
    constexpr auto MAX_CAP = TestFixture::Queue::MAX_CAPACITY;

    iox::cxx::vector<int, MAX_CAP> removedElements;
    auto removeHandler = [&](const element_t& value) { removedElements.push_back(std::move(value)); };

    uint64_t cap = MAX_CAP / 2;

    EXPECT_TRUE(q.setCapacity(cap));
    EXPECT_EQ(q.capacity(), cap);

    uint64_t element = 0;
    while (q.tryPush(element++))
        ;

    EXPECT_EQ(q.capacity(), cap);
    EXPECT_EQ(q.size(), cap);

    auto cap2 = cap + MAX_CAP / 4; // roughly 3 quarters of max (integer division)
    EXPECT_TRUE(q.setCapacity(cap2));

    EXPECT_EQ(q.capacity(), cap2);
    EXPECT_EQ(q.size(), cap);

    auto cap3 = cap2 - cap; // roughly a quarter of max

    EXPECT_TRUE(q.setCapacity(cap3, removeHandler));
    EXPECT_EQ(q.capacity(), cap3);
    EXPECT_EQ(q.size(), cap3);

    // cap3 elements remain, the first cap - cap3 elements are removed

    // were the least recent elements removed?
    EXPECT_EQ(removedElements.size(), cap - cap3);
    element = 0;
    for (auto& removedElement : removedElements)
    {
        EXPECT_EQ(removedElement, element++);
    }

    // are the remaining elements correct? (i.e. we did not remove too many elements)
    for (element = cap - cap3; element < cap; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }

    // refill to verify the capacity can really be used

    element = 0;
    while (q.tryPush(element++))
        ;

    for (element = 0; element < cap3; ++element)
    {
        auto result = q.pop();
        ASSERT_TRUE(result.has_value());
        EXPECT_EQ(result.value(), element);
    }
}

} // namespace<|endoftext|>


<commit_before>//---------------------------------------------------------------------------//
/*!
 * \file moab_transfer_1.cpp
 * \author Stuart R. Slattery
 * \brief Moab mesh transfer example.
 */
//---------------------------------------------------------------------------//

#include <iostream>
#include <vector>
#include <cmath>
#include <sstream>
#include <algorithm>
#include <cassert>
#include <ctime>
#include <cstdlib>

#include <DTK_MeshManager.hpp>
#include <DTK_MeshTools.hpp>
#include <DTK_SharedDomainMap.hpp>
#include <DTK_FieldEvaluator.hpp>
#include <DTK_MeshTraits.hpp>
#include <DTK_MeshTraitsFieldAdapter.hpp>

#include "MoabMesh.hpp"
#include "ArrayField.hpp"
#include "PeaksEvaluator.hpp"

#include <Teuchos_GlobalMPISession.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_CommHelpers.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_TypeTraits.hpp>
#include <Teuchos_VerboseObject.hpp>
#include <Teuchos_StandardCatchMacros.hpp>

//---------------------------------------------------------------------------//
// Example driver.
//---------------------------------------------------------------------------//
int main(int argc, char* argv[])
{
  // Typedefs.
  typedef MoabMesh::Container MeshType;

  // Setup communication.
  Teuchos::GlobalMPISession mpiSession(&argc,&argv);

  Teuchos::RCP<Teuchos::FancyOStream>
    out = Teuchos::VerboseObjectBase::getDefaultOStream();
  bool success = true;
  bool verbose = true;

  Teuchos::RCP<const Teuchos::Comm<int> > comm =
    Teuchos::DefaultComm<int>::getComm();

  try {

    // Setup source mesh. Partitioned in x.
    int mesh_dim = 2;
    MoabMesh source_mesh( comm, "tri_peaks.vtk", moab::MBTRI, 0 );
    Teuchos::ArrayRCP<Teuchos::RCP<MeshType> >
      source_blocks( 1, source_mesh.meshContainer() );
    Teuchos::RCP<DataTransferKit::MeshManager<MeshType> >
      source_mesh_manager =
      Teuchos::rcp( new DataTransferKit::MeshManager<MeshType>(
          source_blocks, comm, mesh_dim ) );

    // Setup target coordinate field. Partitioned in y.
    MoabMesh target_mesh( comm, "quad_mesh.vtk", moab::MBQUAD, 1 );
    Teuchos::RCP<DataTransferKit::FieldManager<MeshType> >
      target_coords_manager =
      Teuchos::rcp( new DataTransferKit::FieldManager<MeshType>(
          target_mesh.meshContainer(), comm ) );

    // Create a peaks function evaluator.
    Teuchos::RCP<DataTransferKit::FieldEvaluator<MeshType::global_ordinal_type,
      ArrayField> >
      peaks_evaluator = Teuchos::rcp( new PeaksEvaluator( source_mesh ) );

    // Create data target.
    int num_targets = DataTransferKit::MeshTools<MeshType>::numVertices(
      *(target_mesh.meshContainer()) );
    Teuchos::RCP<ArrayField> data_target = Teuchos::rcp(
      new ArrayField( num_targets, 1 ) );
    Teuchos::RCP<DataTransferKit::FieldManager<ArrayField> >
      target_space_manager =
      Teuchos::rcp( new DataTransferKit::FieldManager<ArrayField>(
          data_target, comm ) );

    // Construct shared domain map.
    DataTransferKit::SharedDomainMap<MeshType,MeshType>
      shared_domain_map( comm, mesh_dim );

    // Call setup. This creates the mapping.
    shared_domain_map.setup( source_mesh_manager, target_coords_manager );

    // Apply the map. This does the field evaluation and moves the data.
    shared_domain_map.apply( peaks_evaluator, target_space_manager );

    // Set the data on the target mesh and write.
    target_mesh.tag( *data_target );
    target_mesh.write( "quad_result.vtk" );

  }
  TEUCHOS_STANDARD_CATCH_STATEMENTS(verbose, std::cerr, success)

    if (verbose) {
      if(success)  *out << "\nCongratulations! All of the tests checked out!\n";
      else         *out << "\nOh no! At least one of the tests failed!\n";
    }

  return ( success ? 0 : 1 );

}

//---------------------------------------------------------------------------//
// end scaling_study_1.cpp
//---------------------------------------------------------------------------//

<commit_msg>Ading usage of Teuchos::CommandLineProcessor (VRI Kanban #2852)<commit_after>//---------------------------------------------------------------------------//
/*!
 * \file moab_transfer_1.cpp
 * \author Stuart R. Slattery
 * \brief Moab mesh transfer example.
 */
//---------------------------------------------------------------------------//

#include <iostream>
#include <vector>
#include <cmath>
#include <sstream>
#include <algorithm>
#include <cassert>
#include <ctime>
#include <cstdlib>

#include <DTK_MeshManager.hpp>
#include <DTK_MeshTools.hpp>
#include <DTK_SharedDomainMap.hpp>
#include <DTK_FieldEvaluator.hpp>
#include <DTK_MeshTraits.hpp>
#include <DTK_MeshTraitsFieldAdapter.hpp>

#include "MoabMesh.hpp"
#include "ArrayField.hpp"
#include "PeaksEvaluator.hpp"

#include <Teuchos_GlobalMPISession.hpp>
#include "Teuchos_CommandLineProcessor.hpp"
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_CommHelpers.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_TypeTraits.hpp>
#include <Teuchos_VerboseObject.hpp>
#include <Teuchos_StandardCatchMacros.hpp>

//---------------------------------------------------------------------------//
// Example driver.
//---------------------------------------------------------------------------//
int main(int argc, char* argv[])
{
  // Typedefs.
  typedef MoabMesh::Container MeshType;
  using Teuchos::CommandLineProcessor;

  // Setup communication.
  Teuchos::GlobalMPISession mpiSession(&argc,&argv);

  Teuchos::RCP<Teuchos::FancyOStream>
    out = Teuchos::VerboseObjectBase::getDefaultOStream();
  bool success = true;
  bool verbose = true;

  Teuchos::RCP<const Teuchos::Comm<int> > comm =
    Teuchos::DefaultComm<int>::getComm();

  try {

    // Read command-line options
    CommandLineProcessor  clp(false); // Don't throw exceptions
    CommandLineProcessor::EParseCommandLineReturn parse_return
      = clp.parse(argc, argv);
    if( parse_return != CommandLineProcessor::PARSE_SUCCESSFUL )
    { return parse_return; }

    // Setup source mesh. Partitioned in x.
    int mesh_dim = 2;
    MoabMesh source_mesh( comm, "tri_peaks.vtk", moab::MBTRI, 0 );
    Teuchos::ArrayRCP<Teuchos::RCP<MeshType> >
      source_blocks( 1, source_mesh.meshContainer() );
    Teuchos::RCP<DataTransferKit::MeshManager<MeshType> >
      source_mesh_manager =
      Teuchos::rcp( new DataTransferKit::MeshManager<MeshType>(
          source_blocks, comm, mesh_dim ) );

    // Setup target coordinate field. Partitioned in y.
    MoabMesh target_mesh( comm, "quad_mesh.vtk", moab::MBQUAD, 1 );
    Teuchos::RCP<DataTransferKit::FieldManager<MeshType> >
      target_coords_manager =
      Teuchos::rcp( new DataTransferKit::FieldManager<MeshType>(
          target_mesh.meshContainer(), comm ) );

    // Create a peaks function evaluator.
    Teuchos::RCP<DataTransferKit::FieldEvaluator<MeshType::global_ordinal_type,
      ArrayField> >
      peaks_evaluator = Teuchos::rcp( new PeaksEvaluator( source_mesh ) );

    // Create data target.
    int num_targets = DataTransferKit::MeshTools<MeshType>::numVertices(
      *(target_mesh.meshContainer()) );
    Teuchos::RCP<ArrayField> data_target = Teuchos::rcp(
      new ArrayField( num_targets, 1 ) );
    Teuchos::RCP<DataTransferKit::FieldManager<ArrayField> >
      target_space_manager =
      Teuchos::rcp( new DataTransferKit::FieldManager<ArrayField>(
          data_target, comm ) );

    // Construct shared domain map.
    DataTransferKit::SharedDomainMap<MeshType,MeshType>
      shared_domain_map( comm, mesh_dim );

    // Call setup. This creates the mapping.
    shared_domain_map.setup( source_mesh_manager, target_coords_manager );

    // Apply the map. This does the field evaluation and moves the data.
    shared_domain_map.apply( peaks_evaluator, target_space_manager );

    // Set the data on the target mesh and write.
    target_mesh.tag( *data_target );
    target_mesh.write( "quad_result.vtk" );

  }
  TEUCHOS_STANDARD_CATCH_STATEMENTS(verbose, std::cerr, success)

    if (verbose) {
      if(success)  *out << "\nCongratulations! All of the tests checked out!\n";
      else         *out << "\nOh no! At least one of the tests failed!\n";
    }

  return ( success ? 0 : 1 );

}

//---------------------------------------------------------------------------//
// end scaling_study_1.cpp
//---------------------------------------------------------------------------//

<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: SalGtkFilePicker.hxx,v $
 *
 *  $Revision: 1.8 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-20 00:13:47 $
 *
 *  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 _SALGTKFILEPICKER_HXX_
#define _SALGTKFILEPICKER_HXX_

//_______________________________________________________________________________________________________________________
//  includes of other projects
//_______________________________________________________________________________________________________________________

#ifndef _CPPUHELPER_COMPBASE9_HXX_
#include <cppuhelper/compbase9.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XINITIALIZATION_HPP_
#include <com/sun/star/lang/XInitialization.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPICKERNOTIFIER_HPP_
#include <com/sun/star/ui/dialogs/XFilePickerNotifier.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILTERMANAGER_HPP_
#include <com/sun/star/ui/dialogs/XFilterManager.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILTERGROUPMANAGER_HPP_
#include <com/sun/star/ui/dialogs/XFilterGroupManager.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPICKERCONTROLACCESS_HPP_
#include <com/sun/star/ui/dialogs/XFilePickerControlAccess.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPREVIEW_HPP_
#include <com/sun/star/ui/dialogs/XFilePreview.hpp>
#endif

#ifndef _COM_SUN_STAR_BEANS_STRINGPAIR_HPP_
#include <com/sun/star/beans/StringPair.hpp>
#endif

#ifndef _SALGTKPICKER_HXX_
#include "SalGtkPicker.hxx"
#endif

#include <memory>

#ifndef _RTL_USTRING_H_
#include <rtl/ustring.hxx>
#endif

#include <list>

//----------------------------------------------------------
// Implementation class for the XFilePicker Interface
//----------------------------------------------------------

//----------------------------------------------------------
// forward declarations
//----------------------------------------------------------

using namespace rtl;

struct FilterEntry;
struct ElementEntry_Impl;

typedef ::std::list < FilterEntry >     FilterList;
typedef ::std::list < ElementEntry_Impl >   ElementList;
typedef ::com::sun::star::beans::StringPair UnoFilterEntry;
typedef ::com::sun::star::uno::Sequence< UnoFilterEntry >   UnoFilterList;  // can be transported more effectively

//----------------------------------------------------------
// class declaration
//----------------------------------------------------------

class SalGtkFilePicker :
        public SalGtkPicker,
    public cppu::WeakComponentImplHelper9<
        ::com::sun::star::ui::dialogs::XFilterManager,
        ::com::sun::star::ui::dialogs::XFilterGroupManager,
        ::com::sun::star::ui::dialogs::XFilePickerControlAccess,
        ::com::sun::star::ui::dialogs::XFilePickerNotifier,
        ::com::sun::star::ui::dialogs::XFilePreview,
    ::com::sun::star::lang::XInitialization,
        ::com::sun::star::util::XCancellable,
    ::com::sun::star::lang::XEventListener,
    ::com::sun::star::lang::XServiceInfo >
{
    public:

        // constructor
        SalGtkFilePicker( const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& xServiceMgr );

        //------------------------------------------------------------------------------------
        // XFilePickerNotifier
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL addFilePickerListener( const ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >& xListener )
            throw( ::com::sun::star::uno::RuntimeException );
        virtual void SAL_CALL removeFilePickerListener( const ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >& xListener )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XExecutableDialog functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setTitle( const ::rtl::OUString& aTitle )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual sal_Int16 SAL_CALL execute(  )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilePicker functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setMultiSelectionMode( sal_Bool bMode )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setDefaultName( const ::rtl::OUString& aName )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setDisplayDirectory( const ::rtl::OUString& aDirectory )
            throw( com::sun::star::lang::IllegalArgumentException,
                ::com::sun::star::uno::RuntimeException );

        virtual ::rtl::OUString SAL_CALL getDisplayDirectory(  )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getFiles(  )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilterManager functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL appendFilter( const ::rtl::OUString& aTitle, const ::rtl::OUString& aFilter )
            throw( ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setCurrentFilter( const ::rtl::OUString& aTitle )
            throw( ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException );

        virtual ::rtl::OUString SAL_CALL getCurrentFilter(  )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilterGroupManager functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL appendFilterGroup( const ::rtl::OUString& sGroupTitle, const ::com::sun::star::uno::Sequence< ::com::sun::star::beans::StringPair >& aFilters )
                throw (::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException);

        //------------------------------------------------------------------------------------
        // XFilePickerControlAccess functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setValue( sal_Int16 nControlId, sal_Int16 nControlAction, const ::com::sun::star::uno::Any& aValue )
                throw (::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Any SAL_CALL getValue( sal_Int16 aControlId, sal_Int16 aControlAction )
                throw (::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL enableControl( sal_Int16 nControlId, sal_Bool bEnable )
            throw(::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setLabel( sal_Int16 nControlId, const ::rtl::OUString& aLabel )
            throw (::com::sun::star::uno::RuntimeException);

        virtual ::rtl::OUString SAL_CALL getLabel( sal_Int16 nControlId )
            throw (::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XFilePreview
        //------------------------------------------------

        virtual ::com::sun::star::uno::Sequence< sal_Int16 > SAL_CALL getSupportedImageFormats(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getTargetColorDepth(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getAvailableWidth(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getAvailableHeight(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL setImage( sal_Int16 aImageFormat, const ::com::sun::star::uno::Any& aImage )
            throw (::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL setShowState( sal_Bool bShowState )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL getShowState(  )
            throw (::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XInitialization
        //------------------------------------------------

        virtual void SAL_CALL initialize( const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& aArguments )
            throw(::com::sun::star::uno::Exception, ::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XCancellable
        //------------------------------------------------

        virtual void SAL_CALL cancel( )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------
        // XEventListener
        //------------------------------------------------

        using cppu::WeakComponentImplHelperBase::disposing;
        virtual void SAL_CALL disposing( const ::com::sun::star::lang::EventObject& aEvent )
            throw(::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XServiceInfo
        //------------------------------------------------

        virtual ::rtl::OUString SAL_CALL getImplementationName(  )
            throw(::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
            throw(::com::sun::star::uno::RuntimeException);

        //------------------------------------------------------------------------------------
        // FilePicker Event functions
        //------------------------------------------------------------------------------------

        void SAL_CALL fileSelectionChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        void SAL_CALL directoryChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        rtl::OUString SAL_CALL helpRequested( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent ) const;
        void SAL_CALL controlStateChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        void SAL_CALL dialogSizeChanged( );

    private:
        // prevent copy and assignment
        SalGtkFilePicker( const SalGtkFilePicker& );
        SalGtkFilePicker& operator=( const SalGtkFilePicker& );

        sal_Bool FilterNameExists( const ::rtl::OUString& rTitle );
        sal_Bool FilterNameExists( const UnoFilterList& _rGroupedFilters );

        void ensureFilterList( const ::rtl::OUString& _rInitialCurrentFilter );

        // to instanciate own services
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory > m_xServiceMgr;

    private:
        ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >
            m_xListener;
        FilterList *m_pFilterList;
        GtkWidget  *m_pVBox;

        GtkWidget  *m_pFilterExpander;
        GtkWidget  *m_pFilterView;
        GtkListStore *m_pFilterStore;

        enum {
            AUTOEXTENSION,
            PASSWORD,
            FILTEROPTIONS,
            READONLY,
            LINK,
            PREVIEW,
            SELECTION,
            TOGGLE_LAST
              };

        GtkWidget  *m_pToggles[ TOGGLE_LAST ];

        bool mbToggleVisibility[TOGGLE_LAST];
        bool mbToggleChecked[TOGGLE_LAST];

        static const rtl::OString m_ToggleLabels[TOGGLE_LAST];

        enum {
            PLAY,
            BUTTON_LAST };

        GtkWidget  *m_pButtons[ BUTTON_LAST ];

        enum {
            VERSION,
            TEMPLATE,
            IMAGE_TEMPLATE,
            LIST_LAST
              };

        GtkWidget *m_pHBoxs[ LIST_LAST ];
        GtkWidget *m_pAligns[ LIST_LAST ];
        GtkWidget *m_pLists[ LIST_LAST ];
        GtkWidget *m_pListLabels[ LIST_LAST ];
        bool mbListVisibility[ LIST_LAST ];
        gulong mnHID_FolderChange;
        gulong mnHID_SelectionChange;

        ::rtl::OUString m_aCurrentFilter;

        bool bVersionWidthUnset;
        sal_Bool mbPreviewState;
        gulong mHID_Preview;
        GtkWidget* m_pPreview;
        sal_Int32 m_PreviewImageWidth;
        sal_Int32 m_PreviewImageHeight;

        GtkWidget  *getWidget( sal_Int16 nControlId, GType *pType = NULL);

        void SetCurFilter( const OUString& rFilter );
        void SetFilters();
        void UpdateFilterfromUI();

        void implChangeType( GtkTreeSelection *selection );
        int implAddFilter( const OUString& rFilter, const OUString& rType);
        int implAddFilterGroup( const OUString& rFilter,
                     const ::com::sun::star::uno::Sequence< ::com::sun::star::beans::StringPair>& _rFilters );
        void updateCurrentFilterFromName(const gchar* filtername);
        void unselect_type();
        void InitialMapping();

        void HandleSetListValue(GtkComboBox *pWidget, sal_Int16 nControlAction,
            const ::com::sun::star::uno::Any& rValue);
        ::com::sun::star::uno::Any HandleGetListValue(GtkComboBox *pWidget, sal_Int16 nControlAction) const;

        static void expander_changed_cb( GtkExpander *expander, SalGtkFilePicker *pobjFP );
        static void preview_toggled_cb (GtkObject *cb, SalGtkFilePicker *pobjFP);
        static void filter_changed_cb (GtkFileChooser *file_chooser, GParamSpec *pspec, SalGtkFilePicker *pobjFP);
        static void type_changed_cb( GtkTreeSelection *selection, SalGtkFilePicker *pobjFP );
        static void folder_changed_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void selection_changed_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void update_preview_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void dialog_mapped_cb(GtkWidget *widget, SalGtkFilePicker *pobjFP);
    public:
         virtual ~SalGtkFilePicker();

};
/* vi:set tabstop=4 shiftwidth=4 expandtab: */
#endif // _SALGTKFILEPICKER_HXX_
<commit_msg>INTEGRATION: CWS fpicker6 (1.8.18); FILE MERGED 2006/08/01 17:45:25 cmc 1.8.18.1: #i63263# multiselect support<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: SalGtkFilePicker.hxx,v $
 *
 *  $Revision: 1.9 $
 *
 *  last change: $Author: ihi $ $Date: 2006-08-03 13:20:16 $
 *
 *  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 _SALGTKFILEPICKER_HXX_
#define _SALGTKFILEPICKER_HXX_

//_______________________________________________________________________________________________________________________
//  includes of other projects
//_______________________________________________________________________________________________________________________

#ifndef _CPPUHELPER_COMPBASE10_HXX_
#include <cppuhelper/compbase10.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XINITIALIZATION_HPP_
#include <com/sun/star/lang/XInitialization.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPICKERNOTIFIER_HPP_
#include <com/sun/star/ui/dialogs/XFilePickerNotifier.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILTERMANAGER_HPP_
#include <com/sun/star/ui/dialogs/XFilterManager.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILTERGROUPMANAGER_HPP_
#include <com/sun/star/ui/dialogs/XFilterGroupManager.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPICKERCONTROLACCESS_HPP_
#include <com/sun/star/ui/dialogs/XFilePickerControlAccess.hpp>
#endif

#ifndef _COM_SUN_STAR_UI_DIALOGS_XFILEPREVIEW_HPP_
#include <com/sun/star/ui/dialogs/XFilePreview.hpp>
#endif

#ifndef _COM_SUN_STAR_BEANS_STRINGPAIR_HPP_
#include <com/sun/star/beans/StringPair.hpp>
#endif

#ifndef _SALGTKPICKER_HXX_
#include "SalGtkPicker.hxx"
#endif

#include <memory>

#ifndef _RTL_USTRING_H_
#include <rtl/ustring.hxx>
#endif

#include <list>

//----------------------------------------------------------
// Implementation class for the XFilePicker Interface
//----------------------------------------------------------

//----------------------------------------------------------
// forward declarations
//----------------------------------------------------------

using namespace rtl;

struct FilterEntry;
struct ElementEntry_Impl;

typedef ::std::list < FilterEntry >     FilterList;
typedef ::std::list < ElementEntry_Impl >   ElementList;
typedef ::com::sun::star::beans::StringPair UnoFilterEntry;
typedef ::com::sun::star::uno::Sequence< UnoFilterEntry >   UnoFilterList;  // can be transported more effectively

//----------------------------------------------------------
// class declaration
//----------------------------------------------------------

class SalGtkFilePicker :
    public SalGtkPicker,
    public cppu::WeakComponentImplHelper10<
        ::com::sun::star::ui::dialogs::XFilterManager,
        ::com::sun::star::ui::dialogs::XFilterGroupManager,
        ::com::sun::star::ui::dialogs::XFilePickerControlAccess,
        ::com::sun::star::ui::dialogs::XFilePickerNotifier,
        ::com::sun::star::ui::dialogs::XFilePreview,
        ::com::sun::star::ui::dialogs::XFilePickerWorkaround,
        ::com::sun::star::lang::XInitialization,
        ::com::sun::star::util::XCancellable,
        ::com::sun::star::lang::XEventListener,
        ::com::sun::star::lang::XServiceInfo >
{
    public:

        // constructor
        SalGtkFilePicker( const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& xServiceMgr );

        //------------------------------------------------------------------------------------
        // XFilePickerNotifier
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL addFilePickerListener( const ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >& xListener )
            throw( ::com::sun::star::uno::RuntimeException );
        virtual void SAL_CALL removeFilePickerListener( const ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >& xListener )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XExecutableDialog functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setTitle( const ::rtl::OUString& aTitle )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual sal_Int16 SAL_CALL execute(  )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilePicker functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setMultiSelectionMode( sal_Bool bMode )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setDefaultName( const ::rtl::OUString& aName )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setDisplayDirectory( const ::rtl::OUString& aDirectory )
            throw( com::sun::star::lang::IllegalArgumentException,
                ::com::sun::star::uno::RuntimeException );

        virtual ::rtl::OUString SAL_CALL getDisplayDirectory(  )
            throw( ::com::sun::star::uno::RuntimeException );

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getFiles()
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilePickerWorkaround functions
        //------------------------------------------------------------------------------------

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getFilesAsURIs()
                throw (::com::sun::star::uno::RuntimeException);

        //------------------------------------------------------------------------------------
        // XFilterManager functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL appendFilter( const ::rtl::OUString& aTitle, const ::rtl::OUString& aFilter )
            throw( ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setCurrentFilter( const ::rtl::OUString& aTitle )
            throw( ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException );

        virtual ::rtl::OUString SAL_CALL getCurrentFilter(  )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------------------------------------------
        // XFilterGroupManager functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL appendFilterGroup( const ::rtl::OUString& sGroupTitle, const ::com::sun::star::uno::Sequence< ::com::sun::star::beans::StringPair >& aFilters )
                throw (::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException);

        //------------------------------------------------------------------------------------
        // XFilePickerControlAccess functions
        //------------------------------------------------------------------------------------

        virtual void SAL_CALL setValue( sal_Int16 nControlId, sal_Int16 nControlAction, const ::com::sun::star::uno::Any& aValue )
                throw (::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Any SAL_CALL getValue( sal_Int16 aControlId, sal_Int16 aControlAction )
                throw (::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL enableControl( sal_Int16 nControlId, sal_Bool bEnable )
            throw(::com::sun::star::uno::RuntimeException );

        virtual void SAL_CALL setLabel( sal_Int16 nControlId, const ::rtl::OUString& aLabel )
            throw (::com::sun::star::uno::RuntimeException);

        virtual ::rtl::OUString SAL_CALL getLabel( sal_Int16 nControlId )
            throw (::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XFilePreview
        //------------------------------------------------

        virtual ::com::sun::star::uno::Sequence< sal_Int16 > SAL_CALL getSupportedImageFormats(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getTargetColorDepth(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getAvailableWidth(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Int32 SAL_CALL getAvailableHeight(  )
            throw (::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL setImage( sal_Int16 aImageFormat, const ::com::sun::star::uno::Any& aImage )
            throw (::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL setShowState( sal_Bool bShowState )
            throw (::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL getShowState(  )
            throw (::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XInitialization
        //------------------------------------------------

        virtual void SAL_CALL initialize( const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& aArguments )
            throw(::com::sun::star::uno::Exception, ::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XCancellable
        //------------------------------------------------

        virtual void SAL_CALL cancel( )
            throw( ::com::sun::star::uno::RuntimeException );

        //------------------------------------------------
        // XEventListener
        //------------------------------------------------

        using cppu::WeakComponentImplHelperBase::disposing;
        virtual void SAL_CALL disposing( const ::com::sun::star::lang::EventObject& aEvent )
            throw(::com::sun::star::uno::RuntimeException);

        //------------------------------------------------
        // XServiceInfo
        //------------------------------------------------

        virtual ::rtl::OUString SAL_CALL getImplementationName(  )
            throw(::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
            throw(::com::sun::star::uno::RuntimeException);

        //------------------------------------------------------------------------------------
        // FilePicker Event functions
        //------------------------------------------------------------------------------------

        void SAL_CALL fileSelectionChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        void SAL_CALL directoryChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        rtl::OUString SAL_CALL helpRequested( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent ) const;
        void SAL_CALL controlStateChanged( ::com::sun::star::ui::dialogs::FilePickerEvent aEvent );
        void SAL_CALL dialogSizeChanged( );

    private:
        // prevent copy and assignment
        SalGtkFilePicker( const SalGtkFilePicker& );
        SalGtkFilePicker& operator=( const SalGtkFilePicker& );

        sal_Bool FilterNameExists( const ::rtl::OUString& rTitle );
        sal_Bool FilterNameExists( const UnoFilterList& _rGroupedFilters );

        void ensureFilterList( const ::rtl::OUString& _rInitialCurrentFilter );

        // to instanciate own services
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory > m_xServiceMgr;

    private:
        ::com::sun::star::uno::Reference< ::com::sun::star::ui::dialogs::XFilePickerListener >
            m_xListener;
        FilterList *m_pFilterList;
        GtkWidget  *m_pVBox;

        GtkWidget  *m_pFilterExpander;
        GtkWidget  *m_pFilterView;
        GtkListStore *m_pFilterStore;

        enum {
            AUTOEXTENSION,
            PASSWORD,
            FILTEROPTIONS,
            READONLY,
            LINK,
            PREVIEW,
            SELECTION,
            TOGGLE_LAST
              };

        GtkWidget  *m_pToggles[ TOGGLE_LAST ];

        bool mbToggleVisibility[TOGGLE_LAST];
        bool mbToggleChecked[TOGGLE_LAST];

        static const rtl::OString m_ToggleLabels[TOGGLE_LAST];

        enum {
            PLAY,
            BUTTON_LAST };

        GtkWidget  *m_pButtons[ BUTTON_LAST ];

        enum {
            VERSION,
            TEMPLATE,
            IMAGE_TEMPLATE,
            LIST_LAST
              };

        GtkWidget *m_pHBoxs[ LIST_LAST ];
        GtkWidget *m_pAligns[ LIST_LAST ];
        GtkWidget *m_pLists[ LIST_LAST ];
        GtkWidget *m_pListLabels[ LIST_LAST ];
        bool mbListVisibility[ LIST_LAST ];
        gulong mnHID_FolderChange;
        gulong mnHID_SelectionChange;

        ::rtl::OUString m_aCurrentFilter;

        bool bVersionWidthUnset;
        sal_Bool mbPreviewState;
        gulong mHID_Preview;
        GtkWidget* m_pPreview;
        sal_Int32 m_PreviewImageWidth;
        sal_Int32 m_PreviewImageHeight;

        GtkWidget  *getWidget( sal_Int16 nControlId, GType *pType = NULL);

        void SetCurFilter( const OUString& rFilter );
        void SetFilters();
        void UpdateFilterfromUI();

        void implChangeType( GtkTreeSelection *selection );
        int implAddFilter( const OUString& rFilter, const OUString& rType);
        int implAddFilterGroup( const OUString& rFilter,
                     const ::com::sun::star::uno::Sequence< ::com::sun::star::beans::StringPair>& _rFilters );
        void updateCurrentFilterFromName(const gchar* filtername);
        void unselect_type();
        void InitialMapping();

        void HandleSetListValue(GtkComboBox *pWidget, sal_Int16 nControlAction,
            const ::com::sun::star::uno::Any& rValue);
        ::com::sun::star::uno::Any HandleGetListValue(GtkComboBox *pWidget, sal_Int16 nControlAction) const;

        static void expander_changed_cb( GtkExpander *expander, SalGtkFilePicker *pobjFP );
        static void preview_toggled_cb (GtkObject *cb, SalGtkFilePicker *pobjFP);
        static void filter_changed_cb (GtkFileChooser *file_chooser, GParamSpec *pspec, SalGtkFilePicker *pobjFP);
        static void type_changed_cb( GtkTreeSelection *selection, SalGtkFilePicker *pobjFP );
        static void folder_changed_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void selection_changed_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void update_preview_cb (GtkFileChooser *file_chooser, SalGtkFilePicker *pobjFP);
        static void dialog_mapped_cb(GtkWidget *widget, SalGtkFilePicker *pobjFP);
    public:
         virtual ~SalGtkFilePicker();

};
/* vi:set tabstop=4 shiftwidth=4 expandtab: */
#endif // _SALGTKFILEPICKER_HXX_
<|endoftext|>


<commit_before>
#include "AConfig.h"
#if(HAS_STD_PILOT)
#include "Device.h"
#include "Pin.h"
#include "Pilot.h"
#include "Timer.h"

Timer pilotTimer;
bool _headingHoldEnabled = false;
int  _headingHoldTarget = 0;
int hdg = 0;
int hdg_Error;
int raw_Left, raw_Right;
int left, right;  // motor outputs in microseconds, +/-500
int loop_Gain = 1;
int integral_Divisor = 100;
long hdg_Error_Integral = 0;
int tgt_Hdg = 0;
bool _depthHoldEnabled = false;
int _depthHoldTarget = 0;
int depth = 0;
int depth_Error = 0;
int raw_lift =0;
int lift = 0;
int target_depth;
int raw_yaw, yaw;

void Pilot::device_setup(){
  pilotTimer.reset();
  Serial.println(F("log:pilot setup complete;"));
}

void Pilot::device_loop(Command command){
//intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth);
    if( command.cmp("holdHeading_toggle")){
      if (_headingHoldEnabled) {
        _headingHoldEnabled = false;
        raw_Left = 0;
        raw_Right = 0;
        hdg_Error_Integral = 0;  // Reset error integrator
        tgt_Hdg = 0;  // 500 = system not in hdg hold

        int argsToSend[] = {1,00}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
        Serial.println(F("log:hold_disabled;"));

      } else {
        _headingHoldEnabled = true;
        if(command.args[0]==0){
          _headingHoldTarget = navdata::HDGD;
        } else {
          _headingHoldTarget = command.args[1];
        }
        tgt_Hdg = _headingHoldTarget;
        Serial.print(F("log:hold_enabled on="));
        Serial.print(tgt_Hdg);
        Serial.println(';');
        Serial.print(F("thdg:"));
        Serial.print(tgt_Hdg);
        Serial.println(';');
      }
    }

    if( command.cmp("holdDepth_toggle")){
      if (_depthHoldEnabled) {
        _depthHoldEnabled = false;
        raw_lift = 0;
        target_depth = 0;  // 500 = system not in hdg hold

        int argsToSend[] = {1,0}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);
        Serial.println(F("log:depth_hold_disabled;"));

      } else {
        _depthHoldEnabled = true;
        if(command.args[0]==0){
          _depthHoldTarget = navdata::DEAP*100;  //casting to cm
        } else {
          _depthHoldTarget = command.args[1];
        }
        target_depth = _depthHoldTarget;
        Serial.print(F("log:dhold_enabled on="));
        Serial.print(target_depth);
        Serial.println(';');
        Serial.print(F("tdpt:"));
        Serial.print(target_depth);
        Serial.println(';');
      }
    }

    if (pilotTimer.elapsed (50)) {

      // Autopilot Test #3 6 Jan 2014
      // Hold vehicle at arbitrary heading
      // Integer math; proportional control plus basic integrator
      // No hysteresis around 180 degree error

      // Check whether hold mode is on

      if (_depthHoldEnabled)
      {
        depth = navdata::DEAP*100;
        depth_Error = target_depth-depth;  //positive error = positive lift = go deaper.

        raw_lift = depth_Error * loop_Gain;
        lift = constrain(raw_lift, -50, 50);

        Serial.println(F("log:dhold pushing command;"));
        Serial.print(F("dp_er:"));
        Serial.print(depth_Error);
        Serial.println(';');
        int argsToSend[] = {1,lift}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);

      }

      if (_headingHoldEnabled)
      {

        // Code for hold mode here
        hdg = navdata::HDGD;

        // Calculate heading error

        hdg_Error = hdg - tgt_Hdg;

        if (hdg_Error > 180)
        {
        hdg_Error = hdg_Error - 360;
        }

        if (hdg_Error < -179)
        {
        hdg_Error = hdg_Error + 360;
        }

        // Run error accumulator (integrator)
        hdg_Error_Integral = hdg_Error_Integral + hdg_Error;

        // Calculator motor outputs
        raw_yaw = -1 * hdg_Error * loop_Gain;

        // raw_Left = raw_Left - (hdg_Error_Integral / integral_Divisor);
        // raw_Right = raw_Right + (hdg_Error_Integral / integral_Divisor);

        // Constrain and output to motors

        yaw = constrain(raw_yaw, -50, 50);
        Serial.println(F("log:hold pushing command;"));
        Serial.print(F("p_er:"));
        Serial.print(hdg_Error);
        Serial.println(';');

        int argsToSend[] = {1,yaw}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
      }

    }
}
#endif

<commit_msg>Revert "Revert "Implementing UI elements for depth and heading hold""<commit_after>
#include "AConfig.h"
#if(HAS_STD_PILOT)
#include "Device.h"
#include "Pin.h"
#include "Pilot.h"
#include "Timer.h"

Timer pilotTimer;
bool _headingHoldEnabled = false;
int  _headingHoldTarget = 0;
int hdg = 0;
int hdg_Error;
int raw_Left, raw_Right;
int left, right;  // motor outputs in microseconds, +/-500
int loop_Gain = 1;
int integral_Divisor = 100;
long hdg_Error_Integral = 0;
int tgt_Hdg = 0;
bool _depthHoldEnabled = false;
int _depthHoldTarget = 0;
int depth = 0;
int depth_Error = 0;
int raw_lift =0;
int lift = 0;
int target_depth;
int raw_yaw, yaw;

void Pilot::device_setup(){
  pilotTimer.reset();
  Serial.println(F("log:pilot setup complete;"));
}

void Pilot::device_loop(Command command){
//intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth);
    if( command.cmp("holdHeading_toggle")){
      if (_headingHoldEnabled) {
        _headingHoldEnabled = false;
        raw_Left = 0;
        raw_Right = 0;
        hdg_Error_Integral = 0;  // Reset error integrator
        tgt_Hdg = 0;  // 500 = system not in hdg hold

        int argsToSend[] = {1,00}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
        Serial.println(F("log:heading_hold_disabled;"));

      } else {
        _headingHoldEnabled = true;
        if(command.args[0]==0){
          _headingHoldTarget = navdata::HDGD;
        } else {
          _headingHoldTarget = command.args[1];
        }
        tgt_Hdg = _headingHoldTarget;
        Serial.print(F("log:heading_hold_enabled on="));
        Serial.print(tgt_Hdg);
        Serial.println(';');
      }
      Serial.print(F("targetHeading:"));
      Serial.print(tgt_Hdg);
      Serial.println(';');
    }

    if( command.cmp("holdDepth_toggle")){
      if (_depthHoldEnabled) {
        _depthHoldEnabled = false;
        raw_lift = 0;
        target_depth = -500;  // -500 = system not in hdg hold

        int argsToSend[] = {1,0}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);
        Serial.println(F("log:depth_hold_disabled;"));

      } else {
        _depthHoldEnabled = true;
        if(command.args[0]==0){
          _depthHoldTarget = navdata::DEAP*100;  //casting to cm
        } else {
          _depthHoldTarget = command.args[1];
        }
        target_depth = _depthHoldTarget;
        Serial.print(F("log:depth_hold_enabled on="));
        Serial.print(target_depth);
        Serial.println(';');
      }
      Serial.print(F("targetDepth:"));
      Serial.print(target_depth);
      Serial.println(';');
    }

    if (pilotTimer.elapsed (50)) {

      // Autopilot Test #3 6 Jan 2014
      // Hold vehicle at arbitrary heading
      // Integer math; proportional control plus basic integrator
      // No hysteresis around 180 degree error

      // Check whether hold mode is on

      if (_depthHoldEnabled)
      {
        depth = navdata::DEAP*100;
        depth_Error = target_depth-depth;  //positive error = positive lift = go deaper.

        raw_lift = depth_Error * loop_Gain;
        lift = constrain(raw_lift, -50, 50);

        Serial.println(F("log:dhold pushing command;"));
        Serial.print(F("dp_er:"));
        Serial.print(depth_Error);
        Serial.println(';');
        int argsToSend[] = {1,lift}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);

      }

      if (_headingHoldEnabled)
      {

        // Code for hold mode here
        hdg = navdata::HDGD;

        // Calculate heading error

        hdg_Error = hdg - tgt_Hdg;

        if (hdg_Error > 180)
        {
        hdg_Error = hdg_Error - 360;
        }

        if (hdg_Error < -179)
        {
        hdg_Error = hdg_Error + 360;
        }

        // Run error accumulator (integrator)
        hdg_Error_Integral = hdg_Error_Integral + hdg_Error;

        // Calculator motor outputs
        raw_yaw = -1 * hdg_Error * loop_Gain;

        // raw_Left = raw_Left - (hdg_Error_Integral / integral_Divisor);
        // raw_Right = raw_Right + (hdg_Error_Integral / integral_Divisor);

        // Constrain and output to motors

        yaw = constrain(raw_yaw, -50, 50);
        Serial.println(F("log:hold pushing command;"));
        Serial.print(F("p_er:"));
        Serial.print(hdg_Error);
        Serial.println(';');

        int argsToSend[] = {1,yaw}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
      }

    }
}
#endif

<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include <spatiotemporalexploration/ExecutionAction.h>
#include <strands_navigation_msgs/MonitoredNavigationAction.h>

#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"

#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h>

using namespace std;

int ptuMovementFinished = 0;

bool drawEmptyCells = false;
bool drawCells = true;
bool saveFlag = false;

ros::Publisher ptu_pub;
sensor_msgs::JointState ptu;
spatiotemporalexploration::AddView measure_srv;
spatiotemporalexploration::Visualize visualize_srv;

ros::ServiceClient *measure_client_ptr, *visualize_client_ptr;

typedef actionlib::SimpleActionServer<spatiotemporalexploration::ExecutionAction> Server;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> *ac_nav_ptr;

void movePtu(float pan,float tilt)
{
    ptuMovementFinished = 0;
    ptu.name[0] ="pan";
    ptu.name[1] ="tilt";
    ptu.position[0] = pan;
    ptu.position[1] = tilt;
    ptu.velocity[0] = ptu.velocity[1] = 1.0;
    ptu_pub.publish(ptu);
}

void ptuCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
    float pan,tilt;
    for (int i = 0;i<3;i++){
        if (msg->name[i] == "pan") pan = msg->position[i];
        if (msg->name[i] == "tilt") tilt = msg->position[i];
    }
    //printf("PTU: %.3f %.3f %i\n",pan,tilt,ptuMovementFinished);
    if (fabs(pan-ptu.position[0])<0.01 && fabs(tilt-ptu.position[1])<0.01) ptuMovementFinished++;
}

void execute(const spatiotemporalexploration::ExecutionGoalConstPtr& goal, Server* as)
{

    ROS_INFO("received new plan");
    as->acceptNewGoal();
    //as->setPreempted();

    strands_navigation_msgs::MonitoredNavigationGoal current_goal;
    current_goal.action_server = "move_base";
    current_goal.target_pose.header.frame_id = "map";

    spatiotemporalexploration::ExecutionFeedback feedback;

    sleep(1);

    int numPoints = 14;
    int point = 0;
    float pan[] =  { 0.00, 0.90, 1.80, 2.70, 2.70, 1.80, 0.90, 0.00,-0.90,-1.80,-2.70,-2.70,-1.80,-0.90,0.00};
    float tilt[] = { 0.50, 0.50, 0.50, 0.50,-0.30,-0.30,-0.30,-0.30,-0.30,-0.30,-0.30, 0.50, 0.50, 0.50,0.00};

    int n = (int) goal->locations.poses.size();

    ROS_INFO("received %d locations to visit in %f minutes", n, 0.0);

    for(int i = 0; i < n; i++)
    {

        char cr_goal[10];
        sprintf(cr_goal, "%d/%d", i, n);
        feedback.current_goal = cr_goal;
        feedback.time_remaining = 0;//TODO
        as->publishFeedback(feedback);

        current_goal.target_pose.pose = goal->locations.poses[i];
        ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
        ac_nav_ptr->sendGoal(current_goal);

//        ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
        ac_nav_ptr->waitForResult(ros::Duration(0.0));

        if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Monitored navigation: SUCCEEDED!");
            point = 0;
            movePtu(pan[point],tilt[point]);

            ros::spinOnce();
            while (ros::ok() && point < numPoints)
            {
                measure_srv.request.stamp = 0.0;
                if (ptuMovementFinished > 10)
                {
                    if(measure_client_ptr->call(measure_srv))
                    {
                        ROS_INFO("Measure added to grid!");
                    }
                    else
                    {
                        ROS_ERROR("Failed to call measure service");
                        exit(1);
                    }

                    point++;
                    movePtu(pan[point],tilt[point]);
                    ros::spinOnce();
                    usleep(500000);
                    if(drawCells){
                        visualize_srv.request.red = visualize_srv.request.blue = 0.0;
                        visualize_srv.request.green = visualize_srv.request.alpha = 1.0;
                        visualize_srv.request.minProbability = 0.9;
                        visualize_srv.request.maxProbability = 1.0;
                        visualize_srv.request.name = "occupied";
                        visualize_srv.request.type = 0;
                        visualize_client_ptr->call(visualize_srv);
                        ros::spinOnce();
                        usleep(100000);
                        if (drawEmptyCells){
                            visualize_srv.request.green = 0.0;
                            visualize_srv.request.red = 1.0;
                            visualize_srv.request.minProbability = 0.0;
                            visualize_srv.request.maxProbability = 0.1;
                            visualize_srv.request.alpha = 0.005;
                            visualize_srv.request.name = "free";
                            visualize_srv.request.type = 0;
                            visualize_client_ptr->call(visualize_srv);
                            ros::spinOnce();
                            usleep(100000);
                        }
                    }
                }
                ros::spinOnce();
            }
        }
        else if(ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::PREEMPTED || ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::ABORTED)
        {
            ROS_INFO("Move base failed, trying next goal in the plan...");
        }

        movePtu(0.0,0.0);
    }
}

int main(int argc,char *argv[])
{
    ros::init(argc, argv, "exploration_executioner");
    ros::NodeHandle n;

    ros::NodeHandle nh("~");

    Server server(n, "executioner", boost::bind(&execute, _1, &server), false);
    ROS_INFO("starting server...");
    server.start();

    actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> ac_nav("monitored_navigation",true);
    ac_nav_ptr = &ac_nav;
    ac_nav.waitForServer();

    //Subscribers
    ros::Subscriber ptu_sub = n.subscribe("/ptu/state", 10, ptuCallback);
    ptu.name.resize(3);
    ptu.position.resize(3);
    ptu.velocity.resize(3);
    ptu_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 10);

    //measure service client
    ros::ServiceClient measure_client = n.serviceClient<spatiotemporalexploration::AddView>("/fremenGrid/depth");
    measure_client_ptr = &measure_client;

    //vizualize client
    ros::ServiceClient visualize_client = n.serviceClient<spatiotemporalexploration::Visualize>("/fremenGrid/visualize");
    visualize_client_ptr = &visualize_client;

    //Dynamic Reconfigure
    dynamic_reconfigure::ReconfigureRequest srv_req;
    dynamic_reconfigure::ReconfigureResponse srv_resp;
    dynamic_reconfigure::DoubleParameter double_param;
    dynamic_reconfigure::Config conf;

    double_param.name = "yaw_goal_tolerance";
    double_param.value = 3.0;
    conf.doubles.push_back(double_param);

    srv_req.config = conf;

    ros::service::call("/move_base/DWAPlannerROS/set_parameters", srv_req, srv_resp);

    ROS_INFO("server started");

    ros::spin();

    return 0;
}
<commit_msg>ptu issue fixed<commit_after>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include <spatiotemporalexploration/ExecutionAction.h>
#include <strands_navigation_msgs/MonitoredNavigationAction.h>

#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"

#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h>

using namespace std;

int ptuMovementFinished = 0;

bool drawEmptyCells = false;
bool drawCells = true;
bool saveFlag = false;

ros::Publisher ptu_pub;
sensor_msgs::JointState ptu;
spatiotemporalexploration::AddView measure_srv;
spatiotemporalexploration::Visualize visualize_srv;

ros::ServiceClient *measure_client_ptr, *visualize_client_ptr;

typedef actionlib::SimpleActionServer<spatiotemporalexploration::ExecutionAction> Server;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> *ac_nav_ptr;

void movePtu(float pan,float tilt)
{
    ptuMovementFinished = 0;
    ptu.name[0] ="pan";
    ptu.name[1] ="tilt";
    ptu.position[0] = pan;
    ptu.position[1] = tilt;
    ptu.velocity[0] = ptu.velocity[1] = 1.0;
    ptu_pub.publish(ptu);
}

void ptuCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
    float pan,tilt;
    for (int i = 0;i<2;i++){
        if (msg->name[i] == "pan") pan = msg->position[i];
        if (msg->name[i] == "tilt") tilt = msg->position[i];
    }
    //printf("PTU: %.3f %.3f %i\n",pan,tilt,ptuMovementFinished);
    if (fabs(pan-ptu.position[0])<0.01 && fabs(tilt-ptu.position[1])<0.01) ptuMovementFinished++;
}

void execute(const spatiotemporalexploration::ExecutionGoalConstPtr& goal, Server* as)
{

    ROS_INFO("received new plan");
    as->acceptNewGoal();
    //as->setPreempted();

    strands_navigation_msgs::MonitoredNavigationGoal current_goal;
    current_goal.action_server = "move_base";
    current_goal.target_pose.header.frame_id = "map";

    spatiotemporalexploration::ExecutionFeedback feedback;

    sleep(1);

    int numPoints = 14;
    int point = 0;
    float pan[] =  { 0.00, 0.90, 1.80, 2.70, 2.70, 1.80, 0.90, 0.00,-0.90,-1.80,-2.70,-2.70,-1.80,-0.90,0.00};
    float tilt[] = { 0.50, 0.50, 0.50, 0.50,-0.30,-0.30,-0.30,-0.30,-0.30,-0.30,-0.30, 0.50, 0.50, 0.50,0.00};

    int n = (int) goal->locations.poses.size();

    ROS_INFO("received %d locations to visit in %f minutes", n, 0.0);

    for(int i = 0; i < n; i++)
    {

        char cr_goal[10];
        sprintf(cr_goal, "%d/%d", i, n);
        feedback.current_goal = cr_goal;
        feedback.time_remaining = 0;//TODO
        as->publishFeedback(feedback);

        current_goal.target_pose.pose = goal->locations.poses[i];
        ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
        ac_nav_ptr->sendGoal(current_goal);

//        ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
        ac_nav_ptr->waitForResult(ros::Duration(0.0));

        if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Monitored navigation: SUCCEEDED!");
            point = 0;
            movePtu(pan[point],tilt[point]);

            ros::spinOnce();
            while (ros::ok() && point < numPoints)
            {
                measure_srv.request.stamp = 0.0;
                if (ptuMovementFinished > 10)
                {
                    if(measure_client_ptr->call(measure_srv))
                    {
                        ROS_INFO("Measure added to grid!");
                    }
                    else
                    {
                        ROS_ERROR("Failed to call measure service");
                        exit(1);
                    }

                    point++;
                    movePtu(pan[point],tilt[point]);
                    ros::spinOnce();
                    usleep(500000);
                    if(drawCells){
                        visualize_srv.request.red = visualize_srv.request.blue = 0.0;
                        visualize_srv.request.green = visualize_srv.request.alpha = 1.0;
                        visualize_srv.request.minProbability = 0.9;
                        visualize_srv.request.maxProbability = 1.0;
                        visualize_srv.request.name = "occupied";
                        visualize_srv.request.type = 0;
                        visualize_client_ptr->call(visualize_srv);
                        ros::spinOnce();
                        usleep(100000);
                        if (drawEmptyCells){
                            visualize_srv.request.green = 0.0;
                            visualize_srv.request.red = 1.0;
                            visualize_srv.request.minProbability = 0.0;
                            visualize_srv.request.maxProbability = 0.1;
                            visualize_srv.request.alpha = 0.005;
                            visualize_srv.request.name = "free";
                            visualize_srv.request.type = 0;
                            visualize_client_ptr->call(visualize_srv);
                            ros::spinOnce();
                            usleep(100000);
                        }
                    }
                }
                ros::spinOnce();
            }
        }
        else if(ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::PREEMPTED || ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::ABORTED)
        {
            ROS_INFO("Move base failed, trying next goal in the plan...");
        }

        movePtu(0.0,0.0);
    }
}

int main(int argc,char *argv[])
{
    ros::init(argc, argv, "exploration_executioner");
    ros::NodeHandle n;

    ros::NodeHandle nh("~");

    Server server(n, "executioner", boost::bind(&execute, _1, &server), false);
    ROS_INFO("starting server...");
    server.start();

    actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> ac_nav("monitored_navigation",true);
    ac_nav_ptr = &ac_nav;
    ac_nav.waitForServer();

    //Subscribers
    ros::Subscriber ptu_sub = n.subscribe("/ptu/state", 10, ptuCallback);
    ptu.name.resize(3);
    ptu.position.resize(3);
    ptu.velocity.resize(3);
    ptu_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 10);

    //measure service client
    ros::ServiceClient measure_client = n.serviceClient<spatiotemporalexploration::AddView>("/fremenGrid/depth");
    measure_client_ptr = &measure_client;

    //vizualize client
    ros::ServiceClient visualize_client = n.serviceClient<spatiotemporalexploration::Visualize>("/fremenGrid/visualize");
    visualize_client_ptr = &visualize_client;

    //Dynamic Reconfigure
    dynamic_reconfigure::ReconfigureRequest srv_req;
    dynamic_reconfigure::ReconfigureResponse srv_resp;
    dynamic_reconfigure::DoubleParameter double_param;
    dynamic_reconfigure::Config conf;

    double_param.name = "yaw_goal_tolerance";
    double_param.value = 3.0;
    conf.doubles.push_back(double_param);

    srv_req.config = conf;

    ros::service::call("/move_base/DWAPlannerROS/set_parameters", srv_req, srv_resp);

    ROS_INFO("server started");

    ros::spin();

    return 0;
}
<|endoftext|>