text:
/**************************************************************************
 *   Created: 2008/02/13 0:39
 *    Author: Eugene V. Palchukovsky
 *    E-mail: eugene@palchukovsky.com
 * -------------------------------------------------------------------
 *   Project: TunnelEx
 *       URL: http://tunnelex.net
 **************************************************************************/

#ifndef INCLUDED_FILE__TUNNELEX__Format_h__0802130039
#define INCLUDED_FILE__TUNNELEX__Format_h__0802130039

#include "StringFwd.hpp"
#include <boost/format.hpp>

namespace TunnelEx {

	template<typename Ch>
	class BasicFormat : public boost::basic_format<Ch> {

	public:

		typedef boost::basic_format<Ch> Base;
		typedef typename Base::CharT ValueType;
		typedef typename Base::string_type::traits_type TraitsType;
		typedef std::basic_ostream<ValueType, TraitsType> OutStream;

	public:

		explicit BasicFormat(const Ch* str = NULL)
				: Base(str) {
			exceptions(boost::io::no_error_bits);
		}
		explicit BasicFormat(const string_type &s)
				: Base(s) {
			exceptions(boost::io::no_error_bits);
		}
		explicit BasicFormat(const Ch *str, const std::locale &loc)
				: Base(str, loc) {
			exceptions(boost::io::no_error_bits);
		}
		explicit BasicFormat(const string_type &s, const std::locale &loc)
				: Base(s, loc) {
			exceptions(boost::io::no_error_bits);
		}

	};

	typedef BasicFormat<char> Format;
	typedef BasicFormat<wchar_t> WFormat;

}

namespace std {

	::TunnelEx::Format::OutStream & operator <<(
			::TunnelEx::Format::OutStream &,
			const std::wstring &);
	::TunnelEx::Format::OutStream & operator <<(
			::TunnelEx::Format::OutStream &,
			const ::TunnelEx::String &);
	::TunnelEx::Format::OutStream & operator <<(
			::TunnelEx::Format::OutStream &,
			const ::TunnelEx::WString &);

	::TunnelEx::WFormat::OutStream & operator <<(
			::TunnelEx::WFormat::OutStream &,
			const std::string &);
	::TunnelEx::WFormat::OutStream & operator <<(
			::TunnelEx::WFormat::OutStream &,
			const ::TunnelEx::String &);
	::TunnelEx::WFormat::OutStream & operator <<(
			::TunnelEx::WFormat::OutStream &,
			const ::TunnelEx::WString &);

}

#endif // INCLUDED_FILE__TUNNELEX__Format_h__0802130039

max_stars_repo_name: palchukovsky/TunnelEx

max_issues_repo_name: palchukovsky/TunnelEx

max_forks_repo_name: palchukovsky/TunnelEx


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

#include "port_drayage_plugin/port_drayage_worker.h"
#include "port_drayage_plugin/port_drayage_state_machine.h"
#include "port_drayage_plugin/port_drayage_plugin.h"
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <sstream>

#include <carma_wm/WMListener.h>
#include <carma_wm/WorldModel.h>
#include <carma_wm/CARMAWorldModel.h>
#include <carma_wm/WMTestLibForGuidance.h>
#include <lanelet2_extension/regulatory_elements/StopRule.h>

TEST(PortDrayageTest, testComposeArrivalMessage)
{
    // Test initial arrival message for pdw initialized with cargo with the Staging Area Entrance as its first destination
    ros::Time::init();
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "321", // Cargo ID
        "TEST_CARMA_HOST_ID", // Host ID
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    // Set the pdw's map projector set its current pose
    std::string base_proj = "+proj=tmerc +lat_0=38.95622708 +lon_0=-77.15066142 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    std_msgs::String georeference_msg;
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr);

    // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged
    cav_msgs::GuidanceState guidance_state;
    guidance_state.state = cav_msgs::GuidanceState::ENGAGED;
    cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state));
    pdw.on_guidance_state(guidance_state_pointer);

    geometry_msgs::PoseStamped pose_msg;
    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr); // Sets the host vehicle's current gps lat/lon position

    cav_msgs::MobilityOperation msg = pdw.compose_arrival_message();

    ASSERT_EQ("carma/port_drayage", msg.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg.header.sender_id);
    ASSERT_FALSE(msg.strategy_params.empty());

    std::istringstream strstream(msg.strategy_params);
    using boost::property_tree::ptree;
    ptree pt;
    boost::property_tree::json_parser::read_json(strstream, pt);

    unsigned long cmv_id = pt.get<unsigned long>("cmv_id");
    std::string operation = pt.get<std::string>("operation");
    bool has_cargo = pt.get<bool>("cargo");
    double vehicle_longitude = pt.get<double>("location.longitude");
    double vehicle_latitude = pt.get<double>("location.latitude");

    ASSERT_EQ(123, cmv_id);
    ASSERT_EQ("ENTER_STAGING_AREA", operation);
    ASSERT_EQ(pt.count("action_id"), 0);
    ASSERT_TRUE(has_cargo);
    ASSERT_NEAR(38.95622708, vehicle_latitude, 0.00000001);
    ASSERT_NEAR(-77.15066142, vehicle_longitude, 0.00000001);

    // Test initial arrival message for pdw initialized without cargo with the Port Entrance as its first destination
    port_drayage_plugin::PortDrayageWorker pdw2{
        123, // CMV ID
        "", // Cargo ID
        "TEST_CARMA_HOST_ID",
        false, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    pdw2.on_new_georeference(georeference_msg_ptr);
    pdw2.on_new_pose(pose_msg_ptr);
    pdw2.on_guidance_state(guidance_state_pointer);

    cav_msgs::MobilityOperation msg2 = pdw2.compose_arrival_message();

    ASSERT_EQ("carma/port_drayage", msg2.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg2.header.sender_id);
    ASSERT_FALSE(msg2.strategy_params.empty());

    std::istringstream strstream2(msg2.strategy_params);
    ptree pt2;
    boost::property_tree::json_parser::read_json(strstream2, pt2);

    unsigned long cmv_id2 = pt2.get<unsigned long>("cmv_id");
    std::string operation2 = pt2.get<std::string>("operation");
    bool has_cargo2 = pt2.get<bool>("cargo");
    double vehicle_longitude2 = pt2.get<double>("location.longitude");
    double vehicle_latitude2 = pt2.get<double>("location.latitude");

    ASSERT_EQ(123, cmv_id2);
    ASSERT_EQ(0, pt2.count("cargo_id")); // Broadcasted arrival message should not include cargo_id since vehicle is not carrying cargo
    ASSERT_EQ("ENTER_PORT", operation2);
    ASSERT_FALSE(has_cargo2); // False since vehicle is not currently carrying cargo
    ASSERT_NEAR(38.95622708, vehicle_latitude2, 0.00000001);
    ASSERT_NEAR(-77.15066142, vehicle_longitude2, 0.00000001);
}

TEST(PortDrayageTest, testCheckStop1)
{
    ros::Time::init();
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "321", // Cargo ID
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    geometry_msgs::TwistStamped twist;
    twist.twist.linear.x = 0;
    geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}};
    pdw.set_current_speed(twistptr);

    cav_msgs::ManeuverPlan plan;
    cav_msgs::Maneuver mvr;
    mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT;
    mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "port_drayage_plugin";
    plan.maneuvers.push_back(mvr);
    cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}};
    pdw.set_maneuver_plan(planptr);

    bool stopped = pdw.check_for_stop(planptr, twistptr);

    ASSERT_TRUE(stopped);
}

TEST(PortDrayageTest, testCheckStop2)
{
    ros::Time::init();
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "321", // Cargo ID
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    geometry_msgs::TwistStamped twist;
    twist.twist.linear.x = 2.0;
    geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}};
    pdw.set_current_speed(twistptr);

    cav_msgs::ManeuverPlan plan;
    cav_msgs::Maneuver mvr;
    mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT;
    mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Port Drayage Plugin";
    plan.maneuvers.push_back(mvr);
    cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}};
    pdw.set_maneuver_plan(planptr);

    bool stopped = pdw.check_for_stop(planptr, twistptr);

    ASSERT_FALSE(stopped);
}

TEST(PortDrayageTest, testCheckStop3)
{
    ros::Time::init();
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "321", // Cargo ID
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    geometry_msgs::TwistStamped twist;
    twist.twist.linear.x = 0.0;
    geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}};
    pdw.set_current_speed(twistptr);

    cav_msgs::ManeuverPlan plan;
    cav_msgs::Maneuver mvr;
    mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT;
    mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Route Following Plugin";
    plan.maneuvers.push_back(mvr);
    plan.maneuvers.push_back(mvr);
    cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}};
    pdw.set_maneuver_plan(planptr);

    bool stopped = pdw.check_for_stop(planptr, twistptr);

    ASSERT_FALSE(stopped);
}

TEST(PortDrayageTest, testCheckStop4)
{
    ros::Time::init();
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "321", // Cargo ID
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    geometry_msgs::TwistStamped twist;
    twist.twist.linear.x = 2.0;
    geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}};
    pdw.set_current_speed(twistptr);

    cav_msgs::ManeuverPlan plan;
    cav_msgs::Maneuver mvr;
    mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT;
    mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Route Following Plugin";
    plan.maneuvers.push_back(mvr);
    cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}};
    pdw.set_maneuver_plan(planptr);

    bool stopped = pdw.check_for_stop(planptr, twistptr);

    ASSERT_FALSE(stopped);
}

// Test State Machine flow strictly from PortDrayageEvents
TEST(PortDrayageTest, testStateMachine1)
{
    port_drayage_plugin::PortDrayageStateMachine pdsm;

    // Verify system initial state
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdsm.get_state());

    // Startup CARMA system and begin automation
    pdsm.process_event(port_drayage_plugin::PortDrayageEvent::DRAYAGE_START);

    // Verify that we are en route to our next destination (port)
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdsm.get_state());

    // Notify state machine we've arrived
    pdsm.process_event(port_drayage_plugin::PortDrayageEvent::ARRIVED_AT_DESTINATION);
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdsm.get_state());

    // Notify state machine we've recieved the next destination
    pdsm.process_event(port_drayage_plugin::PortDrayageEvent::RECEIVED_NEW_DESTINATION);
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdsm.get_state());

    // Rest of the state machine to be implemented and validated in future stories
}

