<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2012 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP
#define MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP

// mapnik
#include <mapnik/global.hpp>
#include <mapnik/geometry.hpp>
#include <mapnik/util/path_iterator.hpp>
#include <mapnik/util/container_adapter.hpp>
#include <mapnik/vertex.hpp>    // for CommandType::SEG_MOVETO

// boost
#include <boost/tuple/tuple.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/fusion/adapted/std_tuple.hpp>
#include <boost/math/special_functions/trunc.hpp> // for vc++ and android whose c++11 libs lack std::trunct

namespace boost { namespace spirit { namespace traits {

// make gcc and darwin toolsets happy.
template <>
struct is_container<mapnik::geometry_container>
    : mpl::false_
{};

}}}

namespace mapnik { namespace json {

namespace karma = boost::spirit::karma;
namespace phoenix = boost::phoenix;

namespace {

#ifdef BOOST_SPIRIT_USE_PHOENIX_V3
struct get_type
{
    typedef int result_type;
    result_type operator() (geometry_type const& geom) const
    {
        return static_cast<int>(geom.type());
    }
};

struct get_first
{
    typedef geometry_type::value_type const result_type;
    result_type operator() (geometry_type const& geom) const
    {
        geometry_type::value_type coord;
        std::get<0>(coord) = geom.vertex(0,&std::get<1>(coord),&std::get<2>(coord));
        return coord;
    }
};

struct get_size
{
    typedef unsigned result_type;
    result_type operator() (geometry_type const& geom) const
    {
        return geom.size();
    }
};

struct multi_geometry_type
{
    typedef std::tuple<unsigned,bool>  result_type;
    result_type operator() (geometry_container const& geom) const
    {
        unsigned type = 0u;
        bool collection = false;

        geometry_container::const_iterator itr = geom.begin();
        geometry_container::const_iterator end = geom.end();

        for ( ; itr != end; ++itr)
        {
            if (type != 0u && itr->type() != type)
            {
                collection = true;
                break;
            }
            type = itr->type();
        }
        if (geom.size() > 1) type +=3;
        return std::tuple<unsigned,bool>(type, collection);
    }
};

struct not_empty
{
    typedef bool result_type;
    result_type operator() (geometry_container const& cont) const
    {
        for (auto const& geom : cont)
        {
            if (geom.size() > 0) return true;
        }
        return false;
    }
};

#else
struct get_type
{
    template <typename T>
    struct result { typedef int type; };

    int operator() (geometry_type const& geom) const
    {
        return static_cast<int>(geom.type());
    }
};

struct get_first
{
    template <typename T>
    struct result { typedef geometry_type::value_type const type; };

    geometry_type::value_type const operator() (geometry_type const& geom) const
    {
        geometry_type::value_type coord;
        std::get<0>(coord) = geom.vertex(0,&std::get<1>(coord),&std::get<2>(coord));
        return coord;
    }
};

struct multi_geometry_type
{
    template <typename T>
    struct result { typedef std::tuple<unsigned,bool> type; };

    std::tuple<unsigned,bool> operator() (geometry_container const& geom) const
    {
        unsigned type = 0u;
        bool collection = false;

        geometry_container::const_iterator itr = geom.begin();
        geometry_container::const_iterator end = geom.end();

        for ( ; itr != end; ++itr)
        {
            if (type != 0u && static_cast<unsigned>(itr->type()) != type)
            {
                collection = true;
                break;
            }
            type = itr->type();
        }
        if (geom.size() > 1) type +=3;
        return std::tuple<unsigned,bool>(type, collection);
    }
};
#endif

template <typename T>
struct json_coordinate_policy : karma::real_policies<T>
{
    typedef boost::spirit::karma::real_policies<T> base_type;
    static int floatfield(T n) { return base_type::fmtflags::fixed; }

    static unsigned precision(T n)
    {
        if (n == 0.0) return 0;
        using namespace boost::spirit;
        return static_cast<unsigned>(14 - boost::math::trunc(log10(traits::get_absolute_value(n))));
    }

    template <typename OutputIterator>
    static bool dot(OutputIterator& sink, T n, unsigned precision)
    {
        if (n == 0) return true;
        return base_type::dot(sink, n, precision);
    }

    template <typename OutputIterator>
    static bool fraction_part(OutputIterator& sink, T n
                              , unsigned adjprec, unsigned precision)
    {
        if (n == 0) return true;
        return base_type::fraction_part(sink, n, adjprec, precision);
    }
};

}

template <typename OutputIterator>
struct geometry_generator_grammar :
        karma::grammar<OutputIterator, geometry_type const& ()>
{

    geometry_generator_grammar()
        : geometry_generator_grammar::base_type(coordinates)
    {
        using boost::spirit::karma::uint_;
        using boost::spirit::bool_;
        using boost::spirit::karma::_val;
        using boost::spirit::karma::_1;
        using boost::spirit::karma::lit;
        using boost::spirit::karma::_a;
        using boost::spirit::karma::_r1;
        using boost::spirit::karma::eps;

        coordinates =  point | linestring | polygon
            ;

        point = &uint_(mapnik::geometry_type::types::Point)[_1 = _type(_val)]
            << point_coord [_1 = _first(_val)]
            ;

        linestring = &uint_(mapnik::geometry_type::types::LineString)[_1 = _type(_val)]
            << lit('[')
            << coords
            << lit(']')
            ;

        polygon = &uint_(mapnik::geometry_type::types::Polygon)[_1 = _type(_val)]
            << lit('[')
            << coords2
            << lit("]]")
            ;

        point_coord = &uint_
            << lit('[')
            << coord_type << lit(',') << coord_type
            << lit(']')
            ;

        polygon_coord %= ( &uint_(mapnik::SEG_MOVETO) << eps[_r1 += 1]
                           << karma::string[ if_ (_r1 > 1) [_1 = "],["]
                                             .else_[_1 = '[' ]]
                           |
                           &uint_(mapnik::SEG_LINETO)
                           << lit(',')) << lit('[') << coord_type << lit(',') << coord_type << lit(']')
            ;

        coords2 %= *polygon_coord(_a)
            ;

        coords = point_coord % lit(',')
            ;

    }
    // rules
    karma::rule<OutputIterator, geometry_type const& ()> coordinates;
    karma::rule<OutputIterator, geometry_type const& ()> point;
    karma::rule<OutputIterator, geometry_type const& ()> linestring;
    karma::rule<OutputIterator, geometry_type const& ()> polygon;
    karma::rule<OutputIterator, geometry_type const& ()> coords;
    karma::rule<OutputIterator, karma::locals<unsigned>, geometry_type const& ()> coords2;
    karma::rule<OutputIterator, geometry_type::value_type ()> point_coord;
    karma::rule<OutputIterator, geometry_type::value_type (unsigned& )> polygon_coord;

    // phoenix functions
    phoenix::function<get_type > _type;
    phoenix::function<get_first> _first;
    //
    karma::real_generator<double, json_coordinate_policy<double> > coord_type;

};

template <typename OutputIterator>
struct multi_geometry_generator_grammar :
        karma::grammar<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                       geometry_container const& ()>
{

    multi_geometry_generator_grammar()
        : multi_geometry_generator_grammar::base_type(start)
    {
        using boost::spirit::karma::lit;
        using boost::spirit::karma::eps;
        using boost::spirit::karma::_val;
        using boost::spirit::karma::_1;
        using boost::spirit::karma::_a;
        using boost::spirit::karma::_r1;
        using boost::spirit::bool_;

        geometry_types.add
            (mapnik::geometry_type::types::Point,"\"Point\"")
            (mapnik::geometry_type::types::LineString,"\"LineString\"")
            (mapnik::geometry_type::types::Polygon,"\"Polygon\"")
            (mapnik::geometry_type::types::Point + 3,"\"MultiPoint\"")
            (mapnik::geometry_type::types::LineString + 3,"\"MultiLineString\"")
            (mapnik::geometry_type::types::Polygon + 3,"\"MultiPolygon\"")
            ;

        start %= ( eps(phoenix::at_c<1>(_a))[_a = multi_type_(_val)]
                   << lit("{\"type\":\"GeometryCollection\",\"geometries\":[")
                   << geometry_collection << lit("]}")
                   |
                   geometry)
            ;

        geometry_collection = -(geometry2 % lit(','))
            ;

        geometry = ( &bool_(true)[_1 = not_empty_(_val)] << lit("{\"type\":")
                     << geometry_types[_1 = phoenix::at_c<0>(_a)][_a = multi_type_(_val)]
                     << lit(",\"coordinates\":")
                     << karma::string[ phoenix::if_ (phoenix::at_c<0>(_a) > 3) [_1 = '['].else_[_1 = ""]]
                     << coordinates
                     << karma::string[ phoenix::if_ (phoenix::at_c<0>(_a) > 3) [_1 = ']'].else_[_1 = ""]]
                     << lit('}')) | lit("null")
            ;

        geometry2 = lit("{\"type\":")
            << geometry_types[_1 = _a][_a = type_(_val)]
            << lit(",\"coordinates\":")
            << path
            << lit('}')
            ;

        coordinates %= path % lit(',')
            ;

    }
    // rules
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> start;
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> geometry_collection;
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> geometry;
    karma::rule<OutputIterator, karma::locals<unsigned>,
                geometry_type const&()> geometry2;
    karma::rule<OutputIterator, geometry_container const&()> coordinates;
    geometry_generator_grammar<OutputIterator>  path;
    // phoenix
    phoenix::function<multi_geometry_type> multi_type_;
    phoenix::function<get_type > type_;
    phoenix::function<not_empty> not_empty_;
    // symbols table
    karma::symbols<unsigned, char const*> geometry_types;
};

}}

