text:
//------------------------------------------------------------------------------
/*
    This file is part of jbcoin-libpp: https://github.com/jbcoin/jbcoin-libpp
    Copyright (c) 2016 Jbcoin Labs Inc.

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <jbcoin/protocol/AccountID.h>
#include <jbcoin/protocol/digest.h>
#include <jbcoin/protocol/HashPrefix.h>
#include <jbcoin/protocol/JsonFields.h>
#include <jbcoin/protocol/Sign.h>
#include <jbcoin/protocol/st.h>
#include <jbcoin/protocol/TxFlags.h>
#include <jbcoin/basics/StringUtilities.h>
#include <jbcoin/json/to_string.h>
#include <jbcoin-libpp/version.hpp>
#include <boost/version.hpp>
#include <algorithm>

std::string serialize(jbcoin::STTx const& tx)
{
    using namespace jbcoin;

    return strHex(tx.getSerializer().peekData());
}

std::shared_ptr<jbcoin::STTx const> deserialize(std::string blob)
{
    using namespace jbcoin;

    auto ret{ strUnHex(blob) };

    if (!ret.second || !ret.first.size())
        Throw<std::runtime_error>("transaction not valid hex");

    SerialIter sitTrans{ makeSlice(ret.first) };
    // Can Throw
    return std::make_shared<STTx const>(std::ref(sitTrans));
}

//------------------------------------------------------------------------------

bool demonstrateSigning(jbcoin::KeyType keyType, std::string seedStr,
    std::string expectedAccount)
{
    using namespace jbcoin;

    auto const seed = parseGenericSeed(seedStr);
    assert(seed);
    auto const keypair = generateKeyPair(keyType, *seed);
    auto const id = calcAccountID(keypair.first);
    assert(toBase58(id) == expectedAccount);

    std::cout << "\n" << to_string(keyType) << " secret \"" << seedStr
        << "\" generates secret key \"" << toBase58(*seed)
        << "\" and public key \"" << toBase58(id) << "\"\n";

    auto const destination = parseBase58<AccountID>(
        "rHb9CJAWyB4rj91VRWn96DkukG4bwdtyTh");
    assert(destination);
    auto const gateway1 = parseBase58<AccountID>(
        "rhub8VRN55s94qWKDv6jmDy1pUykJzF3wq");
    assert(gateway1);
    auto const gateway2 = parseBase58<AccountID>(
        "razqQKzJRdB4UxFPWf5NEpEG3WMkmwgcXA");
    assert(gateway2);
    STTx noopTx(ttPAYMENT,
        [&](auto& obj)
    {
        // General transaction fields
        obj[sfAccount] = id;
        obj[sfFee] = STAmount{ 100 };
        obj[sfFlags] = tfFullyCanonicalSig;
        obj[sfSequence] = 18;
        obj[sfSigningPubKey] = keypair.first.slice();
        // Payment-specific fields
        obj[sfAmount] = STAmount(Issue(to_currency("USD"), *gateway1),
            1234, 5);
        obj[sfDestination] = *destination;
        obj[sfSendMax] = STAmount(Issue(to_currency("CNY"), *gateway2),
            56789, 7);
    });

    std::cout << "\nBefore signing: \n" <<
        noopTx.getJson(0).toStyledString() << "\n" <<
        "Serialized: " << noopTx.getJson(0, true)[jss::tx] << "\n";

    noopTx.sign(keypair.first, keypair.second);

    auto const serialized = serialize(noopTx);
    std::cout << "\nAfter signing: \n" <<
        noopTx.getJson(0).toStyledString() << "\n" <<
        "Serialized: " << serialized << "\n";

    auto const deserialized = deserialize(serialized);
    assert(deserialized);
    assert(deserialized->getTransactionID() == noopTx.getTransactionID());
    std::cout << "Deserialized: " <<
        deserialized->getJson(0).toStyledString() << "\n";

    auto const check1 = noopTx.checkSign(false);

    std::cout << "Check 1: " << (check1.first ? "Good" : "Bad!") << "\n";
    assert(check1.first);

    // Use the function primitives, which are hidden by the `STTx`
    // interface, to check the signature again and verify that
    // `STTx` is working properly.
    auto const& signatureSlice = noopTx[sfTxnSignature];
    Blob const data = [&]
    {
        // This is a copy of the static `getSigningData` function body
        // which is needed by `verify`.
        Serializer s;
        s.add32(HashPrefix::txSign);
        noopTx.addWithoutSigningFields(s);
        return s.getData();
    }();

    // STTx::checkSign calls `verify` indirectly via `checkSingleSign`
    auto const check2 = verify(
        keypair.first,
        makeSlice(data),
        signatureSlice,
        true);

    std::cout << "Check 2: " << (check2 ? "Good" : "Bad!") << "\n";

    return check1.first && check2;
}

bool exerciseSingleSign ()
{
    std::vector<bool> passes;

    passes.emplace_back(demonstrateSigning(jbcoin::KeyType::secp256k1,
        "alice", "rG1QQv2nh2gr7RCZ1P8YYcBUKCCN633jCn"));

    passes.emplace_back(demonstrateSigning(jbcoin::KeyType::ed25519,
        "alice", "r9mC1zjD9u5SJXw56pdPhxoDSHaiNcisET"));

    // Genesis account w/ not-so-secret key.
    // Never hardcode a real secret key.
    passes.emplace_back(demonstrateSigning(jbcoin::KeyType::secp256k1,
        "snoPBrXtMeMyMHUVTgbuqAfg1SUTb", "rHb9CJAWyB4rj91VRWn96DkukG4bwdtyTh"));

    {
        passes.emplace_back(false);
        try
        {
            auto const empty = deserialize("");
        }
        catch (std::runtime_error e)
        {
            passes.back() = true;
            assert(e.what() == std::string("transaction not valid hex"));
        }
    }

    auto const allPass = std::all_of(passes.begin(), passes.end(),
        [](auto p) { return p; });
    assert(allPass);
    std::cout << (allPass ?
        "All single signing checks pass.\n" :
        "Some single signing checks fail.\n");

    return allPass;
}

//------------------------------------------------------------------------------

// Demonstrate multisigning.

// Helper function that asserts if for some reason we can't create a seed.
jbcoin::Seed getSeed (std::string const& seedText)
{
    // WARNING!
    // Never use jbcoin::parseGenericSeed() for secure code.  Call
    // jbcoin::randomSeed() instead, since it is cryptographically secure.
    boost::optional<jbcoin::Seed> const possibleSeed {
        jbcoin::parseGenericSeed (seedText)};

    // Should not be necessary in production code, since you used
    // jbcoin::randomSeed().  Right?
    assert (possibleSeed);

    return *possibleSeed;
}

// Holds identifying information for an account.
class Credentials
{
    std::string const name_;
    jbcoin::KeyType const keyType_;
    jbcoin::Seed const seed_;
    std::pair<jbcoin::PublicKey, jbcoin::SecretKey> const keys_;
    jbcoin::AccountID const id_;

public:
    Credentials (
        std::string name,
        jbcoin::KeyType keyType = jbcoin::KeyType::secp256k1)
    : name_ (name)
    , keyType_ (keyType)
    , seed_ (getSeed (name_))
    , keys_ (jbcoin::generateKeyPair (keyType_, seed_))
    , id_ (jbcoin::calcAccountID (keys_.first))
    {
    }

    std::string const& name() const { return name_; }
    jbcoin::KeyType const& keyType() const { return keyType_; }
    jbcoin::Seed const& seed() const { return seed_; }
    jbcoin::SecretKey const& secretKey() const { return keys_.second; }
    jbcoin::PublicKey const& publicKey() const { return keys_.first; }
    jbcoin::AccountID const& id() const { return id_; }
};

// Build a transaction that can be multisigned.  All fields must be filled in,
// including sequence and fee, before any signatures are applied.  If the
// contents of the transaction are modified then any previously provided
// multi-signatures will become invalid.
jbcoin::STTx buildMultisignTx (
    jbcoin::AccountID const& id, std::uint32_t seq, std::uint32_t fee)
{
    using namespace jbcoin;

    STTx noopTx {ttACCOUNT_SET,
        [id, seq, fee] (auto& obj)
    {
        obj[sfAccount] = id;
        obj[sfFlags] = tfFullyCanonicalSig;
        obj[sfFee] = STAmount {fee};               // Must be already filled in
        obj[sfSequence] = seq;                     // Must be already filled in
        obj[sfSigningPubKey] = Slice {nullptr, 0}; // Must be present and empty
    }};

    std::cout << "\nBefore signing: \n"
        << noopTx.getJson(0, false).toStyledString()  << std::endl;

    return noopTx;
}

// Apply one multi-signature to the supplied transaction.  The signer
// provides their AccountID, PublicKey, and SecretKey.
bool multisign (jbcoin::STTx& tx, Credentials const& signer)
{
    using namespace jbcoin;

    // Get the TxnSignature.
    Serializer s = buildMultiSigningData (tx, signer.id());

    auto const multisig = jbcoin::sign (
        signer.publicKey(), signer.secretKey(), s.slice());

    // Make the signer object that we'll inject into the array.
    STObject element (sfSigner);
    element[sfAccount] = signer.id();
    element[sfSigningPubKey] = signer.publicKey();
    element[sfTxnSignature] = multisig;

    // If a Signers array does not yet exist make one.
    if (! tx.isFieldPresent (sfSigners))
        tx.setFieldArray (sfSigners, {});

    // Insert the signer into the array.
    STArray& signers {tx.peekFieldArray (sfSigners)};
    signers.emplace_back (std::move (element));

    // Sort the Signers array by Account.  If it is not sorted when submitted
    // to the network then it will be rejected.
    std::sort (signers.begin(), signers.end(),
        [](STObject const& a, STObject const& b)
    {
        return (a[sfAccount] < b[sfAccount]);
    });

    // Verify that the signature is valid.
    bool const pass = tx.checkSign(true).first;
    assert (pass);

    // To submit multisigned JSON to the network use this RPC command:
    // $ jbcoind submit_multisigned '<all JSON>'
    std::cout << "\nMultisigned JSON: \n"
        << tx.getJson(0, false).toStyledString()  << std::endl;

    // Alternatively, to submit the multisigned blob to the network:
    //  1. Extract the hex string (including the quotes) following "tx"
    //  2. Then use this RPC command:
    //     $ jbcoind submit <quoted hex string>
    std::cout << "Multisigned blob:"
        << tx.getJson(0, true) << std::endl;

    return pass;
}

// Outlines a multisign process where:
//  1. A multi-signable transaction is built.
//  2. That transaction is signed by one signer.
//  3. The transaction is signed by a different signer.
bool exerciseMultiSign()
{
    using namespace jbcoin;

    // Create credentials for the folks involved in the transaction.
    Credentials const alice {"alice"};
    Credentials const billy {"billy"};
    Credentials const carol {"carol"};

    // Create a transaction on alice's account.  alice doesn't sign it.
    STTx tx {buildMultisignTx (alice.id(), 2, 100)};

    // billy and carol sign alice's transaction for her.
    bool allPass = multisign (tx, billy);
    allPass &= multisign (tx, carol);

    return allPass;
}

//------------------------------------------------------------------------------

int main (int argc, char** argv)
{
#if defined(__GNUC__) && !defined(__clang__)
    auto constexpr gccver = (__GNUC__ * 100 * 100) +
                            (__GNUC_MINOR__ * 100) +
                            __GNUC_PATCHLEVEL__;

    static_assert (gccver >= 50100,
        "GCC version 5.1.0 or later is required to compile jbcoind.");
#endif

    static_assert (BOOST_VERSION >= 105700,
        "Boost version 1.57 or later is required to compile jbcoind");

    // Display the version
    std::cout << "jbcoin-libpp_demo version " <<
        JBCOIN_LIBPP_VERSION_STRING << "\n";

    // Demonstrate single signing.
    auto allPass = exerciseSingleSign();

    // Demonstrate multisigning.
    allPass &= exerciseMultiSign();

    assert(allPass);
    std::cout << (allPass ?
        "All checks pass.\n" : "Some checks fail.\n");

    return allPass ? 0 : 1;
}

max_stars_repo_name: trongnmchainos/validator-keys-tool

max_issues_repo_name: trongnmchainos/validator-keys-tool

max_forks_repo_name: trongnmchainos/validator-keys-tool


text:
// Copyright 2021 Tier IV, Inc.
//
// 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.

#ifndef BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_
#define BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp"

#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>
#include <route_handler/route_handler.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>
#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometry.hpp>

#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_routing/RoutingGraphContainer.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <limits>
#include <memory>
#include <string>
#include <vector>

namespace tier4_autoware_utils
{
template <>
inline geometry_msgs::msg::Point getPoint(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
{
  return p.point.pose.position;
}

template <>
inline geometry_msgs::msg::Pose getPose(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
{
  return p.point.pose;
}
}  // namespace tier4_autoware_utils

namespace tf2
{
inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<tf2::Transform> & out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  tf2::Transform tmp;
  fromMsg(msg.pose, tmp);
  out.setData(tmp);
}

// Remove after this commit is released
// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a
inline geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out)
{
  out.x = in.getX();
  out.y = in.getY();
  out.z = in.getZ();
  return out;
}

// Remove after this commit is released
// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a
inline void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
{
  out = tf2::Vector3(in.x, in.y, in.z);
}

template <>
inline void doTransform(
  const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  tf2::Transform t;
  fromMsg(transform.transform, t);
  tf2::Vector3 v_in;
  fromMsg(t_in, v_in);
  tf2::Vector3 v_out = t * v_in;
  toMsg(v_out, t_out);
}
}  // namespace tf2

namespace behavior_path_planner
{
namespace util
{
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_perception_msgs::msg::PredictedPath;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseArray;
using geometry_msgs::msg::PoseStamped;
using geometry_msgs::msg::Twist;
using geometry_msgs::msg::Vector3;
using nav_msgs::msg::OccupancyGrid;
using route_handler::RouteHandler;
using tier4_autoware_utils::LineString2d;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;

struct FrenetCoordinate3d
{
  double length{0.0};    // longitudinal
  double distance{0.0};  // lateral
};

// data conversions

Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);

std::vector<Point> convertToPointArray(const PathWithLaneId & path);

std::vector<Point> convertToGeometryPointArray(const PathWithLaneId & path);

PoseArray convertToGeometryPoseArray(const PathWithLaneId & path);

PredictedPath convertToPredictedPath(
  const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose,
  const double duration, const double resolution, const double acceleration);

bool convertToFrenetCoordinate3d(
  const PathWithLaneId & path, const Point & search_point_geom,
  FrenetCoordinate3d * frenet_coordinate);

bool convertToFrenetCoordinate3d(
  const std::vector<Point> & linestring, const Point search_point_geom,
  FrenetCoordinate3d * frenet_coordinate);

std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets);

// distance (arclength) calculation

double l2Norm(const Vector3 vector);

double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets);

double getDistanceToNextIntersection(
  const Pose & current_pose, const lanelet::ConstLanelets & lanelets);

double getDistanceToCrosswalk(
  const Pose & current_pose, const lanelet::ConstLanelets & lanelets,
  const lanelet::routing::RoutingGraphContainer & overall_graphs);

double getSignedDistance(
  const Pose & current_pose, const Pose & goal_pose, const lanelet::ConstLanelets & lanelets);

double getArcLengthToTargetLanelet(
  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane,
  const Pose & pose);

// object collision check

Pose lerpByPose(const Pose & p1, const Pose & p2, const double t);

Point lerpByLength(const std::vector<Point> & array, const double length);

bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt);

bool lerpByDistance(
  const behavior_path_planner::PullOutPath & path, const double & s, Pose * lerped_pt,
  const lanelet::ConstLanelets & road_lanes);

bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon);

PredictedPath resamplePredictedPath(
  const PredictedPath & input_path, const double resolution, const double duration);

double getDistanceBetweenPredictedPaths(
  const PredictedPath & path1, const PredictedPath & path2, const double start_time,
  const double end_time, const double resolution);

double getDistanceBetweenPredictedPathAndObject(
  const PredictedObject & object, const PredictedPath & path, const double start_time,
  const double end_time, const double resolution);

double getDistanceBetweenPredictedPathAndObjectPolygon(
  const PredictedObject & object, const PullOutPath & ego_path,
  const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution,
  const lanelet::ConstLanelets & road_lanes);

/**
 * @brief Get index of the obstacles inside the lanelets with start and end length
 * @return Indices corresponding to the obstacle inside the lanelets
 */
