<commit_before>// Ouzel by Elviss Strazdins

#ifndef OUZEL_FORMATS_INI_HPP
#define OUZEL_FORMATS_INI_HPP

#include <algorithm>
#include <array>
#include <cstdint>
#include <functional>
#include <iterator>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

namespace ouzel::ini
{
    class ParseError final: public std::logic_error
    {
    public:
        using std::logic_error::logic_error;
    };

    class Section final
    {
    public:
        using Values = std::map<std::string, std::string>;

        Section() = default;

        explicit Section(const std::string& initName):
            name(initName)
        {
        }

        Values::iterator begin() noexcept
        {
            return values.begin();
        }

        Values::iterator end() noexcept
        {
            return values.end();
        }

        Values::const_iterator begin() const noexcept
        {
            return values.begin();
        }

        Values::const_iterator end() const noexcept
        {
            return values.end();
        }

        const std::string& getName() const noexcept { return name; }
        void setName(const std::string& newName) { name = newName; }

        const Values& getValues() const noexcept { return values; }

        bool hasValue(const std::string& key) const
        {
            return values.find(key) != values.end();
        }

        std::string& operator[](const std::string& key)
        {
            return values[key];
        }

        std::string operator[](const std::string& key) const
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                return valueIterator->second;

            return std::string();
        }

        const std::string& getValue(const std::string& key, const std::string& defaultValue = {}) const
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                return valueIterator->second;

            return defaultValue;
        }

        void setValue(const std::string& key, const std::string& value)
        {
            values[key] = value;
        }

        void deleteValue(const std::string& key)
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                values.erase(valueIterator);
        }

        std::size_t getSize() const noexcept
        {
            return values.size();
        }

    private:
        std::string name;
        Values values;
    };

    class Data final
    {
    public:
        using Sections = std::map<std::string, Section>;

        Data() = default;

        const Sections& getSections() const noexcept { return sections; }

        Sections::iterator begin() noexcept
        {
            return sections.begin();
        }

        Sections::iterator end() noexcept
        {
            return sections.end();
        }

        Sections::const_iterator begin() const noexcept
        {
            return sections.begin();
        }

        Sections::const_iterator end() const noexcept
        {
            return sections.end();
        }

        bool hasSection(const std::string& name) const
        {
            const auto sectionIterator = sections.find(name);
            return sectionIterator != sections.end();
        }

        Section& operator[](const std::string& name)
        {
            return sections[name];
        }

        Section operator[](const std::string& name) const
        {
            if (const auto sectionIterator = sections.find(name); sectionIterator != sections.end())
                return sectionIterator->second;

            return Section();
        }

        void eraseSection(const std::string& name)
        {
            if (const auto sectionIterator = sections.find(name); sectionIterator != sections.end())
                sections.erase(sectionIterator);
        }

        std::size_t getSize() const noexcept
        {
            return sections.size();
        }

    private:
        Sections sections;
    };

    inline namespace detail
    {
        constexpr std::array<std::uint8_t, 3> utf8ByteOrderMark = {0xEF, 0xBB, 0xBF};
    }

    template <class Iterator>
    Data parse(Iterator begin, Iterator end)
    {
        class Parser final
        {
        public:
            static Data parse(Iterator begin, Iterator end)
            {
                Data result;

                std::string section;

                for (auto iterator = hasByteOrderMark(begin, end) ? begin + 3 : begin; iterator != end;)
                {
                    if (isWhitespace(static_cast<char>(*iterator)) ||
                        static_cast<char>(*iterator) == '\n' ||
                        static_cast<char>(*iterator) == '\r') // line starts with a whitespace
                        ++iterator; // skip the white space
                    else if (static_cast<char>(*iterator) == '[') // section
                    {
                        ++iterator; // skip the left bracket

                        bool parsedSection = false;

                        for (;;)
                        {
                            if (iterator == end ||
                                static_cast<char>(*iterator) == '\n' ||
                                static_cast<char>(*iterator) == '\r')
                            {
                                if (!parsedSection)
                                    throw ParseError{"Unexpected end of section"};

                                ++iterator; // skip the newline
                                break;
                            }
                            else if (static_cast<char>(*iterator) == ';')
                            {
                                ++iterator; // skip the semicolon

                                if (!parsedSection)
                                    throw ParseError{"Unexpected comment"};

                                while (iterator != end)
                                {
                                    if (static_cast<char>(*iterator) == '\n' ||
                                        static_cast<char>(*iterator) == '\r')
                                    {
                                        ++iterator; // skip the newline
                                        break;
                                    }

                                    ++iterator;
                                }
                                break;
                            }
                            else if (static_cast<char>(*iterator) == ']')
                                parsedSection = true;
                            else if (static_cast<char>(*iterator) != ' ' &&
                                     static_cast<char>(*iterator) != '\t')
                            {
                                if (parsedSection)
                                    throw ParseError{"Unexpected character after section"};
                            }

                            if (!parsedSection)
                                section.push_back(static_cast<char>(*iterator));

                            ++iterator;
                        }

                        trim(section);

                        if (section.empty())
                            throw ParseError{"Invalid section name"};

                        result[section] = Section{};
                    }
                    else if (static_cast<char>(*iterator) == ';') // comment
                    {
                        while (++iterator != end)
                        {
                            if (static_cast<char>(*iterator) == '\r' ||
                                static_cast<char>(*iterator) == '\n')
                            {
                                ++iterator; // skip the newline
                                break;
                            }
                        }
                    }
                    else // key, value pair
                    {
                        std::string key;
                        std::string value;
                        bool parsedKey = false;

                        while (iterator != end)
                        {
                            if (static_cast<char>(*iterator) == '\r' ||
                                static_cast<char>(*iterator) == '\n')
                            {
                                ++iterator; // skip the newline
                                break;
                            }
                            else if (static_cast<char>(*iterator) == '=')
                            {
                                if (!parsedKey)
                                    parsedKey = true;
                                else
                                    throw ParseError{"Unexpected character"};
                            }
                            else if (static_cast<char>(*iterator) == ';')
                            {
                                ++iterator; // skip the semicolon

                                while (iterator != end)
                                {
                                    if (static_cast<char>(*iterator) == '\r' ||
                                        static_cast<char>(*iterator) == '\n')
                                    {
                                        ++iterator; // skip the newline
                                        break;
                                    }

                                    ++iterator;
                                }
                                break;
                            }
                            else
                            {
                                if (!parsedKey)
                                    key.push_back(static_cast<char>(*iterator));
                                else
                                    value.push_back(static_cast<char>(*iterator));
                            }

                            ++iterator;
                        }

                        if (key.empty())
                            throw ParseError{"Invalid key name"};

                        trim(key);
                        trim(value);

                        result[section][key] = value;
                    }
                }

                return result;
            }

        private:
            static bool hasByteOrderMark(Iterator begin, Iterator end) noexcept
            {
                for (const auto b : utf8ByteOrderMark)
                    if (begin == end || static_cast<char>(*begin) != b)
                        return false;
                    else
                        ++begin;
                return true;
            }

            static constexpr bool isWhitespace(const char c) noexcept
            {
                return c == ' ' || c == '\t';
            }

            static std::string& leftTrim(std::string& s)
            {
                s.erase(s.begin(), std::find_if(s.begin(), s.end(),
                                                [](char c) noexcept {return !isWhitespace(c);}));
                return s;
            }

            static std::string& rightTrim(std::string& s)
            {
                s.erase(std::find_if(s.rbegin(), s.rend(),
                                     [](char c) noexcept {return !isWhitespace(c);}).base(), s.end());
                return s;
            }

            static std::string& trim(std::string& s)
            {
                return leftTrim(rightTrim(s));
            }
        };

        return Parser::parse(begin, end);
    }

    inline Data parse(const char* data)
    {
        auto end = data;
        while (*end) ++end;
        return parse(data, end);
    }

    template <class T>
    Data parse(const T& data)
    {
        return parse(std::begin(data), std::end(data));
    }

    inline std::string encode(const Data& data, bool byteOrderMark = false)
    {
        std::string result;

        if (byteOrderMark) result.assign(utf8ByteOrderMark.begin(),
                                         utf8ByteOrderMark.end());

        for (const auto& [name, section] : data)
        {
            if (!name.empty())
            {
                result.push_back('[');
                result.insert(result.end(), name.begin(), name.end());
                result.push_back(']');
                result.push_back('\n');
            }

            for (const auto& [key, value] : section)
            {
                result.insert(result.end(), key.begin(), key.end());
                result.push_back('=');
                result.insert(result.end(), value.begin(), value.end());
                result.push_back('\n');
            }
        }

        return result;
    }
}