// Test communication between PortDrayageWorker and PortDrayageStateMachine for State Machine flow
TEST(PortDrayageTest, testPortDrayageStateMachine2)
{
    // Create PortDrayageWorker object with _cmv_id of 123 and no cargo
    //std::function<void(cav_msgs::MobilityOperation)> fun = [](cav_msgs::MobilityOperation){return;};
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "", // Cargo ID; empty string indicates CMV begins without no cargo
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    // State Machine should begin in INACTIVE state
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdw.get_port_drayage_state());

    // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged
    cav_msgs::GuidanceState guidance_state;
    guidance_state.state = cav_msgs::GuidanceState::ENGAGED;
    cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state));
    pdw.on_guidance_state(guidance_state_pointer);

    ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdw.get_port_drayage_state());

    // State Machine should transition to AWAITING_DIRECTION if a 'ROUTE_LOADED' event occurs immediately after a 'ROUTE_COMPLETED' event
    cav_msgs::RouteEvent route_event_1;
    route_event_1.event = cav_msgs::RouteEvent::ROUTE_COMPLETED;
    cav_msgs::RouteEventConstPtr route_event_pointer_1(new cav_msgs::RouteEvent(route_event_1));
    pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed

    cav_msgs::RouteEvent route_event_2;
    route_event_2.event = cav_msgs::RouteEvent::ROUTE_LOADED;
    cav_msgs::RouteEventConstPtr route_event_pointer_2(new cav_msgs::RouteEvent(route_event_2));
    pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active

    ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.get_port_drayage_state());

    // State Machine should transition to 'EN_ROUTE_TO_RECEIVED_DESTINATION' if a new port drayage MobilityOperation message is received

    // Create a MobilityOperationConstPtr with a cmv_id that is intended for this specific vehicle
    // Note: The strategy_params using the schema for messages of this type that have strategy "carma/port_drayage"
    cav_msgs::MobilityOperation mobility_operation_msg;
    mobility_operation_msg.strategy = "carma/port_drayage";
    mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"location\"\
        : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\
        : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\
        : \"32\"}";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr);

    ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdw.get_port_drayage_state());

    // State Machine should transition to 'AWAITING_DIRECTION' again if a ROUTE_COMPLETED event occurs while the vehicle is stopped
    pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed
    pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active
    ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.get_port_drayage_state());
}

TEST(PortDrayageTest, testEstimateDistanceToStop)
{
    ASSERT_EQ(port_drayage_plugin::estimate_distance_to_stop(10.0, 1.0), 50.0);
}

TEST(PortDrayageTest, testEstimateTimeToStop)
{
    ASSERT_EQ(port_drayage_plugin::estimate_time_to_stop(10.0, 1.0), 20.0);
}

TEST(PortDrayageTest, testPlanManeuverCb)
{
    cav_srvs::PlanManeuversRequest req;
    cav_srvs::PlanManeuversResponse resp;

    port_drayage_plugin::PortDrayagePlugin pdp{nullptr, nullptr};
    pdp.declaration = 1;

    ASSERT_EQ(pdp.plan_maneuver_cb(req,resp),false);

    geometry_msgs::Twist _cur_speed;
    _cur_speed.linear.x = 1;
    _cur_speed.linear.y = 2;

    pdp._cur_speed = _cur_speed;

    geometry_msgs::Pose pose;
    pose.position.x = 0;
    pose.position.y = 0;
    pose.position.z = 0;

    geometry_msgs::PoseStamped pose_stamped;

    pose_stamped.pose = pose;

    std::shared_ptr<geometry_msgs::PoseStamped> curr_pose_ = std::make_shared<geometry_msgs::PoseStamped>(pose_stamped);
    pdp.curr_pose_ = curr_pose_;

    std::shared_ptr<carma_wm::CARMAWorldModel> cmw = std::make_shared<carma_wm::CARMAWorldModel>();

    auto pl1 = carma_wm::test::getPoint(0, 0, 0);
    auto pl2 = carma_wm::test::getPoint(0, 1, 0);
    auto pl3 = carma_wm::test::getPoint(0, 2, 0);
    auto pr1 = carma_wm::test::getPoint(1, 0, 0);
    auto pr2 = carma_wm::test::getPoint(1, 1, 0);
    auto pr3 = carma_wm::test::getPoint(1, 2, 0);

    std::vector<lanelet::Point3d> left_1 = { pl1, pl2 };
    std::vector<lanelet::Point3d> right_1 = { pr1, pr2 };
    auto ll_1 = carma_wm::test::getLanelet(left_1, right_1, lanelet::AttributeValueString::SolidSolid, lanelet::AttributeValueString::Dashed);

    std::vector<lanelet::Point3d> left_2 = { pl2, pl3 };
    std::vector<lanelet::Point3d> right_2 = { pr2, pr3 };
    auto ll_2 = carma_wm::test::getLanelet(left_2, right_2);

    // 1. Confirm all pointers are false (done above)
    // Ensure that none of the returned pointers are valid if the map has not been set
    ASSERT_FALSE((bool)cmw->getMap());
    ASSERT_FALSE((bool)cmw->getRoute());
    ASSERT_FALSE((bool)cmw->getMapRoutingGraph());

    lanelet::Id stop_line_id = lanelet::utils::getId();
    lanelet::LineString3d virtual_stop_line(stop_line_id, {pl2, pr2});
    // Creat passing control line for solid dashed line
    std::shared_ptr<lanelet::StopRule> stop_and_wait(new lanelet::StopRule(lanelet::StopRule::buildData(
    lanelet::utils::getId(), { virtual_stop_line }, { lanelet::Participants::Vehicle })));

    ll_1.addRegulatoryElement(stop_and_wait);

    // 2. Build map but do not assign
    // Create basic map and verify that the map and routing graph can be build, but the route remains false
    lanelet::LaneletMapPtr map = lanelet::utils::createMap({ ll_1, ll_2 }, {});

    // // traffic lights are created from a linestring that shows a traffic light and optionally a stop line.
    // lanelet::LineString3d stop_rule = lanelet::LineString3d(lanelet::utils::getId(), {lanelet::Point3d{lanelet::utils::getId(), 1, 0, 0}, lanelet::Point3d{lanelet::utils::getId(), 1, 1, 0}, lanelet::Point3d{lanelet::utils::getId(), 1, 2, 0}});
    // trafficLight.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::TrafficLight;
    // // this creates our traffic light. Regelems are passed around as shared pointers.
    // lanelet::RegulatoryElementPtr trafficLightRegelem = lanelet::TrafficLight::make(lanelet::utils::getId(), {}, {trafficLight});

    // // 3. Build routing graph but do not assign
    // // Build routing graph from map
    lanelet::traffic_rules::TrafficRulesUPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
    lanelet::Locations::Germany, lanelet::Participants::VehicleCar);

    lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*map, *traffic_rules);

    // 4. Generate route
    auto optional_route = map_graph->getRoute(ll_1, ll_2);
    ASSERT_TRUE((bool)optional_route);

    lanelet::routing::Route route = std::move(*optional_route);
    carma_wm::LaneletRoutePtr route_ptr = std::make_shared<lanelet::routing::Route>(std::move(route));
    // 5. Try to set route without map and ensure it passes
    cmw->setRoute(route_ptr);
    // 6. getRoute is true but other pointers are false
    ASSERT_FALSE((bool)cmw->getMap());
    ASSERT_TRUE((bool)cmw->getRoute());
    ASSERT_FALSE((bool)cmw->getMapRoutingGraph());

    cmw->setMap(map);
    // 8. All pointers exist
    ASSERT_TRUE((bool)cmw->getMap());
    ASSERT_TRUE((bool)cmw->getRoute());
    ASSERT_TRUE((bool)cmw->getMapRoutingGraph());
    // 9. Call setRoute again to confirm no errors
    cmw->setRoute(route_ptr);
    // 10. All pointers exist
    ASSERT_TRUE((bool)cmw->getMap());
    ASSERT_TRUE((bool)cmw->getRoute());
    ASSERT_TRUE((bool)cmw->getMapRoutingGraph());

    pdp.wm_ = cmw;

    ASSERT_EQ(pdp.plan_maneuver_cb(req,resp),true);

    ros::Time time;
    time.sec = 1;
    time.nsec = 0;

    cav_msgs::Maneuver maneuver = pdp.compose_stop_and_wait_maneuver_message(1, 2, 1, 2, 1, time, 10);
    ASSERT_EQ(maneuver.stop_and_wait_maneuver.end_time,time + ros::Duration(15));

    maneuver = pdp.compose_lane_following_maneuver_message(1, 2, 1, 2, 1, time);
    ASSERT_NEAR(maneuver.lane_following_maneuver.end_time.sec, 1, 0.0001);
}