std::vector<size_t> filterObjectsByLanelets(
  const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets,
  const double start_arc_length, const double end_arc_length);

/**
 * @brief Get index of the obstacles inside the lanelets
 * @return Indices corresponding to the obstacle inside the lanelets
 */
std::vector<size_t> filterObjectsByLanelets(
  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);

std::vector<size_t> filterObjectsByPath(
  const PredictedObjects & objects, const std::vector<size_t> & object_indices,
  const PathWithLaneId & ego_path, const double vehicle_width);

PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v);

PredictedObjects filterObjectsByVelocity(
  const PredictedObjects & objects, double min_v, double max_v);

// drivable area generation

void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image);

void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid);

cv::Point toCVPoint(
  const Point & geom_point, const double width_m, const double height_m, const double resolution);

OccupancyGrid generateDrivableArea(
  const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length,
  const std::shared_ptr<const PlannerData> planner_data);

lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
  const std::shared_ptr<const PlannerData> & planner_data);
// goal management

/**
 * @brief Modify the path points near the goal to smoothly connect the input path and the goal point
 * @details Remove the path points that are forward from the goal by the distance of
 * search_radius_range. Then insert the goal into the path. The previous goal point generated
 * from the goal posture information is also inserted for the smooth connection of the goal pose.
 * @param [in] search_radius_range distance on path to be modified for goal insertion
 * @param [in] search_rad_range [unused]
 * @param [in] input original path
 * @param [in] goal original goal pose
 * @param [in] goal_lane_id [unused]
 * @param [in] output_ptr output path with modified points for the goal
 */