#endif // OUZEL_FORMATS_INI_HPP
<commit_msg>Pass string_view to ini functions<commit_after>// Ouzel by Elviss Strazdins

#ifndef OUZEL_FORMATS_INI_HPP
#define OUZEL_FORMATS_INI_HPP

#include <algorithm>
#include <array>
#include <cstdint>
#include <functional>
#include <map>
#include <stdexcept>
#include <string>
#include <string_view>

namespace ouzel::ini
{
    class ParseError final: public std::logic_error
    {
    public:
        using std::logic_error::logic_error;
    };

    using Values = std::map<std::string, std::string, std::less<>>;

    class Section final
    {
    public:
        Section() = default;

        explicit Section(const std::string& initName):
            name(initName)
        {
        }

        auto begin() noexcept
        {
            return values.begin();
        }

        auto end() noexcept
        {
            return values.end();
        }

        auto begin() const noexcept
        {
            return values.begin();
        }

        auto end() const noexcept
        {
            return values.end();
        }

        const std::string& getName() const noexcept { return name; }
        void setName(const std::string& newName) { name = newName; }

        const Values& getValues() const noexcept { return values; }

        bool hasValue(const std::string_view key) const
        {
            return values.find(key) != values.end();
        }

        std::string& operator[](const std::string_view key)
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                return valueIterator->second;
            else
            {
                const auto& [newIterator, success] = values.insert({std::string{key}, std::string{}});
                (void)success;
                return newIterator->second;
            }
        }

        std::string operator[](const std::string_view key) const
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                return valueIterator->second;