#endif // MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP
<commit_msg>geojson parser : add phoenix v2 not_empty implementation<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2012 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP
#define MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP

// mapnik
#include <mapnik/global.hpp>
#include <mapnik/geometry.hpp>
#include <mapnik/util/path_iterator.hpp>
#include <mapnik/util/container_adapter.hpp>
#include <mapnik/vertex.hpp>    // for CommandType::SEG_MOVETO

// boost
#include <boost/tuple/tuple.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/fusion/adapted/std_tuple.hpp>
#include <boost/math/special_functions/trunc.hpp> // for vc++ and android whose c++11 libs lack std::trunct

namespace boost { namespace spirit { namespace traits {

// make gcc and darwin toolsets happy.
template <>
struct is_container<mapnik::geometry_container>
    : mpl::false_
{};

}}}

namespace mapnik { namespace json {

namespace karma = boost::spirit::karma;
namespace phoenix = boost::phoenix;

namespace {

#ifdef BOOST_SPIRIT_USE_PHOENIX_V3
struct get_type
{
    typedef int result_type;
    result_type operator() (geometry_type const& geom) const
    {
        return static_cast<int>(geom.type());
    }
};

struct get_first
{
    typedef geometry_type::value_type const result_type;
    result_type operator() (geometry_type const& geom) const
    {
        geometry_type::value_type coord;
        std::get<0>(coord) = geom.vertex(0,&std::get<1>(coord),&std::get<2>(coord));
        return coord;
    }
};

struct get_size
{
    typedef unsigned result_type;
    result_type operator() (geometry_type const& geom) const
    {
        return geom.size();
    }
};

struct multi_geometry_type
{
    typedef std::tuple<unsigned,bool>  result_type;
    result_type operator() (geometry_container const& geom) const
    {
        unsigned type = 0u;
        bool collection = false;

        geometry_container::const_iterator itr = geom.begin();
        geometry_container::const_iterator end = geom.end();

        for ( ; itr != end; ++itr)
        {
            if (type != 0u && itr->type() != type)
            {
                collection = true;
                break;
            }
            type = itr->type();
        }
        if (geom.size() > 1) type +=3;
        return std::tuple<unsigned,bool>(type, collection);
    }
};

struct not_empty
{
    typedef bool result_type;
    result_type operator() (geometry_container const& cont) const
    {
        for (auto const& geom : cont)
        {
            if (geom.size() > 0) return true;
        }
        return false;
    }
};

#else
struct get_type
{
    template <typename T>
    struct result { typedef int type; };

    int operator() (geometry_type const& geom) const
    {
        return static_cast<int>(geom.type());
    }
};

struct get_first
{
    template <typename T>
    struct result { typedef geometry_type::value_type const type; };

    geometry_type::value_type const operator() (geometry_type const& geom) const
    {
        geometry_type::value_type coord;
        std::get<0>(coord) = geom.vertex(0,&std::get<1>(coord),&std::get<2>(coord));
        return coord;
    }
};

struct multi_geometry_type
{
    template <typename T>
    struct result { typedef std::tuple<unsigned,bool> type; };

    std::tuple<unsigned,bool> operator() (geometry_container const& geom) const
    {
        unsigned type = 0u;
        bool collection = false;

        geometry_container::const_iterator itr = geom.begin();
        geometry_container::const_iterator end = geom.end();

        for ( ; itr != end; ++itr)
        {
            if (type != 0u && static_cast<unsigned>(itr->type()) != type)
            {
                collection = true;
                break;
            }
            type = itr->type();
        }
        if (geom.size() > 1) type +=3;
        return std::tuple<unsigned,bool>(type, collection);
    }
};

struct not_empty
{
    template <typename T>
    struct result { typedef bool type; };

    bool operator() (geometry_container const& cont) const
    {
        geometry_container::const_iterator itr = cont.begin();
        geometry_container::const_iterator end = cont.end();

        for (; itr!=end; ++itr)
        {
            if (itr->size() > 0) return true;
        }
        return false;
    }
};

#endif

template <typename T>
struct json_coordinate_policy : karma::real_policies<T>
{
    typedef boost::spirit::karma::real_policies<T> base_type;
    static int floatfield(T n) { return base_type::fmtflags::fixed; }

    static unsigned precision(T n)
    {
        if (n == 0.0) return 0;
        using namespace boost::spirit;
        return static_cast<unsigned>(14 - boost::math::trunc(log10(traits::get_absolute_value(n))));
    }

    template <typename OutputIterator>
    static bool dot(OutputIterator& sink, T n, unsigned precision)
    {
        if (n == 0) return true;
        return base_type::dot(sink, n, precision);
    }

    template <typename OutputIterator>
    static bool fraction_part(OutputIterator& sink, T n
                              , unsigned adjprec, unsigned precision)
    {
        if (n == 0) return true;
        return base_type::fraction_part(sink, n, adjprec, precision);
    }
};

}

template <typename OutputIterator>
struct geometry_generator_grammar :
        karma::grammar<OutputIterator, geometry_type const& ()>
{

    geometry_generator_grammar()
        : geometry_generator_grammar::base_type(coordinates)
    {
        using boost::spirit::karma::uint_;
        using boost::spirit::bool_;
        using boost::spirit::karma::_val;
        using boost::spirit::karma::_1;
        using boost::spirit::karma::lit;
        using boost::spirit::karma::_a;
        using boost::spirit::karma::_r1;
        using boost::spirit::karma::eps;

        coordinates =  point | linestring | polygon
            ;

        point = &uint_(mapnik::geometry_type::types::Point)[_1 = _type(_val)]
            << point_coord [_1 = _first(_val)]
            ;

        linestring = &uint_(mapnik::geometry_type::types::LineString)[_1 = _type(_val)]
            << lit('[')
            << coords
            << lit(']')
            ;

        polygon = &uint_(mapnik::geometry_type::types::Polygon)[_1 = _type(_val)]
            << lit('[')
            << coords2
            << lit("]]")
            ;

        point_coord = &uint_
            << lit('[')
            << coord_type << lit(',') << coord_type
            << lit(']')
            ;

        polygon_coord %= ( &uint_(mapnik::SEG_MOVETO) << eps[_r1 += 1]
                           << karma::string[ if_ (_r1 > 1) [_1 = "],["]
                                             .else_[_1 = '[' ]]
                           |
                           &uint_(mapnik::SEG_LINETO)
                           << lit(',')) << lit('[') << coord_type << lit(',') << coord_type << lit(']')
            ;

        coords2 %= *polygon_coord(_a)
            ;

        coords = point_coord % lit(',')
            ;

    }
    // rules
    karma::rule<OutputIterator, geometry_type const& ()> coordinates;
    karma::rule<OutputIterator, geometry_type const& ()> point;
    karma::rule<OutputIterator, geometry_type const& ()> linestring;
    karma::rule<OutputIterator, geometry_type const& ()> polygon;
    karma::rule<OutputIterator, geometry_type const& ()> coords;
    karma::rule<OutputIterator, karma::locals<unsigned>, geometry_type const& ()> coords2;
    karma::rule<OutputIterator, geometry_type::value_type ()> point_coord;
    karma::rule<OutputIterator, geometry_type::value_type (unsigned& )> polygon_coord;

    // phoenix functions
    phoenix::function<get_type > _type;
    phoenix::function<get_first> _first;
    //
    karma::real_generator<double, json_coordinate_policy<double> > coord_type;

};

template <typename OutputIterator>
struct multi_geometry_generator_grammar :
        karma::grammar<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                       geometry_container const& ()>
{

    multi_geometry_generator_grammar()
        : multi_geometry_generator_grammar::base_type(start)
    {
        using boost::spirit::karma::lit;
        using boost::spirit::karma::eps;
        using boost::spirit::karma::_val;
        using boost::spirit::karma::_1;
        using boost::spirit::karma::_a;
        using boost::spirit::karma::_r1;
        using boost::spirit::bool_;

        geometry_types.add
            (mapnik::geometry_type::types::Point,"\"Point\"")
            (mapnik::geometry_type::types::LineString,"\"LineString\"")
            (mapnik::geometry_type::types::Polygon,"\"Polygon\"")
            (mapnik::geometry_type::types::Point + 3,"\"MultiPoint\"")
            (mapnik::geometry_type::types::LineString + 3,"\"MultiLineString\"")
            (mapnik::geometry_type::types::Polygon + 3,"\"MultiPolygon\"")
            ;

        start %= ( eps(phoenix::at_c<1>(_a))[_a = multi_type_(_val)]
                   << lit("{\"type\":\"GeometryCollection\",\"geometries\":[")
                   << geometry_collection << lit("]}")
                   |
                   geometry)
            ;

        geometry_collection = -(geometry2 % lit(','))
            ;

        geometry = ( &bool_(true)[_1 = not_empty_(_val)] << lit("{\"type\":")
                     << geometry_types[_1 = phoenix::at_c<0>(_a)][_a = multi_type_(_val)]
                     << lit(",\"coordinates\":")
                     << karma::string[ phoenix::if_ (phoenix::at_c<0>(_a) > 3) [_1 = '['].else_[_1 = ""]]
                     << coordinates
                     << karma::string[ phoenix::if_ (phoenix::at_c<0>(_a) > 3) [_1 = ']'].else_[_1 = ""]]
                     << lit('}')) | lit("null")
            ;

        geometry2 = lit("{\"type\":")
            << geometry_types[_1 = _a][_a = type_(_val)]
            << lit(",\"coordinates\":")
            << path
            << lit('}')
            ;

        coordinates %= path % lit(',')
            ;

    }
    // rules
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> start;
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> geometry_collection;
    karma::rule<OutputIterator, karma::locals<std::tuple<unsigned,bool> >,
                geometry_container const&()> geometry;
    karma::rule<OutputIterator, karma::locals<unsigned>,
                geometry_type const&()> geometry2;
    karma::rule<OutputIterator, geometry_container const&()> coordinates;
    geometry_generator_grammar<OutputIterator>  path;
    // phoenix
    phoenix::function<multi_geometry_type> multi_type_;
    phoenix::function<get_type > type_;
    phoenix::function<not_empty> not_empty_;
    // symbols table
    karma::symbols<unsigned, char const*> geometry_types;
};

}}