bool setGoal(
  const double search_radius_range, const double search_rad_range, const PathWithLaneId & input,
  const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr);

/**
 * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which
 *  causes the path to twist near the goal.
 * @details Return the goal point projected on the straight line of the segment of lanelet
 *  closest to the original goal.
 * @param [in] goal original goal pose
 * @param [in] goal_lanelet lanelet containing the goal pose
 */
const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet);

PathWithLaneId refinePathForGoal(
  const double search_radius_range, const double search_rad_range, const PathWithLaneId & input,
  const Pose & goal, const int64_t goal_lane_id);

PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path);

bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);

// path management

// TODO(Horibe) There is a similar function in route_handler. Check.
std::shared_ptr<PathWithLaneId> generateCenterLinePath(
  const std::shared_ptr<const PlannerData> & planner_data);

PathPointWithLaneId insertStopPoint(double length, PathWithLaneId * path);

double getDistanceToShoulderBoundary(
  const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose);

// misc

lanelet::Polygon3d getVehiclePolygon(
  const Pose & vehicle_pose, const double vehicle_width, const double base_link2front);

std::vector<Polygon2d> getTargetLaneletPolygons(
  const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
  const std::string & target_type);

void shiftPose(Pose * pose, double shift_length);

// route handler
PathWithLaneId getCenterLinePath(
  const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence,
  const Pose & pose, const double backward_path_length, const double forward_path_length,
  const BehaviorPathPlannerParameters & parameter);

PathWithLaneId setDecelerationVelocity(
  const RouteHandler & route_handler, const PathWithLaneId & input,
  const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
  const double lane_change_buffer);

PathWithLaneId setDecelerationVelocity(
  const RouteHandler & route_handler, const PathWithLaneId & input,
  const lanelet::ConstLanelets & lanelet_sequence, const double distance_after_pullover,
  const double pullover_distance_min, const double distance_before_pull_over,
  const double deceleration_interval, Pose goal_pose);

// object label
std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);

}  // namespace util
}  // namespace behavior_path_planner

#endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

ext: hpp


text:
// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_COMPARABLE_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_COMPARABLE_DISTANCE_HPP

#include <boost/geometry/algorithms/distance.hpp>

namespace boost { namespace geometry
{

/*!
\brief \brief_calc2{comparable distance measurement}
\ingroup distance
\details The free function comparable_distance does not necessarily calculate the distance,
    but it calculates a distance measure such that two distances are comparable to each other.
    For example: for the Cartesian coordinate system, Pythagoras is used but the square root
    is not taken, which makes it faster and the results of two point pairs can still be
    compared to each other.
\tparam Geometry1 first geometry type
\tparam Geometry2 second geometry type
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\return \return_calc{comparable distance}

\qbk{[include reference/algorithms/comparable_distance.qbk]}
 */
template <typename Geometry1, typename Geometry2>
inline typename default_distance_result<Geometry1, Geometry2>::type comparable_distance(
                Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef typename point_type<Geometry1>::type point1_type;
    typedef typename point_type<Geometry2>::type point2_type;

    // Define a point-point-distance-strategy
    // for either the normal case, either the reversed case

    typedef typename strategy::distance::services::comparable_type
        <
            typename boost::mpl::if_c
                <
                    geometry::reverse_dispatch
                        <Geometry1, Geometry2>::type::value,
                    typename strategy::distance::services::default_strategy
                            <point_tag, point2_type, point1_type>::type,
                    typename strategy::distance::services::default_strategy
                            <point_tag, point1_type, point2_type>::type
                >::type
        >::type strategy_type;

    return distance(geometry1, geometry2, strategy_type());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_COMPARABLE_DISTANCE_HPP

max_stars_repo_name: bobzabcik/OpenStudio

max_issues_repo_name: bobzabcik/OpenStudio

max_forks_repo_name: bobzabcik/OpenStudio


text:
#include <aslam/calibration/model/sensors/MotionCaptureSensor.h>

#include <boost/make_shared.hpp>
#include <glog/logging.h>

#include <aslam/backend/TransformationExpression.hpp>

#include "aslam/calibration/input/MotionCaptureSource.h"
#include "aslam/calibration/calibrator/CalibratorI.h"
#include "aslam/calibration/data/MeasurementsContainer.h"
#include "aslam/calibration/error-terms/ErrorTermPose.h"
#include <aslam/calibration/model/Model.h>
#include <aslam/calibration/model/ModuleTools.h>
#include <aslam/calibration/model/Sensor.h>
#include <aslam/calibration/tools/ErrorTermStatistics.h>
#include <aslam/calibration/tools/ErrorTermStatisticsWithProblemAndPredictor.h>

namespace aslam {
namespace calibration {

struct MotionCaptureSensorMeasurements{
  PoseMeasurements poses;
};

MotionCaptureSystem::MotionCaptureSystem(Model& model, std::string name, sm::value_store::ValueStoreRef config):
  Module(model, name, config),
  PoseCv(this),
  DelayCv(this),
  CalibratableMinimal(this)
{
}

void MotionCaptureSystem::writeConfig(std::ostream& out) const {
  MODULE_WRITE_PARAM(hasTranslation());
  MODULE_WRITE_PARAM(hasRotation());
  MODULE_WRITE_PARAM(hasDelay());
}

void MotionCaptureSystem::registerWithModel() {
  Module::registerWithModel();
  getModel().addCalibrationVariables({rotationVariable, translationVariable, getDelayVariablePtr()});
}

void MotionCaptureSystem::setActive(bool spatial, bool temporal) {
  if(isUsed()){
    PoseCv::setActive(spatial);
    DelayCv::setActive(temporal);
  }
}

MotionCaptureSensor::MotionCaptureSensor(MotionCaptureSystem& motionCaptureSystem, std::string name, sm::value_store::ValueStoreRef config) :
  AbstractPoseSensor(motionCaptureSystem.getModel(), name, config),
  motionCaptureSystem(motionCaptureSystem)
{
  if(isUsed()) {
    if(!hasDelay()){
      DelayCv::operator=(motionCaptureSystem);
    }
  }
}

MotionCaptureSensor::~MotionCaptureSensor() {
}

const PoseMeasurements & MotionCaptureSensor::fetchMeasurementsFromSourceInto(Timestamp from, Timestamp till, ModuleStorage & storage) const
{
  auto & poses = getMeasurementsMutable(storage);
  if(getMotionCaptureSource()){
    auto posesFromSource = getMotionCaptureSource()->getPoses(from, till);
    poses.reserve(posesFromSource.size());
    for(auto & p : posesFromSource){
      poses.emplace_back(p.time, {p.p, p.q});
    }
  }
  return poses;
}

void MotionCaptureSensor::preProcessNewWindow(CalibratorI& calib) {
  if(getMotionCaptureSource()){
    auto currentEffectiveBatchInterval = calib.getCurrentEffectiveBatchInterval();
    if(hasDelay()){
      currentEffectiveBatchInterval.start += getDelayLowerBound();
      currentEffectiveBatchInterval.end += getDelayUpperBound();
    }
    auto & poses = fetchMeasurementsFromSourceInto(currentEffectiveBatchInterval.start, currentEffectiveBatchInterval.end, calib.getCurrentStorage());
    LOG(INFO) << "Found " << poses.size() << " motion capture measurements for " << getName();
  }
}

Interval MotionCaptureSensor::getSnappedWindow(CalibratorI& calib, const Interval& i) { //TODO C use interval parameter!
  SM_ASSERT_FALSE(std::runtime_error, hasDelay(), "Delay not supported here!");
  preProcessNewWindow(calib);
  auto & storage = calib.getCurrentStorage();
  SM_ASSERT_TRUE(std::runtime_error, hasMeasurements(storage), "Could not find measurements!");

  auto poses = getAllMeasurements(storage);
  Interval res(poses.front().first, poses.back().first);
  SM_ASSERT_GE(std::runtime_error, res.start, i.start, "");
  SM_ASSERT_LE(std::runtime_error, res.end, i.end, "");
  return res;
}

void MotionCaptureSensor::addMeasurementErrorTerms(CalibratorI& calib, const CalibrationConfI & /*ec*/, ErrorTermReceiver & problem, const bool observeOnly) const {
  const std::string errorTermGroupName = getName() + "Pose";
  auto & storage = calib.getCurrentStorage();
  if(!hasMeasurements(storage)){
    LOG(WARNING) << "No measurements available for " << errorTermGroupName;
    return;
  }

  ErrorTermStatisticsWithProblemAndPredictor es(calib, errorTermGroupName, problem, observeOnly);

  ErrorTermGroupReference etgr(errorTermGroupName);

  aslam::backend::TransformationExpression mCSFromGlobalTransformation = motionCaptureSystem.getTransformationToParentExpression().inverse();

  auto interval = calib.getCurrentEffectiveBatchInterval();

  auto uLow = interval.start + getDelayUpperBound();
  auto uUpp = interval.end + getDelayLowerBound();

  auto & delay = getDelayExpression();
  Timestamp currentDelay = delay.evaluate();
  if(currentDelay < getDelayLowerBound() || currentDelay > getDelayUpperBound()){
    throw std::runtime_error("Delay already out of bounds!");
  }

  const Eigen::Matrix3d cov_t = getCovPosition().getValue();
  const Eigen::Matrix3d cov_r = getCovOrientation().getValue();

  for (auto & m : getAllMeasurements(storage)) {
    const Timestamp timestamp = m.first;
    const bool timestampIsPossiblyOutOfBounds = uLow > timestamp || uUpp < timestamp;
    if(timestampIsPossiblyOutOfBounds && !hasDelay()){
      LOG(INFO) << "Dropping out of bounds pose measurement at " << calib.secsSinceStart(timestamp) << "!";
      continue;
    }

    const auto & pose = m.second;

    aslam::backend::TransformationExpression T_m_s = getTransformationExpressionToAtMeasurementTimestamp(calib, timestamp, motionCaptureSystem.getReferenceFrame(), true);
    boost::shared_ptr<ErrorTermPose> e_pose(new ErrorTermPose(aslam::backend::TransformationExpression(mCSFromGlobalTransformation * T_m_s), pose, cov_t, cov_r, etgr));

    if (timestampIsPossiblyOutOfBounds) {
      LOG(INFO) << "Adding conditional PoseErrorTerm for pose measurement at " << calib.secsSinceStart(timestamp) << " because it could go out of bounds!";
      if(uLow > timestamp){
        e_pose = addConditionShared<ErrorTermPose>(*e_pose, [=](){ return timestamp - delay.evaluate() >= interval.start; });
      }
      if(uUpp < timestamp){
        e_pose = addConditionShared<ErrorTermPose>(*e_pose, [=](){ return timestamp - delay.evaluate() <= interval.end; });
      }
    }

    es.add(timestamp, e_pose, false);
  }
  es.printInto(LOG(INFO));
}

} /* namespace calibration */
} /* namespace aslam */

ext: cpp


text:
//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_LOCALE_BOUNDARY_SEGMENT_HPP_INCLUDED
#define BOOST_LOCALE_BOUNDARY_SEGMENT_HPP_INCLUDED
#include <boost/locale/config.hpp>
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable : 4275 4251 4231 4660)
#endif
#include <locale>
#include <string>
#include <iosfwd>
#include <iterator>

namespace boost {
namespace locale {
namespace boundary {
    /// \cond INTERNAL
    namespace details {
        template<typename LeftIterator,typename RightIterator>
        int compare_text(LeftIterator l_begin,LeftIterator l_end,RightIterator r_begin,RightIterator r_end)
        {
            typedef LeftIterator left_iterator;
            typedef RightIterator right_iterator;
            typedef typename std::iterator_traits<left_iterator>::value_type char_type;
            typedef std::char_traits<char_type> traits;
            while(l_begin!=l_end && r_begin!=r_end) {
                char_type lchar = *l_begin++;
                char_type rchar = *r_begin++;
                if(traits::eq(lchar,rchar))
                    continue;
                if(traits::lt(lchar,rchar))
                    return -1;
                else
                    return 1;
            }
            if(l_begin==l_end && r_begin==r_end)
                return 0;
            if(l_begin==l_end)
                return -1;
            else
                return 1;
        }

