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

#include <gtest/gtest.h>
#include <rapidjson/document.h>
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <boost/process.hpp>
#include <pqxx/pqxx>

#include "common/files.hpp"
#include "common/types.hpp"
#include "main/iroha_conf_loader.hpp"

// workaround for redefining -WERROR problem
#undef RAPIDJSON_HAS_STDSTRING

#include "framework/config_helper.hpp"
#include "model/converters/json_common.hpp"

using namespace boost::process;
using namespace boost::filesystem;
using namespace std::chrono_literals;
using iroha::operator|;

class IrohadTest : public testing::Test {
 public:
  void SetUp() override {
    timeout = 1s;
    setPaths();
    auto config = parse_iroha_config(path_config_.string());
    blockstore_path_ = config[config_members::BlockStorePath].GetString();
    pgopts_ = integration_framework::getPostgresCredsOrDefault(
        config[config_members::PgOpt].GetString());
    // we need a separate file here in case if target environment
    // has custom database connection options set
    // via environment variables
    auto config_copy_json = parse_iroha_config(path_config_.string());
    config_copy_json[config_members::PgOpt].SetString(pgopts_.data(),
                                                      pgopts_.size());
    auto config_copy_string =
        iroha::model::converters::jsonToString(config_copy_json);
    std::ofstream copy_file(config_copy_);
    copy_file.write(config_copy_string.data(), config_copy_string.size());
  }

  void TearDown() override {
    iroha::remove_dir_contents(blockstore_path_);
    dropPostgres();
    boost::filesystem::remove(config_copy_);
  }

  std::string params(const boost::optional<std::string> &config_path,
                     const boost::optional<std::string> &genesis_block,
                     const boost::optional<std::string> &keypair_path) {
    std::string res;
    config_path | [&res](auto &&s) { res += " --config " + s; };
    genesis_block | [&res](auto &&s) { res += " --genesis_block " + s; };
    keypair_path | [&res](auto &&s) { res += " --keypair_name " + s; };
    return res;
  }

  std::string setDefaultParams() {
    return params(config_copy_, path_genesis_.string(), path_keypair_.string());
  }

 private:
  void setPaths() {
    path_irohad_ = boost::filesystem::path(PATHIROHAD);
    irohad_executable = path_irohad_ / "irohad";
    path_example_ = path_irohad_.parent_path().parent_path() / "example";
    path_config_ = path_example_ / "config.sample";
    path_genesis_ = path_example_ / "genesis.block";
    path_keypair_ = path_example_ / "node0";
    config_copy_ = path_config_.string() + std::string(".copy");
  }

  void dropPostgres() {
    auto connection = std::make_shared<pqxx::lazyconnection>(pgopts_);
    try {
      connection->activate();
    } catch (const pqxx::broken_connection &e) {
      FAIL() << "Connection to PostgreSQL broken: " << e.what();
    }

    const auto drop = R"(
DROP TABLE IF EXISTS account_has_signatory;
DROP TABLE IF EXISTS account_has_asset;
DROP TABLE IF EXISTS role_has_permissions;
DROP TABLE IF EXISTS account_has_roles;
DROP TABLE IF EXISTS account_has_grantable_permissions;
DROP TABLE IF EXISTS account;
DROP TABLE IF EXISTS asset;
DROP TABLE IF EXISTS domain;
DROP TABLE IF EXISTS signatory;
DROP TABLE IF EXISTS peer;
DROP TABLE IF EXISTS role;
DROP TABLE IF EXISTS height_by_hash;
DROP TABLE IF EXISTS height_by_account_set;
DROP TABLE IF EXISTS index_by_creator_height;
DROP TABLE IF EXISTS index_by_id_height_asset;
)";

    pqxx::work txn(*connection);
    txn.exec(drop);
    txn.commit();
    connection->disconnect();
  }

 public:
  boost::filesystem::path irohad_executable;
  std::chrono::milliseconds timeout = 1s;

 private:
  boost::filesystem::path path_irohad_;
  boost::filesystem::path path_example_;
  boost::filesystem::path path_config_;
  boost::filesystem::path path_genesis_;
  boost::filesystem::path path_keypair_;
  std::string pgopts_;
  std::string blockstore_path_;
  std::string config_copy_;
};

/*
 * @given path to irohad executable and paths to files irohad is needed to be
 * run (config, genesis block, keypair)
 * @when run irohad with all parameters it needs to operate as a full node
 * @then irohad should be started and running until timeout expired
 */
TEST_F(IrohadTest, RunIrohad) {
  child c(irohad_executable.string() + setDefaultParams());
  std::this_thread::sleep_for(timeout);
  ASSERT_TRUE(c.running());
  c.terminate();
}

max_stars_repo_name: steephengeorge/iroha

max_issues_repo_name: steephengeorge/iroha

max_forks_repo_name: steephengeorge/iroha


text:
#include <boost/ut.hpp>
#include <cinttypes>
#include <libxbitset/bitset.hpp>

using namespace boost::ut;

struct Register
{
  volatile uint64_t CTRL1;
  volatile uint64_t CTRL2;
};

Register memory{};
Register* reg = &memory;