            return std::string();
        }

        const std::string& getValue(const std::string_view key, const std::string& defaultValue = {}) const
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                return valueIterator->second;

            return defaultValue;
        }

        void setValue(const std::string_view key, const std::string& value)
        {
            values.insert({std::string{key}, value});
        }

        void deleteValue(const std::string_view key)
        {
            if (const auto valueIterator = values.find(key); valueIterator != values.end())
                values.erase(valueIterator);
        }

        std::size_t getSize() const noexcept
        {
            return values.size();
        }

    private:
        std::string name;
        Values values;
    };

    class Data final
    {
    public:
        using Sections = std::map<std::string, Section, std::less<>>;

        Data() = default;

        const Sections& getSections() const noexcept { return sections; }

        auto begin() noexcept
        {
            return sections.begin();
        }

        auto end() noexcept
        {
            return sections.end();
        }

        auto begin() const noexcept
        {
            return sections.begin();
        }

        auto end() const noexcept
        {
            return sections.end();
        }

        bool hasSection(const std::string_view name) const
        {
            return sections.find(name) != sections.end();
        }

        Section& operator[](const std::string_view name)
        {
            if (const auto sectionIterator = sections.find(name); sectionIterator != sections.end())
                return sectionIterator->second;
            else
            {
                const auto& [newIterator, success] = sections.insert({std::string{name}, Section{}});
                (void)success;
                return newIterator->second;
            }
        }

        Section operator[](const std::string_view name) const
        {
            if (const auto sectionIterator = sections.find(name); sectionIterator != sections.end())
                return sectionIterator->second;

            return Section{};
        }

        void eraseSection(const std::string_view name)
        {
            if (const auto sectionIterator = sections.find(name); sectionIterator != sections.end())
                sections.erase(sectionIterator);
        }

        std::size_t getSize() const noexcept
        {
            return sections.size();
        }

    private:
        Sections sections;
    };

    inline namespace detail
    {
        constexpr std::array<std::uint8_t, 3> utf8ByteOrderMark = {0xEF, 0xBB, 0xBF};
    }

    template <class Iterator>
    Data parse(Iterator begin, Iterator end)
    {
        class Parser final
        {
        public:
            static Data parse(Iterator begin, Iterator end)
            {
                Data result;

                std::string section;

                for (auto iterator = hasByteOrderMark(begin, end) ? begin + 3 : begin; iterator != end;)
                {
                    if (isWhitespace(static_cast<char>(*iterator)) ||
                        static_cast<char>(*iterator) == '\n' ||
                        static_cast<char>(*iterator) == '\r') // line starts with a whitespace
                        ++iterator; // skip the white space
                    else if (static_cast<char>(*iterator) == '[') // section
                    {
                        ++iterator; // skip the left bracket

                        bool parsedSection = false;

                        for (;;)
                        {
                            if (iterator == end ||
                                static_cast<char>(*iterator) == '\n' ||
                                static_cast<char>(*iterator) == '\r')
                            {
                                if (!parsedSection)
                                    throw ParseError{"Unexpected end of section"};

                                ++iterator; // skip the newline
                                break;
                            }
                            else if (static_cast<char>(*iterator) == ';')
                            {
                                ++iterator; // skip the semicolon

                                if (!parsedSection)
                                    throw ParseError{"Unexpected comment"};

                                while (iterator != end)
                                {
                                    if (static_cast<char>(*iterator) == '\n' ||
                                        static_cast<char>(*iterator) == '\r')
                                    {
                                        ++iterator; // skip the newline
                                        break;
                                    }

                                    ++iterator;
                                }
                                break;
                            }
                            else if (static_cast<char>(*iterator) == ']')
                                parsedSection = true;
                            else if (static_cast<char>(*iterator) != ' ' &&
                                     static_cast<char>(*iterator) != '\t')
                            {
                                if (parsedSection)
                                    throw ParseError{"Unexpected character after section"};
                            }

                            if (!parsedSection)
                                section.push_back(static_cast<char>(*iterator));

                            ++iterator;
                        }

                        trim(section);

                        if (section.empty())
                            throw ParseError{"Invalid section name"};

                        result[section] = Section{};
                    }
                    else if (static_cast<char>(*iterator) == ';') // comment
                    {
                        while (++iterator != end)
                        {
                            if (static_cast<char>(*iterator) == '\r' ||
                                static_cast<char>(*iterator) == '\n')
                            {
                                ++iterator; // skip the newline
                                break;
                            }
                        }
                    }
                    else // key, value pair
                    {
                        std::string key;
                        std::string value;
                        bool parsedKey = false;

                        while (iterator != end)
                        {
                            if (static_cast<char>(*iterator) == '\r' ||
                                static_cast<char>(*iterator) == '\n')
                            {
                                ++iterator; // skip the newline
                                break;
                            }
                            else if (static_cast<char>(*iterator) == '=')
                            {
                                if (!parsedKey)
                                    parsedKey = true;
                                else
                                    throw ParseError{"Unexpected character"};
                            }
                            else if (static_cast<char>(*iterator) == ';')
                            {
                                ++iterator; // skip the semicolon

                                while (iterator != end)
                                {
                                    if (static_cast<char>(*iterator) == '\r' ||
                                        static_cast<char>(*iterator) == '\n')
                                    {
                                        ++iterator; // skip the newline
                                        break;
                                    }

                                    ++iterator;
                                }
                                break;
                            }
                            else
                            {
                                if (!parsedKey)
                                    key.push_back(static_cast<char>(*iterator));
                                else
                                    value.push_back(static_cast<char>(*iterator));
                            }

                            ++iterator;
                        }

                        if (key.empty())
                            throw ParseError{"Invalid key name"};

                        trim(key);
                        trim(value);

                        result[section][key] = value;
                    }
                }

                return result;
            }

        private:
            static bool hasByteOrderMark(Iterator begin, Iterator end) noexcept
            {
                for (const auto b : utf8ByteOrderMark)
                    if (begin == end || static_cast<std::uint8_t>(*begin) != b)
                        return false;
                    else
                        ++begin;
                return true;
            }

            static constexpr bool isWhitespace(const char c) noexcept
            {
                return c == ' ' || c == '\t';
            }

            static std::string& leftTrim(std::string& s)
            {
                s.erase(s.begin(), std::find_if(s.begin(), s.end(),
                                                [](char c) noexcept {return !isWhitespace(c);}));
                return s;
            }

            static std::string& rightTrim(std::string& s)
            {
                s.erase(std::find_if(s.rbegin(), s.rend(),
                                     [](char c) noexcept {return !isWhitespace(c);}).base(), s.end());
                return s;
            }

            static std::string& trim(std::string& s)
            {
                return leftTrim(rightTrim(s));
            }
        };

        return Parser::parse(begin, end);
    }

    inline Data parse(const char* data)
    {
        auto end = data;
        while (*end) ++end;
        return parse(data, end);
    }

    template <class T>
    Data parse(const T& data)
    {
        return parse(std::begin(data), std::end(data));
    }

    inline std::string encode(const Data& data, bool byteOrderMark = false)
    {
        std::string result;

        if (byteOrderMark) result.assign(utf8ByteOrderMark.begin(),
                                         utf8ByteOrderMark.end());

        for (const auto& [name, section] : data)
        {
            if (!name.empty())
            {
                result.push_back('[');
                result.insert(result.end(), name.begin(), name.end());
                result.push_back(']');
                result.push_back('\n');
            }

            for (const auto& [key, value] : section)
            {
                result.insert(result.end(), key.begin(), key.end());
                result.push_back('=');
                result.insert(result.end(), value.begin(), value.end());
                result.push_back('\n');
            }
        }

        return result;
    }
}

#endif // OUZEL_FORMATS_INI_HPP
<|endoftext|>


<commit_before>//
// Copyright (c) 2019, University of Edinburgh
// 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.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <exotica_core/tools/box_qp.h>
#include <exotica_core/tools/box_qp_old.h>
#include <exotica_ddp_solver/control_limited_ddp_solver.h>

REGISTER_MOTIONSOLVER_TYPE("ControlLimitedDDPSolver", exotica::ControlLimitedDDPSolver)