TEST(PortDrayageTest, testComposeSetActiveRouteRequest)
{
    // Create PortDrayageWorker object with _cmv_id of "123"
    port_drayage_plugin::PortDrayageWorker pdw{
        123,
        "TEST_CARGO_ID",
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    // Create a MobilityOperationConstPtr with a cmv_id that is intended for this specific vehicle
    // Note: The strategy_params using the schema for messages of this type that have strategy "carma/port_drayage"
    cav_msgs::MobilityOperation mobility_operation_msg;
    mobility_operation_msg.strategy = "carma/port_drayage";
    mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"cargo\": \"false\", \"location\"\
        : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\
        : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"MOVING_TO_LOADING_AREA\", \"action_id\"\
        : \"32\", \"next_action\": \"33\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr);

    // Verify the results of PortDrayageWorker's compose_set_active_route_request() method
    cav_srvs::SetActiveRoute route_req = pdw.compose_set_active_route_request(*pdw._latest_mobility_operation_msg.dest_latitude, *pdw._latest_mobility_operation_msg.dest_longitude);
    ASSERT_EQ(cav_srvs::SetActiveRouteRequest::DESTINATION_POINTS_ARRAY, route_req.request.choice);
    ASSERT_EQ("MOVING_TO_LOADING_AREA", route_req.request.routeID);
    ASSERT_EQ(1, route_req.request.destination_points.size());
    ASSERT_EQ(38.9550038, route_req.request.destination_points[0].latitude);
    ASSERT_EQ(-77.1481983, route_req.request.destination_points[0].longitude);
    ASSERT_EQ(false, route_req.request.destination_points[0].elevation_exists);
}

// Test Case for testing all potential inbound Port Drayage MobilityOperation messages
TEST(PortDrayageTest, testInboundAndComposedMobilityOperation)
{
    // Create PortDrayageWorker object with cmv_id of 123 that is not carrying cargo
    port_drayage_plugin::PortDrayageWorker pdw{
        123, // CMV ID
        "", // Cargo ID; empty string indicates the CMV is not carrying cargo
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged
    cav_msgs::GuidanceState guidance_state;
    guidance_state.state = cav_msgs::GuidanceState::ENGAGED;
    cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state));
    pdw.on_guidance_state(guidance_state_pointer);

    // State Machine should transition to AWAITING_DIRECTION if a 'ROUTE_LOADED' event occurs immediately after a 'ROUTE_COMPLETED' event
    cav_msgs::RouteEvent route_event_1;
    route_event_1.event = cav_msgs::RouteEvent::ROUTE_COMPLETED;
    cav_msgs::RouteEventConstPtr route_event_pointer_1(new cav_msgs::RouteEvent(route_event_1));
    pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed

    cav_msgs::RouteEvent route_event_2;
    route_event_2.event = cav_msgs::RouteEvent::ROUTE_LOADED;
    cav_msgs::RouteEventConstPtr route_event_pointer_2(new cav_msgs::RouteEvent(route_event_2));
    pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active

    // Create a "PICKUP" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg;
    mobility_operation_msg.strategy = "carma/port_drayage";
    mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"location\"\
        : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\
        : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\
        : \"32\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr);

    // Check that the received message was parsed and stored correctly
    ASSERT_EQ("321", *pdw._latest_mobility_operation_msg.cargo_id);
    ASSERT_EQ("PICKUP", pdw._latest_mobility_operation_msg.operation);
    ASSERT_NEAR(-77.1503421, *pdw._latest_mobility_operation_msg.start_longitude, 0.00000001);
    ASSERT_NEAR(38.9554377, *pdw._latest_mobility_operation_msg.start_latitude, 0.00000001);
    ASSERT_NEAR(-77.1481983, *pdw._latest_mobility_operation_msg.dest_longitude, 0.00000001);
    ASSERT_NEAR(38.9550038, *pdw._latest_mobility_operation_msg.dest_latitude, 0.00000001);
    ASSERT_EQ("32", *pdw._latest_mobility_operation_msg.current_action_id);

    // Create a MobilityOperationConstPtr with a cmv_id that is not intended for this specific vehicle
    cav_msgs::MobilityOperation mobility_operation_msg2;
    mobility_operation_msg2.strategy = "carma/port_drayage";
    mobility_operation_msg2.strategy_params = "{ \"cmv_id\": \"444\", \"cargo_id\": \"567\", \"location\"\
        : { \"latitude\": \"48.9554377\", \"longitude\": \"-67.1503421\" }, \"destination\": { \"latitude\"\
        : \"48.9550038\", \"longitude\": \"-57.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\
        : \"44\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr2(new cav_msgs::MobilityOperation(mobility_operation_msg2));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr2);

    // Check that the contents of the received message was not parsed and stored since it was not intended for this CMV
    ASSERT_EQ("321", *pdw._latest_mobility_operation_msg.cargo_id);
    ASSERT_EQ("PICKUP", pdw._latest_mobility_operation_msg.operation);
    ASSERT_NEAR(-77.1503421, *pdw._latest_mobility_operation_msg.start_longitude, 0.00000001);
    ASSERT_NEAR(38.9554377, *pdw._latest_mobility_operation_msg.start_latitude, 0.00000001);
    ASSERT_NEAR(-77.1481983, *pdw._latest_mobility_operation_msg.dest_longitude, 0.00000001);
    ASSERT_NEAR(38.9550038, *pdw._latest_mobility_operation_msg.dest_latitude, 0.00000001);
    ASSERT_EQ("32", *pdw._latest_mobility_operation_msg.current_action_id);

    // Test composeArrivalMessage for when CMV has arrived at the Loading Area

    // Set the pdw's map projector and its current pose
    std::string base_proj = "+proj=tmerc +lat_0=38.95622708 +lon_0=-77.15066142 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    std_msgs::String georeference_msg;
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr);

    geometry_msgs::PoseStamped pose_msg;
    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Loading Area
    cav_msgs::MobilityOperation msg = pdw.compose_arrival_message();
    std::istringstream strstream(msg.strategy_params);
    using boost::property_tree::ptree;
    ptree pt;
    boost::property_tree::json_parser::read_json(strstream, pt);

    unsigned long cmv_id = pt.get<unsigned long>("cmv_id");
    std::string cargo_id = pt.get<std::string>("cargo_id");
    bool has_cargo = pt.get<bool>("cargo");
    std::string action_id = pt.get<std::string>("action_id");
    std::string operation = pt.get<std::string>("operation");
    double vehicle_longitude = pt.get<double>("location.longitude");
    double vehicle_latitude = pt.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg.header.sender_id);
    ASSERT_FALSE(msg.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_FALSE(has_cargo);
    ASSERT_EQ("321", cargo_id);
    ASSERT_EQ("PICKUP", operation);
    ASSERT_EQ("32", action_id);
    ASSERT_NEAR(38.95622708, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.15066142, vehicle_longitude, 0.001);

    // Create an "EXIT_STAGING_AREA" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg3;
    mobility_operation_msg3.strategy = "carma/port_drayage";
    mobility_operation_msg3.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.9103493\", \"longitude\": \"-77.1499283\" }, \"operation\": \"EXIT_STAGING_AREA\", \"action_id\"\
        : \"34\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr3(new cav_msgs::MobilityOperation(mobility_operation_msg3));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr3);

    // Test composeArrivalMessage for when CMV has arrived at the Staging Area Exit

    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.9103493 +lon_0=-77.1499283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr2(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr2);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr2(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr2); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Staging Area Exit
    cav_msgs::MobilityOperation msg2 = pdw.compose_arrival_message();
    std::istringstream strstream2(msg2.strategy_params);
    ptree pt2;
    boost::property_tree::json_parser::read_json(strstream2, pt2);

    cmv_id = pt2.get<unsigned long>("cmv_id");
    cargo_id = pt2.get<std::string>("cargo_id");
    has_cargo = pt2.get<bool>("cargo");
    action_id = pt2.get<std::string>("action_id");
    operation = pt2.get<std::string>("operation");
    vehicle_longitude = pt2.get<double>("location.longitude");
    vehicle_latitude = pt2.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg2.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg2.header.sender_id);
    ASSERT_FALSE(msg2.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("321", cargo_id);
    ASSERT_EQ("EXIT_STAGING_AREA", operation);
    ASSERT_EQ("34", action_id);
    ASSERT_NEAR(38.9103493, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.1499283, vehicle_longitude, 0.001);

    // Create an "ENTER_PORT" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg4;
    mobility_operation_msg4.strategy = "carma/port_drayage";
    mobility_operation_msg4.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.9199993\", \"longitude\": \"-77.1434283\" }, \"operation\": \"ENTER_PORT\", \"action_id\"\
        : \"36\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr4(new cav_msgs::MobilityOperation(mobility_operation_msg4));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr4);

    // Test composeArrivalMessage for when CMV has arrived at the Port Entrance

    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.9199993 +lon_0=-77.1434283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr3(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr3);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr3(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr3); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg3 = pdw.compose_arrival_message();
    std::istringstream strstream3(msg3.strategy_params);
    ptree pt3;
    boost::property_tree::json_parser::read_json(strstream3, pt3);

    cmv_id = pt3.get<unsigned long>("cmv_id");
    has_cargo = pt3.get<bool>("cargo");
    cargo_id = pt3.get<std::string>("cargo_id");
    action_id = pt3.get<std::string>("action_id");
    operation = pt3.get<std::string>("operation");
    vehicle_longitude = pt3.get<double>("location.longitude");
    vehicle_latitude = pt3.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg3.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg3.header.sender_id);
    ASSERT_FALSE(msg3.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("321", cargo_id);
    ASSERT_EQ("ENTER_PORT", operation);
    ASSERT_EQ("36", action_id);
    ASSERT_NEAR(38.9199993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.1434283, vehicle_longitude, 0.001);

    // Create a "DROPOFF" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg5;
    mobility_operation_msg5.strategy = "carma/port_drayage";
    mobility_operation_msg5.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"destination\": { \"latitude\"\
        : \"38.34259993\", \"longitude\": \"-77.1224283\" }, \"operation\": \"DROPOFF\", \"action_id\"\
        : \"37\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr5(new cav_msgs::MobilityOperation(mobility_operation_msg5));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr5);

    // Test composeArrivalMessage for when CMV has arrived at the Dropoff location

    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.34259993 +lon_0=-77.1224283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr4(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr4);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr4(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr4); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg4 = pdw.compose_arrival_message();
    std::istringstream strstream4(msg4.strategy_params);
    ptree pt4;
    boost::property_tree::json_parser::read_json(strstream4, pt4);

    cmv_id = pt4.get<unsigned long>("cmv_id");
    cargo_id = pt4.get<std::string>("cargo_id");
    has_cargo = pt4.get<bool>("cargo");
    action_id = pt4.get<std::string>("action_id");
    operation = pt4.get<std::string>("operation");
    vehicle_longitude = pt4.get<double>("location.longitude");
    vehicle_latitude = pt4.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg4.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg4.header.sender_id);
    ASSERT_FALSE(msg4.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("321", cargo_id);
    ASSERT_EQ("DROPOFF", operation);
    ASSERT_EQ("37", action_id);
    ASSERT_NEAR(38.34259993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.1224283, vehicle_longitude, 0.001);

    // Create a "PICKUP" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg6;
    mobility_operation_msg6.strategy = "carma/port_drayage";
    mobility_operation_msg6.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"422\", \"destination\": { \"latitude\"\
        : \"38.3119993\", \"longitude\": \"-77.2314283\" }, \"operation\": \"PICKUP\", \"action_id\"\
        : \"38\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr6(new cav_msgs::MobilityOperation(mobility_operation_msg6));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr6);

    // Test composeArrivalMessage for when CMV has arrived at the Pickup location
    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.3119993 +lon_0=-77.2314283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr5(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr5);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr5(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr5); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg5 = pdw.compose_arrival_message();
    std::istringstream strstream5(msg5.strategy_params);
    ptree pt5;
    boost::property_tree::json_parser::read_json(strstream5, pt5);

    cmv_id = pt5.get<unsigned long>("cmv_id");
    cargo_id = pt5.get<std::string>("cargo_id");
    has_cargo = pt5.get<bool>("cargo");
    action_id = pt5.get<std::string>("action_id");
    operation = pt5.get<std::string>("operation");
    vehicle_longitude = pt5.get<double>("location.longitude");
    vehicle_latitude = pt5.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg5.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg5.header.sender_id);
    ASSERT_FALSE(msg5.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_FALSE(has_cargo);
    ASSERT_EQ("422", cargo_id);
    ASSERT_EQ("PICKUP", operation);
    ASSERT_EQ("38", action_id);
    ASSERT_NEAR(38.3119993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.2314283, vehicle_longitude, 0.001);

    // Create an "PORT_CHECKPOINT" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg7;
    mobility_operation_msg7.strategy = "carma/port_drayage";
    mobility_operation_msg7.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.3339993\", \"longitude\": \"-77.2594283\" }, \"operation\": \"PORT_CHECKPOINT\", \"action_id\"\
        : \"39\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr7(new cav_msgs::MobilityOperation(mobility_operation_msg7));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr7);

    // Test composeArrivalMessage for when CMV has arrived at the Port Checkpoint
    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.3339993 +lon_0=-77.2594283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr6(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr6);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr6(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr6); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg6 = pdw.compose_arrival_message();
    std::istringstream strstream6(msg6.strategy_params);
    ptree pt6;
    boost::property_tree::json_parser::read_json(strstream6, pt6);

    cmv_id = pt6.get<unsigned long>("cmv_id");
    cargo_id = pt6.get<std::string>("cargo_id");
    has_cargo = pt6.get<bool>("cargo");
    action_id = pt6.get<std::string>("action_id");
    operation = pt6.get<std::string>("operation");
    vehicle_longitude = pt6.get<double>("location.longitude");
    vehicle_latitude = pt6.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg6.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg6.header.sender_id);
    ASSERT_FALSE(msg6.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("422", cargo_id);
    ASSERT_EQ("PORT_CHECKPOINT", operation);
    ASSERT_EQ("39", action_id);
    ASSERT_NEAR(38.3339993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.2594283, vehicle_longitude, 0.001);

    // Create a "HOLDING_AREA" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg8;
    mobility_operation_msg8.strategy = "carma/port_drayage";
    mobility_operation_msg8.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.4139993\", \"longitude\": \"-77.2595583\" }, \"operation\": \"HOLDING_AREA\", \"action_id\"\
        : \"40\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr8(new cav_msgs::MobilityOperation(mobility_operation_msg8));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr8);

    // Test composeArrivalMessage for when CMV has arrived at the Holding Area
    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.4139993 +lon_0=-77.2595583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr7(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr7);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr7(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr7); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg7 = pdw.compose_arrival_message();
    std::istringstream strstream7(msg7.strategy_params);
    ptree pt7;
    boost::property_tree::json_parser::read_json(strstream7, pt7);

    cmv_id = pt7.get<unsigned long>("cmv_id");
    cargo_id = pt7.get<std::string>("cargo_id");
    has_cargo = pt7.get<bool>("cargo");
    action_id = pt7.get<std::string>("action_id");
    operation = pt7.get<std::string>("operation");
    vehicle_longitude = pt7.get<double>("location.longitude");
    vehicle_latitude = pt7.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg7.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg7.header.sender_id);
    ASSERT_FALSE(msg7.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("422", cargo_id);
    ASSERT_EQ("HOLDING_AREA", operation);
    ASSERT_EQ("40", action_id);
    ASSERT_NEAR(38.4139993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.2595583, vehicle_longitude, 0.001);

   // Create an "EXIT_PORT" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg9;
    mobility_operation_msg9.strategy = "carma/port_drayage";
    mobility_operation_msg9.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.6639993\", \"longitude\": \"-77.8395583\" }, \"operation\": \"EXIT_PORT\", \"action_id\"\
        : \"41\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr9(new cav_msgs::MobilityOperation(mobility_operation_msg9));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr9);

    // Test composeArrivalMessage for when CMV has arrived at the Port Exit
    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.6639993 +lon_0=-77.8395583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr8(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr8);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr8(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr8); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg8 = pdw.compose_arrival_message();
    std::istringstream strstream8(msg8.strategy_params);
    ptree pt8;
    boost::property_tree::json_parser::read_json(strstream8, pt8);

    cmv_id = pt8.get<unsigned long>("cmv_id");
    cargo_id = pt8.get<std::string>("cargo_id");
    has_cargo = pt8.get<bool>("cargo");
    action_id = pt8.get<std::string>("action_id");
    operation = pt8.get<std::string>("operation");
    vehicle_longitude = pt8.get<double>("location.longitude");
    vehicle_latitude = pt8.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg8.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg8.header.sender_id);
    ASSERT_FALSE(msg8.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("422", cargo_id);
    ASSERT_EQ("EXIT_PORT", operation);
    ASSERT_EQ("41", action_id);
    ASSERT_NEAR(38.6639993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.8395583, vehicle_longitude, 0.001);

   // Create an "ENTER_STAGING_AREA" MobilityOperationConstPtr for pdw
    cav_msgs::MobilityOperation mobility_operation_msg10;
    mobility_operation_msg10.strategy = "carma/port_drayage";
    mobility_operation_msg10.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\
        : \"38.6889993\", \"longitude\": \"-77.8124583\" }, \"operation\": \"ENTER_STAGING_AREA\", \"action_id\"\
        : \"42\" }";
    cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr10(new cav_msgs::MobilityOperation(mobility_operation_msg10));
    pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr10);

    // Test composeArrivalMessage for when CMV has arrived at the Staging Area Entrance
    // Set the pdw's map projector and its current pose
    base_proj = "+proj=tmerc +lat_0=38.6889993 +lon_0=-77.8124583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m "
                             "+no_defs";
    georeference_msg.data = base_proj;
    std_msgs::StringConstPtr georeference_msg_ptr9(new std_msgs::String(georeference_msg));
    pdw.on_new_georeference(georeference_msg_ptr9);

    pose_msg.pose.position.x = 0.0;
    pose_msg.pose.position.y = 0.0;
    geometry_msgs::PoseStampedConstPtr pose_msg_ptr9(new geometry_msgs::PoseStamped(pose_msg));
    pdw.on_new_pose(pose_msg_ptr9); // Sets the host vehicle's current gps lat/lon position

    // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance
    cav_msgs::MobilityOperation msg9 = pdw.compose_arrival_message();
    std::istringstream strstream9(msg9.strategy_params);
    ptree pt9;
    boost::property_tree::json_parser::read_json(strstream9, pt9);

    cmv_id = pt9.get<unsigned long>("cmv_id");
    cargo_id = pt9.get<std::string>("cargo_id");
    has_cargo = pt9.get<bool>("cargo");
    action_id = pt9.get<std::string>("action_id");
    operation = pt9.get<std::string>("operation");
    vehicle_longitude = pt9.get<double>("location.longitude");
    vehicle_latitude = pt9.get<double>("location.latitude");

    // Verify the contents of the broadcasted message
    ASSERT_EQ("carma/port_drayage", msg9.strategy);
    ASSERT_EQ("TEST_CARMA_HOST_ID", msg9.header.sender_id);
    ASSERT_FALSE(msg9.strategy_params.empty());
    ASSERT_EQ(123, cmv_id);
    ASSERT_TRUE(has_cargo);
    ASSERT_EQ("422", cargo_id);
    ASSERT_EQ("ENTER_STAGING_AREA", operation);
    ASSERT_EQ("42", action_id);
    ASSERT_NEAR(38.6889993, vehicle_latitude, 0.001);
    ASSERT_NEAR(-77.8124583, vehicle_longitude, 0.001);
}