int main()
{
  static constexpr auto intensity =
    xstd::bitrange{ .position = 16, .width = 8 };
  static constexpr auto clock_divider = xstd::bitrange::from<42, 50>();
  xstd::bitmanip(reg->CTRL1)
    .insert<intensity>(7)
    .set(4)
    .reset(0)
    .insert<clock_divider>(3);

  expect(that % 0x0000'0C00'0007'0010 == reg->CTRL1);
}

ext: cpp


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

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_box_templated
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_BOX_TEMPLATED

#include <iostream>

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

template <typename P>
struct my_box
{
    P ll, ur;
};

// Register the box type
BOOST_GEOMETRY_REGISTER_BOX_TEMPLATED(my_box, ll, ur)

int main()
{
    typedef my_box<boost::geometry::model::d2::point_xy<double> > box;
    box b = boost::geometry::make<box>(0, 0, 2, 2);
    std::cout << "Area: "  << boost::geometry::area(b) << std::endl;
    return 0;
}

//]

//[register_box_templated_output
/*`
Output:
[pre
Area: 4
]
*/
//]

max_stars_repo_name: olegshnitko/libboost

max_issues_repo_name: olegshnitko/libboost

max_forks_repo_name: olegshnitko/libboost


text:
// http://www.cplusplus.com/forum/general/16532/

// GB boost header files compile error

#include <cstdio>
#include <string>
#include <iostream>
#include <boost/iostreams/device/file_descriptor.hpp>
#include <boost/iostreams/stream.hpp>

namespace io = boost::iostreams;

int main()
{
    FILE* pipe = popen("find . -type f", "r");

    io::stream_buffer<io::file_descriptor_source> fpstream(fileno(pipe));
    std::istream in(&fpstream);

    std::string line;
    while (in)
    {
        std::getline(in, line);
        std::cout << line << "\n";
    }
    return 0;
}

max_stars_repo_path: 2A. C++ Advanced (STL)/Attic/boost-popen.cpp


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

#ifndef FORMATTING_FORMAT_HPP
#define FORMATTING_FORMAT_HPP

// mapnik
#include <mapnik/text/formatting/base.hpp>
#include <mapnik/text/text_properties.hpp>

// boost
#include <boost/property_tree/ptree_fwd.hpp>

namespace mapnik {

class feature_impl;

namespace formatting {

class MAPNIK_DECL format_node: public node
{
public:
    void to_xml(boost::property_tree::ptree & xml) const;
    static node_ptr from_xml(xml_node const& xml, fontset_map const& fontsets);
    virtual void apply(evaluated_format_properties_ptr const& p, feature_impl const& feature, attributes const& vars, text_layout & output) const;
    virtual void add_expressions(expression_set & output) const;

    void set_child(node_ptr child);
    node_ptr get_child() const;

    boost::optional<std::string> face_name;
    boost::optional<font_set> fontset;
    boost::optional<symbolizer_base::value_type> text_size;
    boost::optional<symbolizer_base::value_type> character_spacing;
    boost::optional<symbolizer_base::value_type> line_spacing;
    boost::optional<symbolizer_base::value_type> text_opacity;
    boost::optional<symbolizer_base::value_type> wrap_before;
    boost::optional<symbolizer_base::value_type> repeat_wrap_char;
    boost::optional<symbolizer_base::value_type> text_transform;
    boost::optional<symbolizer_base::value_type> fill;
    boost::optional<symbolizer_base::value_type> halo_fill;
    boost::optional<symbolizer_base::value_type> halo_radius;
    boost::optional<symbolizer_base::value_type> ff_settings;

private:
    node_ptr child_;
};
} //ns formatting
} //ns mapnik

#endif // FORMATTING_FORMAT_HPP

ext: hpp


text:
#include <iostream>
#include <cmath>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/algorithm/string.hpp>

#include <cpprest/http_client.h>
#include <cpprest/filestream.h>
#include <cpprest/filestream.h>

using namespace web::http;
using namespace web::http::client;
using namespace Concurrency::streams;

#include "date.h"
#include "libs/json.hpp"
#include "http_requests.h"

using json = nlohmann::json;

int Date::GetCurrentWeek() {
    std::string service("Week/GetCurrentWeek/" +
                        std::to_string(73) + "/" + std::to_string(3));
    http_response req = HttpRequest::MakeRequest(service);
    if (req.status_code() != 200) {
        std::cout << "Error: cannot get current date for week calculation" << std::endl;
        exit(1);
    }
    json j = json::parse(req.extract_utf8string().get());
    if (!j["Id"].is_null()) {
        return j["Id"].get<int>();
    }
    std::cout << "Error: cannot get current date for week calculation" << std::endl;
    exit(1);
}

boost::posix_time::ptime Date::DateFromIsoString(std::string date) {
    //Adapt the string
    date.pop_back(); // remove the trailing Z
    boost::erase_all(date, "-");
    boost::erase_all(date, ":");

    return boost::posix_time::from_iso_string(date);
}

max_stars_repo_name: corenting/chronos-cli

max_issues_repo_name: corenting/chronos-cli

max_forks_repo_name: corenting/chronos-cli


text:
/*
 * Copyright (c) 2008, 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, Inc. 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 <wx/event.h>

#include <boost/bind.hpp>

#include <OGRE/OgreRoot.h>
#include <OGRE/OgreViewport.h>
#include <OGRE/OgreRenderWindow.h>

#include "IMultiVizInputEventsHandler.h"
#include "MultiVizKeyEvent.h"
#include "MultiVizMouseEvent.h"

#include "Panels/RenderPanel.h"

namespace rviz
{

    // Passes all default parameters to wxOgreRenderWindow except the size. It
    // seems like it's a problem with GTK, but when I create a RenderPanel for
    // a camera view, only a part of the image is painted at first. For example,
    // when there is no input, "No Image" is shown. However,
    // wxDefaultSize is 20 x 20 and only this part of the "No Image" is painted.
    // If I create the RenderPanel at the size of the parent, which is certainly
    // bigger than what it will actually be once layed out, then everything is
    // painted properly.
    RenderPanel::RenderPanel(wxWindow* parent, int panelNumber, Ogre::SceneManager* sceneMgr, eu::nifti::ocu::gui::IMultiVizInputEventsHandler* evtHandler)
    : wxOgreRenderWindow(Ogre::Root::getSingletonPtr(), parent, wxID_ANY, wxDefaultPosition, parent->GetSize(), wxSUNKEN_BORDER, wxDefaultValidator, true)
    , mouse_x_(0)
    , mouse_y_(0)
    , sceneMgr(sceneMgr)
    , camera(NULL)
    , panelNumber(panelNumber)
    , evtHandler(evtHandler)
    {
        assert(panelNumber >= 0 && panelNumber <= 3);
        assert(sceneMgr != NULL);
        assert(evtHandler != NULL);

        SetFocus();
        Connect(wxEVT_CHAR, wxKeyEventHandler(RenderPanel::onKeyEvent), NULL, this);

        Connect(wxEVT_LEFT_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_MIDDLE_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_RIGHT_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_MOTION, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_LEFT_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_MIDDLE_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_RIGHT_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_MOUSEWHEEL, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Connect(wxEVT_LEFT_DCLICK, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
    }

    RenderPanel::~RenderPanel()
    {
        // At the end of the application, the scene manager has been destroyed, so there is no point in destroying the camera
        if(Ogre::Root::getSingletonPtr()->getSceneManagerIterator().hasMoreElements())
        {
            assert(camera != NULL);
            sceneMgr->destroyCamera(camera);
        }

        Disconnect(wxEVT_CHAR, wxKeyEventHandler(RenderPanel::onKeyEvent), NULL, this);
        Disconnect(wxEVT_LEFT_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_MIDDLE_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_RIGHT_DOWN, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_MOTION, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_LEFT_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_MIDDLE_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_RIGHT_UP, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_MOUSEWHEEL, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
        Disconnect(wxEVT_LEFT_DCLICK, wxMouseEventHandler(RenderPanel::onMouseEvent), NULL, this);
    }

    void RenderPanel::initialize()
    {
        // Creates an Ogre camera with a unique name
        std::stringstream ss;
        static int count = 0;
        ss << "RenderPanelCamera" << count++;
        camera = sceneMgr->createCamera(ss.str());

        wxOgreRenderWindow::setCamera(camera);

        // I have to do this crazy construction because wxWidgets automatically deletes the drop target. Therefore, I need to make it a dummy object, and not the render panel directly.
        RenderPanelTextDropTarget *dropTarget = new RenderPanelTextDropTarget(this);
        SetDropTarget(dropTarget);
    }

    // Todo Benoit: Try using viewports instead of 4 render panels for quad display
    //    void RenderPanel::addCameraAndViewport(std::string name)
    //    {
    //
    //    }

    void RenderPanel::onMouseEvent(wxMouseEvent& evt)
    {
        eu::nifti::ocu::gui::MultiVizMouseEvent niftiEvt(evt, mouse_x_, mouse_y_, NULL);

        mouse_x_ = evt.GetX();
        mouse_y_ = evt.GetY();

        SetFocus();

        evtHandler->handleMouseEvent(niftiEvt, panelNumber);
    }

    void RenderPanel::onKeyEvent(wxKeyEvent& evt)
    {
        eu::nifti::ocu::gui::MultiVizKeyEvent niftiEvt(evt, NULL);
        evtHandler->handleKeyEvent(niftiEvt, panelNumber);
    }

    void RenderPanel::onSize(wxSizeEvent &evt)
    {
        ogre_tools::wxOgreRenderWindow::onSize(evt);

        evtHandler->handleSizeEvent(evt, panelNumber);
    }

    bool RenderPanel::OnDropText(wxCoord x, wxCoord y, const wxString& text)
    {
        evtHandler->handleDragAndDropEvent(text, panelNumber);

        return true;
    }

    Ogre::Camera* RenderPanel::getCamera() const
    {
        return camera;
    }

    Ogre::SceneManager* RenderPanel::getSceneManager() const
    {
        return sceneMgr;
    }

    int RenderPanel::getPanelNumber() const
    {
        return panelNumber;
    }

    void RenderPanel::setPanelNumber(int panelNumber)
    {
        this->panelNumber = panelNumber;
    }

    ////////////////////////////////
    ////////////////////////////////
    //                            //
    //  RenderPanelTextDropTarget //
    //                            //
    ////////////////////////////////
    ////////////////////////////////

    RenderPanelTextDropTarget::RenderPanelTextDropTarget(RenderPanel *owner)
    :owner(owner)
    {

    }

    bool RenderPanelTextDropTarget::OnDropText(wxCoord x, wxCoord y, const wxString& text)
    {
        return owner->OnDropText(x, y, text);
    }

} // namespace rviz


text:
#include <iostream>
#include <cmath>

using ::std::string;

#include <vector>

using ::std::vector;

#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>

using ::pcl::PointCloud;
using ::pcl::PointXYZ;
using ::pcl::computeCentroid;
using ::pcl::transformPointCloud;
using ::pcl::io::loadPLYFile;
using ::pcl::visualization::CloudViewer;

#include <Eigen/Dense>

using ::Eigen::Matrix4d;

#include "my_point_cloud.hpp"

#define PI 3.14159265

MyPCL::MyPointCloud::MyPointCloud(string path) {
    int pcl_load_status;

    cloud.reset(new PointCloud<PointXYZ>());
    pcl_load_status = loadPLYFile(path, *cloud);

    if (pcl_load_status == -1) {
        PCL_ERROR("Reading error!\n");
        exit(pcl_load_status);
    }
}

MyPCL::MyPointCloud::~MyPointCloud() {}

void MyPCL::MyPointCloud::view() {
    CloudViewer viewer("My PointCloud viewer");

    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {} // Busy waiting
}

PointXYZ MyPCL::MyPointCloud::centroid() {
    size_t centroid_dimension;
    PointXYZ centroid;

    centroid_dimension = computeCentroid(*cloud, centroid);

    if (centroid_dimension < 3)
        exit(-1);

    return centroid;
}

void MyPCL::MyPointCloud::apply_offset(PointXYZ offset) {
    apply_transformation(offset, PointXYZ(1, 1, 1), PointXYZ(0, 0, 0));
}

void MyPCL::MyPointCloud::apply_scale(double x_s, double y_s, double z_s) {
    apply_transformation(PointXYZ(0, 0, 0), PointXYZ(x_s, y_s, z_s), PointXYZ(0, 0, 0));
}

void MyPCL::MyPointCloud::apply_scale(double scale) {
    apply_scale(scale, scale, scale);
}

void MyPCL::MyPointCloud::apply_rotation(double x_r, double y_r, double z_r) {
    apply_transformation(PointXYZ(0, 0, 0), PointXYZ(1, 1, 1), PointXYZ(x_r, y_r, z_r));
}

void MyPCL::MyPointCloud::apply_transformation(PointXYZ offset, PointXYZ scale, PointXYZ rotation) {
    Matrix4d m_x, m_y, m_z;
    m_x = Matrix4d::Identity();
    m_y = Matrix4d::Identity();
    m_z = Matrix4d::Identity();

    auto to_rad = [] (double degree) {
        return degree / 180 * PI;
    };

    double rad_x, rad_y, rad_z;
    rad_x = to_rad(rotation.x);
    rad_y = to_rad(rotation.y);
    rad_z = to_rad(rotation.z);

    m_x(1, 1) = cos(rad_x);
    m_x(1, 2) = -sin(rad_x);
    m_x(2, 1) = sin(rad_x);
    m_x(2, 2) = cos(rad_x);

    m_y(0, 0) = cos(rad_y);
    m_y(2, 0) = sin(rad_y);
    m_y(0, 2) = -sin(rad_y);
    m_y(2, 2) = cos(rad_y);

    m_z(0, 0) = cos(rad_z);
    m_z(0, 1) = -sin(rad_z);
    m_z(1, 0) = sin(rad_z);
    m_z(1, 1) = cos(rad_z);

    Matrix4d m_rotation;
    m_rotation = m_x * m_y * m_z;

    Matrix4d m_scale;
    m_scale = Matrix4d::Identity();

    // Setting the scaling matrix
    /*
     * x_s 0   0   0
     * 0   y_s 0   0
     * 0   0   z_s 0
     * 0   0   0   1
     */

    m_scale(0, 0) = scale.x;
    m_scale(1, 1) = scale.y;
    m_scale(2, 2) = scale.z;

    Matrix4d m_translation;
    m_translation = Matrix4d::Identity();

    // Setting the translation matrix
    /*
     * 1 0 0 x
     * 0 1 0 y
     * 0 0 1 z
     * 0 0 0 1
     */
    m_translation(0, 3) = offset.x;
    m_translation(1, 3) = offset.y;
    m_translation(2, 3) = offset.z;

    PointCloud<PointXYZ>::Ptr transformed;
    transformed.reset(new PointCloud<PointXYZ>());

    Matrix4d m_transform;
    m_transform = m_scale * m_rotation * m_translation;

    transformPointCloud(*cloud, *transformed, m_transform);
    cloud = transformed;
}