        template<typename Left,typename Right>
        int compare_text(Left const &l,Right const &r)
        {
            return compare_text(l.begin(),l.end(),r.begin(),r.end());
        }

        template<typename Left,typename Char>
        int compare_string(Left const &l,Char const *begin)
        {
            Char const *end = begin;
            while(*end!=0)
                end++;
            return compare_text(l.begin(),l.end(),begin,end);
        }

        template<typename Right,typename Char>
        int compare_string(Char const *begin,Right const &r)
        {
            Char const *end = begin;
            while(*end!=0)
                end++;
            return compare_text(begin,end,r.begin(),r.end());
        }

    }
    /// \endcond

    ///
    /// \addtogroup boundary
    /// @{

    ///
    /// \brief a segment object that represents a pair of two iterators that define the range where
    /// this segment exits and a rule that defines it.
    ///
    /// This type of object is dereferenced by the iterators of segment_index. Using a rule() member function
    /// you can get a specific rule this segment was selected with. For example, when you use
    /// word boundary analysis, you can check if the specific word contains Kana letters by checking (rule() & \ref word_kana)!=0
    /// For a sentence analysis you can check if the sentence is selected because a sentence terminator is found (\ref sentence_term) or
    /// there is a line break (\ref sentence_sep).
    ///
    /// This object can be automatically converted to std::basic_string with the same type of character. It is also
    /// valid range that has begin() and end() member functions returning iterators on the location of the segment.
    ///
    /// \see
    ///
    /// - \ref segment_index
    /// - \ref boundary_point
    /// - \ref boundary_point_index
    ///
    template<typename IteratorType>
    class segment : public std::pair<IteratorType,IteratorType> {
    public:
        ///
        /// The type of the underlying character
        ///
        typedef typename std::iterator_traits<IteratorType>::value_type char_type;
        ///
        /// The type of the string it is converted to
        ///
        typedef std::basic_string<char_type> string_type;
        ///
        /// The value that iterators return  - the character itself
        ///
        typedef char_type value_type;
        ///
        /// The iterator that allows to iterate the range
        ///
        typedef IteratorType iterator;
        ///
        /// The iterator that allows to iterate the range
        ///
        typedef IteratorType const_iterator;
        ///
        /// The type that represent a difference between two iterators
        ///
        typedef typename std::iterator_traits<IteratorType>::difference_type difference_type;

        ///
        /// Default constructor
        ///
        segment() {}
        ///
        /// Create a segment using two iterators and a rule that represents this point
        ///
        segment(iterator b,iterator e,rule_type r) :
            std::pair<IteratorType,IteratorType>(b,e),
            rule_(r)
        {
        }
        ///
        /// Set the start of the range
        ///
        void begin(iterator const &v)
        {
            this->first = v;
        }
        ///
        /// Set the end of the range
        ///
         void end(iterator const &v)
        {
            this->second = v;
        }

        ///
        /// Get the start of the range
        ///
        IteratorType begin() const
        {
            return this->first;
        }
        ///
        /// Set the end of the range
        ///
        IteratorType end() const
        {
            return this->second;
        }

        ///
        /// Convert the range to a string automatically
        ///
        template <class T, class A>
        operator std::basic_string<char_type, T, A> ()const
        {
            return std::basic_string<char_type, T, A>(this->first, this->second);
        }

        ///
        /// Create a string from the range explicitly
        ///
        string_type str() const
        {
            return string_type(begin(),end());
        }

        ///
        /// Get the length of the text chunk
        ///

        size_t length() const
        {
            return std::distance(begin(),end());
        }

        ///
        /// Check if the segment is empty
        ///
        bool empty() const
        {
            return begin() == end();
        }

        ///
        /// Get the rule that is used for selection of this segment.
        ///
        rule_type rule() const
        {
            return rule_;
        }
        ///
        /// Set a rule that is used for segment selection
        ///
        void rule(rule_type r)
        {
            rule_ = r;
        }

        // make sure we override std::pair's operator==

        /// Compare two segments
        bool operator==(segment const &other)
        {
            return details::compare_text(*this,other) == 0;
        }

        /// Compare two segments
        bool operator!=(segment const &other)
        {
            return details::compare_text(*this,other) != 0;
        }