TEST(PortDrayageTest, testComposeUIInstructions)
{
    // Create PortDrayageWorker object with _cmv_id of "123"
    port_drayage_plugin::PortDrayageWorker pdw{
        123,
        "", // Empty string indicates CMV is not carrying cargo
        "TEST_CARMA_HOST_ID",
        true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance
        [](cav_msgs::MobilityOperation){},
        [](cav_msgs::UIInstructions){},
        1.0,
        true, // Flag to enable port drayage operations
        [](cav_srvs::SetActiveRoute){return true;}
    };

    // First 'operation' is for 'PICKUP'. Verify the created UI Instructions message:
    std::string current_operation = "PICKUP";
    std::string previous_operation = "";

    cav_msgs::UIInstructions ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation);

    ASSERT_EQ(ui_instructions_msg.msg, "A new Port Drayage route with operation type 'PICKUP' has been received. "
                                  "Select YES to engage the system on the route, or select NO to remain "
                                  "disengaged.");
    ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED);
    ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active");

    // Second received MobilityOperation message is for a 'DROPOFF' operation. The previous 'PICKUP' operation has been completed.
    current_operation = "DROPOFF";
    previous_operation = "PICKUP";

    ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation);

    ASSERT_EQ(ui_instructions_msg.msg, "The pickup action was completed successfully. A new Port Drayage route with operation type 'DROPOFF' has been received. "
                                  "Select YES to engage the system on the route, or select NO to remain "
                                  "disengaged.");
    ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED);
    ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active");

    // Third received MobilityOperation message is for a 'EXIT_STAGING_AREA' operation. The previous 'DROPOFF' operation has been completed.
    current_operation = "EXIT_STAGING_AREA";
    previous_operation = "DROPOFF";

    ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation);

    ASSERT_EQ(ui_instructions_msg.msg, "The dropoff action was completed successfully. A new Port Drayage route with operation type 'EXIT_STAGING_AREA' has been received. "
                                  "Select YES to engage the system on the route, or select NO to remain "
                                  "disengaged.");
    ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED);
    ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active");

    // Fourth received MobilityOperation message is for a 'ENTER_PORT' operation. The previous 'EXIT_STAGING_AREA' operation has been completed.
    current_operation = "ENTER_PORT";
    previous_operation = "EXIT_STAGING_AREA";

    ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation);

    ASSERT_EQ(ui_instructions_msg.msg, "A new Port Drayage route with operation type 'ENTER_PORT' has been received. "
                                  "Select YES to engage the system on the route, or select NO to remain "
                                  "disengaged.");
    ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED);
    ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active");

    // Fifth received MobilityOperation message is for a 'ENTER_PORT' operation. The previous 'EXIT_STAGING_AREA' operation has been completed.
    current_operation = "EXIT_PORT";
    previous_operation = "HOLDING_AREA";

    ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation);

    ASSERT_EQ(ui_instructions_msg.msg, "The inspection was completed successfully. A new Port Drayage route with operation type 'EXIT_PORT' has been received. "
                                  "Select YES to engage the system on the route, or select NO to remain "
                                  "disengaged.");
    ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED);
    ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active");
}