namespace exotica
{
void ControlLimitedDDPSolver::Instantiate(const ControlLimitedDDPSolverInitializer& init)
{
    parameters_ = init;
    base_parameters_ = AbstractDDPSolverInitializer(ControlLimitedDDPSolverInitializer(parameters_));
}

void ControlLimitedDDPSolver::BackwardPass()
{
    const Eigen::MatrixXd& control_limits = dynamics_solver_->get_control_limits();

    Vx_ = prob_->GetStateCostJacobian(T_ - 1);
    Vxx_ = prob_->GetStateCostHessian(T_ - 1);

    // concatenation axis for tensor products
    //  See https://eigen.tuxfamily.org/dox-devel/unsupported/eigen_tensors.html#title14
    Eigen::array<Eigen::IndexPair<int>, 1> dims = {Eigen::IndexPair<int>(1, 0)};

    for (int t = T_ - 2; t >= 0; t--)
    {
        Eigen::VectorXd x = prob_->get_X(t), u = prob_->get_U(t);

        dynamics_solver_->ComputeDerivatives(x, u);
        Eigen::MatrixXd fx = dynamics_solver_->get_fx() * dynamics_solver_->get_dt() + Eigen::MatrixXd::Identity(dynamics_solver_->get_num_state_derivative(), dynamics_solver_->get_num_state_derivative());
        Eigen::MatrixXd fu = dynamics_solver_->get_fu() * dynamics_solver_->get_dt();

        Qx_ = dt_ * prob_->GetStateCostJacobian(t) + fx.transpose() * Vx_;  // lx + fx @ Vx_
        Qu_ = dt_ * prob_->GetControlCostJacobian(t) + fu.transpose() * Vx_;

        if (parameters_.UseSecondOrderDynamics)
        {
            // clang-format off
            Eigen::Tensor<double, 1> Vx_tensor = Eigen::TensorMap<Eigen::Tensor<double, 1>>(Vx_.data(), NDX_);
            Qxx_ = dt_ * prob_->GetStateCostHessian(t) + fx.transpose() * Vxx_ * fx +
                Eigen::TensorToMatrix(
                    (Eigen::Tensor<double, 2>)dynamics_solver_->fxx(x, u).contract(Vx_tensor, dims), NDX_, NDX_
                ) * dt_;

            Quu_ = dt_ * prob_->GetControlCostHessian() + fu.transpose() * Vxx_ * fu +
                Eigen::TensorToMatrix(
                    (Eigen::Tensor<double, 2>)dynamics_solver_->fuu(x, u).contract(Vx_tensor, dims), NU_, NU_
                ) * dt_;

            Qux_ = dt_ * prob_->GetStateControlCostHessian() + fu.transpose() * Vxx_ * fx +
                Eigen::TensorToMatrix((Eigen::Tensor<double, 2>)dynamics_solver_->fxu(x, u).contract(Vx_tensor, dims), NU_, NDX_
                ) * dt_;
            // clang-format on
        }
        else
        {
            Qxx_ = dt_ * prob_->GetStateCostHessian(t) + fx.transpose() * Vxx_ * fx;
            Quu_ = dt_ * prob_->GetControlCostHessian() + fu.transpose() * Vxx_ * fu;

            // NOTE: Qux = Qxu for all robotics systems I have seen
            //  this might need to be changed later on
            Qux_ = dt_ * prob_->GetStateControlCostHessian() + fu.transpose() * Vxx_ * fx;
        }

        Eigen::VectorXd low_limit = control_limits.col(0) - u,
                        high_limit = control_limits.col(1) - u;

        BoxQPSolution boxqp_sol = ExoticaBoxQP(Quu_, Qu_, low_limit, high_limit, u, 0.1, 100, 1e-5, lambda_);

        Quu_inv_.setZero();
        for (unsigned int i = 0; i < boxqp_sol.free_idx.size(); ++i)
            for (unsigned int j = 0; j < boxqp_sol.free_idx.size(); ++j)
                Quu_inv_(boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);

        // Compute controls
        K_gains_[t] = -Quu_inv_ * Qux_;
        k_gains_[t] = boxqp_sol.x;

        for (unsigned int j = 0; j < boxqp_sol.clamped_idx.size(); ++j)
            K_gains_[t](boxqp_sol.clamped_idx[j]) = 0;

        Vx_ = Qx_ - K_gains_[t].transpose() * Quu_ * k_gains_[t];
        Vxx_ = Qxx_ - K_gains_[t].transpose() * Quu_ * K_gains_[t];
    }
}

}  // namespace exotica
<commit_msg>[exotica_ddp_solver] ControlLimitedDDPSolver: Add state regularization<commit_after>//
// Copyright (c) 2019, University of Edinburgh
// 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.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <exotica_core/tools/box_qp.h>
#include <exotica_core/tools/box_qp_old.h>
#include <exotica_ddp_solver/control_limited_ddp_solver.h>

REGISTER_MOTIONSOLVER_TYPE("ControlLimitedDDPSolver", exotica::ControlLimitedDDPSolver)

namespace exotica
{
void ControlLimitedDDPSolver::Instantiate(const ControlLimitedDDPSolverInitializer& init)
{
    parameters_ = init;
    base_parameters_ = AbstractDDPSolverInitializer(ControlLimitedDDPSolverInitializer(parameters_));
}

void ControlLimitedDDPSolver::BackwardPass()
{
    const Eigen::MatrixXd& control_limits = dynamics_solver_->get_control_limits();

    Vx_ = prob_->GetStateCostJacobian(T_ - 1);
    Vxx_ = prob_->GetStateCostHessian(T_ - 1);

    // concatenation axis for tensor products
    //  See https://eigen.tuxfamily.org/dox-devel/unsupported/eigen_tensors.html#title14
    Eigen::array<Eigen::IndexPair<int>, 1> dims = {Eigen::IndexPair<int>(1, 0)};

    for (int t = T_ - 2; t >= 0; t--)
    {
        Eigen::VectorXd x = prob_->get_X(t), u = prob_->get_U(t);

        dynamics_solver_->ComputeDerivatives(x, u);
        Eigen::MatrixXd fx = dynamics_solver_->get_fx() * dynamics_solver_->get_dt() + Eigen::MatrixXd::Identity(dynamics_solver_->get_num_state_derivative(), dynamics_solver_->get_num_state_derivative());
        Eigen::MatrixXd fu = dynamics_solver_->get_fu() * dynamics_solver_->get_dt();

        Qx_ = dt_ * prob_->GetStateCostJacobian(t) + fx.transpose() * Vx_;  // lx + fx @ Vx_
        Qu_ = dt_ * prob_->GetControlCostJacobian(t) + fu.transpose() * Vx_;

        // State regularization
        Vxx_.diagonal().array() += lambda_;

        if (parameters_.UseSecondOrderDynamics)
        {
            // clang-format off
            Eigen::Tensor<double, 1> Vx_tensor = Eigen::TensorMap<Eigen::Tensor<double, 1>>(Vx_.data(), NDX_);
            Qxx_ = dt_ * prob_->GetStateCostHessian(t) + fx.transpose() * Vxx_ * fx +
                Eigen::TensorToMatrix(
                    (Eigen::Tensor<double, 2>)dynamics_solver_->fxx(x, u).contract(Vx_tensor, dims), NDX_, NDX_
                ) * dt_;

            Quu_ = dt_ * prob_->GetControlCostHessian() + fu.transpose() * Vxx_ * fu +
                Eigen::TensorToMatrix(
                    (Eigen::Tensor<double, 2>)dynamics_solver_->fuu(x, u).contract(Vx_tensor, dims), NU_, NU_
                ) * dt_;

            Qux_ = dt_ * prob_->GetStateControlCostHessian() + fu.transpose() * Vxx_ * fx +
                Eigen::TensorToMatrix((Eigen::Tensor<double, 2>)dynamics_solver_->fxu(x, u).contract(Vx_tensor, dims), NU_, NDX_
                ) * dt_;
            // clang-format on
        }
        else
        {
            Qxx_ = dt_ * prob_->GetStateCostHessian(t) + fx.transpose() * Vxx_ * fx;
            Quu_ = dt_ * prob_->GetControlCostHessian() + fu.transpose() * Vxx_ * fu;

            // NOTE: Qux = Qxu for all robotics systems I have seen
            //  this might need to be changed later on
            Qux_ = dt_ * prob_->GetStateControlCostHessian() + fu.transpose() * Vxx_ * fx;
        }

        Eigen::VectorXd low_limit = control_limits.col(0) - u,
                        high_limit = control_limits.col(1) - u;

        // BoxQPSolution boxqp_sol = BoxQP(Quu_, Qu_, low_limit, high_limit, u, 0.1, 100, 1e-5, 1e-12);
        // BoxQPSolution boxqp_sol = ExoticaBoxQP(Quu_, Qu_, low_limit, high_limit, u, 0.1, 100, 1e-5, 1e-12);
        // Timer t2;
        // Quu_.diagonal().array() += lambda_;
        BoxQPSolution boxqp_sol = ExoticaBoxQP(Quu_, Qu_, low_limit, high_limit, u, 0.1, 100, 1e-5, lambda_);
        // double t2_taken = t2.GetDuration();
        // Timer t1;
        // BoxQPSolution boxqp_sol_new = BoxQP(Quu_, Qu_, low_limit, high_limit, u, 0.1, 100, 1e-5, parameters_.RegularizationRate);
        // double t1_taken = t1.GetDuration();
        // HIGHLIGHT("New=" << t1_taken << ", old=" << t2_taken);

        Quu_inv_.setZero();
        for (unsigned int i = 0; i < boxqp_sol.free_idx.size(); ++i)
            for (unsigned int j = 0; j < boxqp_sol.free_idx.size(); ++j)
                Quu_inv_(boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);

        // Compute controls
        K_gains_[t] = -Quu_inv_ * Qux_;
        k_gains_[t] = boxqp_sol.x;

        for (unsigned int j = 0; j < boxqp_sol.clamped_idx.size(); ++j)
            K_gains_[t](boxqp_sol.clamped_idx[j]) = 0;

        Vx_ = Qx_ - K_gains_[t].transpose() * Quu_ * k_gains_[t];
        Vxx_ = Qxx_ - K_gains_[t].transpose() * Quu_ * K_gains_[t];
    }
}

}  // namespace exotica
<|endoftext|>