    private:
        rule_type rule_;

    };

    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator==(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) == 0;
    }
    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator!=(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) != 0;
    }

    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator<(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) < 0;
    }
    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator<=(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) <= 0;
    }
    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator>(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) > 0;
    }
    /// Compare two segments
    template<typename IteratorL,typename IteratorR>
    bool operator>=(segment<IteratorL> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) >= 0;
    }

    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator==(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) == 0;
    }
    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator!=(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) != 0;
    }

    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator<(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) < 0;
    }
    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator<=(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) <= 0;
    }
    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator>(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) > 0;
    }
    /// Compare string and segment
    template<typename CharType,typename Traits,typename Alloc,typename IteratorR>
    bool operator>=(std::basic_string<CharType,Traits,Alloc> const &l,segment<IteratorR> const &r)
    {
        return details::compare_text(l,r) >= 0;
    }

    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator==(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) == 0;
    }
    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator!=(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) != 0;
    }

    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator<(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) < 0;
    }
    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator<=(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) <= 0;
    }
    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator>(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) > 0;
    }
    /// Compare string and segment
    template<typename Iterator,typename CharType,typename Traits,typename Alloc>
    bool operator>=(segment<Iterator> const &l,std::basic_string<CharType,Traits,Alloc> const &r)
    {
        return details::compare_text(l,r) >= 0;
    }

    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator==(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) == 0;
    }
    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator!=(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) != 0;
    }

    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator<(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) < 0;
    }
    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator<=(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) <= 0;
    }
    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator>(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) > 0;
    }
    /// Compare C string and segment
    template<typename CharType,typename IteratorR>
    bool operator>=(CharType const *l,segment<IteratorR> const &r)
    {
        return details::compare_string(l,r) >= 0;
    }

    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator==(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) == 0;
    }
    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator!=(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) != 0;
    }

    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator<(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) < 0;
    }
    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator<=(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) <= 0;
    }
    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator>(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) > 0;
    }
    /// Compare C string and segment
    template<typename Iterator,typename CharType>
    bool operator>=(segment<Iterator> const &l,CharType const *r)
    {
        return details::compare_string(l,r) >= 0;
    }

    typedef segment<std::string::const_iterator> ssegment;      ///< convenience typedef
    typedef segment<std::wstring::const_iterator> wssegment;    ///< convenience typedef
    #ifdef BOOST_HAS_CHAR16_T
    typedef segment<std::u16string::const_iterator> u16ssegment;///< convenience typedef
    #endif
    #ifdef BOOST_HAS_CHAR32_T
    typedef segment<std::u32string::const_iterator> u32ssegment;///< convenience typedef
    #endif

    typedef segment<char const *> csegment;                     ///< convenience typedef
    typedef segment<wchar_t const *> wcsegment;                 ///< convenience typedef
    #ifdef BOOST_HAS_CHAR16_T
    typedef segment<char16_t const *> u16csegment;              ///< convenience typedef
    #endif
    #ifdef BOOST_HAS_CHAR32_T
    typedef segment<char32_t const *> u32csegment;              ///< convenience typedef
    #endif

    ///
    /// Write the segment to the stream character by character
    ///
    template<typename CharType,typename TraitsType,typename Iterator>
    std::basic_ostream<CharType,TraitsType> &operator<<(
            std::basic_ostream<CharType,TraitsType> &out,
            segment<Iterator> const &tok)
    {
        for(Iterator p=tok.begin(),e=tok.end();p!=e;++p)
            out << *p;
        return out;
    }

    /// @}

} // boundary
} // locale
} // boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestInplaceReduce
#include <boost/test/unit_test.hpp>

#include <iostream>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/detail/inplace_reduce.hpp>
#include <boost/compute/container/vector.hpp>

#include "quirks.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(sum_int)
{
    if(is_apple_cpu_device(device)) {
        std::cerr
            << "skipping all inplace_reduce tests due to Apple platform"
            << " behavior when local memory is used on a CPU device"
            << std::endl;
        return;
    }

    int data[] = { 1, 5, 3, 4, 9, 3, 5, 3 };
    boost::compute::vector<int> vector(data, data + 8, queue);

    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(33));

    vector.assign(data, data + 8);
    vector.push_back(3);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(36));
}

BOOST_AUTO_TEST_CASE(multiply_int)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    int data[] = { 1, 5, 3, 4, 9, 3, 5, 3 };
    boost::compute::vector<int> vector(data, data + 8, queue);

    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::multiplies<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(24300));

    vector.assign(data, data + 8);
    vector.push_back(3);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::multiplies<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(72900));
}

BOOST_AUTO_TEST_CASE(reduce_iota)
{
    if(is_apple_cpu_device(device)) {
        return;
    }

    // 1 value
    boost::compute::vector<int> vector(1, context);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(0));

    // TEST: 12 values
    vector.resize(12);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(11 * 12 / 2));

    // TEST: 16 values
    vector.resize(16);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(15 * 16 / 2));

    // TEST: 48 values
    vector.resize(48);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(47 * 48 / 2));

    // TEST: 64 values
    vector.resize(64);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(63 * 64 / 2));

    // TEST: 66 values
    vector.resize(66);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(65 * 66 / 2));

    // TEST: 72 values
    vector.resize(72);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(71 * 72 / 2));

    // TEST: 80 values
    vector.resize(80);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(79 * 80 / 2));

    // TEST: 88 values
    vector.resize(88);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(87 * 88 / 2));

    // TEST: 90 values
    vector.resize(90);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(89 * 90 / 2));

    // TEST: 92 values
    vector.resize(92);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(91 * 92 / 2));

    // TEST: 94 values
    vector.resize(94);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(93 * 94 / 2));

    // TEST: 96 values
    vector.resize(96);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(95 * 96 / 2));

    // TEST: 100 values
    vector.resize(100);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(99 * 100 / 2));

    // TEST: 102 values
    vector.resize(102);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(101 * 102 / 2));

    // TEST: 104 values
    vector.resize(104);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(103 * 104 / 2));

    // TEST: 106 values
    vector.resize(106);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(105 * 106 / 2));

    // TEST: 120 values
    vector.resize(120);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(119 * 120 / 2));

    // TEST: 128 values
    vector.resize(128);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(127 * 128 / 2));

    // TEST: 130 values
    vector.resize(130);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(129 * 130 / 2));

    // TEST: 152 values
    vector.resize(152);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(151 * 152 / 2));

    // TEST: 160 values
    vector.resize(160);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(159 * 160 / 2));

    // TEST: 510 values
    vector.resize(510);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(509 * 510 / 2));

    // TEST: 512 values
    vector.resize(512);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(511 * 512 / 2));

    // TEST: 514 values
    vector.resize(514);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(513 * 514 / 2));

    // TEST: 574 values
    vector.resize(574);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(573 * 574 / 2));

    // 1000 values
    vector.resize(1000);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(499500));

    // 2499 values
    vector.resize(2499);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(3121251));

    // 4096 values
    vector.resize(4096);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(8386560));

    // TEST: 4094 values
    vector.resize(4094);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(4093 * 4094 / 2));

    // TEST: 4098 values
    vector.resize(4098);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(4097 * 4098 / 2));

    // 5000 values
    vector.resize(5000);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(12497500));

    // TEST: 5004 values
    vector.resize(5004);
    boost::compute::iota(vector.begin(), vector.end(), int(0), queue);
    boost::compute::detail::inplace_reduce(vector.begin(),
                                           vector.end(),
                                           boost::compute::plus<int>(),
                                           queue);
    queue.finish();
    BOOST_CHECK_EQUAL(int(vector[0]), int(5003 * 5004 / 2));
}

BOOST_AUTO_TEST_SUITE_END()


text:
///////////////////////////////////////////////////////////////////////////////
/// \file no_elements.hpp
/// A base class which defines an empty elements property map
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_STORAGE_DETAIL_NO_ELEMENTS_EAN_05_16_2006
#define BOOST_TIME_SERIES_STORAGE_DETAIL_NO_ELEMENTS_EAN_05_16_2006

#include <boost/assert.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/detail/pod_singleton.hpp>

namespace boost { namespace time_series { namespace storage
{

    template<typename Value>
    struct invalid_iterator
      : iterator_facade<
            invalid_iterator<Value>
          , Value
          , std::random_access_iterator_tag
        >
    {
        invalid_iterator()
        {}
    private:
        friend class boost::iterator_core_access;

        void increment()
        {
            BOOST_ASSERT(false);
        }

        void decrement()
        {
            BOOST_ASSERT(false);
        }

        void advance(std::ptrdiff_t)
        {
            BOOST_ASSERT(false);
        }

        bool equal(invalid_iterator const &) const
        {
            return true;
        }

        std::ptrdiff_t distance_to(invalid_iterator const &) const
        {
            return 0;
        }

        Value &dereference() const
        {
            BOOST_ASSERT(false);
            Value *p = 0;
            return *p;
        }
    };

    /// A base class which defines an empty elements property map
    ///
    template<typename Value>
    struct no_elements_array
    {
        typedef invalid_iterator<Value const> iterator;
        typedef invalid_iterator<Value const> const_iterator;

        iterator begin() { return iterator(); }
        iterator end() { return iterator(); }

        const_iterator begin() const { return const_iterator(); }
        const_iterator end() const { return const_iterator(); }
    };