#endif // MAPNIK_JSON_GEOMETRY_GENERATOR_GRAMMAR_HPP
<|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/common/vehicle_state/vehicle_state.h"
#include <cmath>
#include "modules/common/math/quaternion.h"
#include "modules/localization/common/localization_gflags.h"
#include "modules/common/log.h"

namespace apollo {
namespace common {
namespace vehicle_state {

VehicleState::VehicleState(
    const localization::LocalizationEstimate &localization) {
  ConstructExceptLinearVelocity(&localization);
  if (localization.has_pose() && localization.pose().has_linear_velocity()) {
    linear_v_ = localization.pose().linear_velocity().y();
  }
}

VehicleState::VehicleState(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis) {
  ConstructExceptLinearVelocity(localization);
  if (chassis != nullptr && chassis->has_speed_mps()) {
    linear_v_ = chassis->speed_mps();
  }
}

void VehicleState::ConstructExceptLinearVelocity(
    const localization::LocalizationEstimate *localization) {
  if (localization == nullptr || !localization->has_pose()) {
    AERROR << "Invalid localization input.";
    return;
  }
  localization_ptr_ = localization;
  if (localization->pose().has_position()) {
    x_ = localization->pose().position().x();
    y_ = localization->pose().position().y();
    z_ = localization->pose().position().z();
  }

  if (localization->pose().has_heading()) {
    heading_ = localization->pose().heading();
  } else {
    const auto &orientation = localization->pose().orientation();
    heading_ = ::apollo::common::math::QuaternionToHeading(
        orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz());
  }

  if (FLAGS_enable_map_reference_unify) {
    angular_v_ = localization->pose().angular_velocity_vrf().z();
    linear_a_ = localization->pose().linear_acceleration_vrf().y();
  } else {
    angular_v_ = localization->pose().angular_velocity().z();
    linear_a_ = localization->pose().linear_acceleration().y();
  }
}

double VehicleState::x() const { return x_; }

double VehicleState::y() const { return y_; }

double VehicleState::z() const { return z_; }

double VehicleState::heading() const { return heading_; }

double VehicleState::linear_velocity() const { return linear_v_; }

double VehicleState::angular_velocity() const { return angular_v_; }

double VehicleState::linear_acceleration() const { return linear_a_; }

void VehicleState::set_x(const double x) { x_ = x; }

void VehicleState::set_y(const double y) { y_ = y; }

void VehicleState::set_z(const double z) { z_ = z; }

void VehicleState::set_heading(const double heading) { heading_ = heading; }

void VehicleState::set_linear_velocity(const double linear_velocity) {
  linear_v_ = linear_velocity;
}

void VehicleState::set_angular_velocity(const double angular_velocity) {
  angular_v_ = angular_velocity;
}

Eigen::Vector2d VehicleState::EstimateFuturePosition(const double t) const {
  Eigen::Vector3d vec_distance(0.0, 0.0, 0.0);
  // Predict distance travel vector
  if (std::fabs(angular_v_) < 0.0001) {
    vec_distance[0] = 0.0;
    vec_distance[1] = linear_v_ * t;
  } else {
    vec_distance[0] =
        -linear_v_ / angular_v_ * (1.0 - std::cos(angular_v_ * t));
    vec_distance[1] = std::sin(angular_v_ * t) * linear_v_ / angular_v_;
  }

  // If we have rotation information, take it into consideration.
  if (localization_ptr_ != nullptr && localization_ptr_->has_pose() &&
      localization_ptr_->pose().has_orientation()) {
    const auto &orientation = localization_ptr_->pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    Eigen::Vector3d pos_vec(x_, y_, z_);
    auto future_pos_3d = quaternion.toRotationMatrix() * vec_distance + pos_vec;
    return Eigen::Vector2d(future_pos_3d[0], future_pos_3d[1]);
  }

  // If no valid rotation information provided from localization,
  // return the estimated future position without rotation.
  return Eigen::Vector2d(vec_distance[0] + x_, vec_distance[1] + y_);
}

Eigen::Vector2d VehicleState::ComputeCOMPosition(
    const double rear_to_com_distance) const {
  // set length as distance between rear wheel and center of mass.
  Eigen::Vector3d v(0.0, rear_to_com_distance, 0.0);
  Eigen::Vector3d pos_vec(x_, y_, z_);
  // Initialize the COM position without rotation
  Eigen::Vector3d com_pos_3d = v + pos_vec;

  // If we have rotation information, take it into consideration.
  if (localization_ptr_ != nullptr && localization_ptr_->has_pose() &&
      localization_ptr_->pose().has_orientation()) {
    const auto &orientation = localization_ptr_->pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    // Update the COM position with rotation
    com_pos_3d = quaternion.toRotationMatrix() * v + pos_vec;
  }

  return Eigen::Vector2d(com_pos_3d[0], com_pos_3d[1]);
}

}  // namespace vehicle_state
}  // namespace common
}  // namespace apollo
<commit_msg>fix linear velocity in localization<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/common/vehicle_state/vehicle_state.h"
#include <cmath>
#include "modules/common/math/quaternion.h"
#include "modules/localization/common/localization_gflags.h"
#include "modules/common/log.h"