// Run all the tests
int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

ext: cpp

max_stars_repo_name: usdot-fhwa-stol/carma-platform

max_issues_repo_name: usdot-fhwa-stol/carma-platform

max_forks_repo_name: usdot-fhwa-stol/carma-platform


text:
// MIT License
//
// Copyright (c) 2019 Lennart Braun
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "analysis.h"

#include <cmath>
#include <sstream>

#include <fmt/format.h>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/core/alloc_construct.hpp>
#include <boost/json.hpp>

#include "communication/transport.h"
#include "utility/runtime_info.h"
#include "utility/version.h"

namespace json = boost::json;

namespace MOTION {
namespace Statistics {

static double compute_duration(const RunTimeStats::time_point_pair& tpp) {
  std::chrono::duration<double, AccumulatedRunTimeStats::resolution> d = tpp.second - tpp.first;
  return d.count();
}

void AccumulatedRunTimeStats::add(const RunTimeStats& stats) {
  for (std::size_t i = 0; i <= static_cast<std::size_t>(RunTimeStats::StatID::MAX); ++i) {
    accumulators_[i](compute_duration(stats.data_[i]));
  }
  ++count_;
}

template <typename C>
static typename C::value_type at(const C& container, RunTimeStats::StatID id) {
  return container.at(static_cast<std::size_t>(id));
}

using StatID = RunTimeStats::StatID;

static std::string format_line(std::string name, std::string unit,
                               AccumulatedRunTimeStats::accumulator_type accumulator,
                               std::size_t field_width) {
  std::stringstream ss;
  ss << fmt::format("{:19s} ", name);
  ss << fmt::format("{:{}.3f} {:s} ", boost::accumulators::mean(accumulator), field_width, unit);
  ss << fmt::format("{:{}.3f} {:s} ", boost::accumulators::median(accumulator), field_width, unit);
  // uncorrected standard deviation
  ss << fmt::format("{:{}.3f} {:s}", std::sqrt(boost::accumulators::variance(accumulator)),
                    field_width, unit);
  ss << "\n";
  return ss.str();
}

std::string AccumulatedRunTimeStats::print_human_readable() const {
  std::size_t field_width = 10;
  std::stringstream ss;
  std::string unit = "ms";

  ss << fmt::format("Run time statistics over {} iterations\n", count_)
     << "---------------------------------------------------------------------------\n"
     << fmt::format("                    {:>{}s}    {:>{}s}    {:>{}s}\n", "mean", field_width,
                    "median", field_width, "stddev", field_width)
     << "---------------------------------------------------------------------------\n"
     << format_line("MT Presetup", unit, at(accumulators_, StatID::mt_presetup), field_width)
     << format_line("MT Setup", unit, at(accumulators_, StatID::mt_setup), field_width)
     << format_line("SP Presetup", unit, at(accumulators_, StatID::sp_presetup), field_width)
     << format_line("SP Setup", unit, at(accumulators_, StatID::sp_setup), field_width)
     << format_line("SB Presetup", unit, at(accumulators_, StatID::sb_presetup), field_width)
     << format_line("SB Setup", unit, at(accumulators_, StatID::sb_setup), field_width)
     << format_line("Base OTs", unit, at(accumulators_, StatID::base_ots), field_width)
     << format_line("OT Extension Setup", unit, at(accumulators_, StatID::ot_extension_setup),
                    field_width)
     << "---------------------------------------------------------------------------\n"
     << format_line("Preprocessing Total", unit, at(accumulators_, StatID::preprocessing),
                    field_width)
     << format_line("Gates Setup", unit, at(accumulators_, StatID::gates_setup), field_width)
     << format_line("Gates Online", unit, at(accumulators_, StatID::gates_online), field_width)
     << "---------------------------------------------------------------------------\n"
     << format_line("Circuit Evaluation", unit, at(accumulators_, StatID::evaluate), field_width);

  return ss.str();
}

json::object AccumulatedRunTimeStats::to_json() const {
  const auto mk_triple = [this](const auto& stat_id) {
    const auto& acc = at(accumulators_, stat_id);
    return json::object({{"mean", boost::accumulators::mean(acc)},
                         {"median", boost::accumulators::median(acc)},
                         // uncorrected standard deviation
                         {"stddev", std::sqrt(boost::accumulators::variance(acc))}});
  };
  return {{"repetitions", count_},
          {"mt_setup", mk_triple(StatID::mt_setup)},
          {"sp_setup", mk_triple(StatID::sp_setup)},
          {"sb_setup", mk_triple(StatID::sb_setup)},
          {"linalgtriple_setup", mk_triple(StatID::linalgtriple_setup)},
          {"base_ots", mk_triple(StatID::base_ots)},
          {"ot_extension_setup", mk_triple(StatID::ot_extension_setup)},
          {"preprocessing", mk_triple(StatID::preprocessing)},
          {"gates_setup", mk_triple(StatID::gates_setup)},
          {"gates_online", mk_triple(StatID::gates_online)},
          {"evaluate", mk_triple(StatID::evaluate)}};
}

void AccumulatedCommunicationStats::add(const Communication::TransportStatistics& stats) {
  accumulators_[idx_num_messages_sent](stats.num_messages_sent);
  accumulators_[idx_num_messages_received](stats.num_messages_received);
  accumulators_[idx_num_bytes_sent](stats.num_bytes_sent);
  accumulators_[idx_num_bytes_received](stats.num_bytes_received);
  ++count_;
}

void AccumulatedCommunicationStats::add(
    const std::vector<Communication::TransportStatistics>& stats) {
  for (const auto& s : stats) {
    add(s);
  }
}

std::string AccumulatedCommunicationStats::print_human_readable() const {
  std::stringstream ss;
  ss << "Communication with each other party:\n"
     << fmt::format("Sent: {:0.3f} MiB in {:d} messages\n",
                    boost::accumulators::mean(accumulators_[idx_num_bytes_sent]) / 1048576,
                    static_cast<std::size_t>(
                        boost::accumulators::mean(accumulators_[idx_num_messages_sent])))
     << fmt::format("Received: {:0.3f} MiB in {:d} messages\n",
                    boost::accumulators::mean(accumulators_[idx_num_bytes_received]) / 1048576,
                    static_cast<std::size_t>(
                        boost::accumulators::mean(accumulators_[idx_num_messages_received])));
  return ss.str();
}

json::object AccumulatedCommunicationStats::to_json() const {
  return {
      {"bytes_sent",
       static_cast<std::size_t>(boost::accumulators::mean(accumulators_[idx_num_bytes_sent]))},
      {"num_messages_sent",
       static_cast<std::size_t>(boost::accumulators::mean(accumulators_[idx_num_messages_sent]))},
      {"bytes_received",
       static_cast<std::size_t>(boost::accumulators::mean(accumulators_[idx_num_bytes_received]))},
      {"num_messages_received", static_cast<std::size_t>(boost::accumulators::mean(
                                    accumulators_[idx_num_messages_received]))}};
}

std::string print_motion_info() {
  std::stringstream ss;
  ss << fmt::format("MOTION version: {} @ {}\n", get_git_version(), get_git_branch())
     << fmt::format("invocation: {}\n", get_cmdline())
     << fmt::format("by {}@{}, PID {}\n", get_username(), get_hostname(), get_pid());
  return ss.str();
}

std::string print_stats(const std::string& experiment_name,
                        const AccumulatedRunTimeStats& exec_stats,
                        const AccumulatedCommunicationStats& comm_stats) {
  std::stringstream ss;
  ss << "===========================================================================\n"
     << experiment_name << "\n"
     << "===========================================================================\n"
     << print_motion_info()
     << "===========================================================================\n"
     << exec_stats.print_human_readable()
     << "===========================================================================\n"
     << comm_stats.print_human_readable()
     << "===========================================================================\n";
  return ss.str();
}

json::object to_json(const std::string& experiment_name, const AccumulatedRunTimeStats& exec_stats,
                     const AccumulatedCommunicationStats& comm_stats) {
  json::object obj({{"experiment", experiment_name},
                    {"meta",
                     {{"invocation", get_cmdline()},
                      {"timestamp", get_timestamp()},
                      {"user", get_username()},
                      {"hostname", get_hostname()},
                      {"pid", get_pid()},
                      {"build-type", get_build_type()},
                      {"git-branch", get_git_branch()},
                      {"git-commit", get_git_commit()},
                      {"git-version", get_git_version()}}}});
  obj.emplace("runtime", exec_stats.to_json());
  obj.emplace("communication", comm_stats.to_json());
  return obj;
}

}  // namespace Statistics
}  // namespace MOTION