    template<typename Value>
    struct msvc_result_no_elements_property_map
    {
        template<typename K>
        struct result;

        template<typename This, typename K>
        struct result<This(K)>
        {
            typedef Value const &type;
        };

        template<typename This, typename K, typename V>
        struct result<This(K, V)>
        {
            typedef void type;
        };
    };

    template<typename Value>
    struct no_elements_property_map
      : msvc_result_no_elements_property_map<Value>
    {
        template<typename K>
        Value const &operator ()(K const &) const
        {
            BOOST_ASSERT(false);
            Value const *p = 0;
            return *p;
        }

        template<typename K, typename V>
        void operator ()(K const &, V const &v) const
        {
            BOOST_ASSERT(false);
        }
    };

    template<typename Value>
    struct no_elements
    {
        typedef no_elements_property_map<Value> result_type;

        template<typename S>
        result_type operator ()(S &) const
        {
            return result_type();
        }
    };

}}}

#endif


text:
#include "network_common.h"

#include <server_lib/event_loop.h>
#include <server_lib/logging_helper.h>

#include <server_lib/network/network_server.h>
#include <server_lib/network/persist_network_client.h>
#include <server_lib/network/raw_builder.h>

#include <mutex>
#include <condition_variable>
#include <chrono>

#include <boost/lexical_cast.hpp>

namespace server_lib {
namespace tests {

    using namespace server_lib::network;

    BOOST_FIXTURE_TEST_SUITE(network_tests, basic_network_fixture)