namespace apollo {
namespace common {
namespace vehicle_state {

VehicleState::VehicleState(
    const localization::LocalizationEstimate &localization) {
  ConstructExceptLinearVelocity(&localization);
  if (localization.has_pose() && localization.pose().has_linear_velocity()) {
    double linear_v_x = localization.pose().linear_velocity().x();
    double linear_v_y = localization.pose().linear_velocity().y();
    linear_v_ = std::hypot(linear_v_x, linear_v_y);
  }
}

VehicleState::VehicleState(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis) {
  ConstructExceptLinearVelocity(localization);
  if (chassis != nullptr && chassis->has_speed_mps()) {
    linear_v_ = chassis->speed_mps();
  }
}

void VehicleState::ConstructExceptLinearVelocity(
    const localization::LocalizationEstimate *localization) {
  if (localization == nullptr || !localization->has_pose()) {
    AERROR << "Invalid localization input.";
    return;
  }
  localization_ptr_ = localization;
  if (localization->pose().has_position()) {
    x_ = localization->pose().position().x();
    y_ = localization->pose().position().y();
    z_ = localization->pose().position().z();
  }

  if (localization->pose().has_heading()) {
    heading_ = localization->pose().heading();
  } else {
    const auto &orientation = localization->pose().orientation();
    heading_ = ::apollo::common::math::QuaternionToHeading(
        orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz());
  }

  if (FLAGS_enable_map_reference_unify) {
    angular_v_ = localization->pose().angular_velocity_vrf().z();
    linear_a_ = localization->pose().linear_acceleration_vrf().y();
  } else {
    angular_v_ = localization->pose().angular_velocity().z();
    linear_a_ = localization->pose().linear_acceleration().y();
  }
}

double VehicleState::x() const { return x_; }

double VehicleState::y() const { return y_; }

double VehicleState::z() const { return z_; }

double VehicleState::heading() const { return heading_; }

double VehicleState::linear_velocity() const { return linear_v_; }

double VehicleState::angular_velocity() const { return angular_v_; }

double VehicleState::linear_acceleration() const { return linear_a_; }

void VehicleState::set_x(const double x) { x_ = x; }

void VehicleState::set_y(const double y) { y_ = y; }

void VehicleState::set_z(const double z) { z_ = z; }

void VehicleState::set_heading(const double heading) { heading_ = heading; }

void VehicleState::set_linear_velocity(const double linear_velocity) {
  linear_v_ = linear_velocity;
}

void VehicleState::set_angular_velocity(const double angular_velocity) {
  angular_v_ = angular_velocity;
}

Eigen::Vector2d VehicleState::EstimateFuturePosition(const double t) const {
  Eigen::Vector3d vec_distance(0.0, 0.0, 0.0);
  // Predict distance travel vector
  if (std::fabs(angular_v_) < 0.0001) {
    vec_distance[0] = 0.0;
    vec_distance[1] = linear_v_ * t;
  } else {
    vec_distance[0] =
        -linear_v_ / angular_v_ * (1.0 - std::cos(angular_v_ * t));
    vec_distance[1] = std::sin(angular_v_ * t) * linear_v_ / angular_v_;
  }

  // If we have rotation information, take it into consideration.
  if (localization_ptr_ != nullptr && localization_ptr_->has_pose() &&
      localization_ptr_->pose().has_orientation()) {
    const auto &orientation = localization_ptr_->pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    Eigen::Vector3d pos_vec(x_, y_, z_);
    auto future_pos_3d = quaternion.toRotationMatrix() * vec_distance + pos_vec;
    return Eigen::Vector2d(future_pos_3d[0], future_pos_3d[1]);
  }

  // If no valid rotation information provided from localization,
  // return the estimated future position without rotation.
  return Eigen::Vector2d(vec_distance[0] + x_, vec_distance[1] + y_);
}

Eigen::Vector2d VehicleState::ComputeCOMPosition(
    const double rear_to_com_distance) const {
  // set length as distance between rear wheel and center of mass.
  Eigen::Vector3d v(0.0, rear_to_com_distance, 0.0);
  Eigen::Vector3d pos_vec(x_, y_, z_);
  // Initialize the COM position without rotation
  Eigen::Vector3d com_pos_3d = v + pos_vec;

  // If we have rotation information, take it into consideration.
  if (localization_ptr_ != nullptr && localization_ptr_->has_pose() &&
      localization_ptr_->pose().has_orientation()) {
    const auto &orientation = localization_ptr_->pose().orientation();
    Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                         orientation.qy(), orientation.qz());
    // Update the COM position with rotation
    com_pos_3d = quaternion.toRotationMatrix() * v + pos_vec;
  }

  return Eigen::Vector2d(com_pos_3d[0], com_pos_3d[1]);
}

}  // namespace vehicle_state
}  // namespace common
}  // namespace apollo
<|endoftext|>


<commit_before>#pragma once

#include <vector>
#include <tuple>
#include <array>

#include <tudocomp/util.hpp>
#include <tudocomp/io.hpp>
#include <tudocomp/Algorithm.hpp>

#include <tudocomp/ds/IntVector.hpp>

//#include <sdsl/suffixarrays.hpp>

#include <sdsl/suffix_trees.hpp>
//#include <sdsl/suffix_arrays.hpp>
//#include <sdsl/csa_bitcompressed.hpp>
//#include <sdsl/csa_uncompressed.hpp>

namespace tdc {
namespace lfs {

template<uint min_lrf = 2 >
class SimSTStrategy : public Algorithm {
private:

    struct cmp_seeds {
        bool operator () (const std::pair< std::vector<uint>::iterator, std::vector<uint>::iterator> p1,
                          const std::pair< std::vector<uint>::iterator, std::vector<uint>::iterator> p2)
        const {
            return *(p1.first) <  *(p2.first);
        }
    };

    typedef sdsl::bp_interval<long unsigned int> node_type;

    //sdsl::t_csa sa =sdsl::csa_uncompressed<>
    typedef sdsl::cst_sct3< sdsl::csa_bitcompressed<> > cst_t;
    cst_t stree;

    inline virtual std::vector<uint> select_starting_positions(int node_id, int length){

        std::vector<uint> selected_starting_positions;
        std::vector<uint> not_selected_starting_positions;
       // if(beginning_positions.size()<2){
       //     return selected_starting_positions;
       // }
        //select occurences greedily non-overlapping:
        selected_starting_positions.reserve(node_begins[node_id].size());

        not_selected_starting_positions.reserve(node_begins[node_id].size());

        int last =  0-length-1;
        int current;
        //int shorter_count = 0;
        int min_shorter = 1;
        for (auto it=node_begins[node_id].begin(); it!=node_begins[node_id].end(); ++it){

            current = *it;
            if(last+length <= (int) current && !dead_positions[current] && !dead_positions[current+length-1]){
                selected_starting_positions.push_back(current);
                last = current;

            } else {
                not_selected_starting_positions.push_back(current);
            }

            if(!dead_positions[current] && dead_positions[current+length-1]){
                while(!dead_positions[current+min_shorter]){
                    min_shorter++;
                }

            }

        }

        if(min_shorter < length){
            node_type node = stree.inv_id(node_id);

            if(min_shorter >= (int)min_lrf){
                //check if parent node is shorter

                node_type parent = stree.parent(node);
                uint depth = stree.depth(parent);
                if(depth < (uint)(min_shorter)){

                    //just re-add node, if the possible replaceable lrf is longer than dpeth of parent node
                    bins[min_shorter].push_back(node_id);
                }
            }
        }
        if(selected_starting_positions.size()>=2){
            node_begins[node_id].assign(not_selected_starting_positions.begin(), not_selected_starting_positions.end());
           // node_begins[node_id] = not_selected_starting_positions;
            return selected_starting_positions;
        } else {
            return std::vector<uint>();
        }
    }

    //(position in text, non_terminal_symbol_number, length_of_symbol);
    typedef std::tuple<uint,uint,uint> non_term;
    typedef std::vector<non_term> non_terminal_symbols;
    typedef std::vector<std::pair<uint,uint>> rules;

    BitVector dead_positions;

    //could be node_type
    std::vector<std::vector<uint> > bins;

    //vector of tuples, associates id to tuple
    //tuple:: min_bp, max_bp, card
    typedef std::tuple<int,int,int> n_tpl;
    typedef std::vector< n_tpl > tuple_vector;
    //tuple_vector node_tuples;

    std::vector<std::vector<uint> > node_begins;

public:

    using Algorithm::Algorithm; //import constructor

    inline static Meta meta() {
        Meta m("lfs_comp", "sim_st");
        return m;
    }