text:
#ifndef _CHUNK_HPP_
#define _CHUNK_HPP_

#include <unordered_map>
#include <queue>
#include <cstdint>
#include <boost/timer.hpp>
#include <boost/array.hpp>
#include <boost/multi_array.hpp>
#include <zlib/zlib.h>

#include "protocol/messages.hpp"

typedef std::vector<uint8_t>::const_iterator ByteVecConstItr;

void Inflate(const std::vector<uint8_t>& _src, std::vector<uint8_t>& _dst);

// Exceptions
class ColumnDoesNotExistException {};

int WorldToColumnRel(int _worldCoord);
int WorldToColumnCoord(int _worldCoord);
int RoundWorldCoord(double _worldCoord);

/*int ColumnRelToWorld(int _relCoord, int _colCoord)
{
	return
}*/

struct ChunkColumnID
{
	int32_t m_columnX;
	int32_t m_columnZ;

	ChunkColumnID(int32_t _columnX, int32_t _columnZ)
		:	m_columnX(_columnX)
		,	m_columnZ(_columnZ)
	{}

	bool operator==(const ChunkColumnID& _other) const
	{
		return _other.m_columnX == m_columnX && _other.m_columnZ == m_columnZ;
	}

};

namespace std {

template <>
struct hash<ChunkColumnID>
{
	size_t operator()(const ChunkColumnID& _subject) const
	{
		union {int32_t x; uint32_t ux;};
		x = _subject.m_columnX;
		union {int32_t z; uint32_t uz;};
		z = _subject.m_columnZ;

		size_t retr = 23;
		retr *= 31;
		retr += ux;
		retr *= 31;
		retr += uz;

		return retr;
	}
};

} // namespace std

struct BlockData
{
	uint16_t m_type;
	uint8_t m_metadata;		// 4 bits when arrives
	uint8_t m_blockLight;	// 4 bits when arrives
	uint8_t m_skyLight;		// only if 'skylight' is true; 4 bits when arrives
};

// 16x256x16 block, 16 chunks
class ChunkColumn
{
public:
	ChunkColumn()
	//	:	m_columnDataRef(m_columnData.c_array(), boost::extents[256][16][16]) // YZX
	{}
	ChunkColumn(const ChunkColumn& _other);
	~ChunkColumn(){}

	size_t load(std::vector<uint8_t>& _src, size_t _offset, int _nNonAirChunks);

	size_t load(std::vector<uint8_t>& _src, size_t _offset, uint16_t _PrimBitmask, uint16_t _addBitmask);
	ByteVecConstItr load(ByteVecConstItr _begin, uint16_t _PrimBitmask, uint16_t _addBitmask);

	inline BlockData& getBlock(int _relX, int _relY, int _relZ);
	inline const BlockData& getBlock(int _relX, int _relY, int _relZ) const;

private:
	boost::array<BlockData, 256 * 16 * 16> m_columnData; // flat representation of 3d array

};

BlockData& ChunkColumn::getBlock(int _relX, int _relY, int _relZ)
{
	return m_columnData[_relY * 256 + _relZ * 16 + _relX];
	//return m_columnDataRef[_relY][_relZ][_relX];
	// boost multi array ref is too slow
}

const BlockData& ChunkColumn::getBlock(int _relX, int _relY, int _relZ) const
{
	return m_columnData[_relY * 256 + _relZ * 16 + _relX];
	//return m_columnDataRef[_relY][_relZ][_relX];
}

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

struct ScheduledBlock
{
	int m_x, m_y, m_z;
	BlockData m_block;
};

class World
{
public:
	World(){}

	~World()
	{
		for(auto itr = m_columnsMap.begin(); itr != m_columnsMap.end(); ++itr)
			delete itr->second;
	}

	void loadColumns(protocol::msg::MapChunkBulk& _columnsMsg);

	inline BlockData& getBlock(int _blockX, int _blockY, int _blockZ);
	inline const BlockData& getBlock(int _blockX, int _blockY, int _blockZ) const;

	// If column that block belongs to does not exist when change block message arrived,
	// this can be used to change block when column appears.
	// Will be called automatically when anyone tries to change block in non-existent column.
	// Move to private?
	void scheduleBlockChange(int _blockX, int _blockY, int _blockZ, BlockData _newBlock);

private:
	std::unordered_map<ChunkColumnID, ChunkColumn*> m_columnsMap;
	// Merge with columns?
	std::unordered_map<ChunkColumnID, std::queue<ScheduledBlock>> m_scheduledBlocks;

};

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

BlockData& World::getBlock(int _blockX, int _blockY, int _blockZ)
{
	auto colItr = m_columnsMap.find(ChunkColumnID(WorldToColumnCoord(_blockX), WorldToColumnCoord(_blockZ)));
	if(colItr == m_columnsMap.end())
		throw ColumnDoesNotExistException();

	return colItr->second->getBlock(WorldToColumnRel(_blockX), _blockY, WorldToColumnRel(_blockZ));
}

const BlockData& World::getBlock(int _blockX, int _blockY, int _blockZ) const
{
	return const_cast<World*>(this)->getBlock(WorldToColumnRel(_blockX), _blockY, WorldToColumnRel(_blockZ));
}

#endif // _CHUNK_HPP_

ext: hpp


text:
#include <skye/detail/argument_wrapper.hpp>
#include <skye/detail/tuple_streaming.hpp>

#include <boost/test/unit_test.hpp>

using namespace skye::detail;

/**
 * Implement simple types to drive the tests.
 */
namespace {
/**
 * A non-copy-constructible class (i.e., one where std::is_copyable<>
 * returns false), to test how such arguments are wrapped.
 */
struct no_copy {
  no_copy(no_copy const &) = delete;
  no_copy()
      : value(42)
  {}
  explicit no_copy(int x)
      : value(x)
  {}

  bool operator==(int rhs) const {
    return value == rhs;
  }
  bool operator!=(int rhs) const {
    return !(*this == rhs);
  }
  bool operator==(no_copy const & rhs) const {
    return value == rhs.value;
  }
  bool operator!=(no_copy const & rhs) const {
    return !(*this == rhs);
  }

  int value;
};

/**
 * The streaming operator for no_copy
 */
std::ostream & operator<<(std::ostream & os, no_copy const & x) {
  return os << "no_copy{value=" << x.value << "}";
}

/**
 * A class without a streaming operator, i.e., one where the following
 * code snippet would fail to compile:
 *
 * @code
 * void function(std::ostream & os, no_stream const & x) {
 *   os << x;
 * }
 * @endcode
 */
struct no_stream {
  no_stream(no_stream const &) = default;
  no_stream& operator=(no_stream const &) = default;
  no_stream() = default;
  explicit no_stream(int x)
      : value(x)
  {}

  bool operator==(int rhs) const {
    return value == rhs;
  }
  bool operator!=(int rhs) const {
    return !(*this == rhs);
  }
  bool operator==(no_stream const & rhs) const {
    return value == rhs.value;
  }
  bool operator!=(no_stream const & rhs) const {
    return !(*this == rhs);
  }

  int value;
};

/**
 * A class without equality and inequality operators.
 */
struct no_compare {
  no_compare(no_compare const &) = default;
  no_compare& operator=(no_compare const &) = default;
  no_compare() = default;
  explicit no_compare(int x)
      : value(x)
  {}

  int value;
};

/**
 * The streaming operator for no_compare.
 */
std::ostream & operator<<(std::ostream & os, no_compare const & x) {
  return os << x.value;
}

} // anonymous namespace

BOOST_AUTO_TEST_CASE( argument_wrapper_int ) {
  int x = 5;
  auto w1 = make_arg_wrapper(x);
  BOOST_CHECK_EQUAL(w1, 5);

  int const & y = x;
  auto w2 = make_arg_wrapper(y);
  bool const is_same = std::is_same<decltype(w1), decltype(w2)>::value;
  BOOST_CHECK_MESSAGE(is_same, "The two wrappers should return the same type");
  BOOST_CHECK_EQUAL(w1, w2);

  std::ostringstream os;
  os << w1;
  BOOST_CHECK_EQUAL(os.str(), "5");
}

BOOST_AUTO_TEST_CASE( argument_wrapper_string ) {
  std::string s("foo bar baz");

  auto w1 = make_arg_wrapper(s);
  BOOST_CHECK_EQUAL(w1, "foo bar baz");

  auto w2 = make_arg_wrapper(std::string("bar baz"));
  BOOST_CHECK_NE(w1, w2);

  std::ostringstream os;
  os << w1;
  BOOST_CHECK_EQUAL(os.str(), "foo bar baz");
}