text: #include <boost/qvm/vec.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/**
 * @file UdpDelaySim.cpp
 * @author  Brian Tomko <brian.j.tomko@nasa.gov>
 *
 * @copyright Copyright © 2021 United States Government as represented by
 * the National Aeronautics and Space Administration.
 * No copyright is claimed in the United States under Title 17, U.S.Code.
 * All Other Rights Reserved.
 *
 * @section LICENSE
 * Released under the NASA Open Source Agreement (NOSA)
 * See LICENSE.md in the source root directory for more information.
 */

#include <iostream>

#include "UdpDelaySim.h"
#include <boost/make_unique.hpp>

UdpDelaySim::UdpDelaySim(uint16_t myBoundUdpPort,
    const std::string & remoteHostnameToForwardPacketsTo,
    const std::string & remotePortToForwardPacketsTo,
    const unsigned int numCircularBufferVectors,
    const unsigned int maxUdpPacketSizeBytes,
    const boost::posix_time::time_duration & sendDelay,
    const bool autoStart) :
    m_resolver(m_ioService),
    m_udpPacketSendDelayTimer(m_ioService),
    m_timerTransferRateStats(m_ioService),
    m_udpSocket(m_ioService),
    M_MY_BOUND_UDP_PORT(myBoundUdpPort),
    M_REMOTE_HOSTNAME_TO_FORWARD_PACKETS_TO(remoteHostnameToForwardPacketsTo),
    M_REMOTE_PORT_TO_FORWARD_PACKETS_TO(remotePortToForwardPacketsTo),
    M_NUM_CIRCULAR_BUFFER_VECTORS(numCircularBufferVectors),
    M_MAX_UDP_PACKET_SIZE_BYTES(maxUdpPacketSizeBytes),
    M_SEND_DELAY(sendDelay),
    m_udpReceiveBuffer(M_MAX_UDP_PACKET_SIZE_BYTES),
    m_circularIndexBuffer(M_NUM_CIRCULAR_BUFFER_VECTORS),
    m_udpReceiveBuffersCbVec(M_NUM_CIRCULAR_BUFFER_VECTORS),
    m_udpReceiveBytesTransferredCbVec(M_NUM_CIRCULAR_BUFFER_VECTORS),
    m_expiriesCbVec(M_NUM_CIRCULAR_BUFFER_VECTORS),
    m_printedCbTooSmallNotice(false),
    m_sendDelayTimerIsRunning(false),
    //stats
    m_lastTotalUdpPacketsReceived(0),
    m_lastTotalUdpBytesReceived(0),
    m_lastTotalUdpPacketsSent(0),
    m_lastTotalUdpBytesSent(0),
    m_countCircularBufferOverruns(0),
    m_countMaxCircularBufferSize(0),
    m_countTotalUdpPacketsReceived(0),
    m_countTotalUdpBytesReceived(0),
    m_countTotalUdpPacketsSent(0),
    m_countTotalUdpBytesSent(0)
{
    for (unsigned int i = 0; i < M_NUM_CIRCULAR_BUFFER_VECTORS; ++i) {
        m_udpReceiveBuffersCbVec[i].resize(M_MAX_UDP_PACKET_SIZE_BYTES);
    }

    if (autoStart) {
        StartIfNotAlreadyRunning(); //TODO EVALUATE IF AUTO START SAFE
    }
}

bool UdpDelaySim::StartIfNotAlreadyRunning() {
    if (!m_ioServiceThreadPtr) {
        //Receiver UDP
        try {
            m_udpSocket.open(boost::asio::ip::udp::v4());
            m_udpSocket.bind(boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), M_MY_BOUND_UDP_PORT)); // //if udpPort is 0 then bind to random ephemeral port
        }
        catch (const boost::system::system_error & e) {
            std::cerr << "Could not bind on UDP port " << M_MY_BOUND_UDP_PORT << std::endl;
            std::cerr << "  Error: " << e.what() << std::endl;

            return false;
        }
        printf("UdpDelaySim bound successfully on UDP port %d\n", m_udpSocket.local_endpoint().port());

        {
            //start timer
            m_lastPtime = boost::posix_time::microsec_clock::universal_time();
            m_timerTransferRateStats.expires_from_now(boost::posix_time::seconds(5));
            m_timerTransferRateStats.async_wait(boost::bind(&UdpDelaySim::TransferRate_TimerExpired, this, boost::asio::placeholders::error));
        }

        {
            //connect
            static const boost::asio::ip::resolver_query_base::flags UDP_RESOLVER_FLAGS = boost::asio::ip::resolver_query_base::canonical_name; //boost resolver flags
            std::cout << "udp resolving remote " << M_REMOTE_HOSTNAME_TO_FORWARD_PACKETS_TO << ":" << M_REMOTE_PORT_TO_FORWARD_PACKETS_TO << std::endl;
            boost::asio::ip::udp::resolver::query query(boost::asio::ip::udp::v4(), M_REMOTE_HOSTNAME_TO_FORWARD_PACKETS_TO, M_REMOTE_PORT_TO_FORWARD_PACKETS_TO, UDP_RESOLVER_FLAGS);
            m_resolver.async_resolve(query, boost::bind(&UdpDelaySim::OnResolve,
                this, boost::asio::placeholders::error,
                boost::asio::placeholders::results));
            m_ioServiceThreadPtr = boost::make_unique<boost::thread>(boost::bind(&boost::asio::io_service::run, &m_ioService)); //resolve serves as initial work
        }
    }
    return true;
}

UdpDelaySim::~UdpDelaySim() {
    Stop();
    std::cout << "stats:\n";
    std::cout << "m_countCircularBufferOverruns " << m_countCircularBufferOverruns << "\n";
    std::cout << "m_countMaxCircularBufferSize " << m_countMaxCircularBufferSize << "\n";
    std::cout << "m_countTotalUdpPacketsReceived " << m_countTotalUdpPacketsReceived << "\n";
    std::cout << "m_countTotalUdpPacketsSent " << m_countTotalUdpPacketsSent << "\n";
}

void UdpDelaySim::Stop() {

    DoUdpShutdown();
    while (m_udpSocket.is_open()) {
        boost::this_thread::sleep(boost::posix_time::milliseconds(250));
    }

    if (m_ioServiceThreadPtr) {
        m_ioServiceThreadPtr->join();
        m_ioServiceThreadPtr.reset(); //delete it
    }

}

void UdpDelaySim::OnResolve(const boost::system::error_code & ec, boost::asio::ip::udp::resolver::results_type results) { // Resolved endpoints as a range.
    if (ec) {
        std::cerr << "Error resolving: " << ec.message() << std::endl;
    }
    else {
        m_udpDestinationEndpoint = *results;
        std::cout << "resolved host to " << m_udpDestinationEndpoint.address() << ":" << m_udpDestinationEndpoint.port() << ".  Binding..." << std::endl;
        StartUdpReceive();
    }
}

void UdpDelaySim::StartUdpReceive() {
    m_udpSocket.async_receive_from(
        boost::asio::buffer(m_udpReceiveBuffer),
        m_remoteEndpointReceived,
        boost::bind(&UdpDelaySim::HandleUdpReceive, this,
            boost::asio::placeholders::error,
            boost::asio::placeholders::bytes_transferred));
}