<commit_before>/******************************************************************
*
* Round for C++
*
* Copyright (C) Satoshi Konno 2015
*
* This is licensed under BSD-style license, see file COPYING.
*
******************************************************************/

#include <boost/test/unit_test.hpp>

#include <sstream>
#include <round/core/Registry.h>

using namespace std;
using namespace Round;

BOOST_AUTO_TEST_SUITE(registry)

BOOST_AUTO_TEST_CASE(RQLRregistryBasicMethodTest) {
  std::string val;
  time_t ts;
  std::stringstream ss;
  std::string sval;
  time_t tval;

  Registry reg;

  BOOST_CHECK_EQUAL(reg.getKey(&sval), false);
  BOOST_CHECK_EQUAL(reg.getValue(&sval), false);
  BOOST_CHECK_EQUAL(reg.getTimestamp(tval), false);
  BOOST_CHECK_EQUAL(reg.getLogicalTimestamp(tval), false);

  time(&ts); ss << "key" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg.setKey(val), true);
  BOOST_CHECK_EQUAL(reg.getKey(&sval), true);
  BOOST_CHECK_EQUAL(val.compare(sval), 0);

  time(&ts); ss << "val" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg.setValue(val), true);
  BOOST_CHECK_EQUAL(reg.getValue(&sval), true);
  BOOST_CHECK_EQUAL(val.compare(sval), 0);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg.setTimestamp(ts), true);
  BOOST_CHECK_EQUAL(reg.getTimestamp(tval), true);
  BOOST_CHECK_EQUAL(tval, ts);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg.setLogicalTimestamp(ts), true);
  BOOST_CHECK_EQUAL(reg.getLogicalTimestamp(tval), true);
  BOOST_CHECK_EQUAL(tval, ts);
}

BOOST_AUTO_TEST_CASE(RQLRregistryEqualsTest) {
  std::string val;
  time_t ts;
  std::stringstream ss;
  std::string sval;

  Registry reg01;

  time(&ts); ss << "key" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg01.setKey(val), true);

  time(&ts); ss << "val" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg01.setValue(val), true);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg01.setTimestamp(ts), true);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg01.setLogicalTimestamp(ts), true);

  Registry reg02;

  BOOST_CHECK_EQUAL(reg01.equals(reg02), false);
  BOOST_CHECK_EQUAL(reg01.equalsWithTimestamp(reg02), false);

  reg02 = reg01;
  BOOST_CHECK_EQUAL(reg01.equals(reg02), true);
  BOOST_CHECK_EQUAL(reg01.equalsWithTimestamp(reg02), true);
}

BOOST_AUTO_TEST_CASE(RQLRregistryMapMethodTest) {
  RegistryMap regMap;
  BOOST_CHECK_EQUAL(regMap.size(), 0);

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

    std::stringstream ss;
    time_t ts;
    time(&ts); ss << ts;

    std::string key = "key" + ss.str();
    std::string val = "val" + ss.str();

    Registry inReg;
    BOOST_CHECK_EQUAL(inReg.setKey(key), true);
    BOOST_CHECK_EQUAL(inReg.setValue(val), true);
    BOOST_CHECK_EQUAL(inReg.setTimestamp(ts), true);
    BOOST_CHECK_EQUAL(inReg.setLogicalTimestamp(ts), true);

    BOOST_CHECK_EQUAL(regMap.size(), n);
    BOOST_CHECK_EQUAL(regMap.set(inReg), true);
    BOOST_CHECK_EQUAL(regMap.size(), (n+1));

    Registry outReg;
    BOOST_CHECK_EQUAL(inReg.equals(inReg), true);
    BOOST_CHECK_EQUAL(inReg.equalsWithTimestamp(inReg), true);
  }
}

BOOST_AUTO_TEST_SUITE_END()
<commit_msg>* Updated tests for Registry.<commit_after>/******************************************************************
*
* Round for C++
*
* Copyright (C) Satoshi Konno 2015
*
* This is licensed under BSD-style license, see file COPYING.
*
******************************************************************/

#include <boost/test/unit_test.hpp>

#include <sstream>
#include <round/core/Registry.h>

using namespace std;
using namespace Round;

BOOST_AUTO_TEST_SUITE(registry)

BOOST_AUTO_TEST_CASE(RQLRregistryBasicMethodTest) {
  std::string val;
  time_t ts;
  std::stringstream ss;
  std::string sval;
  time_t tval;

  Registry reg;

  BOOST_CHECK_EQUAL(reg.getKey(&sval), false);
  BOOST_CHECK_EQUAL(reg.getValue(&sval), false);
  BOOST_CHECK_EQUAL(reg.getTimestamp(tval), false);
  BOOST_CHECK_EQUAL(reg.getLogicalTimestamp(tval), false);

  time(&ts); ss << "key" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg.setKey(val), true);
  BOOST_CHECK_EQUAL(reg.getKey(&sval), true);
  BOOST_CHECK_EQUAL(val.compare(sval), 0);

  time(&ts); ss << "val" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg.setValue(val), true);
  BOOST_CHECK_EQUAL(reg.getValue(&sval), true);
  BOOST_CHECK_EQUAL(val.compare(sval), 0);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg.setTimestamp(ts), true);
  BOOST_CHECK_EQUAL(reg.getTimestamp(tval), true);
  BOOST_CHECK_EQUAL(tval, ts);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg.setLogicalTimestamp(ts), true);
  BOOST_CHECK_EQUAL(reg.getLogicalTimestamp(tval), true);
  BOOST_CHECK_EQUAL(tval, ts);
}