    inline void compute_rules(io::InputView & input, rules & dictionary, non_terminal_symbols & nts_symbols){

        //build suffixtree
        DLOG(INFO)<<"build suffixtree";

        dead_positions = BitVector(input.size(), 0);

        StatPhase::wrap("Constructing ST", [&]{
             sdsl::construct_im(stree, (const char*) input.data(), 1);
        });

        StatPhase::wrap("Computing LRF", [&]{
            DLOG(INFO)<<"computing lrf";

            //array of vectors for bins of nodes with string depth

            bins.resize(stree.size()+1);
            uint node_counter = 0;

            typedef sdsl::cst_bfs_iterator<cst_t> iterator;
            iterator begin = iterator(&stree, stree.root());
            iterator end   = iterator(&stree, stree.root(), true, true);

            StatPhase::wrap("Iterate over ST", [&]{
                DLOG(INFO)<<"iterate st";

                for (iterator it = begin; it != end; ++it) {
                    bins[stree.depth(*it)].push_back(stree.id(*it));
                    node_counter++;
                }
            });

            node_begins.resize(node_counter);

            //node_tuples.resize(node_counter);

            uint nts_number =0;
            StatPhase::wrap("Iterate over Node Bins", [&]{
                for(uint i = bins.size()-1; i>=min_lrf; i--){
                    auto bin_it = bins[i].begin();
                    while (bin_it!= bins[i].end()){
                        node_type node = stree.inv_id(*bin_it);

                        if(stree.is_leaf(node)){
                            uint bp = stree.csa[stree.lb(node)];
                            node_begins[*bin_it].push_back(bp);
                        }
                        else {

                            if(node_begins[*bin_it].empty()){

                                for (auto& child: stree.children(node)) {
                                    int child_id = stree.id(child);

                                    if(!node_begins[child_id].empty()){

                                        node_begins[*bin_it].insert(node_begins[*bin_it].begin(), node_begins[child_id].begin(), node_begins[child_id].end());
                                    }

                                }

                                std::sort(node_begins[*bin_it].begin(), node_begins[*bin_it].end());
                            }
                        }
                        if(node_begins[*bin_it].empty()){

                            bin_it++;
                            continue;
                        }
                        //check tuple
                        if(!(node_begins[*bin_it].size()>=2) && !( (  (uint)( node_begins[*bin_it].back()   - node_begins[*bin_it].front() )) >= i )){

                            bin_it++;
                            continue;
                        }
                            //do this
                           //Add new rule
                            //and add new non-terminal symbols
                            std::vector<uint> selected_bp = select_starting_positions(*bin_it, i);
                            if(! (selected_bp.size() >=2) ){
                                bin_it++;
                                continue;
                            }
                            //vector of text position, length
                            std::pair<uint,uint> rule = std::make_pair(selected_bp.at(0), i);
                            dictionary.push_back(rule);

                            //iterate over selected pos, add non terminal symbols
                            for(auto bp_it = selected_bp.begin(); bp_it != selected_bp.end(); bp_it++){
                                //(position in text, non_terminal_symbol_number, length_of_symbol);
                                non_term nts = std::make_tuple(*bp_it, nts_number, i);
                                nts_symbols.push_back(nts);
                                //mark as used
                                for(uint pos = 0; pos<i;pos++){
                                    dead_positions[pos+ *bp_it] = 1;
                                }
                            }
                            nts_number++;

                            bin_it++;

                        }
                    }

            });
        });

        StatPhase::wrap("Sorting occurences", [&]{

            DLOG(INFO) << "sorting occurences";
            std::sort(nts_symbols.begin(), nts_symbols.end());

        });

    }
};
}

}
<commit_msg>removed merger comparator<commit_after>#pragma once

#include <vector>
#include <tuple>
#include <array>

#include <tudocomp/util.hpp>
#include <tudocomp/io.hpp>
#include <tudocomp/Algorithm.hpp>

#include <tudocomp/ds/IntVector.hpp>

//#include <sdsl/suffixarrays.hpp>

#include <sdsl/suffix_trees.hpp>
//#include <sdsl/suffix_arrays.hpp>
//#include <sdsl/csa_bitcompressed.hpp>
//#include <sdsl/csa_uncompressed.hpp>

namespace tdc {
namespace lfs {

template<uint min_lrf = 2 >
class SimSTStrategy : public Algorithm {
private:

    typedef sdsl::bp_interval<long unsigned int> node_type;

    //sdsl::t_csa sa =sdsl::csa_uncompressed<>
    typedef sdsl::cst_sct3< sdsl::csa_bitcompressed<> > cst_t;
    cst_t stree;

    inline virtual std::vector<uint> select_starting_positions(int node_id, int length){

        std::vector<uint> selected_starting_positions;
        std::vector<uint> not_selected_starting_positions;
       // if(beginning_positions.size()<2){
       //     return selected_starting_positions;
       // }
        //select occurences greedily non-overlapping:
        selected_starting_positions.reserve(node_begins[node_id].size());

        not_selected_starting_positions.reserve(node_begins[node_id].size());

        int last =  0-length-1;
        int current;
        //int shorter_count = 0;
        int min_shorter = 1;
        for (auto it=node_begins[node_id].begin(); it!=node_begins[node_id].end(); ++it){

            current = *it;
            if(last+length <= (int) current && !dead_positions[current] && !dead_positions[current+length-1]){
                selected_starting_positions.push_back(current);
                last = current;

            } else {
                not_selected_starting_positions.push_back(current);
            }

            if(!dead_positions[current] && dead_positions[current+length-1]){
                while(!dead_positions[current+min_shorter]){
                    min_shorter++;
                }

            }

        }

        if(min_shorter < length){
            node_type node = stree.inv_id(node_id);

            if(min_shorter >= (int)min_lrf){
                //check if parent node is shorter

                node_type parent = stree.parent(node);
                uint depth = stree.depth(parent);
                if(depth < (uint)(min_shorter)){

                    //just re-add node, if the possible replaceable lrf is longer than dpeth of parent node
                    bins[min_shorter].push_back(node_id);
                }
            }
        }
        if(selected_starting_positions.size()>=2){
            node_begins[node_id].assign(not_selected_starting_positions.begin(), not_selected_starting_positions.end());
           // node_begins[node_id] = not_selected_starting_positions;
            return selected_starting_positions;
        } else {
            return std::vector<uint>();
        }
    }

    //(position in text, non_terminal_symbol_number, length_of_symbol);
    typedef std::tuple<uint,uint,uint> non_term;
    typedef std::vector<non_term> non_terminal_symbols;
    typedef std::vector<std::pair<uint,uint>> rules;

    BitVector dead_positions;

    //could be node_type
    std::vector<std::vector<uint> > bins;

    //vector of tuples, associates id to tuple
    //tuple:: min_bp, max_bp, card
    typedef std::tuple<int,int,int> n_tpl;
    typedef std::vector< n_tpl > tuple_vector;
    //tuple_vector node_tuples;

    std::vector<std::vector<uint> > node_begins;

public:

    using Algorithm::Algorithm; //import constructor

    inline static Meta meta() {
        Meta m("lfs_comp", "sim_st");
        return m;
    }