void UdpDelaySim::HandleUdpReceive(const boost::system::error_code & error, std::size_t bytesTransferred) {
    if (!error) {
        const unsigned int writeIndex = m_circularIndexBuffer.GetIndexForWrite(); //store the volatile
        if (writeIndex == CIRCULAR_INDEX_BUFFER_FULL) {
            ++m_countCircularBufferOverruns;
            if (!m_printedCbTooSmallNotice) {
                m_printedCbTooSmallNotice = true;
                std::cout << "notice in UdpDelaySim::HandleUdpReceive(): buffers full.. you might want to increase the circular buffer size! This UDP packet will be dropped!" << std::endl;
            }
        }
        else { //not full.. swap packet in to circular buffer
            m_udpReceiveBuffer.swap(m_udpReceiveBuffersCbVec[writeIndex]);
            m_udpReceiveBytesTransferredCbVec[writeIndex] = bytesTransferred;
            m_expiriesCbVec[writeIndex] = boost::posix_time::microsec_clock::universal_time() + M_SEND_DELAY;
            m_circularIndexBuffer.CommitWrite(); //write complete at this point
            ++m_countTotalUdpPacketsReceived;
            m_countTotalUdpBytesReceived += bytesTransferred;
            const uint64_t cbSize = m_circularIndexBuffer.NumInBuffer();
            m_countMaxCircularBufferSize = std::max(m_countMaxCircularBufferSize, cbSize);
            TryRestartSendDelayTimer();
        }
        StartUdpReceive(); //restart operation only if there was no error
    }
    else if (error != boost::asio::error::operation_aborted) {
        std::cerr << "critical error in UdpDelaySim::HandleUdpReceive(): " << error.message() << std::endl;
        DoUdpShutdown();
    }
}

void UdpDelaySim::DoUdpShutdown() {
    //final code to shut down udp sockets
    m_udpPacketSendDelayTimer.cancel();
    m_timerTransferRateStats.cancel();
    if (m_udpSocket.is_open()) {
        try {
            std::cout << "closing UdpDelaySim UDP socket.." << std::endl;
            m_udpSocket.close();
        }
        catch (const boost::system::system_error & e) {
            std::cout << "notice in UdpDelaySim::DoUdpShutdown calling udpSocket.close: " << e.what() << std::endl;
        }
    }

}

//restarts the send delay timer if it is not running and there are packets available
void UdpDelaySim::TryRestartSendDelayTimer() {
    if (!m_sendDelayTimerIsRunning) {
        const unsigned int consumeIndex = m_circularIndexBuffer.GetIndexForRead(); //store the volatile
        if (consumeIndex != CIRCULAR_INDEX_BUFFER_EMPTY) { //if not empty
            m_udpPacketSendDelayTimer.expires_at(m_expiriesCbVec[consumeIndex]);
            m_udpPacketSendDelayTimer.async_wait(boost::bind(&UdpDelaySim::OnSendDelay_TimerExpired, this, boost::asio::placeholders::error, consumeIndex));
            m_sendDelayTimerIsRunning = true;
        }
    }
}

void UdpDelaySim::OnSendDelay_TimerExpired(const boost::system::error_code& e, const unsigned int consumeIndex) {
    //m_sendDelayTimerIsRunning = false; (HandleUdpSend shall set this false)
    if (e != boost::asio::error::operation_aborted) {
        // Timer was not cancelled, take necessary action.

        const std::vector<uint8_t> & vecDataToSend = m_udpReceiveBuffersCbVec[consumeIndex];
        const uint64_t bytesTransferred = m_udpReceiveBytesTransferredCbVec[consumeIndex];
        m_udpSocket.async_send_to(boost::asio::buffer(vecDataToSend.data(), bytesTransferred), m_udpDestinationEndpoint,
            boost::bind(&UdpDelaySim::HandleUdpSend, this,
                boost::asio::placeholders::error,
                boost::asio::placeholders::bytes_transferred));
    }
    else {
        //std::cout << "timer cancelled\n";
    }
}

void UdpDelaySim::HandleUdpSend(const boost::system::error_code& error, std::size_t bytes_transferred) {
    if (error) {
        std::cerr << "error in UdpDelaySim::HandleUdpSend: " << error.message() << std::endl;
        DoUdpShutdown();
    }
    else {
        ++m_countTotalUdpPacketsSent;
        m_countTotalUdpBytesSent += bytes_transferred;
        m_sendDelayTimerIsRunning = false;
        m_circularIndexBuffer.CommitRead();
        TryRestartSendDelayTimer();
    }
}

void UdpDelaySim::TransferRate_TimerExpired(const boost::system::error_code& e) {
    if (e != boost::asio::error::operation_aborted) {
        // Timer was not cancelled, take necessary action.
        boost::posix_time::ptime finishedTime = boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::time_duration diff = finishedTime - m_lastPtime;
        const uint64_t diffTotalUdpPacketsReceived = m_countTotalUdpPacketsReceived - m_lastTotalUdpPacketsReceived;
        const uint64_t diffTotalUdpBytesReceived = m_countTotalUdpBytesReceived - m_lastTotalUdpBytesReceived;
        const uint64_t diffTotalUdpPacketsSent = m_countTotalUdpPacketsSent - m_lastTotalUdpPacketsSent;
        const uint64_t diffTotalUdpBytesSent = m_countTotalUdpBytesSent - m_lastTotalUdpBytesSent;
        if (diffTotalUdpPacketsReceived || diffTotalUdpPacketsSent) {
            const double diffTotalMicroseconds = static_cast<double>(diff.total_microseconds());
            double ratePacketsRxPerSecond = (diffTotalUdpPacketsReceived * 1e6) / diffTotalMicroseconds;
            double rateBytesRxMbps = (diffTotalUdpBytesReceived * 8.0) / diffTotalMicroseconds;
            double ratePacketsTxPerSecond = (diffTotalUdpPacketsSent * 1e6) / diffTotalMicroseconds;
            double rateBytesTxMbps = (diffTotalUdpBytesSent * 8.0) / diffTotalMicroseconds;
            const unsigned int cbSize = m_circularIndexBuffer.NumInBuffer();

            printf("RX: %0.4f Mbits/sec, %0.1f Packets/sec   TX: %0.4f Mbits/sec, %0.1f Packets/sec  Buffered: %d\n",
                rateBytesRxMbps, ratePacketsRxPerSecond, rateBytesTxMbps, ratePacketsTxPerSecond, cbSize);
        }

        m_lastTotalUdpPacketsReceived = m_countTotalUdpPacketsReceived;
        m_lastTotalUdpBytesReceived = m_countTotalUdpBytesReceived;
        m_lastTotalUdpPacketsSent = m_countTotalUdpPacketsSent;
        m_lastTotalUdpBytesSent = m_countTotalUdpBytesSent;
        m_lastPtime = finishedTime;
        m_timerTransferRateStats.expires_from_now(boost::posix_time::seconds(2));
        m_timerTransferRateStats.async_wait(boost::bind(&UdpDelaySim::TransferRate_TimerExpired, this, boost::asio::placeholders::error));
    }
    else {
        std::cout << "transfer rate timer stopped\n";
    }
}

max_stars_repo_name: nasa/HDTN

max_issues_repo_name: nasa/HDTN

max_forks_repo_name: nasa/HDTN


text:
/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/application.h"
#include "bio/remo.h"
USING_BIO_NS

#include <boost/filesystem/path.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/regex.hpp>
using namespace boost;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include <iostream>
#include <fstream>
#include <string>
using namespace std;

struct ShowReMosApp : Application
{
	std::string remo_extraction_filename;
	std::string sequence;

	ShowReMosApp()
	{
		get_options().add_options()
			("remo,r", po::value(&remo_extraction_filename)->default_value("remo_space.bin"), "remo extraction file")
			("sequence,s", po::value(&sequence)->default_value("."), "regex to match sequence name")
			;

		get_positional_options().add("sequence", 1);
	}

	int task()
	{
		if ("" == sequence)
		{
			throw std::logic_error( "No regex defined to match gene against" );
		}
		else
		{
			cout << "Showing remos that match regex: " << sequence << "\n";
		}

		//deserialise the binary remo archive
		ReMoExtraction::ptr_t remo;
		{
			fs::path
				remo_extraction_archive(
					remo_extraction_filename
				);
			cout << "Deserialising remo extraction from \"" << remo_extraction_archive._BOOST_FS_NATIVE() << "\"\n";
			remo = ReMoExtraction::deserialise(remo_extraction_archive);
		}

		//show the remos that match the sequence
		unsigned num_matched = 0;
		regex re(sequence);
		smatch what;
		{
			const unsigned num_groups = remo->sequence_groups.size();
			cout << "Searching " << num_groups << " sequence groups\n";

			for (ReMoSequenceGroup::list_t::const_iterator sg = remo->sequence_groups.begin();
				remo->sequence_groups.end() != sg;
				++sg)
			{
				bool matched = false;
				//check each of the sequences in the group to see if the id matches
				for (ReMoSequence::list_t::const_iterator s = sg->get()->sequences.begin();
					sg->get()->sequences.end() != s && ! matched;
					++s)
				{
					matched = regex_search(s->get()->id, what, re);
				}

				if (matched)
				{
					//print out group
					cout << **sg << "\n";
					++num_matched;
				}
			}
		}
		cout << "Matched " << num_matched << " groups\n";

		return 0;
	}
};