BOOST_AUTO_TEST_CASE(RQLRregistryEqualsTest) {
  std::string val;
  time_t ts;
  std::stringstream ss;
  std::string sval;

  Registry reg01;

  time(&ts); ss << "key" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg01.setKey(val), true);

  time(&ts); ss << "val" << ts; val = ss.str();
  BOOST_CHECK_EQUAL(reg01.setValue(val), true);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg01.setTimestamp(ts), true);

  time(&ts); ts += rand();
  BOOST_CHECK_EQUAL(reg01.setLogicalTimestamp(ts), true);

  Registry reg02;

  BOOST_CHECK_EQUAL(reg01.equals(reg02), false);
  BOOST_CHECK_EQUAL(reg01.equalsWithTimestamp(reg02), false);

  reg02 = reg01;
  BOOST_CHECK_EQUAL(reg01.equals(reg02), true);
  BOOST_CHECK_EQUAL(reg01.equalsWithTimestamp(reg02), true);
}

BOOST_AUTO_TEST_CASE(RQLRregistryMapMethodTest) {
  RegistryMap regMap;
  BOOST_CHECK_EQUAL(regMap.size(), 0);

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

    std::stringstream ss;
    time_t ts;

    Registry inReg;

    time(&ts); ts += rand(); ss << ts;
    std::string key = "key" + ss.str();
    std::string val = "val" + ss.str();
    BOOST_CHECK_EQUAL(inReg.setKey(key), true);
    BOOST_CHECK_EQUAL(inReg.setValue(val), true);

    time(&ts); ts += rand();
    BOOST_CHECK_EQUAL(inReg.setTimestamp(ts), true);

    time(&ts); ts += rand();
    BOOST_CHECK_EQUAL(inReg.setLogicalTimestamp(ts), true);

    BOOST_CHECK_EQUAL(regMap.size(), n);
    BOOST_CHECK_EQUAL(regMap.set(inReg), true);
    BOOST_CHECK_EQUAL(regMap.size(), (n+1));

    Registry outReg;

    BOOST_CHECK_EQUAL(inReg.equals(inReg), true);
    BOOST_CHECK_EQUAL(inReg.equalsWithTimestamp(inReg), true);
  }
}

BOOST_AUTO_TEST_SUITE_END()
<|endoftext|>


<commit_before>/// Common commands for all bitstreams
///
/// (c) Koheron

#ifndef __DRIVERS_COMMON_HPP__
#define __DRIVERS_COMMON_HPP__

#include <array>

#include <drivers/dev_mem.hpp>
#include <drivers/wr_register.hpp>
#include <drivers/addresses.hpp>
#include <drivers/init.hpp>

class Common
{
  public:
    Common(Klib::DevMem& dev_mem_);
    ~Common();

    int Open();

    #pragma tcp-server exclude
    void Close();

    std::array<uint32_t, BITSTREAM_ID_SIZE> get_bitstream_id();

    uint64_t get_dna();
    void set_led(uint32_t value);
    uint32_t get_led();
    void ip_on_leds();

    void load_settings() {
        Init init(dev_mem);
        init.load_settings();
    };

    enum Status {
        CLOSED,
        OPENED,
        FAILED
    };

    #pragma tcp-server is_failed
    bool IsFailed() const {return status == FAILED;}

  private:
    Klib::DevMem& dev_mem;
    int status;
    std::array<uint32_t, BITSTREAM_ID_SIZE> bitstream_id;

    // Memory maps IDs
    Klib::MemMapID config_map;
    Klib::MemMapID status_map;
};

#endif // __DRIVERS_COMMON_HPP__<commit_msg>add ip_on_leds to init<commit_after>/// Common commands for all bitstreams
///
/// (c) Koheron

#ifndef __DRIVERS_COMMON_HPP__
#define __DRIVERS_COMMON_HPP__

#include <array>

#include <drivers/dev_mem.hpp>
#include <drivers/wr_register.hpp>
#include <drivers/addresses.hpp>
#include <drivers/init.hpp>

class Common
{
  public:
    Common(Klib::DevMem& dev_mem_);
    ~Common();

    int Open();

    #pragma tcp-server exclude
    void Close();

    std::array<uint32_t, BITSTREAM_ID_SIZE> get_bitstream_id();

    uint64_t get_dna();
    void set_led(uint32_t value);
    uint32_t get_led();
    void ip_on_leds();

    void init() {
        ip_on_leds();
        Init init(dev_mem);
        init.load_settings();
    };

    enum Status {
        CLOSED,
        OPENED,
        FAILED
    };

    #pragma tcp-server is_failed
    bool IsFailed() const {return status == FAILED;}

  private:
    Klib::DevMem& dev_mem;
    int status;
    std::array<uint32_t, BITSTREAM_ID_SIZE> bitstream_id;

    // Memory maps IDs
    Klib::MemMapID config_map;
    Klib::MemMapID status_map;
};

#endif // __DRIVERS_COMMON_HPP__<|endoftext|>


<commit_before>// Copyright 2015-2020 Elviss Strazdins. All rights reserved.

#ifndef OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP
#define OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP

#include <stdexcept>
#include <dispatch/dispatch.h>

namespace ouzel::graphics::metal
{
    class DispatchSemaphore final
    {
    public:
        explicit DispatchSemaphore(long value):
            semaphore{dispatch_semaphore_create(value)}
        {
            if (!semaphore)
                throw std::runtime_error("Failed to create dispatch semaphore");
        }

        ~DispatchSemaphore()
        {
            if (semaphore) dispatch_release(semaphore);
        }

        DispatchSemaphore(DispatchSemaphore&& other) noexcept:
            semaphore{other.semaphore}
        {
            other.semaphore = nullptr;
        }

        DispatchSemaphore(const DispatchSemaphore& other):
            semaphore{other.semaphore}
        {
            if (semaphore) dispatch_retain(semaphore);
        }

        DispatchSemaphore& operator=(DispatchSemaphore&& other) noexcept
        {
            if (this == &other) return *this;
            semaphore = other.semaphore;
            other.semaphore = nullptr;
            return *this;
        }

        DispatchSemaphore& operator=(const DispatchSemaphore& other)
        {
            if (this == &other) return *this;
            if (semaphore) dispatch_release(semaphore);
            semaphore = other.semaphore;
            if (semaphore) dispatch_retain(semaphore);
            return *this;
        }

        operator dispatch_semaphore_t() const noexcept
        {
            return semaphore;
        }

    private:
        dispatch_semaphore_t semaphore = nullptr;
    };
}

#endif // OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP
<commit_msg>Fix memory leak in the move assignment operator of DispatchSemaphore<commit_after>// Copyright 2015-2020 Elviss Strazdins. All rights reserved.

#ifndef OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP
#define OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP

#include <stdexcept>
#include <dispatch/dispatch.h>

namespace ouzel::graphics::metal
{
    class DispatchSemaphore final
    {
    public:
        explicit DispatchSemaphore(long value):
            semaphore{dispatch_semaphore_create(value)}
        {
            if (!semaphore)
                throw std::runtime_error("Failed to create dispatch semaphore");
        }