    inline void compute_rules(io::InputView & input, rules & dictionary, non_terminal_symbols & nts_symbols){

        //build suffixtree
        DLOG(INFO)<<"build suffixtree";

        dead_positions = BitVector(input.size(), 0);

        StatPhase::wrap("Constructing ST", [&]{
             sdsl::construct_im(stree, (const char*) input.data(), 1);
        });

        StatPhase::wrap("Computing LRF", [&]{
            DLOG(INFO)<<"computing lrf";

            //array of vectors for bins of nodes with string depth

            bins.resize(stree.size()+1);
            uint node_counter = 0;

            typedef sdsl::cst_bfs_iterator<cst_t> iterator;
            iterator begin = iterator(&stree, stree.root());
            iterator end   = iterator(&stree, stree.root(), true, true);

            StatPhase::wrap("Iterate over ST", [&]{
                DLOG(INFO)<<"iterate st";

                for (iterator it = begin; it != end; ++it) {
                    bins[stree.depth(*it)].push_back(stree.id(*it));
                    node_counter++;
                }
            });

            node_begins.resize(node_counter);

            //node_tuples.resize(node_counter);

            uint nts_number =0;
            StatPhase::wrap("Iterate over Node Bins", [&]{
                for(uint i = bins.size()-1; i>=min_lrf; i--){
                    auto bin_it = bins[i].begin();
                    while (bin_it!= bins[i].end()){
                        node_type node = stree.inv_id(*bin_it);

                        if(stree.is_leaf(node)){
                            uint bp = stree.csa[stree.lb(node)];
                            node_begins[*bin_it].push_back(bp);
                        }
                        else {

                            if(node_begins[*bin_it].empty()){

                                for (auto& child: stree.children(node)) {
                                    int child_id = stree.id(child);

                                    if(!node_begins[child_id].empty()){

                                        node_begins[*bin_it].insert(node_begins[*bin_it].begin(), node_begins[child_id].begin(), node_begins[child_id].end());
                                    }

                                }

                                std::sort(node_begins[*bin_it].begin(), node_begins[*bin_it].end());
                            }
                        }
                        if(node_begins[*bin_it].empty()){

                            bin_it++;
                            continue;
                        }
                        //check tuple
                        if(!(node_begins[*bin_it].size()>=2) && !( (  (uint)( node_begins[*bin_it].back()   - node_begins[*bin_it].front() )) >= i )){

                            bin_it++;
                            continue;
                        }
                            //do this
                           //Add new rule
                            //and add new non-terminal symbols
                            std::vector<uint> selected_bp = select_starting_positions(*bin_it, i);
                            if(! (selected_bp.size() >=2) ){
                                bin_it++;
                                continue;
                            }
                            //vector of text position, length
                            std::pair<uint,uint> rule = std::make_pair(selected_bp.at(0), i);
                            dictionary.push_back(rule);

                            //iterate over selected pos, add non terminal symbols
                            for(auto bp_it = selected_bp.begin(); bp_it != selected_bp.end(); bp_it++){
                                //(position in text, non_terminal_symbol_number, length_of_symbol);
                                non_term nts = std::make_tuple(*bp_it, nts_number, i);
                                nts_symbols.push_back(nts);
                                //mark as used
                                for(uint pos = 0; pos<i;pos++){
                                    dead_positions[pos+ *bp_it] = 1;
                                }
                            }
                            nts_number++;

                            bin_it++;

                        }
                    }

            });
        });

        StatPhase::wrap("Sorting occurences", [&]{

            DLOG(INFO) << "sorting occurences";
            std::sort(nts_symbols.begin(), nts_symbols.end());

        });

    }
};
}

}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2016 The ZLMediaKit project authors. All Rights Reserved.
 *
 * This file is part of ZLMediaKit(https://github.com/xiongziliang/ZLMediaKit).
 *
 * Use of this source code is governed by MIT license that can be found in the
 * LICENSE file in the root of the source tree. All contributing project authors
 * may be found in the AUTHORS file in the root of the source tree.
 */

#if defined(ENABLE_RTPPROXY)
#include "RtpProcess.h"
#include "Util/File.h"
#include "Http/HttpTSPlayer.h"
#define RTP_APP_NAME "rtp"

namespace mediakit{

string printSSRC(uint32_t ui32Ssrc) {
    char tmp[9] = { 0 };
    ui32Ssrc = htonl(ui32Ssrc);
    uint8_t *pSsrc = (uint8_t *) &ui32Ssrc;
    for (int i = 0; i < 4; i++) {
        sprintf(tmp + 2 * i, "%02X", pSsrc[i]);
    }
    return tmp;
}

static string printAddress(const struct sockaddr *addr){
    return StrPrinter << SockUtil::inet_ntoa(((struct sockaddr_in *) addr)->sin_addr) << ":" << ntohs(((struct sockaddr_in *) addr)->sin_port);
}

RtpProcess::RtpProcess(uint32_t ssrc) {
    _ssrc = ssrc;
    _track = std::make_shared<SdpTrack>();
    _track->_interleaved = 0;
    _track->_samplerate = 90000;
    _track->_type = TrackVideo;
    _track->_ssrc = _ssrc;

    _media_info._schema = RTP_APP_NAME;
    _media_info._vhost = DEFAULT_VHOST;
    _media_info._app = RTP_APP_NAME;
    _media_info._streamid = printSSRC(_ssrc);

    GET_CONFIG(string,dump_dir,RtpProxy::kDumpDir);
    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".rtp", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_rtp.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }

    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".mp2", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_ps.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }

    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".video", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_video.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }
}

RtpProcess::~RtpProcess() {
    DebugP(this);
    if (_addr) {
        delete _addr;
    }

    uint64_t duration = (_last_rtp_time.createdTime() - _last_rtp_time.elapsedTime()) / 1000;
    WarnP(this) << "RTP推流器("
                << _media_info._vhost << "/"
                << _media_info._app << "/"
                << _media_info._streamid
                << ")断开,耗时(s):" << duration;

    //流量统计事件广播
    GET_CONFIG(uint32_t, iFlowThreshold, General::kFlowThreshold);
    if (_total_bytes > iFlowThreshold * 1024) {
        NoticeCenter::Instance().emitEvent(Broadcast::kBroadcastFlowReport, _media_info, _total_bytes, duration, false, static_cast<SockInfo &>(*this));
    }
}

bool RtpProcess::inputRtp(const Socket::Ptr &sock, const char *data, int data_len,const struct sockaddr *addr,uint32_t *dts_out) {
    GET_CONFIG(bool,check_source,RtpProxy::kCheckSource);
    //检查源是否合法
    if(!_addr){
        _addr = new struct sockaddr;
        _sock = sock;
        memcpy(_addr,addr, sizeof(struct sockaddr));
        DebugP(this) << "bind to address:" << printAddress(_addr);
        //推流鉴权
        emitOnPublish();
    }

    if(!_muxer){
        //无权限推流
        return false;
    }

    if(check_source && memcmp(_addr,addr,sizeof(struct sockaddr)) != 0){
        DebugP(this) << "address dismatch:" << printAddress(addr) << " != " << printAddress(_addr);
        return false;
    }

    _total_bytes += data_len;
    _last_rtp_time.resetTime();
    bool ret = handleOneRtp(0,_track,(unsigned char *)data,data_len);
    if(dts_out){
        *dts_out = _dts;
    }
    return ret;
}

//判断是否为ts负载
static inline bool checkTS(const uint8_t *packet, int bytes){
    return bytes % TS_PACKET_SIZE == 0 && packet[0] == TS_SYNC_BYTE;
}

void RtpProcess::onRtpSorted(const RtpPacket::Ptr &rtp, int) {
    if(rtp->sequence != _sequence + 1 && rtp->sequence != 0){
        WarnP(this) << "rtp丢包:" << rtp->sequence << " != " << _sequence << "+1" << ",公网环境下请使用tcp方式推流";
    }
    _sequence = rtp->sequence;
    if(_save_file_rtp){
        uint16_t  size = rtp->size() - 4;
        size = htons(size);
        fwrite((uint8_t *) &size, 2, 1, _save_file_rtp.get());
        fwrite((uint8_t *) rtp->data() + 4, rtp->size() - 4, 1, _save_file_rtp.get());
    }
    decodeRtp(rtp->data() + 4 ,rtp->size() - 4);
}

void RtpProcess::onRtpDecode(const uint8_t *packet, int bytes, uint32_t timestamp, int flags) {
    if(_save_file_ps){
        fwrite((uint8_t *)packet,bytes, 1, _save_file_ps.get());
    }

    if (!_decoder) {
        //创建解码器
        if (checkTS(packet, bytes)) {
            //猜测是ts负载
            InfoP(this) << "judged to be TS";
            _decoder = DecoderImp::createDecoder(DecoderImp::decoder_ts, this);
        } else {
            //猜测是ps负载
            InfoP(this) << "judged to be PS";
            _decoder = DecoderImp::createDecoder(DecoderImp::decoder_ps, this);
        }
    }

    if (_decoder) {
        auto ret = _decoder->input((uint8_t *) packet, bytes);
        if (ret != bytes) {
            WarnP(this) << ret << " != " << bytes << " " << flags;
        }
    }
}

void  RtpProcess::inputFrame(const Frame::Ptr &frame){
    _dts = frame->dts();
    if (_save_file_video && frame->getTrackType() == TrackVideo) {
        fwrite((uint8_t *) frame->data(), frame->size(), 1, _save_file_video.get());
    }
    _muxer->inputFrame(frame);
}

void  RtpProcess::addTrack(const Track::Ptr & track){
    _muxer->addTrack(track);
}

bool RtpProcess::alive() {
    GET_CONFIG(int,timeoutSec,RtpProxy::kTimeoutSec)
    if(_last_rtp_time.elapsedTime() / 1000 < timeoutSec){
        return true;
    }
    return false;
}

string RtpProcess::get_peer_ip() {
    if(_addr){
        return SockUtil::inet_ntoa(((struct sockaddr_in *) _addr)->sin_addr);
    }
    return "0.0.0.0";
}

uint16_t RtpProcess::get_peer_port() {
    if(!_addr){
        return 0;
    }
    return ntohs(((struct sockaddr_in *) _addr)->sin_port);
}

string RtpProcess::get_local_ip() {
    if(_sock){
        return _sock->get_local_ip();
    }
    return "0.0.0.0";
}

uint16_t RtpProcess::get_local_port() {
    if(_sock){
       return _sock->get_local_port();
    }
    return 0;
}

string RtpProcess::getIdentifier() const{
    return _media_info._streamid;
}

int RtpProcess::totalReaderCount(){
    return _muxer ? _muxer->totalReaderCount() : 0;
}

void RtpProcess::setListener(const std::weak_ptr<MediaSourceEvent> &listener){
    if(_muxer){
        _muxer->setMediaListener(listener);
    }else{
        _listener = listener;
    }
}