int
main(int argc, char * argv[])
{
	return ShowReMosApp().main(argc, argv);
}

max_stars_repo_name: JohnReid/biopsy

max_issues_repo_name: JohnReid/biopsy

max_forks_repo_name: JohnReid/biopsy


text:
/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <deque>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/smart_ptr/make_shared.hpp>
#include <sprig/nk/foundation.hpp>

namespace sprig {
	namespace nk {
		//
		// basic_unit
		//
		class basic_unit
			: public unit
		{
		public:
			typedef std::deque<byte_type> memory_type;
		private:
			memory_type input_;
			memory_type output_;
			unit_info info_;
		public:
			basic_unit() {}
		public:
			unit_id_type const get_id() {
				return 0;
			}
			unit_info const get_info() const {
				return info_;
			}
			void set_info(unit_info const& info) {
				info_ = info;
			}
			void push_input(byte_type const in) {
				input_.push_back(in);
			}
			popped_byte_type const pop_input() {
				popped_byte_type result;
				if (!input_.empty()) {
					result = input_.front();
					input_.pop_front();
				}
				return result;
			}
			bool const empty_input() {
				return input_.empty();
			}
			void push_output(byte_type const out) {
				output_.push_back(out);
			}
			popped_byte_type const pop_output() {
				popped_byte_type result;
				if (!output_.empty()) {
					result = output_.front();
					output_.pop_front();
				}
				return result;
			}
			bool const empty_output() {
				return output_.empty();
			}
		};

		//
		// basic_engine
		//
		class basic_engine
			: public engine
		{
		public:
			typedef std::deque<byte_type> memory_type;
		private:
			memory_type forward_;
			memory_type backward_;
			bool is_allocated_;
			size_type loaded_size_;
			bool is_executable_;
			unit_info info_;
		private:
			bool const is_out_of_bounds(ptr_type const p) {
				return p >= 0
					? p >= static_cast<ptr_type>(forward_.size())
					: -p - 1 >= static_cast<ptr_type>(backward_.size())
					;
			}
			byte_type& at(ptr_type const p) {
				return p >= 0
					? forward_.at(p)
					: backward_.at(-p - 1)
					;
			}
			void push(ptr_type const p) {
				p >= 0
					? forward_.push_back(0)
					: backward_.push_back(0)
					;
			}
			void move(ptr_type const p) {
				if (is_out_of_bounds(p)) {
					push(p);
				}
			}
			bool const search_forward(unit& u, unit_info& info) {
				ptr_type p = info.pptr;
				for (int i = 1; ; ++p) {
					if (is_out_of_bounds(p)) {
						info.state = abnormal;
						info.error = out_of_bounds_forward;
						return false;
					}
					if (at(p) == '[') {
						++i;
					} else if (at(p) == ']') {
						--i;
					}
					if (!i) {
						info.state = normal;
						info.error = no_error;
						info.pptr = p + 1;
						return true;
					}
				}
				return true;
			}
			bool const search_backward(unit& u, unit_info& info) {
				ptr_type p = info.pptr;
				for (int i = -1; ; --p) {
					if (is_out_of_bounds(p)) {
						info.state = abnormal;
						info.error = out_of_bounds_backward;
						return false;
					}
					if (at(p) == '[') {
						++i;
					} else if (at(p) == ']') {
						--i;
					}
					if (!i) {
						info.state = normal;
						info.error = no_error;
						info.pptr = p;
						return true;
					}
				}
				return true;
			}
		public:
			basic_engine()
				: is_allocated_(false)
				, loaded_size_(0)
			{}
			template<typename Range>
			void load_forward(Range const& range) {
				std::copy(
					boost::begin(range),
					boost::end(range),
					std::back_inserter(forward_)
					);
			}
			template<typename Range>
			void load_backward(Range const& range) {
				std::copy(
					boost::begin(range),
					boost::end(range),
					std::front_inserter(backward_)
					);
			}
			template<typename Iterator>
			void save_forward(Iterator iter) {
				std::copy(
					boost::begin(forward_),
					boost::end(forward_),
					iter
					);
			}
			template<typename Iterator>
			void save_backward(Iterator iter) {
				std::copy(
					boost::begin(backward_),
					boost::end(backward_),
					iter
					);
			}
		public:
			created_unit_type create_unit() {
				return created_unit_type(new basic_unit());
			}
			interval_type const referenced_interval(unit& u) const {
				return interval_type(-static_cast<ptr_type>(backward_.size()), static_cast<ptr_type>(forward_.size()));
			}
			bool const allocate(unit& u, size_type const s) {
				if (is_allocated(u)) {
					return false;
				}
				memory_type new_forward_(s, 0);
				memory_type new_backward_;
				forward_.swap(new_forward_);
				backward_.swap(new_backward_);
				loaded_size_ = 0;
				is_allocated_ = true;
				return true;
			}
			size_type const allocated_size(unit& u) const {
				return forward_.size();
			}
			bool const is_allocated(unit& u) const {
				return is_allocated_;
			}
			bool const load(unit& u, byte_type const c) {
				if (!is_allocated(u) || is_full_loaded(u)) {
					return false;
				}
				forward_.at(loaded_size_++) = c;
				return true;
			}
			bool const is_full_loaded(unit& u) const {
				return is_allocated(u) && loaded_size(u) == allocated_size(u);
			}
			size_type const loaded_size(unit& u) const {
				return loaded_size_;
			}
			bool const is_executable(unit& u) const {
				return is_allocated(u) /*&& is_full_loaded(u)*/ && is_executable_;
			}
			bool const begin_execution(unit& u) {
				is_executable_ = true;
				return true;
			}
			bool const end_execution(unit& u) {
				is_executable_ = false;
				return true;
			}
			bool const execute(unit& u) {
				unit_info info(info_);
				if (!is_executable(u)) {
					info.state = abnormal;
					info.error = un_executable;
					u.set_info(info_);
					return false;
				}
				switch (info_.error) {
				case out_of_bounds_forward:
					search_forward(u, info);
					u.set_info(info_ = info);
					return true;
				case out_of_bounds_backward:
					search_backward(u, info);
					u.set_info(info_ = info);
					return true;
				}
				if (is_out_of_bounds(info_.pptr)) {
					info.state = stopped;
					info.error = no_error;
					u.set_info(info_ = info);
					return true;
				}
				switch (at(info_.pptr)) {
				case '>':
					move(++info.dptr);
					info.state = normal;
					info.error = no_error;
					++info.pptr;
					break;
				case '<':
					move(--info.dptr);
					info.state = normal;
					info.error = no_error;
					++info.pptr;
					break;
				case '+':
					++at(info.dptr);
					info.state = normal;
					info.error = no_error;
					++info.pptr;
					break;
				case '-':
					--at(info.dptr);
					info.state = normal;
					info.error = no_error;
					++info.pptr;
					break;
				case '.':
					u.push_output(at(info.dptr));
					info.state = normal;
					info.error = no_error;
					++info.pptr;
					break;
				case ',':
					if (u.empty_input()) {
						info.state = requires_input;
						info.error = no_error;
					} else {
						at(info.dptr) = *u.pop_input();
						info.state = normal;
						info.error = no_error;
						++info.pptr;
					}
					break;
				case '[':
					if (!at(info.dptr)) {
						++info.pptr;
						search_forward(u, info);
					} else {
						info.state = normal;
						info.error = no_error;
						++info.pptr;
					}
					break;
				case ']':
					--info.pptr;
					search_backward(u, info);
					break;
				default:
					info.state = normal;
					info.error = no_error;
					++info.pptr;
				}
				u.set_info(info_ = info);
				return true;
			}
		};
	}	// namespace nk
}	// namespace sprig

#endif	// #ifndef SPRIG_NK_BASIC_HPP

max_stars_repo_name: bolero-MURAKAMI/Sprig

max_issues_repo_name: bolero-MURAKAMI/Sprig

max_forks_repo_name: bolero-MURAKAMI/Sprig


text:
// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "OpenDrive.h"

#include <boost/filesystem/operations.hpp>
#include <fstream>
#include <streambuf>

namespace fs = boost::filesystem;

bool Match(const char *str, const char *test) {
  return 0 == fnmatch(test, str, 0);
}

void ValidateFilePath(std::string &filepath, const std::string &ext) {
  fs::path path(filepath);
  if (path.extension().empty() && !ext.empty()) {
    if (ext[0] != '.') {
      path += '.';
    }
    path += ext;
  }
  auto parent = path.parent_path();
  if (!parent.empty()) {
    fs::create_directories(parent);
  }
  filepath = path.string();
}

std::vector<std::string> ListFolder(
    const std::string &folder_path,
    const std::string &wildcard_pattern) {
  fs::path root(folder_path);
  if (!fs::exists(root) || !fs::is_directory(root)) {
    std::cout << "[error]" << std::endl;
  }

  std::vector<std::string> results;
  fs::directory_iterator end;
  for (fs::directory_iterator it(root); it != end; ++it) {
    if (fs::is_regular_file(*it)) {
      const std::string filename = it->path().filename().string();
      if (Match(filename.c_str(), wildcard_pattern.c_str())) {
        results.emplace_back(filename);
      }
    }
  }
  return results;
}

