text:
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <gtest/gtest.h>

#include "rosrt/rosrt.h"

#include <ros/ros.h>

#include <std_msgs/UInt32.h>

#include <boost/thread.hpp>

using namespace rosrt;

void publishThread(ros::Publisher& pub, bool& done)
{
  uint32_t i = 0;
  std_msgs::UInt32 msg;
  while (!done)
  {
    msg.data = i;
    pub.publish(msg);
    ros::WallDuration(0.0001).sleep();
    ++i;
  }
}

TEST(Subscriber, singleSubscriber)
{
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<std_msgs::UInt32>("test", 1);
  bool done = false;
  boost::thread t(boost::bind(publishThread, boost::ref(pub), boost::ref(done)));

  Subscriber<std_msgs::UInt32> sub(2, nh, "test");

  resetThreadAllocInfo();

  uint32_t count = 0;
  int32_t last = -1;
  while (count < 10000)
  {
    std_msgs::UInt32ConstPtr msg = sub.poll();
    if (msg)
    {
      ASSERT_GT((int32_t)msg->data, last);
      last = msg->data;
      ++count;
    }
  }

  ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL);

  done = true;
  t.join();
}

TEST(Subscriber, multipleSubscribersSameTopic)
{
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<std_msgs::UInt32>("test", 1);
  bool done = false;
  boost::thread t(boost::bind(publishThread, boost::ref(pub), boost::ref(done)));

  const size_t sub_count = 10;
  Subscriber<std_msgs::UInt32> subs[sub_count];
  uint32_t counts[sub_count];
  int32_t lasts[sub_count];
  for (size_t i = 0; i < sub_count; ++i)
  {
    subs[i].initialize(2);
    subs[i].subscribe(nh, "test");
    counts[i] = 0;
    lasts[i] = -1;
  }

  resetThreadAllocInfo();

  bool all_done = false;
  while (!all_done)
  {
    all_done = true;

    for (size_t i = 0; i < sub_count; ++i)
    {
      std_msgs::UInt32ConstPtr msg = subs[i].poll();
      if (msg)
      {
        ASSERT_GT((int32_t)msg->data, lasts[i]);
        lasts[i] = msg->data;
        ++counts[i];
      }

      if (counts[i] < 10000)
      {
        all_done = false;
      }
    }
  }

  ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL);

  done = true;
  t.join();
}

TEST(Subscriber, multipleSubscribersMultipleTopics)
{
  const size_t sub_count = 10;

  ros::NodeHandle nh;
  bool done = false;
  boost::thread_group tg;

  ros::Publisher pubs[sub_count];
  Subscriber<std_msgs::UInt32> subs[sub_count];
  uint32_t counts[sub_count];
  int32_t lasts[sub_count];
  for (size_t i = 0; i < sub_count; ++i)
  {
    std::stringstream topic;
    topic << "test" << i;
    pubs[i] = nh.advertise<std_msgs::UInt32>(topic.str(), 1);
    tg.create_thread(boost::bind(publishThread, boost::ref(pubs[i]), boost::ref(done)));

    subs[i].initialize(2);
    subs[i].subscribe(nh, topic.str());
    counts[i] = 0;
    lasts[i] = -1;
  }

  bool all_done = false;
  while (!all_done)
  {
    all_done = true;

    for (size_t i = 0; i < sub_count; ++i)
    {
      std_msgs::UInt32ConstPtr msg = subs[i].poll();
      if (msg)
      {
        ASSERT_GT((int32_t)msg->data, lasts[i]);
        lasts[i] = msg->data;
        ++counts[i];
      }

      if (counts[i] < 10000)
      {
        all_done = false;
      }
    }
  }

  done = true;
  tg.join_all();
}

void subscribeThread(Subscriber<std_msgs::UInt32>& sub, bool& failed)
{
  resetThreadAllocInfo();

  const int count = 10000;
  int my_count = 0;
  int32_t last = -1;
  while (true)
  {
    std_msgs::UInt32ConstPtr msg = sub.poll();
    if (msg)
    {
      if (last >= (int32_t)msg->data)
      {
        ROS_ERROR_STREAM("Thread " << boost::this_thread::get_id() << " last is greater than msg " << last << " >= " << msg->data);
        failed = true;
        break;
      }

      last = msg->data;
      ++my_count;

      //ROS_INFO_STREAM("Thread " << boost::this_thread::get_id() << " " << my_count);
    }

    if (my_count >= count)
    {
      break;
    }

    ros::WallDuration(0.0001).sleep();
  }

  if (getThreadAllocInfo().total_ops > 0UL)
  {
    ROS_ERROR_STREAM("Thread " << boost::this_thread::get_id() << " performed " << getThreadAllocInfo().total_ops << " memory operations (malloc/free/etc.)");
    failed = true;
  }
}