        ~DispatchSemaphore()
        {
            if (semaphore) dispatch_release(semaphore);
        }

        DispatchSemaphore(DispatchSemaphore&& other) noexcept:
            semaphore{other.semaphore}
        {
            other.semaphore = nullptr;
        }

        DispatchSemaphore(const DispatchSemaphore& other):
            semaphore{other.semaphore}
        {
            if (semaphore) dispatch_retain(semaphore);
        }

        DispatchSemaphore& operator=(DispatchSemaphore&& other) noexcept
        {
            if (this == &other) return *this;
            if (semaphore) dispatch_release(semaphore);
            semaphore = other.semaphore;
            other.semaphore = nullptr;
            return *this;
        }

        DispatchSemaphore& operator=(const DispatchSemaphore& other)
        {
            if (this == &other) return *this;
            if (semaphore) dispatch_release(semaphore);
            semaphore = other.semaphore;
            if (semaphore) dispatch_retain(semaphore);
            return *this;
        }

        operator dispatch_semaphore_t() const noexcept
        {
            return semaphore;
        }

    private:
        dispatch_semaphore_t semaphore = nullptr;
    };
}

#endif // OUZEL_GRAPHICS_DISPATCHSEMAPHORE_HPP
<|endoftext|>


<commit_before>#include "WavSource.h"

#define DR_WAV_IMPLEMENTATION
#include <HgSound/dr_wav.h>

#include <include/ThreadPool.h>

namespace HgSound
{

ThreadPool ioThreads(2);

BufferedWavSource::~BufferedWavSource()
{
	delete[] m_samples;
}

StreamingWavSource::~StreamingWavSource()
{
}

SamplePacket BufferedWavSource::getNextSamples(IAudioSourceState& state) const
{
	SamplePacket p;

	p.sampleCount = m_count;
	p.sampleBytes = 4;
	p.audioSamples = (char*)m_samples;
	p.hasMorePackets = false;

	return p;
}

void BufferedWavSource::initializeState(std::unique_ptr<IAudioSourceState>& state) const
{
	state.reset(); //buffered sources don't have a state
}

void StreamingWavSource::initializeState(std::unique_ptr<IAudioSourceState>& state) const
{
	auto tmp = std::make_unique<StreamingWavSource::State>();
	tmp->open(m_path.c_str());

	state = std::move(tmp);
}

SamplePacket Decode(StreamingWavSource::State* state)
{
	SamplePacket p;

	drwav* wav = state->get_drwav();

	drwav_uint64 framesToRead = wav->sampleRate / 10; // 1/10th of a second
	auto samplesCount = framesToRead * wav->channels;

	auto buffer = state->getBackBuffer();
	size_t framesDecoded = drwav_read_pcm_frames_f32(wav, framesToRead, buffer);
	state->swapBuffers();

	p.sampleBytes = 4;
	p.sampleCount = framesDecoded * wav->channels;
	p.audioSamples = (char*)buffer;
	p.hasMorePackets = !(framesDecoded < framesToRead);

	return p;
}

SamplePacket StreamingWavSource::getNextSamples(IAudioSourceState& s) const
{
	auto state = reinterpret_cast<StreamingWavSource::State*>(&s);

	SamplePacket r;

	//if the future is not ready replay the previous samples
	if (state->nextToPlay.wait_for(std::chrono::seconds(0)) != std::future_status::ready)
	{
		return state->currentPlaying;
	}
	r = state->nextToPlay.get();
	state->currentPlaying = r;

	state->nextToPlay = ioThreads.enqueue([state] {
		return Decode(state);
	});

	return r;
}

StreamingWavSource::State::~State()
{
	drwav_uninit(&m_wav);

	if (m_frontBuffer) delete[] m_frontBuffer;
	if (m_backBuffer) delete[] m_backBuffer;
}

void StreamingWavSource::State::open(const char* path)
{
	auto wav = get_drwav();
	if (!drwav_init_file(wav, path, nullptr))
	{
		fprintf(stderr, "Could not open wave file \"%s\"\n", path);
		return;
	}

	drwav_uint64 framesToRead = wav->sampleRate / 10; // 1/10th of a second
	auto samplesCount = framesToRead * wav->channels;

	m_frontBuffer = new float[samplesCount]();
	m_backBuffer = new float[samplesCount]();

	auto state = this;
	nextToPlay = ioThreads.enqueue([state] {
		return Decode(state);
	});

	nextToPlay.wait();
}

}<commit_msg>wait for thread to return before destroying<commit_after>#include "WavSource.h"

#define DR_WAV_IMPLEMENTATION
#include <HgSound/dr_wav.h>

#include <include/ThreadPool.h>