BOOST_AUTO_TEST_CASE( argument_wrapper_copy_references ) {
  using namespace skye::detail;

  int a = 1;
  int & b = a;
  int const & c = b;

  BOOST_CHECK_EQUAL(argument_traits<decltype(a)>::copyable, true);
  BOOST_CHECK_EQUAL(argument_traits<decltype(b)>::copyable, true);
  BOOST_CHECK_EQUAL(argument_traits<decltype(c)>::copyable, true);

  int x = make_arg_wrapper(a);
  BOOST_CHECK_EQUAL(x, a);
  int y = make_arg_wrapper(b);
  BOOST_CHECK_EQUAL(y, a);
  int z = make_arg_wrapper(c);
  BOOST_CHECK_EQUAL(z, a);
}

BOOST_AUTO_TEST_CASE( argument_wrapper_no_copy ) {
  no_copy x(42);
  no_copy y(42);

  auto w1 = make_arg_wrapper(x);
  BOOST_CHECK_NE(w1, y);

  std::ostringstream os;
  os << w1;
  BOOST_CHECK_EQUAL(os.str(), "[::place_holder::]");
}

BOOST_AUTO_TEST_CASE( argument_wrapper_no_stream ) {
  no_stream x(42);

  auto w1 = make_arg_wrapper(x);
  BOOST_CHECK_MESSAGE(
      w1 == no_stream(42),
      "Mismatched values, got=" << x.value << ", expected=" << 42);

  std::ostringstream os;
  os << w1;
  BOOST_CHECK_EQUAL(os.str(), "[::non_streamable::]");
}

BOOST_AUTO_TEST_CASE( argument_wrapper_no_compare ) {
  no_compare x(42);

  auto w1 = make_arg_wrapper(x);
  auto w2 = make_arg_wrapper(x);
  BOOST_CHECK_MESSAGE(
      w1 != w2,
      "Mismatched values, got=" << w1 << ", expected=" << x.value);

  BOOST_CHECK_EQUAL(w1.value.value, w2.value.value);

  std::ostringstream os;
  os << w1;
  BOOST_CHECK_EQUAL(os.str(), "42");
}

BOOST_AUTO_TEST_CASE( tuple_streaming_empty ) {
  std::tuple<> empty;
  std::ostringstream os;
  os << empty;
  BOOST_CHECK_EQUAL(os.str(), "<>");
}

BOOST_AUTO_TEST_CASE( tuple_streaming_single ) {
  std::tuple<int> single(42);
  std::ostringstream os;
  os << single;
  BOOST_CHECK_EQUAL(os.str(), "<42>");
}

BOOST_AUTO_TEST_CASE( tuple_streaming_triple ) {
  std::tuple<int,std::string,std::string> x(42, "x", "y");
  std::ostringstream os;
  os << x;
  BOOST_CHECK_EQUAL(os.str(), "<42,x,y>");
}

BOOST_AUTO_TEST_CASE( wrap_simple_args ) {
  using namespace skye::detail;

  auto w = wrap_args_as_tuple(1, 2, 3, std::string("42"));
  std::ostringstream os;
  os << w;
  BOOST_CHECK_EQUAL(os.str(), "<1,2,3,42>");
}

BOOST_AUTO_TEST_CASE( wrap_simple_ref_args ) {
  using namespace skye::detail;

  int x = 1;
  int y = 2;
  int & a = x;
  int const & b = y;
  std::string const c("abc");
  std::string const & d = c;

  auto w = wrap_args_as_tuple(a, b, c, d, c, b, a);
  std::ostringstream os;
  os << w;
  BOOST_CHECK_EQUAL(os.str(), "<1,2,abc,abc,abc,2,1>");
}

BOOST_AUTO_TEST_CASE( wrap_no_copy ) {
  using namespace skye::detail;

  no_copy a;
  no_copy & b = a;

  {
    std::ostringstream os;
    os << b;
    BOOST_CHECK_EQUAL(os.str(), "no_copy{value=42}");
  }

  no_copy const & c = a;
  auto w = wrap_args_as_tuple(a, b, c, 1, 2);
  std::ostringstream os;
  os << w;
  BOOST_CHECK_EQUAL(os.str(),
                    "<[::place_holder::],[::place_holder::]"
                    ",[::place_holder::],1,2>");
}

max_stars_repo_name: coryan/Skye

max_issues_repo_name: coryan/Skye

max_forks_repo_name: coryan/Skye


text:
#include <string>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

// parameters
int image_width;
int image_height;
int camera_idx;
std::string image_filename;

bool optparse(int argc, char *argv[])
{
	namespace po = boost::program_options;
	po::options_description opt("cvcap options");
	opt.add_options()
		("help,h", "show help")
		("resolution,r", po::value<std::string>()->default_value("640x480"), "resolution")
		("camera,c", po::value<int>()->default_value(0), "capture device number")
		("filename,f", po::value<std::string>()->default_value("image.jpg"), "capture image filename");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, opt), vm);

	if (vm.count("help")) {
		std::cerr << opt << std::endl;
		return false;
	}

	image_filename  = vm["filename"].as<std::string>();
	camera_idx      = vm["camera"].as<int>();

	// parse resolution string
	std::string resolution_str = vm["resolution"].as<std::string>();
	std::vector<std::string> res;
	boost::split(res, resolution_str, boost::is_any_of("x"));
	if (res.size() != 2) {
		std::cerr << "error : invalid resolution format string..."  << std::endl;
		std::cerr << opt << std::endl;
		return false;
	}
	image_width  = boost::lexical_cast<int>(res[0]);
	image_height = boost::lexical_cast<int>(res[1]);

	return true;
}

int main(int argc, char *argv[])
{
	if (optparse(argc, argv) == false) return 1;

	cv::VideoCapture capture;
	if (capture.open(0) == false) {
		std::cerr << "error : capture.open(0)" << std::endl;
		return 1;
	}

	cv::Mat capture_img;

	// dummy capture (for auto iris...)
	for (int i = 0; i < 30; ++i) {
		capture >> capture_img;
	}

	capture >> capture_img;

	// resize
	cv::Mat resized_img;
	cv::resize(capture_img, resized_img, cv::Size(image_width, image_height));
	if (cv::imwrite(image_filename.c_str(), resized_img) == false) {
		std::cerr << "error : failed to cv::imwrite()...filename=" << image_filename << std::endl;
		return 1;
	}

	std::cerr << "info : image capture completed...filename=" << image_filename << std::endl;

	return 0;
}

max_stars_repo_name: yoggy/cvcap

max_issues_repo_name: yoggy/cvcap

max_forks_repo_name: yoggy/cvcap


text:
//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot 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)
//
// Official repository: https://github.com/CPPAlliance/http_proto
//

#ifndef BOOST_HTTP_PROTO_REQUEST_HPP
#define BOOST_HTTP_PROTO_REQUEST_HPP

#include <boost/http_proto/detail/config.hpp>
#include <boost/http_proto/basic_header.hpp>
#include <boost/http_proto/headers.hpp>
#include <boost/http_proto/method.hpp>
#include <boost/http_proto/version.hpp>

namespace boost {
namespace http_proto {

#ifndef BOOST_HTTP_PROTO_DOCS
class request_view;
#endif

/** Container for HTTP requests
*/
class BOOST_SYMBOL_VISIBLE
    request
    : public basic_header
{
    http_proto::method method_;
    http_proto::version version_;
    std::size_t method_len_;
    std::size_t target_len_;

public:
    /** The field values for this request
    */
    headers fields;

    /** Constructor
    */
    BOOST_HTTP_PROTO_DECL
    request();

    BOOST_HTTP_PROTO_DECL
    request(request&&) noexcept;

    BOOST_HTTP_PROTO_DECL
    request(request const&);

    BOOST_HTTP_PROTO_DECL
    request& operator=(request&&) noexcept;

    BOOST_HTTP_PROTO_DECL
    request& operator=(request const&);

    //--------------------------------------------
    //
    // Observers
    //
    //--------------------------------------------

    /** Return the method of this request as a known-method enum

        If the method returned is equal to
        @ref method::unknown, the method may
        be obtained as a string instead, by
        calling @ref method_str.
    */
    http_proto::method
    method() const noexcept
    {
        return method_;
    }

    /** Return the method of this request as a string
    */
    string_view
    method_str() const noexcept
    {
        return string_view(
            fields.owner_str().data(),
            method_len_);
    }

    /** Return the target of this request as a string
    */
    string_view
    target() const noexcept
    {
        return string_view(
            fields.owner_str().data() +
                method_len_ + 1,
            target_len_);
    }

    /** Return the HTTP version of this request
    */
    http_proto::version
    version() const noexcept
    {
        return version_;
    }

    /** Return a read-only view to the request
    */
    BOOST_HTTP_PROTO_DECL
    operator
    request_view() const noexcept;

    /** Return the serialized string of this request
    */
    BOOST_HTTP_PROTO_DECL
    string_view
    get_const_buffer() const noexcept override;

    //--------------------------------------------
    //
    // Modifiers
    //
    //--------------------------------------------

    /** Clear the contents, but not the capacity
    */
    BOOST_HTTP_PROTO_DECL
    void
    clear() noexcept;

    /** Set the method of the request to the enum
    */
    void
    set_method(http_proto::method m)
    {
        set(m, to_string(m),
            target(), version());
    }

    /** Set the method of the request to the string
    */
    void
    set_method(string_view s)
    {
        set(string_to_method(s), s,
            target(), version());
    }

    /** Set the target string of the request

        This function sets the request-target.
        The caller is responsible for ensuring
        that the string passed is syntactically
        valid.
    */
    void
    set_target(string_view s)
    {
        set(
            method_, method_str(),
            s, version());
    }

    /** Set the HTTP version of the request
    */
    void
    set_version(
        http_proto::version v)
    {
        set(
            method_, method_str(),
            target(), v);
    }

    /** Set the method, target, and version of the request

        This is more efficient than setting the
        properties individually.
    */
    void
    set(http_proto::method m,
        string_view t,
        http_proto::version v =
            http_proto::version::http_1_1)
    {
        set(m, to_string(m), t, v);
    }

    /** Set the method, target, and version of the request

        This is more efficient than setting the
        properties individually.
    */
    void
    set(string_view m,
        string_view t,
        http_proto::version v =
            http_proto::version::http_1_1)
    {
        set(string_to_method(m), m, t, v);
    }

    /** Swap this with another instance
    */
    BOOST_HTTP_PROTO_DECL
    void
    swap(request& other) noexcept;

    /** Swap two instances
    */
    friend
    void
    swap(
        request& v1,
        request& v2) noexcept
    {
        v1.swap(v2);
    }

private:
    BOOST_HTTP_PROTO_DECL
    void
    set(http_proto::method m,
        string_view ms,
        string_view t,
        http_proto::version v);
};

} // http_proto
} // boost