TEST(Subscriber, multipleThreadsSameTopic)
{
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<std_msgs::UInt32>("test", 1);
  bool done = false;
  boost::thread t(boost::bind(publishThread, boost::ref(pub), boost::ref(done)));
  boost::thread_group tg;

  const size_t sub_count = 10;
  Subscriber<std_msgs::UInt32> subs[sub_count];
  bool failed[sub_count];
  for (size_t i = 0; i < sub_count; ++i)
  {
    subs[i].initialize(2);
    subs[i].subscribe(nh, "test");
    failed[i] = false;

    tg.create_thread(boost::bind(subscribeThread, boost::ref(subs[i]), boost::ref(failed[i])));
  }

  tg.join_all();
  done = true;
  t.join();

  for (size_t i = 0; i < sub_count; ++i)
  {
    EXPECT_FALSE(failed[i]);
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_rt_subscriber");
  testing::InitGoogleTest(&argc, argv);

  ros::NodeHandle nh;
  rosrt::init();

  return RUN_ALL_TESTS();
}


text:
// vim:ts=2:sw=2:expandtab:autoindent:filetype=cpp:

// Copyright (c) 2007 Aristid Breitkreuz, Ruediger Sonderfeld, Christian Uhrhan
//
// Distributed under the Boost Software License, Version 1.0 and under the MIT
// License.
//
// See accompanying LICENSES.txt or
// <http://fragments.sourceforge.net/LICENSES.txt> or, for the Boost Software
// License, <http://www.boost.org/LICENSE_1_0.txt>.
//
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef FRAGMENTS_CONTAINER_TYPE_HPP
#define FRAGMENTS_CONTAINER_TYPE_HPP

#include <fragments/container/concepts/type_provider.hpp>
#include <boost/mpl/vector.hpp>

namespace fragments { namespace container {

template<typename T>
struct type {
  typedef boost::mpl::vector1<concepts::type_provider> concept;

  template<typename Before>
  struct fragment : public Before {
    typedef T value_type;
    typedef T *pointer;
    typedef T const *const_pointer;
    typedef T &reference;
    typedef T const &const_reference;

    fragment() {}

    template<typename ArgPack>
    fragment(ArgPack const &args)
      : Before(args)
    {}
  };
};

}}

#endif

ext: hpp

max_stars_repo_name: aristidb/fragments

max_issues_repo_name: aristidb/fragments

max_forks_repo_name: aristidb/fragments


text:
/**
 * @file multi_discrete_space.hpp
 * @author David Albert (david.albert@insa-lyon.fr)
 * @brief File for MultiDiscreteSpace class.
 * @version 1.0
 * @date 01/02/2021
 *
 * @copyright Copyright (c) 2021
 *
 */
#pragma once

#include <vector>
#include <assert.h>
#include <boost/bimap.hpp>

#include <sdm/types.hpp>
#include <sdm/core/item.hpp>
#include <sdm/core/space/discrete_space.hpp>
#include <sdm/core/joint.hpp>

/**
 * @namespace sdm
 *
 * @brief Namespace grouping all tools required for sequential decision making.
 *
 */
namespace sdm
{
    /**
     * @brief This class provide a way to instantiate multi discrete space (i.e. list of discrete spaces). Typically it is used to store a set of spaces, one by agent (i.e. action_spaces in POSG). This can be view as a set of discrete spaces or as a discrete space of joint items.
     *
     * @tparam std::shared_ptr<Item> The type of items in each sub-discrete space.
     */
    class MultiDiscreteSpace : public DiscreteSpace,
                               public Joint<std::shared_ptr<Space>>
    {
    public:
        using value_type = JointItem;
        using iterator_type = DiscreteSpace::iterator_type;

        /**
         * @brief Instantiate a default discrete space (MultiDiscreteSpace class)
         */
        MultiDiscreteSpace();

        /**
         * @brief Instantiate a multi discrete space from the list its sub-spaces (as shared pointer).
         *
         */
        MultiDiscreteSpace(const std::vector<std::shared_ptr<Space>> &sub_spaces, bool store_items = true);

        /**
         * @brief Instantiate a multi discrete space from the list its sub-spaces (as shared pointer).
         *
         */
        MultiDiscreteSpace(const std::vector<std::vector<std::shared_ptr<Item>>> &values, bool store_items = true);

        /**
         * @brief Copy constructor
         *
         * @param copy the space to be copied
         */
        MultiDiscreteSpace(const MultiDiscreteSpace &copy);

        /**
         * @brief Instantiate a multi discrete space using a list of dimensions (one by single space). This constructor is only available for classes where std::shared_ptr<Item> is an integer (long, int, short, etc).
         *
         * @param num_items the number of items in each spaces. If {k_1, k_2} then {0, 1, ..., k_1 - 1} are possible items in first subspace and {0, 1, ..., k_2 - 1} in the second subspace.
         */
        template <bool TBool = std::is_integral<std::shared_ptr<Item>>::value>
        MultiDiscreteSpace(const std::enable_if_t<TBool, std::vector<std::shared_ptr<Item>>> &num_items);

        /**
         * @brief   Get a specific index of an item for a given agent
         * @param   ag_id index of the agent
         * @param   item the item we want to get the index
         */
        number getItemIndex(number ag_id, const std::shared_ptr<Item> &item) const;

        /**
         * @brief Get a specific item from its index
         *
         * @param index the index
         * @return a pointer on the item
         */
        std::shared_ptr<Item> getItem(number index) const;

        /**
         * @brief   Get a specific item for a given agent (from its index)
         * @param   ag_id index of the agent
         * @param   item_index the index of the item we want to get
         */
        std::shared_ptr<Item> getItem(number ag_id, number item_index) const;

        /**
         * @brief Get the number of sub-space.
         */
        number getNumSpaces() const;

        /**
         * @brief Get a specific subspace
         *
         * @param index the index of the space
         * @return a shared pointer on a specific space
         */
        std::shared_ptr<Space> getSpace(number index) const;

        template <bool TBool = std::is_integral<std::shared_ptr<Item>>::value>
        void setSpaces(const std::enable_if_t<TBool, std::vector<std::shared_ptr<Item>>> &num_items);
        void setSpaces(const std::vector<std::vector<std::shared_ptr<Item>>> &);
        void setSpaces(const std::vector<std::shared_ptr<Space>> &spaces);

        /*!
         * @brief Transform joint item to its index in the list of all joint items.
         * @param jitem the joint item we want to get the index
         * @return the corresponding index
         */
        number getJointItemIndex(std::shared_ptr<JointItem> &jitem) const;
        // number getJointItemIndex(const std::vector<std::shared_ptr<Item>> &) const;

        /*!
         * @brief Get the corresponding joint item from its index.
         */
        std::shared_ptr<Item> getJointItem(number) const;

        std::string str() const;

        virtual iterator_type begin();
        virtual iterator_type end();

        MultiDiscreteSpace &operator=(const MultiDiscreteSpace &);
        bool operator==(const MultiDiscreteSpace &other) const;
        bool operator!=(const MultiDiscreteSpace &other) const;

        /**
         * @brief Verify is the multi discrete space contains the JointItem;
         *
         * @return true
         * @return false
         */
        bool contains(const std::shared_ptr<Item> &) const;

        friend std::ostream &operator<<(std::ostream &os, const MultiDiscreteSpace &sp)
        {
            os << sp.str();
            return os;
        }

    protected:
        using jitems_bimap = DiscreteSpace::items_bimap;
        using jitems_bimap_value = jitems_bimap::value_type;

        /**
         * @brief Sets the number of joint items
         *
         */
        void setNumJItems(number);

        inline std::shared_ptr<DiscreteSpace> cast(const std::shared_ptr<Space> &space) const;

    };

} // namespace sdm

max_stars_repo_name: SDMStudio/sdms

max_issues_repo_name: SDMStudio/sdms

max_forks_repo_name: SDMStudio/sdms


text:
#include "delphi_esr_application.h"

#include <cav_msgs/ExternalObjectList.h>
#include <cav_srvs/GetDriversWithCapabilities.h>
#include <boost/math/constants/constants.hpp>

namespace delphi
{
//these are helpers for the diagnostic messages
std::ostream& operator<<(std::ostream &o,SpeedDirection n) { switch(n){
        case SpeedDirection::Forward: return o<<"Forward";
        case SpeedDirection::Reverse: return o<<"Reverse";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,SteeringWheelSign n) { switch(n){
        case SteeringWheelSign::CounterClockwise: return o<<"CounterClockwise";
        case SteeringWheelSign::Clockwise: return o<<"Clockwise";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,Validity n) { switch(n){
        case Validity::UnavailableOrInvalid: return o<<"UnavailableOrInvalid";
        case Validity::AvailableAndValid: return o<<"AvailableAndValid";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,SensorModeCommand n) { switch(n){
        case SensorModeCommand::DoNotRadiate: return o<<"DoNotRadiate";
        case SensorModeCommand::Radiate: return o<<"Radiate";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,WindShieldWiperStatus n) { switch(n){
        case WindShieldWiperStatus::Off: return o<<"Off";
        case WindShieldWiperStatus::On: return o<<"On";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,RawDataEnable n) { switch(n){
        case RawDataEnable::Filtered: return o<<"Filtered";
        case RawDataEnable::Raw: return o<<"Raw";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,RadiatingStatus n) { switch(n){
        case RadiatingStatus::NotRadiating: return o<<"NotRadiating";
        case RadiatingStatus::Radiating: return o<<"Radiating";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,ErrorStatus n) { switch(n){
        case ErrorStatus::NotFailed: return o<<"NotFailed";
        case ErrorStatus::Failed: return o<<"Failed";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,BlockedStatus n) { switch(n){
        case BlockedStatus::NotBlocked: return o<<"NotBlocked";
        case BlockedStatus::Blocked: return o<<"Blocked";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,OverHeatStatus n) { switch(n){
        case OverHeatStatus::NotOverTemp: return o<<"NotOverTemp";
        case OverHeatStatus::OverTemp: return o<<"OverTemp";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,GroupingMode n) { switch(n){
        case GroupingMode::None: return o<<"None";
        case GroupingMode::MovingTargetsOnly: return o<<"MovingTargetsOnly";
        case GroupingMode::StationaryTargetsOnly: return o<<"StationaryTargetsOnly";
        case GroupingMode::StationayAndMovingTargets: return o<<"StationayAndMovingTargets";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,MRLRMode n) { switch(n){
        case MRLRMode::Reserved: return o<<"Reserved";
        case MRLRMode::MROnly: return o<<"MROnly";
        case MRLRMode::LROnly: return o<<"LROnly";
        case MRLRMode::MRAndLR: return o<<"MRAndLR";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,MedRangeMode n) { switch(n){
        case MedRangeMode::NoUpdate: return o<<"NoUpdate";
        case MedRangeMode::MRUpdate: return o<<"MRUpdate";
        case MedRangeMode::LRUpdate: return o<<"LRUpdate";
        case MedRangeMode::BothMRandLRUpdate: return o<<"BothMRandLRUpdate";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,TrackStatus n) { switch(n){
        case TrackStatus::NoTarget: return o<<"NoTarget";
        case TrackStatus::NewTarget: return o<<"NewTarget";
        case TrackStatus::NewUpdatedTarget: return o<<"NewUpdatedTarget";
        case TrackStatus::UpdatedTarget: return o<<"UpdatedTarget";
        case TrackStatus::CoastedTarget: return o<<"CoastedTarget";
        case TrackStatus::MergedTarget: return o<<"MergedTarget";
        case TrackStatus::InvalidCOastedTarget: return o<<"InvalidCOastedTarget";
        case TrackStatus::NewCoastedTarget: return o<<"NewCoastedTarget";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,InterfaceVersion n) { switch(n){
        case InterfaceVersion::TDPwith64tracks: return o<<"TDPwith64tracks";
        case InterfaceVersion::ADPDV1with64tracks: return o<<"ADPDV1with64tracks";
        case InterfaceVersion::ADPDV1forDV1MOART: return o<<"ADPDV1forDV1MOART";
        case InterfaceVersion::ADPDV2: return o<<"ADPDV2";
        case InterfaceVersion::ADPDV3: return o<<"ADPDV3";
        case InterfaceVersion::ADPPV: return o<<"ADPPV";
        case InterfaceVersion::ADPPV_2011: return o<<"ADPPV_2011";
        case InterfaceVersion::Reserved7: return o<<"Reserved7";
        case InterfaceVersion::Reserved8: return o<<"Reserved8";
        case InterfaceVersion::Reserved9: return o<<"Reserved9";
        case InterfaceVersion::Reserved10: return o<<"Reserved10";
        case InterfaceVersion::Reserved11: return o<<"Reserved11";
        case InterfaceVersion::Reserved12: return o<<"Reserved12";
        case InterfaceVersion::Reserved13: return o<<"Reserved13";
        case InterfaceVersion::Reserved14: return o<<"Reserved14";
        case InterfaceVersion::Reserved15: return o<<"Reserved15";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,PowerMode n) { switch(n){
        case PowerMode::DSPInit: return o<<"DSPInit";
        case PowerMode::RadiateOff: return o<<"RadiateOff";
        case PowerMode::RadiateOn: return o<<"RadiateOn";
        case PowerMode::DSPShutdown: return o<<"DSPShutdown";
        case PowerMode::DSPOff: return o<<"DSPOff";
        case PowerMode::HostShutdown: return o<<"HostShutdown";
        case PowerMode::Test: return o<<"Test";
        case PowerMode::Invalid: return o<<"Invalid";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,AlignmentStatus n) { switch(n){
        case AlignmentStatus::Off: return o<<"Off";
        case AlignmentStatus::Bust: return o<<"Bust";
        case AlignmentStatus::Success: return o<<"Success";
        case AlignmentStatus::FailNoTarget: return o<<"FailNoTarget";
        case AlignmentStatus::FailDevTooLarge: return o<<"FailDevTooLarge";
        case AlignmentStatus::FailVarTooLarge: return o<<"FailVarTooLarge";
        default: return o<<"(invalid value)"; }}
std::ostream& operator<<(std::ostream &o,TurnSignalStatus n) { switch(n){
        case TurnSignalStatus::None: return o<<"None";
        case TurnSignalStatus::Left: return o<<"Left";
        case TurnSignalStatus::Right: return o<<"Right";
        default: return o<<"(invalid value)"; }}
}

DelphiESRApplication::DelphiESRApplication(int argc, char **argv, const std::string &name) : DriverApplication(argc,argv,name), received_update_(false) {
}

DelphiESRApplication::~DelphiESRApplication() {
    client_.reset();
}

std::vector<std::string> &DelphiESRApplication::get_api() {
    return api_;
}

void DelphiESRApplication::publish_updates() {
    cav_msgs::ExternalObjectList out_list;
    out_list.header.frame_id = sensor_frame_;
    out_list.header.stamp = ros::Time::fromBoost(sensor_burst.stamp);

    for(auto it : sensor_burst.tracks)
    {
        if(it.track_status == delphi::TrackStatus::NoTarget) continue;

        cav_msgs::ExternalObject obj;

        obj.header.frame_id = sensor_frame_;
        obj.header.stamp = out_list.header.stamp;
        obj.presence_vector = cav_msgs::ExternalObject::SIZE_PRESENCE_VECTOR;
        obj.size.y = it.width / 2.0;
        obj.size.x = 0.0;
        obj.size.z = 0.0;

        obj.presence_vector |= cav_msgs::ExternalObject::POSE_PRESENCE_VECTOR;
        obj.pose.pose.position.x = it.range*cos(it.angle*boost::math::constants::pi<double>()/180.0);
        obj.pose.pose.position.y = it.range*sin(it.angle*boost::math::constants::pi<double>()/180.0);
        obj.pose.pose.position.z = 0;

        obj.presence_vector |= cav_msgs::ExternalObject::VELOCITY_PRESENCE_VECTOR;
        obj.velocity.twist.linear.x = it.range_rate*cos(it.angle*boost::math::constants::pi<double>()/180.0)
                                      + it.lat_rate*sin(it.angle*boost::math::constants::pi<double>()/180.0);

        obj.velocity.twist.linear.y = it.range_rate*sin(it.angle*boost::math::constants::pi<double>()/180.0)
                                      - it.lat_rate*cos(it.angle*boost::math::constants::pi<double>()/180.0);

        obj.velocity.twist.linear.z = 0;

        obj.presence_vector |= cav_msgs::ExternalObject::RANGE_RATE_PRESENCE_VECTOR;
        obj.range_rate = it.range_rate;

        obj.presence_vector |= cav_msgs::ExternalObject::AZIMUTH_RATE_PRESENCE_VECTOR;
        obj.azimuth_rate = -it.lat_rate/it.range;

        obj.presence_vector |= cav_msgs::ExternalObject::CONFIDENCE_PRESENCE_VECTOR;
        obj.confidence = it.track_status == delphi::TrackStatus::NewTarget ? 0.5 : 1.0;

        out_list.objects.push_back(obj);
    }

    objects_pub_.publish(out_list);

    diag_updater_.update();
}

void DelphiESRApplication::velocity_cb(const geometry_msgs::TwistStampedConstPtr &msg) {
    rx0_.speed = static_cast<float>(std::fabs(msg->twist.linear.x));
    rx0_.speed_direction = msg->twist.linear.x > 0 ? delphi::SpeedDirection::Forward : delphi::SpeedDirection::Reverse;
    rx0_.yaw_rate = static_cast<float>(msg->twist.angular.z * 180.0 / M_PI);

    rx0_.yaw_rate_validity = delphi::Validity::AvailableAndValid;
    rx1_.vehicle_speed_validity = delphi::Validity::AvailableAndValid;

    last_velocity_update_ = msg;
}

void DelphiESRApplication::initialize() {

    //are we getting velocities
    pnh_->param<bool>("use_velocities", recv_velocities_, false);
    if(recv_velocities_)
    {
        std::string topic = "velocity";
        pnh_->param<std::string>("velocity_topic",topic,topic);
        if(topic.empty())
        {
            ROS_INFO_STREAM("Looking for available velocity driver");
            ros::service::waitForService("get_drivers_with_capabilities");
            ros::ServiceClient client = nh_->serviceClient<cav_srvs::GetDriversWithCapabilities>("get_drivers_with_capabilities");
            cav_srvs::GetDriversWithCapabilities srv;
            srv.request.capabilities.push_back("position/velocity");
            while(srv.response.driver_data.empty())
                client.call(srv);

            topic = srv.response.driver_data.front();
        }

        velocity_sub_ = pnh_->subscribe<geometry_msgs::TwistStamped>(topic,1,&DelphiESRApplication::velocity_cb,this);
    }

    //initialize static variables sent as commands
    {
        pnh_->param<int32_t>("radius_curvature", rx0_.radius_curvature, rx0_.radius_curvature);
        rx1_.radiate_command = delphi::SensorModeCommand::Radiate;

        pnh_->param<float>("lateral_mounting_offset", rx1_.lateral_mounting_offset,rx1_.lateral_mounting_offset);
        pnh_->param<float>("angle_misalignment", rx1_.angle_misalignment,rx1_.angle_misalignment);
        pnh_->param<bool>("mmr_upside_down", rx1_.mmr_upside_down,rx1_.mmr_upside_down);
        pnh_->param<bool>("blockage_disable", rx1_.blockage_disable,rx1_.blockage_disable);
        pnh_->param<bool>("use_angle_misalignment", rx1_.use_angle_misalignment,rx1_.use_angle_misalignment);
        pnh_->param<bool>("lr_transmit", rx1_.lr_transmit,rx1_.lr_transmit);
        pnh_->param<bool>("mr_transmit", rx1_.mr_transmit,rx1_.mr_transmit);
        int32_t temp;
        if(pnh_->getParam("maximum_tracks",temp))
            rx1_.maximum_tracks = static_cast<uint8_t>(temp);

        if(pnh_->getParam("high_yaw_angle", temp))
            rx1_.high_yaw_angle = static_cast<int8_t>(temp);

        if(pnh_->getParam("raw_data_enable", temp))
            rx1_.raw_data_enable = static_cast<delphi::RawDataEnable>(temp);

        if(pnh_->getParam("grouping_mode", temp))
            rx1_.grouping_mode = static_cast<delphi::GroupingMode>(temp);
    }

    //setup object topic
    pnh_->param<std::string>("sensor_frame", sensor_frame_,"delphi");
    objects_pub_=pnh_->advertise<cav_msgs::ExternalObjectList>("sensor/objects",10);
    api_.push_back(objects_pub_.getTopic());

    std::string can_device_name;
    pnh_->param<std::string>("device_name", can_device_name, "can0");
    //setup DelphiESRCANClient
    std::shared_ptr<cav::SocketCANInterface> device = std::make_shared<cav::SocketCANInterface>(can_device_name);
    client_.reset(new DelphiESRCANClient<cav::SocketCANInterface>(device));
    client_->init();

    client_->onBurstReceived.connect(std::bind(&DelphiESRApplication::burst_cb, this, std::placeholders::_1));
    client_->onError.connect([this](const boost::system::error_code&ec){ ROS_WARN_STREAM_THROTTLE(1, "Error with delphi client: " << ec.message());});

    //diagnostics updater
    diag_updater_.setHardwareID("Delphi-ESR2.5");
    diag_updater_.add("updater",this, &DelphiESRApplication::produce_diagnostics);

    auto status = getStatus();
    status.status = cav_msgs::DriverStatus::OPERATIONAL;
    status.radar = true;

    setStatus(status);
    last_radar_update_ = ros::Time::now();

    spin_rate = 50;

}

void DelphiESRApplication::produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK,"Delphi Status Messages");

    stat.add("Status1.comm_error",sensor_burst.esrStatus1.comm_error);
    stat.add("Status1.radius_curvature_calc",sensor_burst.esrStatus1.radius_curvature_calc);
    stat.add("Status1.vehicle_speed_calc",sensor_burst.esrStatus1.vehicle_speed_calc);
    stat.add("Status1.yaw_rate_calc",sensor_burst.esrStatus1.yaw_rate_calc);

    stat.add("Status2.temperature",(int)sensor_burst.esrStatus2.temperature);
    stat.add("Status2.maximum_tracks_ack",(int)sensor_burst.esrStatus2.maximum_tracks_ack);
    stat.add("Status2.sw_version",(int)sensor_burst.esrStatus2.sw_version);
    stat.add("Status2.internal_error",sensor_burst.esrStatus2.internal_error);
    stat.add("Status2.steering_angle_ack",(int)sensor_burst.esrStatus2.steering_angle_ack);
    stat.add("Status2.overheat_error",sensor_burst.esrStatus2.overheat_error);
    stat.add("Status2.grouping_mode",sensor_burst.esrStatus2.grouping_mode);
    stat.add("Status2.raw_data_mode",sensor_burst.esrStatus2.raw_data_mode);
    stat.add("Status2.xcvr_operational",sensor_burst.esrStatus2.xcvr_operational);
    stat.add("Status2.veh_spd_comp_factor",sensor_burst.esrStatus2.veh_spd_comp_factor);
    stat.add("Status2.yaw_rate_bias",sensor_burst.esrStatus2.yaw_rate_bias);
    stat.add("Status2.range_perf_error",sensor_burst.esrStatus2.range_perf_error);

    stat.add("Status3.sw_version_pld",(int)sensor_burst.esrStatus3.sw_version_pld);
    stat.add("Status3.serial_num",(int)sensor_burst.esrStatus3.serial_num);
    stat.add("Status3.interface_version",(int)sensor_burst.esrStatus3.interface_version);
    stat.add("Status3.hardware_version",(int)sensor_burst.esrStatus3.hardware_version);
    stat.add("Status3.host_sw_version",(int)sensor_burst.esrStatus3.host_sw_version);

    stat.add("Status4.LR_only_grating_lobe_detected",sensor_burst.esrStatus4.LR_only_grating_lobe_detected);
    stat.add("Status4.sidelobe_blockage",sensor_burst.esrStatus4.sidelobe_blockage);
    stat.add("Status4.partial_blockage",sensor_burst.esrStatus4.partial_blockage);
    stat.add("Status4.auto_align_angle",sensor_burst.esrStatus4.auto_align_angle);
    stat.add("Status4.path_ID_FCW_stat",(int)sensor_burst.esrStatus4.path_ID_FCW_stat);
    stat.add("Status4.path_ID_FCW_move",(int)sensor_burst.esrStatus4.path_ID_FCW_move);
    stat.add("Status4.path_ID_CMBB_stat",(int)sensor_burst.esrStatus4.path_ID_CMBB_stat);
    stat.add("Status4.path_ID_CMBB_move",(int)sensor_burst.esrStatus4.path_ID_CMBB_move);
    stat.add("Status4.path_ID_ACC",(int)sensor_burst.esrStatus4.path_ID_ACC);
    stat.add("Status4.mode",sensor_burst.esrStatus4.mode);
    stat.add("Status4.truck_target_detected",sensor_burst.esrStatus4.truck_target_detected);

    stat.add("Status5.supply_10v_a2d",(int)sensor_burst.esrStatus5.supply_10v_a2d);
    stat.add("Status5.supply_3p3v_a2d",(int)sensor_burst.esrStatus5.supply_3p3v_a2d);
    stat.add("Status5.supply_5vd_a2d",(int)sensor_burst.esrStatus5.supply_5vd_a2d);
    stat.add("Status5.supply_5va_a2d",(int)sensor_burst.esrStatus5.supply_5va_a2d);
    stat.add("Status5.temp2_a2d",(int)sensor_burst.esrStatus5.temp2_a2d);
    stat.add("Status5.temp1_a2d",(int)sensor_burst.esrStatus5.temp1_a2d);
    stat.add("Status5.ignp_a2d",(int)sensor_burst.esrStatus5.ignp_a2d);
    stat.add("Status5.swbatt_a2d",(int)sensor_burst.esrStatus5.swbatt_a2d);

    stat.add("Status6.wave_diff_a2d",(int)sensor_burst.esrStatus6.wave_diff_a2d);
    stat.add("Status6.supply_n5v_a2d",(int)sensor_burst.esrStatus6.supply_n5v_a2d);
    stat.add("Status6.supply_1p8v_a2d",(int)sensor_burst.esrStatus6.supply_1p8v_a2d);
    stat.add("Status6.sw_version_dsp_3rd_byte",(int)sensor_burst.esrStatus6.sw_version_dsp_3rd_byte);
    stat.add("Status6.service_alignment_updates_done",(int)sensor_burst.esrStatus6.service_alignment_updates_done);
    stat.add("Status6.vertical_alignment_updated",sensor_burst.esrStatus6.vertical_alignment_updated);
    stat.add("Status6.found_target",sensor_burst.esrStatus6.found_target);
    stat.add("Status6.recommend_unconverge",sensor_burst.esrStatus6.recommend_unconverge);
    stat.add("Status6.factory_align_status2",sensor_burst.esrStatus6.factory_align_status2);
    stat.add("Status6.factory_align_status1",sensor_burst.esrStatus6.factory_align_status1);
    stat.add("Status6.system_power_mode",sensor_burst.esrStatus6.system_power_mode);
    stat.add("Status6.factory_misalignment",sensor_burst.esrStatus6.factory_misalignment);
    stat.add("Status6.vertical_misalignment",sensor_burst.esrStatus6.vertical_misalignment);

    stat.add("Status7.active_fault0",(int)sensor_burst.esrStatus7.active_fault0);
    stat.add("Status7.active_fault1",(int)sensor_burst.esrStatus7.active_fault1);
    stat.add("Status7.active_fault2",(int)sensor_burst.esrStatus7.active_fault2);
    stat.add("Status7.active_fault3",(int)sensor_burst.esrStatus7.active_fault3);
    stat.add("Status7.active_fault4",(int)sensor_burst.esrStatus7.active_fault4);
    stat.add("Status7.active_fault5",(int)sensor_burst.esrStatus7.active_fault5);
    stat.add("Status7.active_fault6",(int)sensor_burst.esrStatus7.active_fault6);
    stat.add("Status7.active_fault7",(int)sensor_burst.esrStatus7.active_fault7);

    stat.add("Status8.history_fault0",(int)sensor_burst.esrStatus8.history_fault0);
    stat.add("Status8.history_fault1",(int)sensor_burst.esrStatus8.history_fault1);
    stat.add("Status8.history_fault2",(int)sensor_burst.esrStatus8.history_fault2);
    stat.add("Status8.history_fault3",(int)sensor_burst.esrStatus8.history_fault3);
    stat.add("Status8.history_fault4",(int)sensor_burst.esrStatus8.history_fault4);
    stat.add("Status8.history_fault5",(int)sensor_burst.esrStatus8.history_fault5);
    stat.add("Status8.history_fault6",(int)sensor_burst.esrStatus8.history_fault6);
    stat.add("Status8.history_fault7",(int)sensor_burst.esrStatus8.history_fault7);

    stat.add("Status9.path_id_acc_2",(int)sensor_burst.esrStatus9.path_id_acc_2);
    stat.add("Status9.path_id_acc_3",(int)sensor_burst.esrStatus9.path_id_acc_3);
    stat.add("Status9.filtered_xohp_of_acc_tgt",sensor_burst.esrStatus9.filtered_xohp_of_acc_tgt);
    stat.add("Status9.water_spray_id",(int)sensor_burst.esrStatus9.water_spray_id);
    stat.add("Status9.serial_num_3rd_byte",(int)sensor_burst.esrStatus9.serial_num_3rd_byte);
    stat.add("Status9.sideslip_angle",sensor_burst.esrStatus9.sideslip_angle);
    stat.add("Status9.avg_pwr_cwblkg",(int)sensor_burst.esrStatus9.avg_pwr_cwblkg);

}

void DelphiESRApplication::burst_cb(const DelphiESRCANClient<cav::SocketCANInterface>::BurstData &data) {
    std::unique_lock<std::mutex> lock(mutex_,std::try_to_lock);
    if(!lock.owns_lock()) return;

    ROS_DEBUG_STREAM("Received burst from sensor");
    sensor_burst = data;
    received_update_ = true;
    last_radar_update_ = ros::Time::now();

}

void DelphiESRApplication::pre_spin() {
    if(received_update_)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        publish_updates();
        received_update_ = false;
    }

    if(ros::Time::now() - last_radar_update_ > ros::Duration(0.5))
    {
        ROS_WARN_STREAM_THROTTLE(1,"Not receiving updates from sensor");
        auto status = getStatus();
        status.status = cav_msgs::DriverStatus::FAULT;
        status.radar = true;

        setStatus(status);
    }
    else if (getStatus().status != cav_msgs::DriverStatus::OPERATIONAL)
    {
        auto status = getStatus();
        status.status = cav_msgs::DriverStatus::OPERATIONAL;
        status.radar = true;

        setStatus(status);
    }
}

void DelphiESRApplication::post_spin() {
    if(last_velocity_update_ && ros::Time::now() - last_velocity_update_->header.stamp > ros::Duration(0.1))
    {
        rx0_.yaw_rate_validity = delphi::Validity::UnavailableOrInvalid;
        rx1_.vehicle_speed_validity = delphi::Validity::UnavailableOrInvalid;
    }

    client_->sendCommands(rx0_,rx1_);
}

void DelphiESRApplication::shutdown() {
    ROS_WARN_STREAM("Shutting down");
    client_.reset();
}

ext: cpp

max_stars_repo_name: mjeronimo/carma-delphi-esr-driver

max_issues_repo_name: mjeronimo/carma-delphi-esr-driver

max_forks_repo_name: mjeronimo/carma-delphi-esr-driver


text:
#ifndef CONST_CROSS_ITERATOR_HPP
#define CONST_CROSS_ITERATOR_HPP

#include <iterator>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/irange.hpp>
#include <boost/log/trivial.hpp>

namespace Stg
{
    template<typename _InputIterator,
            typename __T = typename std::iterator_traits<_InputIterator>::value_type::value_type>
    struct ConstCrossIterator : public boost::iterator_facade<
            ConstCrossIterator<_InputIterator>,
            __T const,
            boost::forward_traversal_tag>
    {
        using It = _InputIterator;
        using Range = std::vector<std::tuple<int, int>>;

        ConstCrossIterator(It i) : _i(i) { }

        ConstCrossIterator(It i, size_t s, int x, int y) : _i(i)
        {
            std::vector<int> v(2 * s + 1);
            std::iota(v.begin(), v.end(), -s);

            for (auto i : v)
            {
                std::vector<int> t {0, i};
                std::sort(t.begin(), t.end());
                do {
                    _range.push_back(std::make_tuple(x - t.front(), y - t.back()));
                }
                while(std::next_permutation(t.begin(), t.end()));
            }
        }

        void increment()
        {
            _range.pop_back();
        }

        __T const& dereference() const
        {
            auto [x, y] = _range.back();
            return _i[x][y];
        }

        bool equal(ConstCrossIterator) const
        {
            return _range.empty();
        }

    private:
        It _i;
        Range _range;
    };
} // namespace Stg

#endif //CONST_CROSS_ITERATOR_HPP

max_stars_repo_name: iccet/stego

max_issues_repo_name: iccet/stego

max_forks_repo_name: iccet/stego


text:
/***************************************************************************
 * \file DataPort.cpp
 * \brief Defines the 10GigE data port interface for an NDR651.
 * \author DA
 * \author NH
 * \author MN
 * \copyright (c) 2017 CyberRadio Solutions, Inc.  All rights reserved.
 *
 ***************************************************************************/

#include "LibCyberRadio/Driver/NDR651/DataPort.h"
#include <boost/format.hpp>

namespace LibCyberRadio
{

    namespace Driver
    {

        namespace NDR651
        {

            DataPort::DataPort(int index,
                    ::LibCyberRadio::Driver::RadioHandler* parent,
                     bool debug,
                     const std::string& sourceIP) :
                ::LibCyberRadio::Driver::DataPort(/* const std::string& name */ (boost::format("NDR651-DP%02d") % \
                        index).str(),
                        /* int index */ index,
                        /* RadioHandler* parent */ parent,
                        /* bool debug */ debug,
                        /* const std::string& sourceIP */ sourceIP,
                        /* int numDataPortDipEntries */ 32,
                        /* int dataPortDipEntryIndexBase */ 0)
            {
                initConfigurationDict();
            }

            DataPort::~DataPort()
            {
            }

            DataPort::DataPort(const DataPort& other) :
                ::LibCyberRadio::Driver::DataPort(other)
            {
            }

            DataPort& DataPort::operator=(const DataPort& other)
            {
                ::LibCyberRadio::Driver::DataPort::operator=(other);
                if ( this != &other )
                {
                }
                return *this;
            }

        } // namespace NDR651

    } /* namespace Driver */

} /* namespace LibCyberRadio */

max_stars_repo_name: CyberRadio/libcyberradio

max_issues_repo_name: CyberRadio/libcyberradio

max_forks_repo_name: CyberRadio/libcyberradio


text:
#include <Eigen/Dense>
#include <Eigen/src/Core/Product.h>

#include <iostream>
#include <fstream>
#include <vector>

/*
TODO
    Improve learning rate decay
    Normal Distribution for weight initialization
    Add Data Augmentation
    Figure out why Loss can get stuck at 0.2 > Seems to be when guesses all go to 0.1
    Find out why this is slower than Python
    Add Convolution
    Batch Normalization
    Adam
    Measure times of different parts of the program
    Get some of these chunks in to functions, such as Forward Pass
    Make np_dot and relu_grad handle bad sized matrices without hard fail
    Beat 95%

DONE
    Load MNIST Data
    Initialize Weights
    Forward Pass
    Backward Pass
    Calculate Loss
    Calculate Accuracy
    Xavier Initialization
    Get convergence on small slice of dataset
    Beat 90% on validation data
    Load data with functions
    Compare speed of float vs double (With Eigen the doubles seem slower than floats)
    Beat 92% on validation data (94.5%+)
    Xavier Initialization Function
*/

int reverseInt(int i) {
    // https://stackoverflow.com/questions/8286668/how-to-read-mnist-data-in-c
    unsigned char c1, c2, c3, c4;
    c1 = i & 255;
    c2 = (i >> 8) & 255;
    c3 = (i >> 16) & 255;
    c4 = (i >> 24) & 255;
    return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
}

Eigen::MatrixXf np_dot(Eigen::MatrixXf error, Eigen::MatrixXf weights) {
    // Basically a Numpy Dot Product. Eigen doesn't seem to do this but who knows maybe they do.
    Eigen::MatrixXf output(1, weights.cols());
    for (int i = 0; i < weights.cols(); i++) {
        float thesum = 0;
        for (int j = 0; j < weights.rows(); j++) {
            thesum += error(j) * weights(j, i);
        }
        output(i) = thesum;
    }
    return output;
}

Eigen::MatrixXf relu_grad(Eigen::MatrixXf error, Eigen::MatrixXf res_rel) {
    // error = np.dot(out.T, error) * (res_rel1 > 0)     This is the (res_rel > 0) portion
    Eigen::MatrixXf output(1, res_rel.rows());
    for (int i = 0; i < res_rel.rows(); i++) {
        if (res_rel(i) > 0) {
            output(i) = error(i);
        }
        else {
            output(i) = 0;
        }
    }
    return output;
}

std::vector<Eigen::MatrixXi> load_y_data(const char *gotstring, int runs) {
    /*
    ######################################################
                       LOAD Y DATA
    ######################################################
    */
    std::vector<Eigen::MatrixXi> Y;
    std::ifstream filey(gotstring, std::ios::binary);
    if (filey.is_open())
    {
        int magic_number = 0;
        int number_of_images = 0;
        int n_rows = 0;
        int n_cols = 0;
        filey.read((char*)&magic_number, sizeof(magic_number));
        magic_number = reverseInt(magic_number);
        filey.read((char*)&number_of_images, sizeof(number_of_images));
        number_of_images = reverseInt(number_of_images);
        std::cout << number_of_images << std::endl;
        int label_int;
        for (int i = 0; i < runs; i++)
        {
            Eigen::MatrixXi label;
            label = Eigen::MatrixXi::Zero(1, 10);
            unsigned char temp = 0;
            filey.read((char*)&temp, sizeof(temp));
            label_int = temp;
            label(0, label_int) = 1;
            Y.push_back(label);
        }
        filey.close();
        return Y;
    }
    else {
        std::cout << "FAILED TO LOAD THE FILE";
        Y.push_back(Eigen::MatrixXi::Zero(1, 10));
        return Y; // Send zeros if load fails
    }
}

std::vector<Eigen::MatrixXf> load_x_data(const char* gotstring, int runs) {
    /*
    ######################################################
                       LOAD X DATA
    ######################################################
    */
    std::vector<Eigen::MatrixXf> X;
    // LOAD THE MNIST DATA http://yann.lecun.com/exdb/mnist/
    // MNIST Data https://stackoverflow.com/questions/23253485/little-endian-reading-mnist-file-numbers-out-of-range
    // MNIST Data https://stackoverflow.com/questions/8286668/how-to-read-mnist-data-in-c
    // MNIST Data https://stackoverflow.com/questions/12993941/how-can-i-read-the-mnist-dataset-with-c
    // MNIST Data https://stackoverflow.com/questions/16871512/how-to-read-pixels-from-mnist-digit-database-and-create-the-iplimage?noredirect=1&lq=1
    // 3D Eigen https://stackoverflow.com/questions/17098218/most-efficient-option-for-build-3d-structures-using-eigen-matrices
    std::ifstream filex(gotstring, std::ios::binary);
    if (filex.is_open())
    {
        int magic_number = 0;
        int number_of_images = 0;
        int n_rows = 0;
        int n_cols = 0;
        filex.read((char*)&magic_number, sizeof(magic_number));
        magic_number = reverseInt(magic_number);
        filex.read((char*)&number_of_images, sizeof(number_of_images));
        number_of_images = reverseInt(number_of_images);
        std::cout << number_of_images << std::endl;
        filex.read((char*)&n_rows, sizeof(n_rows));
        n_rows = reverseInt(n_rows);
        std::cout << "n_rows = " << n_rows << std::endl;
        filex.read((char*)&n_cols, sizeof(n_cols));
        n_cols = reverseInt(n_cols);
        std::cout << "n_cols = " << n_cols << std::endl;
        for (int i = 0; i < runs; i++)
        {
            Eigen::MatrixXf thisImage(28, 28);
            Eigen::MatrixXf thisImageFlat(1, 784);
            for (int r = 0; r < n_rows; ++r)
            {
                for (int c = 0; c < n_cols; ++c)
                {
                    unsigned char temp = 0;
                    filex.read((char*)&temp, sizeof(temp));
                    int mnist_int = temp;
                    float mnist_float = mnist_int / 255.0;
                    thisImage(r, c) = mnist_float;
                }
            }
            thisImage.resize(1, 784); // Flatten the image
            X.push_back(thisImage);
        }
        filex.close();
        return X;
    }
    else {
        std::cout << "FAILED TO LOAD THE FILE";
        X.push_back(Eigen::MatrixXf::Zero(1, 784));
        return X; // Send zeros if load fails
    }
}

Eigen::MatrixXf box_muller(Eigen::MatrixXf mat) {
    /*
    From Uniform Distribution to a sort of triangle distribution
    https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
    */
    Eigen::MatrixXf second(mat.rows(), mat.cols()); // Box-Muller requires two sets of random data
    second = Eigen::MatrixXf::Random(mat.rows(), mat.cols());
    second = ((second.array() + 1) / (float)2);
    mat = sqrt(-2 * log(mat.array())) * cos(2 * 3.14 * second.array()); // Box-Muller
    for (int r = 0; r < mat.rows(); r++) { // Sometimes I get a NAN probably getting negative square roots due to Eigen's Random algo
        for (int c = 0; c < mat.cols(); c++) {
            if (mat(r, c) == mat(r, c)) {
            }
            else {
                mat(r, c) = 0.500;
            }
        }
    }
    return mat;
}

Eigen::MatrixXf xavier_initialization(Eigen::MatrixXf mat) {
    /*
    Xavier Initialization to keep weights sane during gradient descent
    */
    mat = mat.array() * sqrt(1 / (float)(mat.rows() + mat.cols()));
    return mat;
}

int main()
{
    // Init Training
    int div_by = 1; // Use this to run on smaller number of MNIST images
    int epochs = 2;
    float lr = 0.25;
    int runs = int(60000 / div_by);

    // Load Y Data
    std::vector<Eigen::MatrixXi> Y_train;
    Y_train = load_y_data("train-labels.idx1-ubyte", runs);
    if (Y_train[0].sum() == 0) {
        return 0; // Fail Elegantly
    }

    // Load X Data
    std::vector<Eigen::MatrixXf> X_train;
    X_train = load_x_data("train-images.idx3-ubyte", runs);
    if (X_train[0].sum() == 0) {
        return 0; // Fail Elegantly
    }

    // Layers
    Eigen::MatrixXf w0;
    Eigen::MatrixXf w1;
    Eigen::MatrixXf out;
    w0 = Eigen::MatrixXf::Random(64, 784); // Eigen only does Uniform -1 to 1
    w1 = Eigen::MatrixXf::Random(32, 64);
    out = Eigen::MatrixXf::Random(10, 32);

    w0 = ((w0.array() + 1) / (float)2); // Change to Uniform 0 to 1
    w1 = ((w1.array() + 1) / (float)2);
    out = ((out.array() + 1) / (float)2);

    w0 = box_muller(w0).array(); // Box-Muller
    w1 = box_muller(w1).array();
    out = box_muller(out).array();

    w0 = xavier_initialization(w0); //  Xavier Initialization
    w1 = xavier_initialization(w1);
    out = xavier_initialization(out);

    // Dx Initialization
    Eigen::MatrixXf old_dx_out;
    Eigen::MatrixXf old_dx_w1;
    Eigen::MatrixXf old_dx_w0;
    old_dx_out = Eigen::MatrixXf::Zero(10, 32);
    old_dx_w1 = Eigen::MatrixXf::Zero(32, 64);
    old_dx_w0 = Eigen::MatrixXf::Zero(64, 784);
    Eigen::MatrixXf vold_dx_out;
    Eigen::MatrixXf vold_dx_w1;
    Eigen::MatrixXf vold_dx_w0;
    vold_dx_out = Eigen::MatrixXf::Zero(10, 32);
    vold_dx_w1 = Eigen::MatrixXf::Zero(32, 64);
    vold_dx_w0 = Eigen::MatrixXf::Zero(64, 784);

    // Result Matrix Initialization
    Eigen::MatrixXf res_w0(64, 1);
    Eigen::MatrixXf res_rel0(64, 1);
    Eigen::MatrixXf res_w1(32, 1);
    Eigen::MatrixXf res_rel1(32, 1);
    Eigen::MatrixXf res_out(10, 1);
    Eigen::MatrixXf guess(10, 1);
    Eigen::MatrixXf error(10, 1);
    Eigen::MatrixXf softMaxValue(10, 1);

    /*
    ######################################################
                       Main Loop
    ######################################################
    */
    std::vector<int> corrects;
    std::vector<float> losses;
    for (int epoch = 0; epoch < epochs; epoch++) {
        float correct_percent;
        float loss;
        float old_loss = 0.0;
        int corrects_sum = 0;
        float lr_mod = 1.0;
        float this_lr = 0.0;
        for (int i = 0; i < runs; i++) {
            // Data Matrices
            Eigen::MatrixXf x(1, 784);
            x = X_train[i];
            Eigen::MatrixXi y(1, 10);
            y = Y_train[i];

            /*
            ######################################################
                              Forward Pass
            ######################################################
            */
            res_w0 << w0 * x.transpose();
            res_rel0 << res_w0.cwiseMax(0); // ReLU
            res_w1 << w1 * res_rel0;
            res_rel1 << res_w1.cwiseMax(0);
            res_out << out * res_rel1;
            softMaxValue = res_out.array() - res_out.maxCoeff();
            guess << exp(softMaxValue.array()) / (exp(softMaxValue.array())).sum(); // guess = np.exp(res_out - res_out.max()) / np.sum(np.exp(res_out - res_out.max()), axis=0

            error << guess(0) - y(0), guess(1) - y(1), guess(2) - y(2), guess(3) - y(3), guess(4) - y(4), guess(5) - y(5), guess(6) - y(6), guess(7) - y(7), guess(8) - y(8), guess(9) - y(9);

            // Loss and Accuracy
            loss = 0;
            for (int k = 0; k < 10; k++) {
                loss += abs(error(k));
            }
            loss = loss / (float)10.0;
            float guess_max = 0;
            int guess_idx = 0;
            for (int k = 0; k < 10; k++) {
                if (guess(k) > guess_max) {
                    guess_max = guess(k);
                    guess_idx = k;
                }
            }
            if (y(guess_idx) == 1) {
                corrects.push_back(1);
                corrects_sum += 1;
            }
            else {
                corrects.push_back(0);
            }

            /*
            ######################################################
                               Backward Pass
            ######################################################
            */
            Eigen::MatrixXf dd(10, 1);
            Eigen::MatrixXf dx_out(10, 32);
            Eigen::MatrixXf error_new(32,1);
            Eigen::MatrixXf dx_w1(32, 64);
            Eigen::MatrixXf error_final(64, 1);
            Eigen::MatrixXf dx_w0(64, 784);

            dd << guess.array() * (1 - guess.array()); // (10, 1)
            error << error(0) * dd(0), error(1) * dd(1), error(2) * dd(2), error(3) * dd(3), error(4) * dd(4), error(5) * dd(5), error(6) * dd(6), error(7) * dd(7), error(8) * dd(8), error(9) * dd(9); // (10, 1)    This isn't what I had in Python
            dx_out << error * res_rel1.transpose(); // (10, 32)
            error_new = relu_grad( np_dot(error.transpose(), out), res_rel1); // (1, 32)
            dx_w1 = error_new.transpose() * res_rel0.transpose(); // (32, 64)
            error_final = relu_grad(np_dot(error_new, w1), res_rel0); // (64, 1)
            dx_w0 = error_final.transpose() * x; // (64, 784)

            // Adjust Weights
            if (guess_max < 0.2) { // Sometimes it will guess all 0.1 so maybe this will nudge it away from doing that
                lr_mod = 1.5;
            }
            this_lr = lr * lr_mod;
            out = out.array() - this_lr * dx_out.array() - 0.25 * this_lr * old_dx_out.array() - 0.1 * this_lr * vold_dx_out.array();
            w0 = w0.array() - this_lr * dx_w0.array() - 0.25 * this_lr * old_dx_w0.array() - 0.1 * this_lr * vold_dx_w0.array();
            w1 = w1.array() - this_lr * dx_w1.array() - 0.25 * this_lr * old_dx_w1.array() - 0.1 * this_lr * vold_dx_w1.array();
            lr_mod = 1.0;

            // Adjust Learning Rate Over Time
            if (i > 0) {
                correct_percent = corrects_sum / (float)i;
                if (i % 1000 == 0) {
                    std::cout << std::endl << "Correct Percent = " << correct_percent << " loss = " << loss << " Epoch = " << epoch << " img_index = " << i;
                }
                if (i > 1000 && correct_percent > 0.4 && correct_percent <= 0.50) {
                    lr = 0.15;
                }
                else if (i > 2000 && correct_percent > 0.50 && correct_percent <= 0.60) {
                    lr = 0.15;
                }
                else if (i > 2000 && correct_percent > 0.60 && correct_percent <= 0.74) {
                    lr = 0.075;
                }
                else if (i > 2000 && correct_percent > 0.74 && correct_percent <= 0.87) {
                    lr = 0.05;
                }
                else if (i > 2000 && correct_percent > 0.87 && correct_percent <= 0.95) {
                    lr = 0.0025;
                }
            }

            // Momentum Updates
            vold_dx_out = old_dx_out;
            vold_dx_w1 = old_dx_w1;
            vold_dx_w0 = old_dx_w0;
            old_dx_out = dx_out;
            old_dx_w1 = dx_w1;
            old_dx_w0 = dx_w0;
            old_loss = loss;
        }
        std::cout << std::endl << "==================================";
        std::cout << std::endl << "Correct Percent = " << correct_percent;
        std::cout << std::endl << "==================================";
    }

    /*
    =====================================================================================================================================================
    End of Training
    ------------------------------
    Begin Validation
    =====================================================================================================================================================
    */

    // Init Validation
    div_by = 1;
    runs = int(10000 / div_by);

    // Load Y Data
    std::vector<Eigen::MatrixXi> Y_test;
    Y_test = load_y_data("t10k-labels.idx1-ubyte", runs);
    if (Y_test[0].sum() == 0) {
        return 0; // Fail Elegantly
    }

    // Load X Data
    std::vector<Eigen::MatrixXf> X_test;
    X_test = load_x_data("t10k-images.idx3-ubyte", runs);
    if (X_test[0].sum() == 0) {
        return 0; // Fail Elegantly
    }

    float loss = 0.0;
    int corrects_sum = 0;
    for (int i = 0; i < runs; i++) {
        // Data Matrices
        Eigen::MatrixXf x(1, 784);
        x = X_test[i];
        Eigen::MatrixXi y(1, 10);
        y = Y_test[i];

        /*
        ######################################################
                          Forward Pass
        ######################################################
        */
        res_w0 << w0 * x.transpose();
        res_rel0 << res_w0.cwiseMax(0); // ReLU
        res_w1 << w1 * res_rel0;
        res_rel1 << res_w1.cwiseMax(0);
        res_out << out * res_rel1;
        softMaxValue = res_out.array() - res_out.maxCoeff();
        guess << exp(softMaxValue.array()) / (exp(softMaxValue.array())).sum(); // guess = np.exp(res_out - res_out.max()) / np.sum(np.exp(res_out - res_out.max()), axis=0)
        error << guess(0) - y(0), guess(1) - y(1), guess(2) - y(2), guess(3) - y(3), guess(4) - y(4), guess(5) - y(5), guess(6) - y(6), guess(7) - y(7), guess(8) - y(8), guess(9) - y(9);

        // Loss and Accuracy
        loss = 0;
        for (int k = 0; k < 10; k++) {
            loss += abs(error(k));
        }
        loss = loss / (float)10.0;
        float guess_max = 0;
        int guess_idx = 0;
        for (int k = 0; k < 10; k++) {
            if (guess(k) > guess_max) {
                guess_max = guess(k);
                guess_idx = k;
            }
        }
        if (y(guess_idx) == 1) {
            corrects.push_back(1);
            corrects_sum += 1;
        }
        else {
            corrects.push_back(0);
        }
    }
    float correct_percent = corrects_sum / (float)runs;
    std::cout << std::endl;
    std::cout << std::endl << "####################################################################";
    std::cout << std::endl << "               " << correct_percent;
    std::cout << std::endl << "####################################################################";
    std::cout << std::endl;
    return 0;
}

max_stars_repo_name: Kinvert/resume

max_issues_repo_name: Kinvert/resume

max_forks_repo_name: Kinvert/resume


text:
#include <voxelized_geometry_tools/ros_interface.hpp>

#include <vector>

#include <Eigen/Geometry>
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>

#include <common_robotics_utilities/color_builder.hpp>
#include <common_robotics_utilities/conversions.hpp>
#include <common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
#include <common_robotics_utilities/utility.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <common_robotics_utilities/zlib_helpers.hpp>
#include <voxelized_geometry_tools/collision_map.hpp>
#include <voxelized_geometry_tools/CollisionMapMessage.h>
#include <voxelized_geometry_tools/tagged_object_collision_map.hpp>
#include <voxelized_geometry_tools/TaggedObjectCollisionMapMessage.h>

namespace voxelized_geometry_tools
{
namespace ros_interface
{
visualization_msgs::Marker ExportForDisplay(
    const CollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const auto color_fn
      = [&] (const CollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    if (cell.Occupancy() > 0.5)
    {
      return collision_color;
    }
    else if (cell.Occupancy() < 0.5)
    {
      return free_color;
    }
    else
    {
      return unknown_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<CollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "collision_map";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::MarkerArray ExportForSeparateDisplay(
    const CollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  visualization_msgs::Marker collision_only_marker
      = ExportForDisplay(collision_map, collision_color, no_color, no_color);
  collision_only_marker.ns = "collision_only";
  visualization_msgs::Marker free_only_marker
      = ExportForDisplay(collision_map, no_color, free_color, no_color);
  free_only_marker.ns = "free_only";
  visualization_msgs::Marker unknown_only_marker
      = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
  unknown_only_marker.ns = "unknown_only";
  visualization_msgs::MarkerArray display_messages;
  display_messages.markers = {collision_only_marker,
                              free_only_marker,
                              unknown_only_marker};
  return display_messages;
}

visualization_msgs::Marker ExportSurfacesForDisplay(
    const CollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  const auto color_fn
      = [&] (const CollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex& index)
  {
    if (collision_map.IsSurfaceIndex(index))
    {
      if (cell.Occupancy() > 0.5)
      {
        return collision_color;
      }
      else if (cell.Occupancy() < 0.5)
      {
        return free_color;
      }
      else
      {
        return unknown_color;
      }
    }
    else
    {
      return no_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<CollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "collision_surfaces";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::MarkerArray ExportSurfacesForSeparateDisplay(
    const CollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  visualization_msgs::Marker collision_only_marker
      = ExportSurfacesForDisplay(
          collision_map, collision_color, no_color, no_color);
  collision_only_marker.ns = "collision_surfaces_only";
  visualization_msgs::Marker free_only_marker
      = ExportSurfacesForDisplay(
          collision_map, no_color, free_color, no_color);
  free_only_marker.ns = "free_surfaces_only";
  visualization_msgs::Marker unknown_only_marker
      = ExportSurfacesForDisplay(
          collision_map, no_color, no_color, unknown_color);
  unknown_only_marker.ns = "unknown_surfaces_only";
  visualization_msgs::MarkerArray display_messages;
  display_messages.markers = {collision_only_marker,
                              free_only_marker,
                              unknown_only_marker};
  return display_messages;
}

visualization_msgs::Marker ExportConnectedComponentsForDisplay(
    const CollisionMap& collision_map,
    const bool color_unknown_components)
{
  const std_msgs::ColorRGBA unknown_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.5, 0.5, 0.5, 1.0);
  const auto color_fn
      = [&] (const CollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    if (cell.Occupancy() != 0.5)
    {
      return LookupComponentColor(cell.Component());
    }
    else
    {
      if (color_unknown_components)
      {
        return LookupComponentColor(cell.Component());
      }
      else
      {
        return unknown_color;
      }
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<CollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "connected_components";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportIndexMapForDisplay(
    const CollisionMap& collision_map,
    const std::unordered_map<
        common_robotics_utilities::voxel_grid::GridIndex, uint8_t>& index_map,
    const std_msgs::ColorRGBA& surface_color)
{
  const auto color_fn
      = [&] (const CollisionCell&,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    return surface_color;
  };
  auto display_rep = ExportVoxelGridIndexMapToRViz<CollisionCell>(
      collision_map, index_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "collision_map_surface";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportIndicesForDisplay(
    const CollisionMap& collision_map,
    const std::vector<common_robotics_utilities::voxel_grid::GridIndex>&
        indices,
    const std_msgs::ColorRGBA& surface_color)
{
  const auto color_fn
      = [&] (const CollisionCell&,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    return surface_color;
  };
  auto display_rep = ExportVoxelGridIndicesToRViz<CollisionCell>(
      collision_map, indices, collision_map.GetFrame(), color_fn);
  display_rep.ns = "collision_map_surface";
  display_rep.id = 1;
  return display_rep;
}

CollisionMapMessage GetMessageRepresentation(const CollisionMap& map)
{
  CollisionMapMessage map_message;
  map_message.header.stamp = ros::Time::now();
  map_message.header.frame_id = map.GetFrame();
  std::vector<uint8_t> buffer;
  CollisionMap::Serialize(map, buffer);
  map_message.serialized_map
      = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
  map_message.is_compressed = true;
  return map_message;
}

CollisionMap LoadFromMessageRepresentation(const CollisionMapMessage& message)
{
  if (message.is_compressed)
  {
    const std::vector<uint8_t> decompressed_map
        = common_robotics_utilities::zlib_helpers::DecompressBytes(
            message.serialized_map);
    return CollisionMap::Deserialize(decompressed_map, 0).Value();
  }
  else
  {
    return CollisionMap::Deserialize(message.serialized_map, 0).Value();
  }
}

visualization_msgs::MarkerArray ExportForDisplay(
    const DynamicSpatialHashedCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const auto color_fn
      = [&] (const CollisionCell& cell,
             const Eigen::Vector4d&)
  {
    if (cell.Occupancy() > 0.5)
    {
      return collision_color;
    }
    else if (cell.Occupancy() < 0.5)
    {
      return free_color;
    }
    else
    {
      return unknown_color;
    }
  };
  auto display_rep
      = ExportDynamicSpatialHashedVoxelGridToRViz<CollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn, color_fn);
  display_rep.first.ns = "dsh_collision_map_chunks";
  display_rep.first.id = 1;
  display_rep.second.ns = "dsh_collision_map_cells";
  display_rep.second.id = 1;
  visualization_msgs::MarkerArray display_markers;
  display_markers.markers = {display_rep.first, display_rep.second};
  return display_markers;
}

visualization_msgs::MarkerArray ExportForSeparateDisplay(
    const DynamicSpatialHashedCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  visualization_msgs::MarkerArray collision_only_markers
      = ExportForDisplay(collision_map, collision_color, no_color, no_color);
  for (auto& collision_only_marker : collision_only_markers.markers)
  {
   collision_only_marker.ns += "_collision_only";
  }
  visualization_msgs::MarkerArray free_only_markers
      = ExportForDisplay(collision_map, no_color, free_color, no_color);
  for (auto& free_only_marker : free_only_markers.markers)
  {
    free_only_marker.ns += "_free_only";
  }
  visualization_msgs::MarkerArray unknown_only_markers
      = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
  for (auto& unknown_only_marker : unknown_only_markers.markers)
  {
    unknown_only_marker.ns += "_unknown_only";
  }
  visualization_msgs::MarkerArray display_messages;
  display_messages.markers.insert(display_messages.markers.end(),
                                  collision_only_markers.markers.begin(),
                                  collision_only_markers.markers.end());
  display_messages.markers.insert(display_messages.markers.end(),
                                  free_only_markers.markers.begin(),
                                  free_only_markers.markers.end());
  display_messages.markers.insert(display_messages.markers.end(),
                                  unknown_only_markers.markers.begin(),
                                  unknown_only_markers.markers.end());
  return display_messages;
}

DynamicSpatialHashedCollisionMapMessage GetMessageRepresentation(
    const DynamicSpatialHashedCollisionMap& map)
{
  DynamicSpatialHashedCollisionMapMessage map_message;
  map_message.header.stamp = ros::Time::now();
  map_message.header.frame_id = map.GetFrame();
  std::vector<uint8_t> buffer;
  DynamicSpatialHashedCollisionMap::Serialize(map, buffer);
  map_message.serialized_map
      = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
  map_message.is_compressed = true;
  return map_message;
}

DynamicSpatialHashedCollisionMap LoadFromMessageRepresentation(
    const DynamicSpatialHashedCollisionMapMessage& message)
{
  if (message.is_compressed)
  {
    const std::vector<uint8_t> decompressed_map
        = common_robotics_utilities::zlib_helpers::DecompressBytes(
            message.serialized_map);
    return DynamicSpatialHashedCollisionMap::Deserialize(
        decompressed_map, 0).Value();
  }
  else
  {
    return DynamicSpatialHashedCollisionMap::Deserialize(
        message.serialized_map, 0).Value();
  }
}

visualization_msgs::Marker ExportForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    if (cell.Occupancy() > 0.5)
    {
      return collision_color;
    }
    else if (cell.Occupancy() < 0.5)
    {
      return free_color;
    }
    else
    {
      return unknown_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_map";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  const bool specifies_colors = object_color_map.empty();
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    const uint32_t object_id = cell.ObjectId();
    if (specifies_colors)
    {
      const auto found_itr = object_color_map.find(object_id);
      if (found_itr != object_color_map.end())
      {
        return found_itr->second;
      }
      else
      {
        return no_color;
      }
    }
    else
    {
      return LookupComponentColor(object_id, 1.0);
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_map";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::MarkerArray ExportForSeparateDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  visualization_msgs::Marker collision_only_marker
      = ExportForDisplay(collision_map, collision_color, no_color, no_color);
  collision_only_marker.ns = "tagged_object_collision_only";
  visualization_msgs::Marker free_only_marker
      = ExportForDisplay(collision_map, no_color, free_color, no_color);
  free_only_marker.ns = "tagged_object_free_only";
  visualization_msgs::Marker unknown_only_marker
      = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
  unknown_only_marker.ns = "tagged_object_unknown_only";
  visualization_msgs::MarkerArray display_messages;
  display_messages.markers = {collision_only_marker,
                              free_only_marker,
                              unknown_only_marker};
  return display_messages;
}

visualization_msgs::Marker ExportSurfacesForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex& index)
  {
    if (collision_map.IsSurfaceIndex(index))
    {
      if (cell.Occupancy() > 0.5)
      {
        return collision_color;
      }
      else if (cell.Occupancy() < 0.5)
      {
        return free_color;
      }
      else
      {
        return unknown_color;
      }
    }
    else
    {
      return no_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_surfaces";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportSurfacesForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  const bool specifies_colors = object_color_map.empty();
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex& index)
  {
    if (collision_map.IsSurfaceIndex(index))
    {
      const uint32_t object_id = cell.ObjectId();
      if (specifies_colors)
      {
        const auto found_itr = object_color_map.find(object_id);
        if (found_itr != object_color_map.end())
        {
          return found_itr->second;
        }
        else
        {
          return no_color;
        }
      }
      else
      {
        return LookupComponentColor(object_id, 1.0);
      }
    }
    else
    {
      return no_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_surfaces";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::MarkerArray ExportSurfacesForSeparateDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std_msgs::ColorRGBA& collision_color,
    const std_msgs::ColorRGBA& free_color,
    const std_msgs::ColorRGBA& unknown_color)
{
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  visualization_msgs::Marker collision_only_marker
      = ExportSurfacesForDisplay(
          collision_map, collision_color, no_color, no_color);
  collision_only_marker.ns = "tagged_object_collision_surfaces_only";
  visualization_msgs::Marker free_only_marker
      = ExportSurfacesForDisplay(
          collision_map, no_color, free_color, no_color);
  free_only_marker.ns = "tagged_object_free_surfaces_only";
  visualization_msgs::Marker unknown_only_marker
      = ExportSurfacesForDisplay(
          collision_map, no_color, no_color, unknown_color);
  unknown_only_marker.ns = "tagged_object_unknown_surfaces_only";
  visualization_msgs::MarkerArray display_messages;
  display_messages.markers = {collision_only_marker,
                              free_only_marker,
                              unknown_only_marker};
  return display_messages;
}

visualization_msgs::Marker ExportConnectedComponentsForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const bool color_unknown_components)
{
  const std_msgs::ColorRGBA unknown_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.5, 0.5, 0.5, 1.0);
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    if (cell.Occupancy() != 0.5)
    {
      return LookupComponentColor(cell.Component());
    }
    else
    {
      if (color_unknown_components)
      {
        return LookupComponentColor(cell.Component());
      }
      else
      {
        return unknown_color;
      }
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_connected_components";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportSpatialSegmentForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const uint32_t object_id, const uint32_t spatial_segment)
{
  const uint32_t number_of_segments
      = collision_map.GetNumSpatialSegments().Value();
  const std_msgs::ColorRGBA no_color
      = common_robotics_utilities::color_builder
          ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell& cell,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    if ((cell.ObjectId() == object_id)
        && (cell.SpatialSegment() == spatial_segment))
    {
      if (number_of_segments < 22)
      {
        return LookupComponentColor(cell.SpatialSegment());
      }
      else
      {
        return common_robotics_utilities::color_builder
            ::InterpolateHotToCold<std_msgs::ColorRGBA>(
                static_cast<double>(cell.SpatialSegment()),
                0.0,
                static_cast<double>(number_of_segments));
      }
    }
    else
    {
      return no_color;
    }
  };
  auto display_rep
      = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
          collision_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_"
                   + std::to_string(object_id)
                   + "_spatial_segment_"
                   + std::to_string(spatial_segment);
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportIndexMapForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std::unordered_map<
        common_robotics_utilities::voxel_grid::GridIndex, uint8_t>& index_map,
    const std_msgs::ColorRGBA& surface_color)
{
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell&,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    return surface_color;
  };
  auto display_rep = ExportVoxelGridIndexMapToRViz<TaggedObjectCollisionCell>(
      collision_map, index_map, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_map_surface";
  display_rep.id = 1;
  return display_rep;
}

visualization_msgs::Marker ExportIndicesForDisplay(
    const TaggedObjectCollisionMap& collision_map,
    const std::vector<common_robotics_utilities::voxel_grid::GridIndex>&
        indices,
    const std_msgs::ColorRGBA& surface_color)
{
  const auto color_fn
      = [&] (const TaggedObjectCollisionCell&,
             const common_robotics_utilities::voxel_grid::GridIndex&)
  {
    return surface_color;
  };
  auto display_rep = ExportVoxelGridIndicesToRViz<TaggedObjectCollisionCell>(
      collision_map, indices, collision_map.GetFrame(), color_fn);
  display_rep.ns = "tagged_object_collision_map_surface";
  display_rep.id = 1;
  return display_rep;
}

TaggedObjectCollisionMapMessage GetMessageRepresentation(
    const TaggedObjectCollisionMap& map)
{
  TaggedObjectCollisionMapMessage map_message;
  map_message.header.stamp = ros::Time::now();
  map_message.header.frame_id = map.GetFrame();
  std::vector<uint8_t> buffer;
  TaggedObjectCollisionMap::Serialize(map, buffer);
  map_message.serialized_map
      = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
  map_message.is_compressed = true;
  return map_message;
}

TaggedObjectCollisionMap LoadFromMessageRepresentation(
    const TaggedObjectCollisionMapMessage& message)
{
  if (message.is_compressed)
  {
    const std::vector<uint8_t> decompressed_map
        = common_robotics_utilities::zlib_helpers::DecompressBytes(
            message.serialized_map);
    return TaggedObjectCollisionMap::Deserialize(decompressed_map, 0).Value();
  }
  else
  {
    return TaggedObjectCollisionMap::Deserialize(
        message.serialized_map, 0).Value();
  }
}
}  // namespace ros_interface
}  // namespace voxelized_geometry_tools


text:
#include <iostream>
/// json parser
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
/// websocket client
#include <websocketpp/config/asio_no_tls_client.hpp>
#include <websocketpp/client.hpp>
/// Mat parser
#include <opencv2/opencv.hpp>
/// PointCloud parser
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

using wsclient = websocketpp::client<websocketpp::config::asio_client>;

std::vector<std::string> binary_message;  /// websocket binary message
boost::property_tree::ptree text_message; /// websocket text message in property tree

/// <summary>
/// parser websocket text message to property tree
/// </summary>
/// <param name="message"></param>
/// <param name="ptree"></param>
void read_json(wsclient::message_ptr message, boost::property_tree::ptree& ptree)
{
    ptree.clear();
    std::istringstream ss(message->get_payload());
    try
    {
        boost::property_tree::read_json(ss, ptree);
    }
    catch (const std::exception& ex)
    {
        std::cout << "read json catch exception: " << ex.what() << std::endl;
    }
}

/// <summary>
/// parser property tree to json string
/// </summary>
/// <param name="ptree"></param>
/// <param name="json"></param>
void write_json(const boost::property_tree::ptree& ptree, std::string& json)
{
    std::ostringstream ss;
    try
    {
        boost::property_tree::write_json(ss, ptree);
        json = ss.str();
        std::cout << "request: " << json << std::endl;
    }
    catch (const std::exception& ex)
    {
        std::cout << "write json catch exception: " << ex.what() << std::endl;
    }
}

/// <summary>
/// depth image to pcl::PointCloud
/// </summary>
/// <param name="color"></param>
/// <param name="depth"></param>
void write_pointcloud(const cv::Mat& color, const cv::Mat& depth)
{
    if (depth.size().area() == 0 || depth.size() != color.size()) {
        return;
    }
    if (color.type() != CV_8UC1 && color.type() != CV_8UC3) {
        return;
    }
    if (depth.type() != CV_32FC3) {
        return;
    }

    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    for (int i = 0; i < depth.rows; ++i)
    {
        for (int j = 0; j < depth.cols; ++j)
        {
            pcl::PointXYZRGB point;
            point.x = depth.at<cv::Vec3f>(i, j)[0];
            point.y = depth.at<cv::Vec3f>(i, j)[1];
            point.z = depth.at<cv::Vec3f>(i, j)[2];
            if (point.z < 0)
                continue;

            if (color.type() == CV_8UC3)
            {
                point.b = color.at<cv::Vec3b>(i, j)[0];
                point.g = color.at<cv::Vec3b>(i, j)[1];
                point.r = color.at<cv::Vec3b>(i, j)[2];
            }
            else
            {
                point.b = point.g = point.r = color.at<uint8_t>(i, j);
            }

            cloud.push_back(point);
        }
    }

    if (cloud.size() > 0)
    {
        pcl::PCDWriter writer;
        writer.writeBinaryCompressed("cloud.pcd", cloud);
    }
}

/// <summary>
/// handle websocket message
/// </summary>
/// <param name=""></param>
/// <param name="message"></param>
void on_message(websocketpp::connection_hdl, wsclient::message_ptr message) {
    if (message->get_opcode() == websocketpp::frame::opcode::text) /// text message
    {
        read_json(message, text_message);
        if (text_message.count("Action") == 0) /// invalid response
        {
            std::cout << "response: " << message->get_payload() << std::endl;
            return;
        }

        if (text_message.get<std::string>("Action") == "GetInfo")
        {
            std::cout << "response: " << message->get_payload() << std::endl;
            //boost::property_tree::write_json("info.json", text_message); /// save sensor info
            return;
        }

        if (text_message.get<std::string>("Action") == "LivePlayback")
        {
            int pos = text_message.get<int>("Position");
            std::string data = websocketpp::base64_decode(text_message.get<std::string>("Data"));
            std::vector<uint8_t> buffer(data.begin(), data.end());
            cv::Mat image = cv::imdecode(buffer, cv::IMREAD_COLOR);
            const char* name = nullptr;
            if (pos == 0)
            {
                name = "Live - Left";
            }
            else
            {
                name = "Live - Right";
            }
            cv::namedWindow(name, cv::WINDOW_NORMAL | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED);
            cv::imshow(name, image);
            cv::waitKey(1);
            return;
        }
        else
            std::cout << "response: " << message->get_payload() << std::endl;

        auto payloads = text_message.get_child_optional("Payloads");
        if (!payloads)
            return;

        /// has binary payloads
        int i = 0;
        cv::Mat color, depth;
        for (auto it = payloads.get().find(""); it != payloads.get().not_found(); ++it)
        {
            if (i >= binary_message.size())
                break;

            std::string payload = it->second.get<std::string>("Payload");
            std::string stype = it->second.get<std::string>("Type");
            std::cout << "parser payload[" << i << "]: " << payload << ", type: " << stype << std::endl;
            if (payload == "Mat") /// parser opencv mat
            {
                int cols = it->second.get<int>("Cols");
                int rows = it->second.get<int>("Rows");
                if (stype == "CV_8UC1")
                {
                    color = cv::Mat(rows, cols, CV_8UC1, const_cast<char*>(binary_message[i].data()));
                }
                else if(stype == "CV_8UC3")
                {
                    color = cv::Mat(rows, cols, CV_8UC3, const_cast<char*>(binary_message[i].data()));
                }
                else if (stype == "CV_32FC3") {
                    depth = cv::Mat(rows, cols, CV_32FC3, const_cast<char*>(binary_message[i].data()));
                }
                else {
                    continue;
                }
            }
            ++i;
        }

        write_pointcloud(color, depth);
    }
    else /// binary message
    {
        binary_message.emplace_back(message->get_payload());
    }
}

int main()
{
    const std::string uri = "ws://192.168.3.82:8080"; /// sensor websocket address

    wsclient client;
    client.set_access_channels(websocketpp::log::alevel::all);
    client.clear_access_channels(websocketpp::log::alevel::frame_payload);
    client.set_error_channels(websocketpp::log::elevel::all);

    client.init_asio();
    client.set_message_handler(&on_message);
    client.set_max_message_size(100000000); /// set max size to receive opencv mat and pcl pointcloud

    websocketpp::lib::error_code ec;
    wsclient::connection_ptr con = client.get_connection(uri, ec);
    if (ec) {
        std::cout << "could not create connection because: " << ec.message() << std::endl;
        return 0;
    }

    client.connect(con);
    std::cout << "connect to " << uri << " sucess";
    auto future = std::async([&]() {
        client.run();
        });

    boost::property_tree::ptree request;
    std::string json;

    int key = -1;
    bool run = true, live = false;
    do
    {
        std::cout << "Input options: ";
        std::cin >> key;
        std::cin.clear();
        request.clear();
        switch (key)
        {
        case 1:/// set sensor fov
            request.add<std::string>("Action", "SetFoV");
            request.add<int>("X", 0);
            request.add<int>("Y", 0);
            request.add<int>("Width", 0);
            request.add<int>("Height", 0);
            break;
        case 2:/// set sensor distance (mm)
            request.add<std::string>("Action", "SetDistance");
            request.add<int>("Min", 100);
            request.add<int>("Max", 1000);
            break;
        case 3:/// set pointcloud style
            request.add<std::string>("Action", "SetStyle");
            request.add<int>("PlainType", 3); // 3: Gray, 4: RGB
            request.add<int>("DisplayColor", 7); // 7: White, 4: Blue
            request.add<int>("DisplayBit", 8);
            break;
        case 4:/// set sensor decoder
            request.add<std::string>("Action", "SetDecodeType");
            request.add<int>("DecodeType", 5); //
            break;
        case 5:///set live exposure
            request.add<std::string>("Action", "SetExposure");
            request.add<uint32_t>("Live", 21000);
            break;
        case 6:/// set sensor exposure and period
            request.add<std::string>("Action", "SetExposure");
            request.add<uint32_t>("Exposure", 10000);
            request.add<uint32_t>("Period", 20000);
            request.add<uint32_t>("Live", 80000);
            break;
        case 7:/// set sensor led current
            request.add<std::string>("Action", "SetCurrent");
            request.add<uint8_t>("Red", 180);
            request.add<uint8_t>("Green", 180);
            request.add<uint8_t>("Blue", 180);
            break;
        case 8:/// start capture images
            binary_message.clear();
            request.add<std::string>("Action", "Capture");
            request.add<bool>("Image", false); /// enable image payload, default is false
            break;
        case 9:/// compute pointcloud and depth image
            binary_message.clear();
            request.add<std::string>("Action", "Compute");
            break;
        case 10:/// compute pointcloud and depth image
            live = !live;
            request.add<std::string>("Action", "SetPlayback");
            request.add<bool>("Playback", live);
            request.add<std::string>("Payload", ".png");
            request.add<int>("Width", 320);
            request.add<int>("Height", 240);
            break;
        case -1:
            run = false;
            //request.add<std::string>("Action", "RebootSensor");
            //break;
        default:/// get sensor info
            request.add<std::string>("Action", "GetInfo");
            break;
        }

        write_json(request, json);
        con->send(json);
    } while (run);

    client.close(con, websocketpp::close::status::normal, "Close");
    return 0;
}

max_stars_repo_name: MagicalBrain/My-note.

max_issues_repo_name: MagicalBrain/My-note.

max_forks_repo_name: MagicalBrain/My-note.


text: #include <boost/hana/fwd/at.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
// Boost.Signals library

// Copyright Douglas Gregor 2001-2004. 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)

// For more information, see http://www.boost.org

#ifndef BOOST_SIGNALS_SLOT_CALL_ITERATOR
#define BOOST_SIGNALS_SLOT_CALL_ITERATOR

#include <functional>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/smart_ptr.hpp>
#include <boost/signals/detail/config.hpp>
#include <boost/signals/connection.hpp>

#ifdef BOOST_HAS_ABI_HEADERS
#  include BOOST_ABI_PREFIX
#endif

namespace boost {
  namespace BOOST_SIGNALS_NAMESPACE {
    namespace detail {
      // A cached return value from a slot
      template<typename T>
      struct cached_return_value {
        cached_return_value(const T& t) : value(t) {}

        T value;
      };

      // Generates a slot call iterator. Essentially, this is an iterator that:
      //   - skips over disconnected slots in the underlying list
      //   - calls the connected slots when dereferenced
      //   - caches the result of calling the slots
      template<typename Function, typename Iterator>
      class slot_call_iterator
        : public iterator_facade<slot_call_iterator<Function, Iterator>,
                                 typename Function::result_type,
                                 single_pass_traversal_tag,
                                 typename Function::result_type const&>
      {
        typedef iterator_facade<slot_call_iterator<Function, Iterator>,
                                typename Function::result_type,
                                single_pass_traversal_tag,
                                typename Function::result_type const&>
          inherited;

        typedef typename Function::result_type result_type;

        friend class iterator_core_access;

      public:
        slot_call_iterator() {}

        slot_call_iterator(Iterator iter_in, Iterator end_in, Function f)
          : iter(iter_in), end(end_in), f(f), cache()
        {
          iter = std::find_if(iter, end, std::not1(is_disconnected()));
        }

        typename inherited::reference
        dereference() const
        {
          if (!cache.get()) {
            cache.reset(new cached_return_value<result_type>(f(*iter)));
          }

          return cache->value;
        }

        void increment()
        {
          iter = std::find_if(++iter, end, std::not1(is_disconnected()));
          cache.reset();
        }

        bool equal(const slot_call_iterator& other) const
        {
          iter = std::find_if(iter, end, std::not1(is_disconnected()));
          other.iter = std::find_if(other.iter, other.end,
                                    std::not1(is_disconnected()));
          return iter == other.iter;
        }

      private:
        mutable Iterator iter;
        Iterator end;
        Function f;
        mutable shared_ptr< cached_return_value<result_type> > cache;
      };
    } // end namespace detail
  } // end namespace BOOST_SIGNALS_NAMESPACE
} // end namespace boost

#ifdef BOOST_HAS_ABI_HEADERS
#  include BOOST_ABI_SUFFIX
#endif

#endif // BOOST_SIGNALS_SLOT_CALL_ITERATOR

max_stars_repo_name: Imperator-Knoedel/Sunset

max_issues_repo_name: Imperator-Knoedel/Sunset

max_forks_repo_name: Imperator-Knoedel/Sunset


text:
#define BOOST_TEST_MODULE yawielTest

#include <boost/test/unit_test.hpp>

max_stars_repo_name: yawiel/yawiel

max_issues_repo_name: yawiel/yawiel

max_forks_repo_name: yawiel/yawiel