namespace util {

  std::vector<std::string> OpenDrive::GetAvailableFiles() {
    return ListFolder(
        LIBOPENDRIVE_TEST_CONTENT_FOLDER "/OpenDrive/",
        "*.xodr");
  }

  std::string OpenDrive::Load(const std::string &filename) {
    const std::string opendrive_folder = LIBOPENDRIVE_TEST_CONTENT_FOLDER "/OpenDrive/";
    std::ifstream file(opendrive_folder + filename);
    return std::string{std::istreambuf_iterator<char>(file), std::istreambuf_iterator<char>()};
  }

} // namespace util


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

#pragma once

#ifdef __SIZEOF_INT128__

#include <stdint.h>

namespace fc {

using int128_t  = __int128_t;
using uint128_t = __uint128_t;

inline uint64_t uint128_lo64(const uint128_t x) {
  return static_cast<uint64_t>(x & 0xffffffffffffffffULL);
}
inline uint64_t uint128_hi64(const uint128_t x) {
  return static_cast<uint64_t>( x >> 64 );
}

} // namespace fc

#else // __SIZEOF_INT128__

#include <boost/multiprecision/integer.hpp>

namespace fc {

using boost::multiprecision::int128_t;
using boost::multiprecision::uint128_t;

inline uint64_t uint128_lo64(const uint128_t& x) {
  return static_cast<uint64_t>(x & 0xffffffffffffffffULL);
}
inline uint64_t uint128_hi64(const uint128_t& x) {
  return static_cast<uint64_t>( x >> 64 );
}

} // namespace fc

#endif // __SIZEOF_INT128__

namespace fc {

inline uint128_t uint128( const uint64_t hi, const uint64_t lo ) {
   return ( uint128_t(hi) << 64 ) + lo;
}

} // namespace fc

max_stars_repo_name: Revolution-Populi/fc

max_issues_repo_name: Revolution-Populi/fc

max_forks_repo_name: Revolution-Populi/fc


text:
/* Copyright 2016-2017 Joaquin M Lopez Munoz.
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org/libs/poly_collection for library home page.
 */

#ifndef BOOST_POLY_COLLECTION_DETAIL_FUNCTION_MODEL_HPP
#define BOOST_POLY_COLLECTION_DETAIL_FUNCTION_MODEL_HPP

#if defined(_MSC_VER)
#pragma once
#endif

#include <boost/core/addressof.hpp>
#include <boost/poly_collection/detail/callable_wrapper.hpp>
#include <boost/poly_collection/detail/callable_wrapper_iterator.hpp>
#include <boost/poly_collection/detail/is_invocable.hpp>
#include <boost/poly_collection/detail/segment_backend.hpp>
#include <boost/poly_collection/detail/split_segment.hpp>
#include <memory>
#include <type_traits>
#include <typeinfo>
#include <utility>

namespace boost{

namespace poly_collection{

namespace detail{

/* model for function_collection */

template<typename Signature>
struct function_model;

/* is_terminal defined out-class to allow for partial specialization */

template<typename T>
struct function_model_is_terminal:std::true_type{};

template<typename Signature>
struct function_model_is_terminal<callable_wrapper<Signature>>:
  std::false_type{};

template<typename R,typename... Args>
struct function_model<R(Args...)>
{
  using value_type=callable_wrapper<R(Args...)>;

  template<typename Callable>
  using is_implementation=is_invocable_r<R,Callable&,Args...>;

  template<typename T>
  using is_terminal=function_model_is_terminal<T>;

  template<typename T>
  static const std::type_info& subtypeid(const T&){return typeid(T);}

  template<typename Signature>
  static const std::type_info& subtypeid(
    const callable_wrapper<Signature>& f)
  {
    return f.target_type();
  }

  template<typename T>
  static void* subaddress(T& x){return boost::addressof(x);}

  template<typename T>
  static const void* subaddress(const T& x){return boost::addressof(x);}

  template<typename Signature>
  static void* subaddress(callable_wrapper<Signature>& f)
  {
    return f.data();
  }

  template<typename Signature>
  static const void* subaddress(const callable_wrapper<Signature>& f)
  {
    return f.data();
  }

  using base_iterator=callable_wrapper_iterator<value_type>;
  using const_base_iterator=callable_wrapper_iterator<const value_type>;
  using base_sentinel=value_type*;
  using const_base_sentinel=const value_type*;
  template<typename Callable>
  using iterator=Callable*;
  template<typename Callable>
  using const_iterator=const Callable*;
  using segment_backend=detail::segment_backend<function_model>;
  template<typename Callable,typename Allocator>
  using segment_backend_implementation=split_segment<
    function_model,
    Callable,
    typename std::allocator_traits<Allocator>::
      template rebind_alloc<Callable>
  >;
  using segment_backend_unique_ptr=
    typename segment_backend::segment_backend_unique_ptr;

  static base_iterator nonconst_iterator(const_base_iterator it)
  {
    return base_iterator{
      const_cast<value_type*>(static_cast<const value_type*>(it))};
  }

  template<typename T>
  static iterator<T> nonconst_iterator(const_iterator<T> it)
  {
    return const_cast<iterator<T>>(it);
  }

  template<typename Callable,typename Allocator>
  static segment_backend_unique_ptr make(const Allocator& al)
  {
    return segment_backend_implementation<Callable,Allocator>::new_(al,al);
  }

private:
  template<typename,typename,typename>
  friend class split_segment;

  template<typename Callable>
  static value_type make_value_type(Callable& x){return value_type{x};}
};

} /* namespace poly_collection::detail */

} /* namespace poly_collection */

} /* namespace boost */

#endif