    BOOST_AUTO_TEST_CASE(persist_connection_close_by_client_check)
    {
        print_current_test_name();

        event_loop server_th;
        event_loop client_th;

        server_th.change_thread_name("!S");
        client_th.change_thread_name("!C");

        raw_builder protocol;

        network_server server;
        persist_network_client client;

        std::string host = get_default_address();
        auto port = get_free_port();

        const std::string ping_data = "ping";
        const std::string pong_data = "pong test";

        std::shared_ptr<app_connection_i> hold_connection;

        bool done_test = false;
        std::mutex done_test_cond_guard;
        std::condition_variable done_test_cond;

        auto server_recieve_callback = [&hold_connection, &ping_data, &pong_data, &protocol](app_connection_i& conn, app_unit& unit) {
            LOG_TRACE("********* server_recieve_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));
            BOOST_REQUIRE_EQUAL(unit.as_string(), ping_data);

            BOOST_REQUIRE_NO_THROW(hold_connection->send(protocol.create(pong_data)).commit());
        };

        auto server_disconnect_callback = [&hold_connection, &client_th, &done_test, &done_test_cond_guard, &done_test_cond](app_connection_i& conn) {
            LOG_TRACE("********* server_disconnect_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));

            hold_connection.reset();

            client_th.post([&] {
                //done test
                std::unique_lock<std::mutex> lck(done_test_cond_guard);
                done_test = true;
                done_test_cond.notify_one();
            });
        };

        auto server_new_connection_callback = [&hold_connection,
                                               &server_recieve_callback, &server_disconnect_callback](const std::shared_ptr<app_connection_i>& connection) {
            LOG_TRACE("********* server_new_connection_callback");

            BOOST_REQUIRE(connection);

            connection->set_on_receive_handler(server_recieve_callback);
            connection->set_on_disconnect_handler(server_disconnect_callback);

            hold_connection = connection;
        };

        auto client_connection_callback = [](const persist_network_client::connect_state state) {
            LOG_TRACE("********* client_disconnect_callback: " << state);
        };

        auto client_ack_callback = [&pong_data, &client_th, &client](app_unit& unit) {
            LOG_TRACE("********* client_ack_callback");

            BOOST_REQUIRE_EQUAL(unit.as_string(), pong_data);

            client_th.post([&]() {
                BOOST_REQUIRE_NO_THROW(client.disconnect());
                BOOST_REQUIRE(!client.is_connected());
                BOOST_REQUIRE(!client.is_reconnecting());
            });
        };

        auto client_run = [&client, &host, &port, &protocol, &client_th,
                           &client_connection_callback, &ping_data, &client_ack_callback]() {
            BOOST_REQUIRE(client.connect(host, port, &protocol, &client_th, client_connection_callback));
            BOOST_REQUIRE(client.is_connected());
            BOOST_REQUIRE(!client.is_reconnecting());

            BOOST_REQUIRE_NO_THROW(client.send(protocol.create(ping_data), client_ack_callback).commit());
        };

        server_th.start([&server, &host, &port, &protocol, &server_th, &server_new_connection_callback, &client_th, &client_run]() {
            BOOST_REQUIRE(server.start(host, port, &protocol, &server_th, server_new_connection_callback));

            client_th.start([&]() { client_run(); });
        });

        BOOST_REQUIRE(waiting_for(done_test, done_test_cond, done_test_cond_guard));
    }

    BOOST_AUTO_TEST_CASE(persist_connection_close_by_server_check)
    {
        print_current_test_name();

        event_loop server_th;
        event_loop client_th;

        server_th.change_thread_name("!S");
        client_th.change_thread_name("!C");

        raw_builder protocol;

        network_server server;
        persist_network_client client;

        std::string host = get_default_address();
        auto port = get_free_port();

        const std::string ping_data = "ping";
        const std::string pong_data = "pong test";

        std::shared_ptr<app_connection_i> hold_connection;

        bool done_test = false;
        std::mutex done_test_cond_guard;
        std::condition_variable done_test_cond;

        auto server_recieve_callback = [&hold_connection, &ping_data, &pong_data, &protocol](app_connection_i& conn, app_unit& unit) {
            LOG_TRACE("********* server_recieve_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));
            BOOST_REQUIRE_EQUAL(unit.as_string(), ping_data);

            BOOST_REQUIRE_NO_THROW(hold_connection->send(protocol.create(pong_data)).commit());
        };

        auto server_disconnect_callback = [&hold_connection](app_connection_i& conn) {
            LOG_TRACE("********* server_disconnect_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));

            hold_connection.reset();
        };

        auto server_new_connection_callback = [&hold_connection,
                                               &server_recieve_callback, &server_disconnect_callback](const std::shared_ptr<app_connection_i>& connection) {
            LOG_TRACE("********* server_new_connection_callback");

            BOOST_REQUIRE(connection);

            connection->set_on_receive_handler(server_recieve_callback);
            connection->set_on_disconnect_handler(server_disconnect_callback);

            hold_connection = connection;
        };

        auto client_connection_callback = [&client_th,
                                           &done_test, &done_test_cond_guard, &done_test_cond](const persist_network_client::connect_state state) {
            LOG_TRACE("********* client_connection_callback: " << state);

            if (persist_network_client::connect_state::stopped == state)
            {
                client_th.post([&] {
                    //done test
                    std::unique_lock<std::mutex> lck(done_test_cond_guard);
                    done_test = true;
                    done_test_cond.notify_one();
                });
            }
        };

        auto client_ack_callback = [&pong_data, &server_th, &server](app_unit& unit) {
            LOG_TRACE("********* client_ack_callback");

            BOOST_REQUIRE_EQUAL(unit.as_string(), pong_data);

            server_th.post([&] {
                server.stop();
            });
        };

        auto client_run = [&client, &host, &port, &protocol, &client_th,
                           &client_connection_callback, &ping_data, &client_ack_callback]() {
            BOOST_REQUIRE(client.connect(host, port, &protocol, &client_th, client_connection_callback));
            BOOST_REQUIRE(client.is_connected());
            BOOST_REQUIRE(!client.is_reconnecting());

            BOOST_REQUIRE_NO_THROW(client.send(protocol.create(ping_data), client_ack_callback).commit());
        };

        server_th.start([&server, &host, &port, &protocol, &server_th, &server_new_connection_callback, &client_th, &client_run]() {
            BOOST_REQUIRE(server.start(host, port, &protocol, &server_th, server_new_connection_callback));

            client_th.start([&]() { client_run(); });
        });

        BOOST_REQUIRE(waiting_for(done_test, done_test_cond, done_test_cond_guard));
    }

    BOOST_AUTO_TEST_CASE(persist_connection_send_check)
    {
        print_current_test_name();

        event_loop server_th;
        event_loop client_th;

        server_th.change_thread_name("!S");
        client_th.change_thread_name("!C");

        raw_builder protocol;

        network_server server;
        persist_network_client client;

        std::string host = get_default_address();
        auto port = get_free_port();

        std::shared_ptr<app_connection_i> hold_connection;

        bool done_test = false;
        std::mutex done_test_cond_guard;
        std::condition_variable done_test_cond;

        auto server_recieve_callback = [&hold_connection](app_connection_i& conn, app_unit& unit) {
            LOG_TRACE("********* server_recieve_callback: " << unit.as_string());

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));

            BOOST_REQUIRE_NO_THROW(hold_connection->send(unit).commit());
        };

        auto server_disconnect_callback = [&hold_connection, &client_th, &done_test, &done_test_cond_guard, &done_test_cond](app_connection_i& conn) {
            LOG_TRACE("********* server_disconnect_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));

            hold_connection.reset();

            client_th.post([&] {
                //done test
                std::unique_lock<std::mutex> lck(done_test_cond_guard);
                done_test = true;
                done_test_cond.notify_one();
            });
        };

        auto server_new_connection_callback = [&hold_connection,
                                               &server_recieve_callback, &server_disconnect_callback](const std::shared_ptr<app_connection_i>& connection) {
            LOG_TRACE("********* server_new_connection_callback");

            BOOST_REQUIRE(connection);

            connection->set_on_receive_handler(server_recieve_callback);
            connection->set_on_disconnect_handler(server_disconnect_callback);

            hold_connection = connection;
        };

        auto client_connection_callback = [](const persist_network_client::connect_state state) {
            LOG_TRACE("********* client_connection_callback: " << state);
        };

        static int message_1 = 1;
        static int message_2 = 2;
        static int message_fin = 3;

        auto client_final_ack_callback = [&client_th, &client](app_unit& unit) {
            LOG_TRACE("********* client_final_ack_callback: " << unit.as_string());

            BOOST_REQUIRE(unit.is_string());
            BOOST_REQUIRE_EQUAL(message_fin, boost::lexical_cast<int>(unit.as_string()));

            client_th.post([&]() {
                BOOST_REQUIRE_NO_THROW(client.disconnect());
                BOOST_REQUIRE(!client.is_connected());
                BOOST_REQUIRE(!client.is_reconnecting());
            });
        };

        auto client_ack_callback = [&client, &protocol, &client_th, &client_final_ack_callback](app_unit& unit) {
            LOG_TRACE("********* client_ack_callback: " << unit.as_string());

            BOOST_REQUIRE(unit.is_string());

            BOOST_REQUIRE_EQUAL(message_1, boost::lexical_cast<int>(unit.as_string()));

            client_th.post([&]() {
                //You can't wait future (f.wait) in client thread because incoming messages are processed here
                std::thread t([&]() {
                    auto f = client.send(protocol.create(std::to_string(message_2)));
                    BOOST_REQUIRE_NO_THROW(client.commit());
                    f.wait();
                    client.send(protocol.create(std::to_string(message_fin)), client_final_ack_callback);
                    client.commit();
                });
                t.detach();
            });
        };

        auto client_run = [&client, &host, &port, &protocol, &client_th,
                           &client_connection_callback, &client_ack_callback]() {
            BOOST_REQUIRE(client.connect(host, port, &protocol, &client_th, client_connection_callback));
            BOOST_REQUIRE(client.is_connected());
            BOOST_REQUIRE(!client.is_reconnecting());

            BOOST_REQUIRE_NO_THROW(client.send(protocol.create(std::to_string(message_1)), client_ack_callback).commit());
        };

        server_th.start([&server, &host, &port, &protocol, &server_th, &server_new_connection_callback, &client_th, &client_run]() {
            BOOST_REQUIRE(server.start(host, port, &protocol, &server_th, server_new_connection_callback));

            client_th.start([&]() { client_run(); });
        });

        BOOST_REQUIRE(waiting_for(done_test, done_test_cond, done_test_cond_guard));
    }

    BOOST_AUTO_TEST_CASE(persist_connection_reconnect_to_server_check)
    {
        /*
         * Send three messages to server.
         *      1 - success
         *      2 - success after lost connection restoring
         *      3 - failed after connection actually lost
        */

        print_current_test_name();

        event_loop server_th;
        event_loop client_th;

        server_th.change_thread_name("!S");
        client_th.change_thread_name("!C");

        raw_builder protocol;

        network_server server;
        persist_network_client client;

        std::string host = get_default_address();
        auto port = get_free_port();

        const std::string ping_data = "ping";
        const std::string pong_data = "pong test";

        std::shared_ptr<app_connection_i> hold_connection;

        bool done_test = false;
        std::mutex done_test_cond_guard;
        std::condition_variable done_test_cond;

        auto server_recieve_callback = [&hold_connection, &ping_data, &pong_data, &protocol](app_connection_i& conn, app_unit& unit) {
            LOG_TRACE("********* server_recieve_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));
            BOOST_REQUIRE_EQUAL(unit.as_string(), ping_data);

            BOOST_REQUIRE_NO_THROW(hold_connection->send(protocol.create(pong_data)).commit());
        };

        auto server_disconnect_callback = [&hold_connection](app_connection_i& conn) {
            LOG_TRACE("********* server_disconnect_callback");

            BOOST_REQUIRE_EQUAL(reinterpret_cast<uint64_t>(&conn), reinterpret_cast<uint64_t>(hold_connection.get()));

            hold_connection.reset();
        };

        auto server_new_connection_callback = [&hold_connection,
                                               &server_recieve_callback, &server_disconnect_callback](const std::shared_ptr<app_connection_i>& connection) {
            LOG_TRACE("********* server_new_connection_callback");

            BOOST_REQUIRE(connection);

            connection->set_on_receive_handler(server_recieve_callback);
            connection->set_on_disconnect_handler(server_disconnect_callback);

            hold_connection = connection;
        };

        int32_t reconnects = 2;

        auto client_ack_callback = [&pong_data, &server_th, &server, &reconnects](app_unit& unit) {
            LOG_TRACE("********* client_ack_callback");

            if (reconnects > 0)
            {
                BOOST_REQUIRE(unit.ok());
                BOOST_REQUIRE_EQUAL(unit.as_string(), pong_data);

                //interrupt connection
                server_th.post([&] {
                    server.stop();
                });
            }
            else // reconnects == -1
            {
                BOOST_REQUIRE(!unit.ok());
                BOOST_REQUIRE_EQUAL(unit.as_string(), "network failure");
            }
        };

        auto client_connection_callback = [&client_th, &client, &protocol, &ping_data, &client_ack_callback,
                                           &done_test, &done_test_cond_guard, &done_test_cond,
                                           &reconnects, &server_th, &server, &host, &port,
                                           &server_new_connection_callback](const persist_network_client::connect_state state) {
            LOG_TRACE("********* client_connection_callback: " << state);

            switch (state)
            {
            case persist_network_client::connect_state::dropped:
            {
                //add message to queue
                BOOST_REQUIRE_NO_THROW(client.send(protocol.create(ping_data), client_ack_callback).commit());

                if (--reconnects > 0)
                {
                    LOG_TRACE("********* try (next server starting)");

                    //interrupt connection
                    server_th.post([&] {
                        LOG_TRACE("********* next server starting");

                        BOOST_REQUIRE(server.start(host, port, &protocol, &server_th, server_new_connection_callback));
                    });
                }
            }
            break;
            case persist_network_client::connect_state::stopped:
            {
                reconnects = -1;

                client_th.post([&] {
                    //done test
                    std::unique_lock<std::mutex> lck(done_test_cond_guard);
                    done_test = true;
                    done_test_cond.notify_one();
                });
            }
            break;
            default:;
            }
        };

        auto client_run = [&client, &host, &port, &protocol, &client_th,
                           &client_connection_callback, &ping_data, &client_ack_callback,
                           reconnects]() {
            uint32_t timeout_ms = 0;
            int32_t max_reconnects = reconnects;
            uint32_t reconnect_interval_ms = 200;

            BOOST_REQUIRE(client.connect(host, port, &protocol, &client_th, client_connection_callback,
                                         timeout_ms, max_reconnects, reconnect_interval_ms));
            BOOST_REQUIRE(client.is_connected());
            BOOST_REQUIRE(!client.is_reconnecting());

            BOOST_REQUIRE_NO_THROW(client.send(protocol.create(ping_data), client_ack_callback).commit());
        };

        server_th.start([&server, &host, &port, &protocol, &server_th, &server_new_connection_callback, &client_th, &client_run]() {
            LOG_TRACE("********* server is starting");

            BOOST_REQUIRE(server.start(host, port, &protocol, &server_th, server_new_connection_callback));

            client_th.start([&]() { client_run(); });
        });

        BOOST_REQUIRE(waiting_for(done_test, done_test_cond, done_test_cond_guard));
    }

    BOOST_AUTO_TEST_SUITE_END()

} // namespace tests
} // namespace server_lib

max_stars_repo_name: romualdo-bar/barbacoa-server-lib

max_issues_repo_name: romualdo-bar/barbacoa-server-lib

max_forks_repo_name: romualdo-bar/barbacoa-server-lib


text:
//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIMD_BROADCAST_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_BROADCAST_INCLUDED

#include <boost/simd/function/scalar/broadcast.hpp>
#include <boost/simd/arch/common/simd/function/broadcast.hpp>

#if defined(BOOST_HW_SIMD_X86)
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_SSE_VERSION
#    include <boost/simd/arch/x86/sse1/simd/function/broadcast.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_SSSE3_VERSION
#    include <boost/simd/arch/x86/ssse3/simd/function/broadcast.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_AVX_VERSION
#    include <boost/simd/arch/x86/avx/simd/function/broadcast.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_AVX2_VERSION
#    include <boost/simd/arch/x86/avx2/simd/function/broadcast.hpp>
#  endif
#endif

#if defined(BOOST_HW_SIMD_PPC)
#  if BOOST_HW_SIMD_PPC >= BOOST_HW_SIMD_PPC_VMX_VERSION
//#    include <boost/simd/arch/power/vmx/simd/function/broadcast.hpp>
#  endif
#endif

#endif


text:
#include <boost/test/unit_test.hpp>
#include "utils/dc_lookup_table.h"

BOOST_AUTO_TEST_CASE(test_lookup)
{
    lookup::table no_data;
    BOOST_TEST(empty(no_data));
    BOOST_TEST(size(no_data) == 0u);
    lookup::table t;
    t.insert(1);
    BOOST_TEST(!empty(t));
    BOOST_TEST(size(t) == 1u);
    BOOST_TEST(mapped(t, 1) == 0);
    // adding the same value should not change the content
    t.insert(1);
    t.insert(1);
    t.insert(1);
    BOOST_TEST(size(t) == 1u);
    BOOST_TEST(mapped(t, 1) == 0);

}

BOOST_AUTO_TEST_CASE(test_lookup_values)
{
    lookup::table tbl;
    tbl.insert(1000);
    tbl.insert(1);
    tbl.insert(2);
    tbl.insert(1);
    tbl.insert(2);
    tbl.insert(1.1);
    BOOST_TEST(!empty(tbl));
    BOOST_TEST(size(tbl) == 4u);    // 4 unique entries
    BOOST_TEST(mapped(tbl, 1) == 1);
    BOOST_TEST(mapped(tbl, 2) == 2);
    BOOST_TEST(mapped(tbl, 1.1) == 3);
    BOOST_TEST(mapped(tbl, 1000) == 0);
}


text:
// Copyright 2015-2020 Tier IV, Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef TRAFFIC_SIMULATOR__MATH__HERMITE_CURVE_HPP_
#define TRAFFIC_SIMULATOR__MATH__HERMITE_CURVE_HPP_

#include <gtest/gtest.h>
#include <quaternion_operation/quaternion_operation.h>

#include <boost/optional.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <traffic_simulator/math/polynomial_solver.hpp>
#include <vector>

namespace traffic_simulator
{
namespace math
{
class HermiteCurve
{
private:
  friend class HermiteCurveTest;
  double ax_, bx_, cx_, dx_;
  double ay_, by_, cy_, dy_;
  double az_, bz_, cz_, dz_;
  traffic_simulator::math::PolynomialSolver solver_;

public:
  HermiteCurve(
    geometry_msgs::msg::Pose start_pose, geometry_msgs::msg::Pose goal_pose,
    geometry_msgs::msg::Vector3 start_vec, geometry_msgs::msg::Vector3 goal_vec);
  HermiteCurve(
    double ax, double bx, double cx, double dx, double ay, double by, double cy, double dy,
    double az, double bz, double cz, double dz);
  std::vector<geometry_msgs::msg::Point> getTrajectory(size_t num_points = 30) const;
  const std::vector<geometry_msgs::msg::Point> getTrajectory(
    double start_s, double end_s, double resolution, bool autoscale = false) const;
  const geometry_msgs::msg::Pose getPose(double s, bool autoscale = false) const;
  const geometry_msgs::msg::Point getPoint(double s, bool autoscale = false) const;
  const geometry_msgs::msg::Vector3 getTangentVector(double s, bool autoscale = false) const;
  const geometry_msgs::msg::Vector3 getNormalVector(double s, bool autoscale = false) const;
  double get2DCurvature(double s, bool autoscale = false) const;
  double getMaximum2DCurvature() const;
  double getLength(size_t num_points) const;
  double getLength() const { return length_; }
  boost::optional<double> getSValue(
    const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0,
    bool autoscale = false) const;
  double getSquaredDistanceIn2D(
    const geometry_msgs::msg::Point & point, double s, bool autoscale = false) const;
  geometry_msgs::msg::Vector3 getSquaredDistanceVector(
    const geometry_msgs::msg::Point & point, double s, bool autoscale = false) const;
  boost::optional<double> getCollisionPointIn2D(
    const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
    bool search_backward = false) const;
  boost::optional<double> getCollisionPointIn2D(
    const std::vector<geometry_msgs::msg::Point> & polygon, bool search_backward = false,
    bool close_start_end = true) const;

private:
  std::pair<double, double> get2DMinMaxCurvatureValue() const;
  double length_;
};
}  // namespace math
}  // namespace traffic_simulator

#endif  // TRAFFIC_SIMULATOR__MATH__HERMITE_CURVE_HPP_


text:
/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/
#ifndef __SiconosVectorTest__
#define __SiconosVectorTest__

#include <cppunit/extensions/HelperMacros.h>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include "BlockVector.hpp"
#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include <cmath>
#include <vector>

class SiconosVectorTest : public CppUnit::TestFixture
{

private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(SiconosVectorTest);

  // Name of the tests suite
  CPPUNIT_TEST_SUITE(SiconosVectorTest);

  // tests to be done ...

  //  CPPUNIT_TEST(testBuildSiconosVector);
  CPPUNIT_TEST(testConstructor0);
  CPPUNIT_TEST(testConstructor1);
  CPPUNIT_TEST(testConstructor2);
  CPPUNIT_TEST(testConstructor3);
  CPPUNIT_TEST(testConstructor3Bis);
  CPPUNIT_TEST(testConstructor4);
  CPPUNIT_TEST(testConstructor5);
  CPPUNIT_TEST(testConstructor6);
  CPPUNIT_TEST(testConstructor7);
  CPPUNIT_TEST(testZero);
  CPPUNIT_TEST(testFill);
  CPPUNIT_TEST(testNorm);
  CPPUNIT_TEST(testResize);
  CPPUNIT_TEST(testSetBlock);
  //  CPPUNIT_TEST(testSetBlock2);
  CPPUNIT_TEST(testSetBlock3);
  CPPUNIT_TEST(testSetBlock4);
  CPPUNIT_TEST(testAssignment);
  CPPUNIT_TEST(testOperators1);
  CPPUNIT_TEST(testOperators2);
  CPPUNIT_TEST(testOperators3);
  CPPUNIT_TEST(testOperators4);
  CPPUNIT_TEST(testOperators4Bis);
  CPPUNIT_TEST(testOperators4Ter);
  CPPUNIT_TEST(testOperators5);
  CPPUNIT_TEST(testOperators5Bis);
  CPPUNIT_TEST(testOperators6);
  CPPUNIT_TEST(testOperators6Bis);
  CPPUNIT_TEST(testOperators7);
  CPPUNIT_TEST(testOperators8);
  CPPUNIT_TEST(testSubscal);
  CPPUNIT_TEST(testStdOstream);
  CPPUNIT_TEST(testStdVectorCast);
  CPPUNIT_TEST(testIterators);
  CPPUNIT_TEST(End);

  CPPUNIT_TEST_SUITE_END();

  // \todo exception test

  void testConstructor0();
  void testConstructor1();
  void testConstructor2();
  void testConstructor3();
  void testConstructor3Bis();
  void testConstructor4();
  void testConstructor5();
  void testConstructor6();
  void testConstructor7();
  void testZero();
  void testFill();
  void testNorm();
  void testResize();
  void testSetBlock();
  void testSetBlock2();
  void testSetBlock3();
  void testSetBlock4();
  void testAssignment();
  void testOperators1();
  void testOperators2();
  void testOperators3();
  void testOperators4();
  void testOperators4Bis();
  void testOperators4Ter();
  void testOperators5();
  void testOperators5Bis();
  void testOperators6();
  void testOperators6Bis();
  void testOperators7();
  void testOperators8();
  void testSubscal();
  void testStdOstream();
  void testStdVectorCast();
  void testIterators();
  void End();
  // Members

  SP::SiconosVector ref, z, tmp1, tmp2, tmp3, tmp4;
  SP::BlockVector zB;
  SPC::SiconosVector x, y;
  SPC::BlockVector xB, yB;
  unsigned int size, size1, size2;
  std::vector<double> vq;
  SP::DenseVect  dv;
  SP::SparseVect  sv;
  double tol;

public:
  void setUp();
  void tearDown();

};

#endif

max_stars_repo_name: bremond/siconos

max_issues_repo_name: bremond/siconos

max_forks_repo_name: bremond/siconos