#endif


text:
// Date Last Altered: $Date: 2008-12-01 13:34:05 -0700 (Mon, 01 Dec 2008) $
// Revision Number: $Revision: 423 $
//--------------------------------------------*-C++-*------------------------------------//
/*! \file IteratorTest.cc
 *  \author Greg Davidson
 *  \date May 29, 2008
 *
 *  \brief Includes all of the unit tests for the Iterator and ReverseIterator
 *         functionality.
 *
 *  This file runs all of the unit tests for the Iterator and ReverseIterator template
 *  classes. */

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

#include "Iterator.hh"
#include "ReverseIterator.hh"

// Begin including the test suites...
// *** Test the iterator on a vector ***
#include "IteratorVectorTest.hh"

// *** Test the reverse iterator on a vector ***
#include "ReverseIteratorTest.hh"

ext: cc

max_stars_repo_name: jlconlin/PhDThesis

max_issues_repo_name: jlconlin/PhDThesis

max_forks_repo_name: jlconlin/PhDThesis


text:
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8
// basic_archive.cpp:

// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com .
// 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)

//  See http://www.boost.org for updates, documentation, and revision history.

//////////////////////////////////////////////////////////////////////
//
//  objects are stored as
//
//      class_id*   // -1 for a null pointer
//      if a new class id
//      [
//          exported key - class name*
//          tracking level - always/never
//          class version
//      ]
//
//      if tracking
//      [
//          object_id
//      ]
//
//      [   // if a new object id
//          data...
//      ]
//
//  * required only for pointers - optional for objects

#define BOOST_ARCHIVE_SOURCE
#include <boost/serialization/config.hpp>
#include <boost/archive/basic_archive.hpp>

namespace boost {
namespace archive {

///////////////////////////////////////////////////////////////////////
// constants used in archive signature
//This should never ever change. note that is not an std::string
// string.
BOOST_SYMBOL_VISIBLE const char *
BOOST_ARCHIVE_SIGNATURE(){
    return "serialization::archive";
}

// this should change if the capabilities are added to the library
// such that archives can be created which can't be read by previous
// versions of this library
// 1 - initial version
// 2 - made address tracking optional
// 3 - numerous changes - can't guarentee compatibility with previous versions
// 4 - Boost 1.34
//     added item_version to properly support versioning for collections
// 5 - Boost 1.36
//     changed serialization of collections: adding version even for primitive
//     types caused backwards compatibility breaking change in 1.35
// 6 - Boost 1.41 17 Nov 2009
//     serializing collection sizes as std::size_t
// 7   Boost 1.42 2 Feb 2010
//     error - changed binary version to 16 bits w/o changing library version #
//     That is - binary archives are recorded with #6 even though they are
//     different from the previous versions.  This means that binary archives
//     created with versions 1.42 and 1.43 will have to be fixed with a special
//     program which fixes the library version # in the header
//     Boost 1.43 6 May 2010
//     no change
// 8 - Boost 1.44
//     separated version_type into library_version_type and class_version_type
//     changed version_type to be stored as 8 bits.
// 10- fixed base64 output/input.
// 11- not changes
// 12- improved serialization of collections
// 13- simplified visibility, removed Borland, removed pfto
// 14- improved visibility, refactor map/set

BOOST_SYMBOL_VISIBLE library_version_type
BOOST_ARCHIVE_VERSION(){
    return library_version_type(14);
}

} // namespace archive
} // namespace boost

max_stars_repo_name: nxplatform/nx-mobile

max_issues_repo_name: nxplatform/nx-mobile

max_forks_repo_name: nxplatform/nx-mobile


text:
#ifndef STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP
#define STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/identity_constrain.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound.
 *
 *
The transform is
 *
 *
\f$f(x) = U - \exp(x)\f$
 *
 *
where \f$U\f$ is the upper bound.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x);
  }
  return ub - exp(x);
}

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound and increment the specified log
 * probability reference with the log absolute Jacobian
 * determinant of the transform.
 *
 *
The transform is as specified for
 * ub_constrain(T, double).  The log absolute Jacobian
 * determinant is
 *
 *
\f$ \log | \frac{d}{dx} -\mbox{exp}(x) + U |
 *     = \log | -\mbox{exp}(x) + 0 | = x\f$.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x, lp).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @param[in,out] lp log density
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub, T& lp) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x, lp);
  }
  lp += x;
  return ub - exp(x);
}

}  // namespace math

}  // namespace stan

#endif

max_stars_repo_name: PhilClemson/math

max_issues_repo_name: PhilClemson/math

max_forks_repo_name: PhilClemson/math


text:
#include <boost/test/unit_test.hpp>
#include "algorithms/greedy/complete_road_cycle_with_gas_stations.hpp"

BOOST_AUTO_TEST_SUITE(TestCompleteRoadCycleWithGasStations)

    BOOST_AUTO_TEST_CASE(test_get_start_gas_station)
    {
        {
            const std::vector<int> gas;
            const std::vector<int> cost;
            const int expected = -1;
            BOOST_CHECK(
                expected == Algo::Greedy::CompleteRoadCycle::get_start_gas_station(gas, cost));
        }

        {
            const std::vector<int> gas = {1};
            const std::vector<int> cost = {5};
            const int expected = -1;
            BOOST_CHECK(
                expected == Algo::Greedy::CompleteRoadCycle::get_start_gas_station(gas, cost));
        }

        {
            const std::vector<int> gas = {2};
            const std::vector<int> cost = {2};
            const int expected = 0;
            BOOST_CHECK(
                expected == Algo::Greedy::CompleteRoadCycle::get_start_gas_station(gas, cost));
        }

        {
            const std::vector<int> gas = {2, 3};
            const std::vector<int> cost = {2, 5};
            const int expected = -1;
            BOOST_CHECK(
                expected == Algo::Greedy::CompleteRoadCycle::get_start_gas_station(gas, cost));
        }

        {
            const std::vector<int> gas = {1,2,3,4,5};
            const std::vector<int> cost = {3,4,5,1,2};
            const int expected = 3;
            BOOST_CHECK(
                expected == Algo::Greedy::CompleteRoadCycle::get_start_gas_station(gas, cost));
        }
    }

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: iamantony/CppNotes

max_issues_repo_name: iamantony/CppNotes

max_forks_repo_name: iamantony/CppNotes


text:
#include <boost/test/unit_test.hpp>
#include "outliers_input.h"
#include "utils/math_utils.h"
#include "utils/algorithms.hpp"
#include "inputs/outlier_data.h"
#include "utils/algorithms.hpp"
#include <iostream>

void check_outlier(double expected, std::vector<double>& founds)
{
    auto i = std::find(std::begin(founds), std::end(founds), expected);
    if (i == std::end(founds)) {
        BOOST_TEST(false, "failed to find outliers "<<expected<<" in the list of founds outliers");
    } else {
        founds.erase(i);
    }
}

BOOST_AUTO_TEST_CASE(test_outliers)
{
    // please note that you have a python program to find the outliers for verifiction
    static const double alpha_val = 0.1;
    std::vector<double> samples(std::begin(outliers_data), std::end(outliers_data));
    auto outliers = utils::grubbstest(samples, alpha_val);
    BOOST_TEST(samples.size() != outliers.size());
    BOOST_TEST(samples.size() > outliers.size());
    BOOST_TEST(outliers.size() > 0u);
    // make sure that we dont have any duplications in the outliers
    BOOST_TEST_REQUIRE(utils::size(expected_outliers) == outliers.size());
    //std::cout<<"we have "<<outliers.size()<<" outliers that we found in the test out of "<<samples.size()<<std::endl;
    std::sort(std::begin(outliers), std::end(outliers));
    for (auto i = 0u; i < outliers.size(); ++i) {
        BOOST_TEST(outliers[i] == expected_outliers[i]);
        //auto r = verifier.insert(val);
        //BOOST_TEST(r.second, "we are not expecting that the value "<<val<<" would already be inside");
    }
}

BOOST_AUTO_TEST_CASE(test_outliers_with_expected)
{
    static const double alpha_val = 0.1;
    const auto data_size = utils::size(data_outlier);
    const auto number_expected_outliers = utils::size(expected_outlier);
    auto outliers{utils::grubbstest(std::begin(data_outlier), std::end(data_outlier), alpha_val)};
    BOOST_TEST_REQUIRE(number_expected_outliers == outliers.size(),
            "the number of outliers we have "<<outliers.size()<<" != "<<number_expected_outliers);
    //std::cout<<"we found outliers\n";
    //out_range(std::cout, outliers);
    //std::cout<<"\nexpected outliers:"<<std::endl;
    //for (auto index : expected_outlier) {
    //    if (index < data_size) {
    //        std::cout<<data_outlier[index]<<", ";
    //    } else {
    //        std::cout<<" out of range: "<<index<<", ";
    //    }
    //}
    //std::cout<<std::endl;
    for (auto index : expected_outlier) {   // these are the locations where we expecting to find outliers
        if (index < data_size) {
            auto outlier = data_outlier[index];
            check_outlier(outlier, outliers);
        } else {
            BOOST_TEST_REQUIRE(false, "we have invalid index "<<index<<" out of range for data "<<data_size);
        }
    }
    //std::cout<<std::endl;
}