void RtpProcess::emitOnPublish() {
    weak_ptr<RtpProcess> weak_self = shared_from_this();
    Broadcast::PublishAuthInvoker invoker = [weak_self](const string &err, bool enableRtxp, bool enableHls, bool enableMP4) {
        auto strongSelf = weak_self.lock();
        if (!strongSelf) {
            return;
        }
        if (err.empty()) {
            strongSelf->_muxer = std::make_shared<MultiMediaSourceMuxer>(strongSelf->_media_info._vhost,
                                                                         strongSelf->_media_info._app,
                                                                         strongSelf->_media_info._streamid, 0,
                                                                         enableRtxp, enableRtxp, enableHls, enableMP4);
            strongSelf->_muxer->setMediaListener(strongSelf->_listener);
            InfoP(strongSelf) << "允许RTP推流";
        } else {
            WarnP(strongSelf) << "禁止RTP推流:" << err;
        }
    };

    //触发推流鉴权事件
    auto flag = NoticeCenter::Instance().emitEvent(Broadcast::kBroadcastMediaPublish, _media_info, invoker, static_cast<SockInfo &>(*this));
    if(!flag){
        //该事件无人监听,默认不鉴权
        GET_CONFIG(bool, toRtxp, General::kPublishToRtxp);
        GET_CONFIG(bool, toHls, General::kPublishToHls);
        GET_CONFIG(bool, toMP4, General::kPublishToMP4);
        invoker("", toRtxp, toHls, toMP4);
    }
}

}//namespace mediakit
#endif//defined(ENABLE_RTPPROXY)<commit_msg>rtp推流解析出frame才刷新保活计时器<commit_after>/*
 * Copyright (c) 2016 The ZLMediaKit project authors. All Rights Reserved.
 *
 * This file is part of ZLMediaKit(https://github.com/xiongziliang/ZLMediaKit).
 *
 * Use of this source code is governed by MIT license that can be found in the
 * LICENSE file in the root of the source tree. All contributing project authors
 * may be found in the AUTHORS file in the root of the source tree.
 */

#if defined(ENABLE_RTPPROXY)
#include "RtpProcess.h"
#include "Util/File.h"
#include "Http/HttpTSPlayer.h"
#define RTP_APP_NAME "rtp"

namespace mediakit{

string printSSRC(uint32_t ui32Ssrc) {
    char tmp[9] = { 0 };
    ui32Ssrc = htonl(ui32Ssrc);
    uint8_t *pSsrc = (uint8_t *) &ui32Ssrc;
    for (int i = 0; i < 4; i++) {
        sprintf(tmp + 2 * i, "%02X", pSsrc[i]);
    }
    return tmp;
}

static string printAddress(const struct sockaddr *addr){
    return StrPrinter << SockUtil::inet_ntoa(((struct sockaddr_in *) addr)->sin_addr) << ":" << ntohs(((struct sockaddr_in *) addr)->sin_port);
}

RtpProcess::RtpProcess(uint32_t ssrc) {
    _ssrc = ssrc;
    _track = std::make_shared<SdpTrack>();
    _track->_interleaved = 0;
    _track->_samplerate = 90000;
    _track->_type = TrackVideo;
    _track->_ssrc = _ssrc;

    _media_info._schema = RTP_APP_NAME;
    _media_info._vhost = DEFAULT_VHOST;
    _media_info._app = RTP_APP_NAME;
    _media_info._streamid = printSSRC(_ssrc);

    GET_CONFIG(string,dump_dir,RtpProxy::kDumpDir);
    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".rtp", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_rtp.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }

    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".mp2", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_ps.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }

    {
        FILE *fp = !dump_dir.empty() ? File::create_file(File::absolutePath(_media_info._streamid + ".video", dump_dir).data(), "wb") : nullptr;
        if(fp){
            _save_file_video.reset(fp,[](FILE *fp){
                fclose(fp);
            });
        }
    }
}

RtpProcess::~RtpProcess() {
    DebugP(this);
    if (_addr) {
        delete _addr;
    }

    uint64_t duration = (_last_rtp_time.createdTime() - _last_rtp_time.elapsedTime()) / 1000;
    WarnP(this) << "RTP推流器("
                << _media_info._vhost << "/"
                << _media_info._app << "/"
                << _media_info._streamid
                << ")断开,耗时(s):" << duration;

    //流量统计事件广播
    GET_CONFIG(uint32_t, iFlowThreshold, General::kFlowThreshold);
    if (_total_bytes > iFlowThreshold * 1024) {
        NoticeCenter::Instance().emitEvent(Broadcast::kBroadcastFlowReport, _media_info, _total_bytes, duration, false, static_cast<SockInfo &>(*this));
    }
}

bool RtpProcess::inputRtp(const Socket::Ptr &sock, const char *data, int data_len,const struct sockaddr *addr,uint32_t *dts_out) {
    GET_CONFIG(bool,check_source,RtpProxy::kCheckSource);
    //检查源是否合法
    if(!_addr){
        _addr = new struct sockaddr;
        _sock = sock;
        memcpy(_addr,addr, sizeof(struct sockaddr));
        DebugP(this) << "bind to address:" << printAddress(_addr);
        //推流鉴权
        emitOnPublish();
    }

    if(!_muxer){
        //无权限推流
        return false;
    }

    if(check_source && memcmp(_addr,addr,sizeof(struct sockaddr)) != 0){
        DebugP(this) << "address dismatch:" << printAddress(addr) << " != " << printAddress(_addr);
        return false;
    }

    _total_bytes += data_len;
    bool ret = handleOneRtp(0,_track,(unsigned char *)data,data_len);
    if(dts_out){
        *dts_out = _dts;
    }
    return ret;
}

//判断是否为ts负载
static inline bool checkTS(const uint8_t *packet, int bytes){
    return bytes % TS_PACKET_SIZE == 0 && packet[0] == TS_SYNC_BYTE;
}

void RtpProcess::onRtpSorted(const RtpPacket::Ptr &rtp, int) {
    if(rtp->sequence != _sequence + 1 && rtp->sequence != 0){
        WarnP(this) << "rtp丢包:" << rtp->sequence << " != " << _sequence << "+1" << ",公网环境下请使用tcp方式推流";
    }
    _sequence = rtp->sequence;
    if(_save_file_rtp){
        uint16_t  size = rtp->size() - 4;
        size = htons(size);
        fwrite((uint8_t *) &size, 2, 1, _save_file_rtp.get());
        fwrite((uint8_t *) rtp->data() + 4, rtp->size() - 4, 1, _save_file_rtp.get());
    }
    decodeRtp(rtp->data() + 4 ,rtp->size() - 4);
}

void RtpProcess::onRtpDecode(const uint8_t *packet, int bytes, uint32_t timestamp, int flags) {
    if(_save_file_ps){
        fwrite((uint8_t *)packet,bytes, 1, _save_file_ps.get());
    }

    if (!_decoder) {
        //创建解码器
        if (checkTS(packet, bytes)) {
            //猜测是ts负载
            InfoP(this) << "judged to be TS";
            _decoder = DecoderImp::createDecoder(DecoderImp::decoder_ts, this);
        } else {
            //猜测是ps负载
            InfoP(this) << "judged to be PS";
            _decoder = DecoderImp::createDecoder(DecoderImp::decoder_ps, this);
        }
    }

    if (_decoder) {
        auto ret = _decoder->input((uint8_t *) packet, bytes);
        if (ret != bytes) {
            WarnP(this) << ret << " != " << bytes << " " << flags;
        }
    }
}

void  RtpProcess::inputFrame(const Frame::Ptr &frame){
    _last_rtp_time.resetTime();
    _dts = frame->dts();
    if (_save_file_video && frame->getTrackType() == TrackVideo) {
        fwrite((uint8_t *) frame->data(), frame->size(), 1, _save_file_video.get());
    }
    _muxer->inputFrame(frame);
}

void  RtpProcess::addTrack(const Track::Ptr & track){
    _muxer->addTrack(track);
}

bool RtpProcess::alive() {
    GET_CONFIG(int,timeoutSec,RtpProxy::kTimeoutSec)
    if(_last_rtp_time.elapsedTime() / 1000 < timeoutSec){
        return true;
    }
    return false;
}

string RtpProcess::get_peer_ip() {
    if(_addr){
        return SockUtil::inet_ntoa(((struct sockaddr_in *) _addr)->sin_addr);
    }
    return "0.0.0.0";
}

uint16_t RtpProcess::get_peer_port() {
    if(!_addr){
        return 0;
    }
    return ntohs(((struct sockaddr_in *) _addr)->sin_port);
}

string RtpProcess::get_local_ip() {
    if(_sock){
        return _sock->get_local_ip();
    }
    return "0.0.0.0";
}

uint16_t RtpProcess::get_local_port() {
    if(_sock){
       return _sock->get_local_port();
    }
    return 0;
}

string RtpProcess::getIdentifier() const{
    return _media_info._streamid;
}