namespace HgSound
{

ThreadPool ioThreads(2);

BufferedWavSource::~BufferedWavSource()
{
	delete[] m_samples;
}

StreamingWavSource::~StreamingWavSource()
{
}

SamplePacket BufferedWavSource::getNextSamples(IAudioSourceState& state) const
{
	SamplePacket p;

	p.sampleCount = m_count;
	p.sampleBytes = 4;
	p.audioSamples = (char*)m_samples;
	p.hasMorePackets = false;

	return p;
}

void BufferedWavSource::initializeState(std::unique_ptr<IAudioSourceState>& state) const
{
	state.reset(); //buffered sources don't have a state
}

void StreamingWavSource::initializeState(std::unique_ptr<IAudioSourceState>& state) const
{
	auto tmp = std::make_unique<StreamingWavSource::State>();
	tmp->open(m_path.c_str());

	state = std::move(tmp);
}

SamplePacket Decode(StreamingWavSource::State* state)
{
	SamplePacket p;

	drwav* wav = state->get_drwav();

	drwav_uint64 framesToRead = wav->sampleRate / 10; // 1/10th of a second
	auto samplesCount = framesToRead * wav->channels;

	auto buffer = state->getBackBuffer();
	size_t framesDecoded = drwav_read_pcm_frames_f32(wav, framesToRead, buffer);
	state->swapBuffers();

	p.sampleBytes = 4;
	p.sampleCount = framesDecoded * wav->channels;
	p.audioSamples = (char*)buffer;
	p.hasMorePackets = !(framesDecoded < framesToRead);

	return p;
}

SamplePacket StreamingWavSource::getNextSamples(IAudioSourceState& s) const
{
	auto state = reinterpret_cast<StreamingWavSource::State*>(&s);

	SamplePacket r;

	//if the future is not ready replay the previous samples
	if (state->nextToPlay.wait_for(std::chrono::seconds(0)) != std::future_status::ready)
	{
		return state->currentPlaying;
	}
	r = state->nextToPlay.get();
	state->currentPlaying = r;

	state->nextToPlay = ioThreads.enqueue([state] {
		return Decode(state);
	});

	return r;
}

StreamingWavSource::State::~State()
{
	if (nextToPlay.valid())
	{
		//if the future is valid, then wait for the IO thread to return. keep state valid.
		nextToPlay.wait();
	}

	drwav_uninit(&m_wav);

	if (m_frontBuffer) delete[] m_frontBuffer;
	if (m_backBuffer) delete[] m_backBuffer;
}

void StreamingWavSource::State::open(const char* path)
{
	auto wav = get_drwav();
	if (!drwav_init_file(wav, path, nullptr))
	{
		fprintf(stderr, "Could not open wave file \"%s\"\n", path);
		return;
	}

	drwav_uint64 framesToRead = wav->sampleRate / 10; // 1/10th of a second
	auto samplesCount = framesToRead * wav->channels;

	m_frontBuffer = new float[samplesCount]();
	m_backBuffer = new float[samplesCount]();

	auto state = this;
	nextToPlay = ioThreads.enqueue([state] {
		return Decode(state);
	});

	nextToPlay.wait();
}

}<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. 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 "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResponse()->HasReceived()) {
    if (!AdapterManager::GetRoutingResponse()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    FrameHistory::instance()->Add(frame_->sequence_num(), std::move(frame_));
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", trajectory_pb);
    trajectory_pb->mutable_header()->set_timestamp_sec(start_timestamp);
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    AdapterManager::PublishPlanning(*trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
                    const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               &publishable_trajectory);

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return true;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fix planning crash by mis-using move.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. 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 "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResponse()->HasReceived()) {
    if (!AdapterManager::GetRoutingResponse()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    if (frame_) {
      auto seq_num = frame_->sequence_num();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", trajectory_pb);
    trajectory_pb->mutable_header()->set_timestamp_sec(start_timestamp);
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    AdapterManager::PublishPlanning(*trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
                    const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               &publishable_trajectory);

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return true;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. 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 "modules/planning/planning.h"

#include <algorithm>
#include <thread>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction done.";
  }

  if (!frame_->Init(config_)) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

void Planning::SetConfig(const PlanningConfig& config) { config_ = config; }

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(apollo::hdmap::BaseMapFile()));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    if (frame_) {
      auto seq_num = frame_->sequence_num();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb) {
  AdapterManager::FillPlanningHeader("planning", trajectory_pb);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  AdapterManager::FillPlanningHeader("planning", trajectory_pb);
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    not_ready->set_reason("localization not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    not_ready->set_reason("chassis not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    not_ready->set_reason("routing not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
    AERROR << "Prediction is not available; skip the planning cycle";
    not_ready->set_reason("prediction not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  const double start_timestamp = Clock::NowInSecond();

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::Status status =
      common::VehicleState::instance()->Update(localization, chassis);

  if (!status.ok()) {
    AERROR << "Update VehicleState failed.";
    not_ready->set_reason("Update VehicleState failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  status = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = Clock::NowInSecond();
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (status.ok()) {
    PublishPlanningPb(trajectory_pb, start_timestamp);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    status.Save(trajectory_pb->mutable_header()->mutable_status());
    PublishPlanningPb(trajectory_pb, start_timestamp);
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

common::Status Planning::Plan(const bool is_on_auto_mode,
                              const double current_time_stamp,
                              const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  std::vector<std::unique_ptr<std::thread>> threads;
  for (auto& reference_line_info : frame_->reference_line_info()) {
    threads.emplace_back(
        new std::thread([this, stitching_trajectory, &reference_line_info] {
          auto status = this->planner_->Plan(
              stitching_trajectory.back(), frame_.get(), &reference_line_info);
          if (!status.ok()) {
            AERROR << "planner failed to make a driving plan.";
          }
        }));
  }
  for (const auto& thread : threads) {
    if (thread->joinable()) {
      thread->join();
    }
  }

  const ReferenceLineInfo* best_reference_line =
      frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg("planner failed to make a driving plan");
    AERROR << msg;
    last_publishable_trajectory_.Clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  auto* ptr_debug = frame_->MutableADCTrajectory()->mutable_debug();
  auto* ptr_latency_stats =
      frame_->MutableADCTrajectory()->mutable_latency_stats();
  ptr_debug->CopyFrom(best_reference_line->debug());
  ptr_latency_stats->CopyFrom(best_reference_line->latency_stats());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  PublishableTrajectory publishable_trajectory(
      current_time_stamp, best_reference_line->trajectory());

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return Status::OK();
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fix debug merge problem.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. 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 "modules/planning/planning.h"

#include <algorithm>
#include <thread>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction done.";
  }

  if (!frame_->Init(config_)) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

void Planning::SetConfig(const PlanningConfig& config) { config_ = config; }

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(apollo::hdmap::BaseMapFile()));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    if (frame_) {
      auto seq_num = frame_->sequence_num();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb) {
  AdapterManager::FillPlanningHeader("planning", trajectory_pb);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  AdapterManager::FillPlanningHeader("planning", trajectory_pb);
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    not_ready->set_reason("localization not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    not_ready->set_reason("chassis not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    not_ready->set_reason("routing not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
    AERROR << "Prediction is not available; skip the planning cycle";
    not_ready->set_reason("prediction not ready");
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  const double start_timestamp = Clock::NowInSecond();

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::Status status =
      common::VehicleState::instance()->Update(localization, chassis);

  if (!status.ok()) {
    AERROR << "Update VehicleState failed.";
    not_ready->set_reason("Update VehicleState failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb);
    return;
  }

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  status = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = Clock::NowInSecond();
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (status.ok()) {
    PublishPlanningPb(trajectory_pb, start_timestamp);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    status.Save(trajectory_pb->mutable_header()->mutable_status());
    PublishPlanningPb(trajectory_pb, start_timestamp);
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

common::Status Planning::Plan(const bool is_on_auto_mode,
                              const double current_time_stamp,
                              const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  std::vector<std::unique_ptr<std::thread>> threads;
  for (auto& reference_line_info : frame_->reference_line_info()) {
    threads.emplace_back(
        new std::thread([this, stitching_trajectory, &reference_line_info] {
          auto status = this->planner_->Plan(
              stitching_trajectory.back(), frame_.get(), &reference_line_info);
          if (!status.ok()) {
            AERROR << "planner failed to make a driving plan.";
          }
        }));
  }
  for (const auto& thread : threads) {
    if (thread->joinable()) {
      thread->join();
    }
  }

  const ReferenceLineInfo* best_reference_line =
      frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg("planner failed to make a driving plan");
    AERROR << msg;
    last_publishable_trajectory_.Clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  auto* ptr_debug = frame_->MutableADCTrajectory()->mutable_debug();
  auto* ptr_latency_stats =
      frame_->MutableADCTrajectory()->mutable_latency_stats();
  ptr_debug->MergeFrom(best_reference_line->debug());
  ptr_latency_stats->MergeFrom(best_reference_line->latency_stats());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  PublishableTrajectory publishable_trajectory(
      current_time_stamp, best_reference_line->trajectory());

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return Status::OK();
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>