text:
/*
 *  stageros
 *  Copyright (c) 2008, Willow Garage, Inc.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/**

@mainpage

@htmlinclude manifest.html
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <signal.h>

// libstage
#include <stage.hh>

// roscpp
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <rosgraph_msgs/Clock.h>

#include <std_srvs/Empty.h>

#include "tf/transform_broadcaster.h"

#define USAGE "stageros <worldfile>"
#define IMAGE "image"
#define DEPTH "depth"
#define CAMERA_INFO "camera_info"
#define ODOM "odom"
#define BASE_SCAN "base_scan"
#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
#define CMD_VEL "cmd_vel"

//bypassing this shit... I hope that all hell doesnt break loose
nav_msgs::Odometry odom_msg;
Stg::Velocity v;
ros::Subscriber real_odom; //gets the postion of the robot from the robot "physical" odometry

// Our node
class StageNode
{

private:

    // roscpp-related bookkeeping
    ros::NodeHandle n_;

    // A mutex to lock access to fields that are used in message callbacks
    boost::mutex msg_lock;

    // The models that we're interested in
    std::vector<Stg::ModelCamera *> cameramodels;
    std::vector<Stg::ModelRanger *> lasermodels;
    std::vector<Stg::ModelPosition *> positionmodels;

    //a structure representing a robot inthe simulator
    struct StageRobot
    {
        //stage related models
        Stg::ModelPosition* positionmodel; //one position
        std::vector<Stg::ModelCamera *> cameramodels; //multiple cameras per position
        std::vector<Stg::ModelRanger *> lasermodels; //multiple rangers per position

        //ros publishers
        ros::Publisher odom_pub; //one odom
        ros::Publisher ground_truth_pub; //one ground truth

        std::vector<ros::Publisher> image_pubs; //multiple images
        std::vector<ros::Publisher> depth_pubs; //multiple depths
        std::vector<ros::Publisher> camera_pubs; //multiple cameras
        std::vector<ros::Publisher> laser_pubs; //multiple lasers

        ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
    };

    std::vector<StageRobot const *> robotmodels_;

    // Used to remember initial poses for soft reset
    std::vector<Stg::Pose> initial_poses;
    ros::ServiceServer reset_srv_;

    ros::Publisher clock_pub_;

    bool isDepthCanonical;
    bool use_model_names;

    // A helper function that is executed for each stage model.  We use it
    // to search for models of interest.
    static void ghfunc(Stg::Model* mod, StageNode* node);

    static bool s_update(Stg::World* world, StageNode* node)
    {
        node->WorldCallback();
        // We return false to indicate that we want to be called again (an
        // odd convention, but that's the way that Stage works).
        return false;
    }

    // Appends the given robot ID to the given message name.  If omitRobotID
    // is true, an unaltered copy of the name is returned.
    const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
    const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;

    tf::TransformBroadcaster tf;

    // Last time that we received a velocity command
    ros::Time base_last_cmd;
    ros::Duration base_watchdog_timeout;

    // Current simulation time
    ros::Time sim_time;

    // Last time we saved global position (for velocity calculation).
    ros::Time base_last_globalpos_time;
    // Last published global pose of each robot
    std::vector<Stg::Pose> base_last_globalpos;

public:
    // Constructor; stage itself needs argc/argv.  fname is the .world file
    // that stage should load.
    StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names);
    ~StageNode();

    // Subscribe to models of interest.  Currently, we find and subscribe
    // to the first 'laser' model and the first 'position' model.  Returns
    // 0 on success (both models subscribed), -1 otherwise.
    int SubscribeModels();

    // Our callback
    void WorldCallback();

    // Do one update of the world.  May pause if the next update time
    // has not yet arrived.
    bool UpdateWorld();

    // Message callback for a MsgBaseVel message, which set velocities.
    void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);

    // Service callback for soft reset
    bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

    // The main simulator object
    Stg::World* world;
};

// since stageros is single-threaded, this is OK. revisit if that changes!
const char *
StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
{
    //ROS_INFO("Robot %lu: Device %s", robotID, name);
    bool umn = this->use_model_names;

    if ((positionmodels.size() > 1 ) || umn)
    {
        static char buf[100];
        std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");

        if ((found==std::string::npos) && umn)
        {
            snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
        }
        else
        {
            snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
        }

        return buf;
    }
    else
        return name;
}

const char *
StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
{
    //ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
    bool umn = this->use_model_names;

    if ((positionmodels.size() > 1 ) || umn)
    {
        static char buf[100];
        std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");

        if ((found==std::string::npos) && umn)
        {
            snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
        }
        else
        {
            snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
        }

        return buf;
    }
    else
    {
        static char buf[100];
        snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
        return buf;
    }
}

void
StageNode::ghfunc(Stg::Model* mod, StageNode* node)
{
  //printf( "inspecting %s, parent\n", mod->Token() );

  if (dynamic_cast<Stg::ModelRanger *>(mod)) {
     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
  }
  if (dynamic_cast<Stg::ModelPosition *>(mod)) {
     Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
      // remember initial poses
      node->positionmodels.push_back(p);
      node->initial_poses.push_back(p->GetGlobalPose());
    }
  if (dynamic_cast<Stg::ModelCamera *>(mod)) {
     node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
  }
}

bool
StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
  ROS_INFO("Resetting stage!");
  for (size_t r = 0; r < this->positionmodels.size(); r++) {
    this->positionmodels[r]->SetPose(this->initial_poses[r]);
    this->positionmodels[r]->SetStall(false);
  }
  return true;
}

void
StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg)
{
    boost::mutex::scoped_lock lock(msg_lock);
    this->positionmodels[idx]->SetSpeed(msg->linear.x,
                                        msg->linear.y,
                                        msg->angular.z);
    this->base_last_cmd = this->sim_time;
}

StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
{
    this->use_model_names = use_model_names;
    this->sim_time.fromSec(0.0);
    this->base_last_cmd.fromSec(0.0);
    double t;
    ros::NodeHandle localn("~");
    if(!localn.getParam("base_watchdog_timeout", t))
        t = 0.2;
    this->base_watchdog_timeout.fromSec(t);

    if(!localn.getParam("is_depth_canonical", isDepthCanonical))
        isDepthCanonical = true;

    // We'll check the existence of the world file, because libstage doesn't
    // expose its failure to open it.  Could go further with checks (e.g., is
    // it readable by this user).
    struct stat s;
    if(stat(fname, &s) != 0)
    {
        ROS_FATAL("The world file %s does not exist.", fname);
        ROS_BREAK();
    }

    // initialize libstage
    Stg::Init( &argc, &argv );

    if(gui)
        this->world = new Stg::WorldGui(600, 400, "Stage (ROS)");
    else
        this->world = new Stg::World();

    this->world->Load(fname);

    // todo: reverse the order of these next lines? try it .

    this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);

    // inspect every model to locate the things we care about
    this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
}

// Subscribe to models of interest.  Currently, we find and subscribe
// to the first 'laser' model and the first 'position' model.  Returns
// 0 on success (both models subscribed), -1 otherwise.
//
// Eventually, we should provide a general way to map stage models onto ROS
// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
    n_.setParam("/use_sim_time", true);

    for (size_t r = 0; r < this->positionmodels.size(); r++)
    {
        StageRobot* new_robot = new StageRobot;
        new_robot->positionmodel = this->positionmodels[r];
        new_robot->positionmodel->Subscribe();

	ROS_INFO( "Subscribed to Stage position model \"%s\"", this->positionmodels[r]->Token() );

        for (size_t s = 0; s < this->lasermodels.size(); s++)
        {
	  if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel)
            {
                new_robot->lasermodels.push_back(this->lasermodels[s]);
                this->lasermodels[s]->Subscribe();
	      ROS_INFO( "subscribed to Stage ranger \"%s\"", this->lasermodels[s]->Token() );
            }
        }

        for (size_t s = 0; s < this->cameramodels.size(); s++)
        {
            if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel)
            {
                new_robot->cameramodels.push_back(this->cameramodels[s]);
                this->cameramodels[s]->Subscribe();

		ROS_INFO( "subscribed to Stage camera model \"%s\"", this->cameramodels[s]->Token() );
            }
        }

	// TODO - print the topic names nicely as well
        ROS_INFO("Robot %s provided %lu rangers and %lu cameras",
		 new_robot->positionmodel->Token(),
		 new_robot->lasermodels.size(),
		 new_robot->cameramodels.size() );

        new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
        new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
        new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));

        for (size_t s = 0;  s < new_robot->lasermodels.size(); ++s)
        {
            if (new_robot->lasermodels.size() == 1)
                new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
            else
                new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));

        }

        for (size_t s = 0;  s < new_robot->cameramodels.size(); ++s)
        {
            if (new_robot->cameramodels.size() == 1)
            {
                new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
                new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
                new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
            }
            else
            {
                new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
                new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
                new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
            }
        }

        this->robotmodels_.push_back(new_robot);
    }
    clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);

    // advertising reset service
    reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this);

    return(0);
}

StageNode::~StageNode()
{
    for (std::vector<StageRobot const*>::iterator r = this->robotmodels_.begin(); r != this->robotmodels_.end(); ++r)
        delete *r;
}

bool
StageNode::UpdateWorld()
{
    return this->world->UpdateAll();
}

void set_position(const nav_msgs::Odometry::ConstPtr& real_odom){
        odom_msg.pose.pose.position.x = real_odom->pose.pose.position.x;
        odom_msg.pose.pose.position.y = real_odom->pose.pose.position.x;
        odom_msg.pose.pose.orientation = real_odom->pose.pose.orientation;
}

void
StageNode::WorldCallback()
{
  if( ! ros::ok() ) {
    ROS_INFO( "ros::ok() is false. Quitting." );
    this->world->QuitAll();
    return;
  }

    boost::mutex::scoped_lock lock(msg_lock);

    this->sim_time.fromSec(world->SimTimeNow() / 1e6);
    // We're not allowed to publish clock==0, because it used as a special
    // value in parts of ROS, #4027.
    if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
    {
        ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
        return;
    }

    // TODO make this only affect one robot if necessary
    if((this->base_watchdog_timeout.toSec() > 0.0) &&
            ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
    {
        for (size_t r = 0; r < this->positionmodels.size(); r++)
            this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
    }

    //loop on the robot models
    for (size_t r = 0; r < this->robotmodels_.size(); ++r)
    {
        StageRobot const * robotmodel = this->robotmodels_[r];

        //loop on the laser devices for the current robot
        for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
        {
            Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s];
            const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->GetSensors();

            if( sensors.size() > 1 )
                ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );

            // for now we access only the zeroth sensor of the ranger - good
            // enough for most laser models that have a single beam origin
            const Stg::ModelRanger::Sensor& sensor = sensors[0];

            if( sensor.ranges.size() )
            {
                // Translate into ROS message format and publish
                sensor_msgs::LaserScan msg;
                msg.angle_min = -sensor.fov/2.0;
                msg.angle_max = +sensor.fov/2.0;
                msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1);
                msg.range_min = sensor.range.min;
                msg.range_max = sensor.range.max;
                msg.ranges.resize(sensor.ranges.size());
                msg.intensities.resize(sensor.intensities.size());

                for(unsigned int i = 0; i < sensor.ranges.size(); i++)
                {
                    msg.ranges[i] = sensor.ranges[i];
                    msg.intensities[i] = sensor.intensities[i];
                }

                if (robotmodel->lasermodels.size() > 1)
                    msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
                else
                    msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));

                msg.header.stamp = sim_time;
                robotmodel->laser_pubs[s].publish(msg);
            }

            // Also publish the base->base_laser_link Tx.  This could eventually move
            // into being retrieved from the param server as a static Tx.
            Stg::Pose lp = lasermodel->GetPose();
            tf::Quaternion laserQ;
            laserQ.setRPY(0.0, 0.0, lp.a);
            tf::Transform txLaser =  tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));

            if (robotmodel->lasermodels.size() > 1)
                tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
                                                      mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                                      mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
            else
                tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
                                                      mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                                      mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
        }

        //the position of the robot
        tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
                                              sim_time,
                                              mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                              mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

        // Get latest odometry data
        // Translate into ROS message format and publish
        //some changes here as well in order to bypass

        Stg::Velocity v = robotmodel->positionmodel->GetVelocity();
        odom_msg.twist.twist.linear.x = v.x;
        odom_msg.twist.twist.linear.y = 0;
        odom_msg.twist.twist.angular.z = v.a;

        //@todo Publish stall on a separate topic when one becomes available
        //this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
        //
        odom_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
        odom_msg.header.stamp = sim_time;

        robotmodel->odom_pub.publish(odom_msg);

        // broadcast odometry transform
        tf::Quaternion odomQ;
        tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
        tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
        tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
                                              mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                              mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

        // Also publish the ground truth pose and velocity
        Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
        tf::Quaternion q_gpose;
        q_gpose.setRPY(0.0, 0.0, gpose.a);
        tf::Transform gt(q_gpose, tf::Point(gpose.x, gpose.y, 0.0));
        // Velocity is 0 by default and will be set only if there is previous pose and time delta>0
        Stg::Velocity gvel(0,0,0,0);
        if (this->base_last_globalpos.size()>r){
            Stg::Pose prevpose = this->base_last_globalpos.at(r);
            double dT = (this->sim_time-this->base_last_globalpos_time).toSec();
            if (dT>0)
                gvel = Stg::Velocity(
                            (gpose.x - prevpose.x)/dT,
                            (gpose.y - prevpose.y)/dT,
                            (gpose.z - prevpose.z)/dT,
                            Stg::normalize(gpose.a - prevpose.a)/dT
                            );
            this->base_last_globalpos.at(r) = gpose;
        }else //There are no previous readings, adding current pose...
            this->base_last_globalpos.push_back(gpose);

        nav_msgs::Odometry ground_truth_msg;
        ground_truth_msg.pose.pose.position.x     = gt.getOrigin().x();
        ground_truth_msg.pose.pose.position.y     = gt.getOrigin().y();
        ground_truth_msg.pose.pose.position.z     = gt.getOrigin().z();
        ground_truth_msg.pose.pose.orientation.x  = gt.getRotation().x();
        ground_truth_msg.pose.pose.orientation.y  = gt.getRotation().y();
        ground_truth_msg.pose.pose.orientation.z  = gt.getRotation().z();
        ground_truth_msg.pose.pose.orientation.w  = gt.getRotation().w();
        ground_truth_msg.twist.twist.linear.x = gvel.x;
        ground_truth_msg.twist.twist.linear.y = gvel.y;
        ground_truth_msg.twist.twist.linear.z = gvel.z;
        ground_truth_msg.twist.twist.angular.z = gvel.a;

        ground_truth_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
        ground_truth_msg.header.stamp = sim_time;

        robotmodel->ground_truth_pub.publish(ground_truth_msg);

        //cameras
        for (size_t s = 0; s < robotmodel->cameramodels.size(); ++s)
        {
            Stg::ModelCamera* cameramodel = robotmodel->cameramodels[s];
            // Get latest image data
            // Translate into ROS message format and publish
            if (robotmodel->image_pubs[s].getNumSubscribers() > 0 && cameramodel->FrameColor())
            {
                sensor_msgs::Image image_msg;

                image_msg.height = cameramodel->getHeight();
                image_msg.width = cameramodel->getWidth();
                image_msg.encoding = "rgba8";
                //this->imageMsgs[r].is_bigendian="";
                image_msg.step = image_msg.width*4;
                image_msg.data.resize(image_msg.width * image_msg.height * 4);

                memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4);

                //invert the opengl weirdness
                int height = image_msg.height - 1;
                int linewidth = image_msg.width*4;

                char* temp = new char[linewidth];
                for (int y = 0; y < (height+1)/2; y++)
                {
                    memcpy(temp,&image_msg.data[y*linewidth],linewidth);
                    memcpy(&(image_msg.data[y*linewidth]),&(image_msg.data[(height-y)*linewidth]),linewidth);
                    memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth);
                }

                if (robotmodel->cameramodels.size() > 1)
                    image_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
                else
                    image_msg.header.frame_id = mapName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
                image_msg.header.stamp = sim_time;

                robotmodel->image_pubs[s].publish(image_msg);
            }

            //Get latest depth data
            //Translate into ROS message format and publish
            //Skip if there are no subscribers
            if (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth())
            {
                sensor_msgs::Image depth_msg;
                depth_msg.height = cameramodel->getHeight();
                depth_msg.width = cameramodel->getWidth();
                depth_msg.encoding = this->isDepthCanonical?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
                //this->depthMsgs[r].is_bigendian="";
                int sz = this->isDepthCanonical?sizeof(float):sizeof(uint16_t);
                size_t len = depth_msg.width * depth_msg.height;
                depth_msg.step = depth_msg.width * sz;
                depth_msg.data.resize(len*sz);

                //processing data according to REP118
                if (this->isDepthCanonical){
                    double nearClip = cameramodel->getCamera().nearClip();
                    double farClip = cameramodel->getCamera().farClip();
                    memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz);
                    float * data = (float*)&(depth_msg.data[0]);
                    for (size_t i=0;i<len;++i)
                        if(data[i]<=nearClip)
                            data[i] = -INFINITY;
                        else if(data[i]>=farClip)
                            data[i] = INFINITY;
                }
                else{
                    int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000);
                    int farClip = (int)(cameramodel->getCamera().farClip() * 1000);
                    for (size_t i=0;i<len;++i){
                        int v = (int)(cameramodel->FrameDepth()[i]*1000);
                        if (v<=nearClip || v>=farClip) v = 0;
                        ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
                    }
                }

                //invert the opengl weirdness
                int height = depth_msg.height - 1;
                int linewidth = depth_msg.width*sz;

                char* temp = new char[linewidth];
                for (int y = 0; y < (height+1)/2; y++)
                {
                    memcpy(temp,&depth_msg.data[y*linewidth],linewidth);
                    memcpy(&(depth_msg.data[y*linewidth]),&(depth_msg.data[(height-y)*linewidth]),linewidth);
                    memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth);
                }

                if (robotmodel->cameramodels.size() > 1)
                    depth_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
                else
                    depth_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
                depth_msg.header.stamp = sim_time;
                robotmodel->depth_pubs[s].publish(depth_msg);
            }

            //sending camera's tf and info only if image or depth topics are subscribed to
            if ((robotmodel->image_pubs[s].getNumSubscribers()>0 && cameramodel->FrameColor())
                    || (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth()))
            {

                Stg::Pose lp = cameramodel->GetPose();
                tf::Quaternion Q; Q.setRPY(
                            (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI,
                            0.0,
                            lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->positionmodel->GetPose().a
                            );

                tf::Transform tr =  tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z));

                if (robotmodel->cameramodels.size() > 1)
                    tf.sendTransform(tf::StampedTransform(tr, sim_time,
                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                                          mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
                else
                    tf.sendTransform(tf::StampedTransform(tr, sim_time,
                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
                                                          mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

                sensor_msgs::CameraInfo camera_msg;
                if (robotmodel->cameramodels.size() > 1)
                    camera_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
                else
                    camera_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
                camera_msg.header.stamp = sim_time;
                camera_msg.height = cameramodel->getHeight();
                camera_msg.width = cameramodel->getWidth();

                double fx,fy,cx,cy;
                cx = camera_msg.width / 2.0;
                cy = camera_msg.height / 2.0;
                double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0;
                double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0;
                //double fx_ = 1.43266615300557*this->cameramodels[r]->getWidth()/tan(fovh);
                //double fy_ = 1.43266615300557*this->cameramodels[r]->getHeight()/tan(fovv);
                fx = cameramodel->getWidth()/(2*tan(fovh/2));
                fy = cameramodel->getHeight()/(2*tan(fovv/2));

                //ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_);

                camera_msg.D.resize(4, 0.0);

                camera_msg.K[0] = fx;
                camera_msg.K[2] = cx;
                camera_msg.K[4] = fy;
                camera_msg.K[5] = cy;
                camera_msg.K[8] = 1.0;

                camera_msg.R[0] = 1.0;
                camera_msg.R[4] = 1.0;
                camera_msg.R[8] = 1.0;

                camera_msg.P[0] = fx;
                camera_msg.P[2] = cx;
                camera_msg.P[5] = fy;
                camera_msg.P[6] = cy;
                camera_msg.P[10] = 1.0;

                robotmodel->camera_pubs[s].publish(camera_msg);

            }

        }
    }

    this->base_last_globalpos_time = this->sim_time;
    rosgraph_msgs::Clock clock_msg;
    clock_msg.clock = sim_time;
    this->clock_pub_.publish(clock_msg);
}

int
main(int argc, char** argv)
{
    if( argc < 2 )
    {
        puts(USAGE);
        exit(-1);
    }

    ros::init(argc, argv, "stageros");
    //here we subscribe to the odometry of the golfcar

    ros::NodeHandle nh;
    real_odom = nh.subscribe("/odom_merge", 5, set_position);

    bool gui = true;
    bool use_model_names = false;
    for(int i=0;i<(argc-1);i++)
    {
        if(!strcmp(argv[i], "-g"))
            gui = false;
        if(!strcmp(argv[i], "-u"))
            use_model_names = true;
    }

    StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);

    if(sn.SubscribeModels() != 0)
        exit(-1);

    boost::thread t = boost::thread(boost::bind(&ros::spin));

    sn.world->Start();

    Stg::World::Run();

    t.join();

    exit(0);
}