int RtpProcess::totalReaderCount(){
    return _muxer ? _muxer->totalReaderCount() : 0;
}

void RtpProcess::setListener(const std::weak_ptr<MediaSourceEvent> &listener){
    if(_muxer){
        _muxer->setMediaListener(listener);
    }else{
        _listener = listener;
    }
}

void RtpProcess::emitOnPublish() {
    weak_ptr<RtpProcess> weak_self = shared_from_this();
    Broadcast::PublishAuthInvoker invoker = [weak_self](const string &err, bool enableRtxp, bool enableHls, bool enableMP4) {
        auto strongSelf = weak_self.lock();
        if (!strongSelf) {
            return;
        }
        if (err.empty()) {
            strongSelf->_muxer = std::make_shared<MultiMediaSourceMuxer>(strongSelf->_media_info._vhost,
                                                                         strongSelf->_media_info._app,
                                                                         strongSelf->_media_info._streamid, 0,
                                                                         enableRtxp, enableRtxp, enableHls, enableMP4);
            strongSelf->_muxer->setMediaListener(strongSelf->_listener);
            InfoP(strongSelf) << "允许RTP推流";
        } else {
            WarnP(strongSelf) << "禁止RTP推流:" << err;
        }
    };

    //触发推流鉴权事件
    auto flag = NoticeCenter::Instance().emitEvent(Broadcast::kBroadcastMediaPublish, _media_info, invoker, static_cast<SockInfo &>(*this));
    if(!flag){
        //该事件无人监听,默认不鉴权
        GET_CONFIG(bool, toRtxp, General::kPublishToRtxp);
        GET_CONFIG(bool, toHls, General::kPublishToHls);
        GET_CONFIG(bool, toMP4, General::kPublishToMP4);
        invoker("", toRtxp, toHls, toMP4);
    }
}

}//namespace mediakit
#endif//defined(ENABLE_RTPPROXY)<|endoftext|>


<commit_before>/*
 Library written for use with Common-Anode 7-Segment Display. For more information on how to use this library,
 please refer to the github README.

 Written by: Derek Duncan
 Data: 2/4/2016
*/

#include "Arduino.h"
#include "SegmentDisplay.h"

SegmentDisplay::SegmentDisplay(int pin1, int pin2, int pin4, int pin5, int pin6, int pin7, int pin9, int pin10)
{
    pinMode(pin1, OUTPUT);
    pinMode(pin2, OUTPUT);
    pinMode(pin4, OUTPUT);
    pinMode(pin5, OUTPUT);
    pinMode(pin6, OUTPUT);
    pinMode(pin7, OUTPUT);
    pinMode(pin9, OUTPUT);
    pinMode(pin10, OUTPUT);

	/*Make sure all of the LEDs are turned off for common anode*/
    digitalWrite(pin1, HIGH);
    digitalWrite(pin2, HIGH);
    digitalWrite(pin4, HIGH);
    digitalWrite(pin5, HIGH);
    digitalWrite(pin6, HIGH);
    digitalWrite(pin7, HIGH);
    digitalWrite(pin9, HIGH);
    digitalWrite(pin10, HIGH);
    /***********************************************************/

    _pin1 = pin1;
    _pin2 = pin2;
    _pin4 = pin4;
    _pin5 = pin5;
    _pin6 = pin6;
    _pin7 = pin7;
    _pin9 = pin9;
    _pin10 = pin10;

}

void SegmentDisplay::displayHex(int number, boolean decimalPointFlag)
{
    int decimalFlag = HIGH;

    if(decimalPointFlag)
    {
        decimalFlag = LOW

    }
    if(number == 0)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 1)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 2)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, HIGH);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 3)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 4)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 5)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, HIGH);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 6)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, HIGH);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 7)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 8)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 9)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 10)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 11)
    {
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, LOW);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, HIGH);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 12)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, HIGH);
	}

	else if(number == 13)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, LOW);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, LOW);
        digitalWrite(_pin7, HIGH);
        digitalWrite(_pin9, HIGH);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 14)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 15)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, HIGH);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);

	}

    else
    {
        digitalWrite(_pin1, HIGH);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, HIGH);
        digitalWrite(_pin10, LOW);

    }
}

void SegmentDisplay::displayDecimalPoint()
{
    digitalWrite(_pin1, HIGH);
    digitalWrite(_pin2, HIGH);
    digitalWrite(_pin4, HIGH);
    digitalWrite(_pin5, LOW);
    digitalWrite(_pin6, HIGH);
    digitalWrite(_pin7, HIGH);
    digitalWrite(_pin9, HIGH);
    digitalWrite(_pin10, HIGH);

}

void SegmentDisplay::testDisplay()
{
	for(int i = 0; i <= 15; i++)
	{
		displayHex(i);
		delay(500);
	}
}

<commit_msg>fixed a few errors in cpp<commit_after>/*
 Library written for use with Common-Anode 7-Segment Display. For more information on how to use this library,
 please refer to the github README.

 Written by: Derek Duncan
 Data: 2/4/2016
*/

#include "Arduino.h"
#include "SegmentDisplay.h"

SegmentDisplay::SegmentDisplay(int pin1, int pin2, int pin4, int pin5, int pin6, int pin7, int pin9, int pin10)
{
    pinMode(pin1, OUTPUT);
    pinMode(pin2, OUTPUT);
    pinMode(pin4, OUTPUT);
    pinMode(pin5, OUTPUT);
    pinMode(pin6, OUTPUT);
    pinMode(pin7, OUTPUT);
    pinMode(pin9, OUTPUT);
    pinMode(pin10, OUTPUT);

	/*Make sure all of the LEDs are turned off for common anode*/
    digitalWrite(pin1, HIGH);
    digitalWrite(pin2, HIGH);
    digitalWrite(pin4, HIGH);
    digitalWrite(pin5, HIGH);
    digitalWrite(pin6, HIGH);
    digitalWrite(pin7, HIGH);
    digitalWrite(pin9, HIGH);
    digitalWrite(pin10, HIGH);

    _pin1 = pin1;
    _pin2 = pin2;
    _pin4 = pin4;
    _pin5 = pin5;
    _pin6 = pin6;
    _pin7 = pin7;
    _pin9 = pin9;
    _pin10 = pin10;

}

void SegmentDisplay::displayHex(int number, boolean decimalPointFlag)
{
    int decimalFlag = HIGH;

    if(decimalPointFlag)
    {
        decimalFlag = LOW;

    }
    if(number == 0)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 1)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 2)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, HIGH);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 3)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 4)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 5)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, HIGH);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 6)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, HIGH);
      digitalWrite(_pin7, HIGH);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 7)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, HIGH);
      digitalWrite(_pin10, HIGH);
    }

    else if(number == 8)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, LOW);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 9)
    {
      digitalWrite(_pin1, HIGH);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 10)
    {
      digitalWrite(_pin1, LOW);
      digitalWrite(_pin2, HIGH);
      digitalWrite(_pin4, LOW);
      digitalWrite(_pin5, decimalFlag);
      digitalWrite(_pin6, LOW);
      digitalWrite(_pin7, LOW);
      digitalWrite(_pin9, LOW);
      digitalWrite(_pin10, LOW);
    }

    else if(number == 11)
    {
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, LOW);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, HIGH);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 12)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, HIGH);
	}

	else if(number == 13)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, LOW);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, LOW);
        digitalWrite(_pin7, HIGH);
        digitalWrite(_pin9, HIGH);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 14)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);
	}

	else if(number == 15)
	{
        digitalWrite(_pin1, LOW);
        digitalWrite(_pin2, HIGH);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, LOW);
        digitalWrite(_pin10, LOW);

	}

    else
    {
        digitalWrite(_pin1, HIGH);
        digitalWrite(_pin2, LOW);
        digitalWrite(_pin4, HIGH);
        digitalWrite(_pin5, decimalFlag);
        digitalWrite(_pin6, HIGH);
        digitalWrite(_pin7, LOW);
        digitalWrite(_pin9, HIGH);
        digitalWrite(_pin10, LOW);

    }
}

void SegmentDisplay::displayDecimalPoint()
{
    digitalWrite(_pin1, HIGH);
    digitalWrite(_pin2, HIGH);
    digitalWrite(_pin4, HIGH);
    digitalWrite(_pin5, LOW);
    digitalWrite(_pin6, HIGH);
    digitalWrite(_pin7, HIGH);
    digitalWrite(_pin9, HIGH);
    digitalWrite(_pin10, HIGH);

}

void SegmentDisplay::testDisplay()
{
	for(int i = 0; i <= 15; i++)
	{
		displayHex(i, false);
		delay(500);
	}
}

<|endoftext|>