text:
//
// windows/basic_stream_handle.hpp
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
// Copyright (c) 2003-2011 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_ASIO_WINDOWS_BASIC_STREAM_HANDLE_HPP
#define BOOST_ASIO_WINDOWS_BASIC_STREAM_HANDLE_HPP

#if defined(_MSC_VER) && (_MSC_VER >= 1200)
# pragma once
#endif // defined(_MSC_VER) && (_MSC_VER >= 1200)

#include <boost/asio/detail/config.hpp>

#if defined(BOOST_ASIO_HAS_WINDOWS_STREAM_HANDLE) \
  || defined(GENERATING_DOCUMENTATION)

#include <cstddef>
#include <boost/asio/error.hpp>
#include <boost/asio/windows/basic_handle.hpp>
#include <boost/asio/windows/stream_handle_service.hpp>
#include <boost/asio/detail/throw_error.hpp>

#include <boost/asio/detail/push_options.hpp>

namespace boost {
namespace asio {
namespace windows {

/// Provides stream-oriented handle functionality.
/**
 * The windows::basic_stream_handle class template provides asynchronous and
 * blocking stream-oriented handle functionality.
 *
 * @par Thread Safety
 * @e Distinct @e objects: Safe.@n
 * @e Shared @e objects: Unsafe.
 *
 * @par Concepts:
 * AsyncReadStream, AsyncWriteStream, Stream, SyncReadStream, SyncWriteStream.
 */
template <typename StreamHandleService = stream_handle_service>
class basic_stream_handle
  : public basic_handle<StreamHandleService>
{
public:
  /// The native representation of a handle.
  typedef typename StreamHandleService::native_type native_type;

  /// Construct a basic_stream_handle without opening it.
  /**
   * This constructor creates a stream handle without opening it. The handle
   * needs to be opened and then connected or accepted before data can be sent
   * or received on it.
   *
   * @param io_service The io_service object that the stream handle will use to
   * dispatch handlers for any asynchronous operations performed on the handle.
   */
  explicit basic_stream_handle(boost::asio::io_service& io_service)
    : basic_handle<StreamHandleService>(io_service)
  {
  }

  /// Construct a basic_stream_handle on an existing native handle.
  /**
   * This constructor creates a stream handle object to hold an existing native
   * handle.
   *
   * @param io_service The io_service object that the stream handle will use to
   * dispatch handlers for any asynchronous operations performed on the handle.
   *
   * @param native_handle The new underlying handle implementation.
   *
   * @throws boost::system::system_error Thrown on failure.
   */
  basic_stream_handle(boost::asio::io_service& io_service,
      const native_type& native_handle)
    : basic_handle<StreamHandleService>(io_service, native_handle)
  {
  }

  /// Write some data to the handle.
  /**
   * This function is used to write data to the stream handle. The function call
   * will block until one or more bytes of the data has been written
   * successfully, or until an error occurs.
   *
   * @param buffers One or more data buffers to be written to the handle.
   *
   * @returns The number of bytes written.
   *
   * @throws boost::system::system_error Thrown on failure. An error code of
   * boost::asio::error::eof indicates that the connection was closed by the
   * peer.
   *
   * @note The write_some operation may not transmit all of the data to the
   * peer. Consider using the @ref write function if you need to ensure that
   * all data is written before the blocking operation completes.
   *
   * @par Example
   * To write a single data buffer use the @ref buffer function as follows:
   * @code
   * handle.write_some(boost::asio::buffer(data, size));
   * @endcode
   * See the @ref buffer documentation for information on writing multiple
   * buffers in one go, and how to use it with arrays, boost::array or
   * std::vector.
   */
  template <typename ConstBufferSequence>
  std::size_t write_some(const ConstBufferSequence& buffers)
  {
    boost::system::error_code ec;
    std::size_t s = this->service.write_some(this->implementation, buffers, ec);
    boost::asio::detail::throw_error(ec);
    return s;
  }

  /// Write some data to the handle.
  /**
   * This function is used to write data to the stream handle. The function call
   * will block until one or more bytes of the data has been written
   * successfully, or until an error occurs.
   *
   * @param buffers One or more data buffers to be written to the handle.
   *
   * @param ec Set to indicate what error occurred, if any.
   *
   * @returns The number of bytes written. Returns 0 if an error occurred.
   *
   * @note The write_some operation may not transmit all of the data to the
   * peer. Consider using the @ref write function if you need to ensure that
   * all data is written before the blocking operation completes.
   */
  template <typename ConstBufferSequence>
  std::size_t write_some(const ConstBufferSequence& buffers,
      boost::system::error_code& ec)
  {
    return this->service.write_some(this->implementation, buffers, ec);
  }

  /// Start an asynchronous write.
  /**
   * This function is used to asynchronously write data to the stream handle.
   * The function call always returns immediately.
   *
   * @param buffers One or more data buffers to be written to the handle.
   * Although the buffers object may be copied as necessary, ownership of the
   * underlying memory blocks is retained by the caller, which must guarantee
   * that they remain valid until the handler is called.
   *
   * @param handler The handler to be called when the write operation completes.
   * Copies will be made of the handler as required. The function signature of
   * the handler must be:
   * @code void handler(
   *   const boost::system::error_code& error, // Result of operation.
   *   std::size_t bytes_transferred           // Number of bytes written.
   * ); @endcode
   * Regardless of whether the asynchronous operation completes immediately or
   * not, the handler will not be invoked from within this function. Invocation
   * of the handler will be performed in a manner equivalent to using
   * boost::asio::io_service::post().
   *
   * @note The write operation may not transmit all of the data to the peer.
   * Consider using the @ref async_write function if you need to ensure that all
   * data is written before the asynchronous operation completes.
   *
   * @par Example
   * To write a single data buffer use the @ref buffer function as follows:
   * @code
   * handle.async_write_some(boost::asio::buffer(data, size), handler);
   * @endcode
   * See the @ref buffer documentation for information on writing multiple
   * buffers in one go, and how to use it with arrays, boost::array or
   * std::vector.
   */
  template <typename ConstBufferSequence, typename WriteHandler>
  void async_write_some(const ConstBufferSequence& buffers,
      WriteHandler handler)
  {
    this->service.async_write_some(this->implementation, buffers, handler);
  }

  /// Read some data from the handle.
  /**
   * This function is used to read data from the stream handle. The function
   * call will block until one or more bytes of data has been read successfully,
   * or until an error occurs.
   *
   * @param buffers One or more buffers into which the data will be read.
   *
   * @returns The number of bytes read.
   *
   * @throws boost::system::system_error Thrown on failure. An error code of
   * boost::asio::error::eof indicates that the connection was closed by the
   * peer.
   *
   * @note The read_some operation may not read all of the requested number of
   * bytes. Consider using the @ref read function if you need to ensure that
   * the requested amount of data is read before the blocking operation
   * completes.
   *
   * @par Example
   * To read into a single data buffer use the @ref buffer function as follows:
   * @code
   * handle.read_some(boost::asio::buffer(data, size));
   * @endcode
   * See the @ref buffer documentation for information on reading into multiple
   * buffers in one go, and how to use it with arrays, boost::array or
   * std::vector.
   */
  template <typename MutableBufferSequence>
  std::size_t read_some(const MutableBufferSequence& buffers)
  {
    boost::system::error_code ec;
    std::size_t s = this->service.read_some(this->implementation, buffers, ec);
    boost::asio::detail::throw_error(ec);
    return s;
  }

  /// Read some data from the handle.
  /**
   * This function is used to read data from the stream handle. The function
   * call will block until one or more bytes of data has been read successfully,
   * or until an error occurs.
   *
   * @param buffers One or more buffers into which the data will be read.
   *
   * @param ec Set to indicate what error occurred, if any.
   *
   * @returns The number of bytes read. Returns 0 if an error occurred.
   *
   * @note The read_some operation may not read all of the requested number of
   * bytes. Consider using the @ref read function if you need to ensure that
   * the requested amount of data is read before the blocking operation
   * completes.
   */
  template <typename MutableBufferSequence>
  std::size_t read_some(const MutableBufferSequence& buffers,
      boost::system::error_code& ec)
  {
    return this->service.read_some(this->implementation, buffers, ec);
  }

  /// Start an asynchronous read.
  /**
   * This function is used to asynchronously read data from the stream handle.
   * The function call always returns immediately.
   *
   * @param buffers One or more buffers into which the data will be read.
   * Although the buffers object may be copied as necessary, ownership of the
   * underlying memory blocks is retained by the caller, which must guarantee
   * that they remain valid until the handler is called.
   *
   * @param handler The handler to be called when the read operation completes.
   * Copies will be made of the handler as required. The function signature of
   * the handler must be:
   * @code void handler(
   *   const boost::system::error_code& error, // Result of operation.
   *   std::size_t bytes_transferred           // Number of bytes read.
   * ); @endcode
   * Regardless of whether the asynchronous operation completes immediately or
   * not, the handler will not be invoked from within this function. Invocation
   * of the handler will be performed in a manner equivalent to using
   * boost::asio::io_service::post().
   *
   * @note The read operation may not read all of the requested number of bytes.
   * Consider using the @ref async_read function if you need to ensure that the
   * requested amount of data is read before the asynchronous operation
   * completes.
   *
   * @par Example
   * To read into a single data buffer use the @ref buffer function as follows:
   * @code
   * handle.async_read_some(boost::asio::buffer(data, size), handler);
   * @endcode
   * See the @ref buffer documentation for information on reading into multiple
   * buffers in one go, and how to use it with arrays, boost::array or
   * std::vector.
   */
  template <typename MutableBufferSequence, typename ReadHandler>
  void async_read_some(const MutableBufferSequence& buffers,
      ReadHandler handler)
  {
    this->service.async_read_some(this->implementation, buffers, handler);
  }
};

} // namespace windows
} // namespace asio
} // namespace boost

#include <boost/asio/detail/pop_options.hpp>

#endif // defined(BOOST_ASIO_HAS_WINDOWS_STREAM_HANDLE)
       //   || defined(GENERATING_DOCUMENTATION)

#endif // BOOST_ASIO_WINDOWS_BASIC_STREAM_HANDLE_HPP

ext: hpp

max_stars_repo_name: nlchao/ofxBoost

max_issues_repo_name: ksundberg/boost-svn

max_forks_repo_name: ksundberg/boost-svn


text:
#include "sensor.hpp"

#include <boost/container/flat_map.hpp>
#include <phosphor-logging/log.hpp>
#include <sdbusplus/asio/property.hpp>
#include <sdbusplus/bus/match.hpp>

#include <functional>

Sensor::Sensor(interfaces::Sensor::Id sensorId, boost::asio::io_context& ioc,
               const std::shared_ptr<sdbusplus::asio::connection>& bus) :
    sensorId(std::move(sensorId)),
    ioc(ioc), bus(bus)
{}

Sensor::Id Sensor::makeId(std::string_view service, std::string_view path)
{
    return Id("Sensor", service, path);
}

Sensor::Id Sensor::id() const
{
    return sensorId;
}

void Sensor::async_read()
{
    uniqueCall([this](auto lock) { async_read(std::move(lock)); });
}

void Sensor::async_read(std::shared_ptr<utils::UniqueCall::Lock> lock)
{
    makeSignalMonitor();

    sdbusplus::asio::getProperty<double>(
        *bus, sensorId.service, sensorId.path,
        "xyz.openbmc_project.Sensor.Value", "Value",
        [lock, id = sensorId, weakSelf = weak_from_this()](
            boost::system::error_code ec, double newValue) {
            if (ec)
            {
                phosphor::logging::log<phosphor::logging::level::WARNING>(
                    "DBus 'GetProperty' call failed on Sensor Value",
                    phosphor::logging::entry("SENSOR_PATH=%s", id.path.c_str()),
                    phosphor::logging::entry("ERROR_CODE=%d", ec.value()));
                return;
            }
            if (auto self = weakSelf.lock())
            {
                self->updateValue(newValue);
            }
        });
}

void Sensor::registerForUpdates(
    const std::weak_ptr<interfaces::SensorListener>& weakListener)
{
    listeners.erase(
        std::remove_if(listeners.begin(), listeners.end(),
                       [](const auto& listener) { return listener.expired(); }),
        listeners.end());

    if (auto listener = weakListener.lock())
    {
        listeners.emplace_back(weakListener);

        if (value)
        {
            listener->sensorUpdated(*this, timestamp, *value);
        }
        else
        {
            async_read();
        }
    }
}

void Sensor::updateValue(double newValue)
{
    timestamp = std::time(0);

    if (value == newValue)
    {
        for (const auto& weakListener : listeners)
        {
            if (auto listener = weakListener.lock())
            {
                listener->sensorUpdated(*this, timestamp);
            }
        }
    }
    else
    {
        value = newValue;

        for (const auto& weakListener : listeners)
        {
            if (auto listener = weakListener.lock())
            {
                listener->sensorUpdated(*this, timestamp, *value);
            }
        }
    }
}

void Sensor::makeSignalMonitor()
{
    if (signalMonitor)
    {
        return;
    }

    using namespace std::string_literals;

    const auto param = "type='signal',member='PropertiesChanged',path='"s +
                       sensorId.path +
                       "',arg0='xyz.openbmc_project.Sensor.Value'"s;

    signalMonitor = std::make_unique<sdbusplus::bus::match::match>(
        *bus, param,
        [weakSelf = weak_from_this()](sdbusplus::message::message& message) {
            signalProc(weakSelf, message);
        });
}

void Sensor::signalProc(const std::weak_ptr<Sensor>& weakSelf,
                        sdbusplus::message::message& message)
{
    if (auto self = weakSelf.lock())
    {
        std::string iface;
        boost::container::flat_map<std::string, ValueVariant>
            changed_properties;
        std::vector<std::string> invalidated_properties;

        message.read(iface, changed_properties, invalidated_properties);

        if (iface == "xyz.openbmc_project.Sensor.Value")
        {
            const auto it = changed_properties.find("Value");
            if (it != changed_properties.end())
            {
                if (auto val = std::get_if<double>(&it->second))
                {
                    self->updateValue(*val);
                }
                else
                {
                    phosphor::logging::log<phosphor::logging::level::ERR>(
                        "Failed to receive Value from Sensor "
                        "PropertiesChanged signal",
                        phosphor::logging::entry("SENSOR_PATH=%s",
                                                 self->sensorId.path.c_str()));
                }
            }
        }
    }
}

ext: cpp

max_stars_repo_name: lxwinspur/telemetry

max_issues_repo_name: lxwinspur/telemetry

max_forks_repo_name: lxwinspur/telemetry


text:
////////////////////////////////////////////////////////////////////////////////
// Copyright 2018 NAVER Corp.
//
// 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.
////////////////////////////////////////////////////////////////////////////////
/**
 * add main process test 03/16
 *
 * */

#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "log.h"
#include "memory_pool.h"
#include "utility.h"
#include "pp_test_utility.h"

#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/thread/condition.hpp>
#include <boost/pool/singleton_pool.hpp>
#include <list>
#include <stack>
#include <stddef.h>
#include <iostream>
using std::list;
using std::stack;

typedef struct _M_PSTest
{
    Pinpoint::PSMutexLock gMutex;
    char buf[1024];
} M_PSTest;

typedef struct
{
  volatile   int a;
  volatile   int b;
} TT;

#define TTLOOPTIME 100000

TEST(utility, double_VS_long)
{
    using namespace Pinpoint::utils;
    srand(12);
    for (int i = 0; i < 10000; i++) {
        int64_t val = random();
        EXPECT_EQ(val, double_to_long_bits(long_bits_to_double(val)));
    }
}

//@deprecated
void process_main(TFlag iFlag)
{
    using namespace boost::interprocess;
    M_PSTest* pMtps;

//    if (iFlag.Flag.RmoveMem) {
//        LOGI("remove mem %d ", shared_memory_object::remove("MySharedMemory"));
//    }

    shared_memory_object shm
            (open_or_create //only create
            , "MySharedMemory" //name
            , read_write //read-write mode
            );

    offset_t size;
    shm.get_size(size);
    if (size == 0) // must a new  shm
    {
        shm.truncate(sizeof (M_PSTest));
    }

    LOGI("MySharedMemory size %d", size);

    mapped_region region
            (shm //What to map
            , read_write //Map it as read-write
            );
    void * addr = region.get_address();
    if (size == 0) { // new
        pMtps = new (addr) M_PSTest;
    } else { // old
        pMtps = static_cast<M_PSTest*> (addr);
    }
    int i = 100;
    while (i--) {
        int err = pMtps->gMutex.try_lock(1000);
        if (err == 0) {
            LOGI("get lock");
            //        sleep(1);
            if (iFlag.Flag.Seg) {
                *(int*) 0 = 10;
            }
            pMtps->gMutex.unlock();
            LOGI("free lock");
        } else {
            LOGI("get lock failed !!!");
        }
    }

    if (iFlag.Flag.RmoveMem)
        shared_memory_object::remove("MySharedMemory");
}

void process_test_share_memory(TFlag iFlag)
{
	Pinpoint::memory::SafeShareMemory<TT> sSm("test1");
    TT * tt = sSm.getData();
    int i = TTLOOPTIME;
    while (i--) {
        sSm.getWriteLock().lock();
        tt->a++;
        if (iFlag.Flag.Seg) {
            MAKE_SEG_ERROR();
        }
        tt->b++;
        sSm.getWriteLock().unlock();

        // test try_lock()
        if (sSm.getWriteLock().try_lock(10) == 0) {
            sSm.getWriteLock().unlock();
        }

    }
}

TEST(test, SafeShareMemory)
{
    using namespace boost::interprocess;

    TFlag tf = {0};
    tf.Flag.Seg = 0;
    // make sure the test1 is exist
    Pinpoint::memory::SafeShareMemory<TT> sSm("test1");
    sleep(1);
    fork_and_execute(process_test_share_memory, tf);

    tf.Flag.Seg = 1;
    fork_and_execute(process_test_share_memory, tf);
    tf.Flag.Seg = 0;
    fork_and_execute(process_test_share_memory, tf);

    WAIT_CHILDREN();

    TT* tt = sSm.getData();

    EXPECT_EQ(tt->a, TTLOOPTIME * 2 + 1);

    EXPECT_EQ(tt->b, TTLOOPTIME * 2);

    LOGI("remove mem %d ", shared_memory_object::remove("test1"));
}


text:
//  Copyright 2018 Stephan Menzel. 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)

#include "mredis/AsyncClient.hpp"

#include "tools/Log.hpp"
#include "tools/Random.hpp"

#include <boost/config.hpp>
#include <boost/thread.hpp>
#include <boost/asio.hpp>
#include <boost/chrono.hpp>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>

#ifndef _WIN32
#include <sys/resource.h>
#endif

#include <iostream>
#include <cstdlib>

#define EASY_NUM_SUBSCRIBERS 1

using namespace moose::mredis;
using namespace moose::tools;

/* This tries to be a multithreaded stress test for mredis.

   Aim is to spot race conditions and problems by having a scenario
   with multiple threads using a single asyncclient instance with
   some being subsub and some pushing

 */

//! subscribe to one value, read integers and assume they will increase gradually
//! occasionally re-subscribe
class EasySubscriberThread {

	public:
		EasySubscriberThread(AsyncClient &n_redis)
				: m_error(false)
				, m_stopped(false)
				, m_redis(n_redis) {
		}

		void stop() noexcept {

			m_stopped.store(true);
		}

		void operator()() noexcept {

			try {
				boost::this_thread::sleep_for(boost::chrono::milliseconds(10));

				while (!m_stopped) {

					m_subscription = m_redis.subscribe("easy_int_test", [this] (const std::string &n_message) {

						boost::int64_t current = boost::lexical_cast<boost::int64_t>(n_message);

						if (current >= m_last) {
							m_last = current;
						} else {
							m_error.store(true);
						}
					});

					// sleep for a random number of seconds, while subscriptions lasts
					boost::this_thread::sleep_for(boost::chrono::seconds(urand(5) + 1));

					m_redis.unsubscribe(m_subscription);
				}
			} catch (const moose_error &merr) {
				std::cerr << "Exception caught by EasySubscriberThread: " << boost::diagnostic_information(merr) << std::endl;
				m_error.store(true);
			}
		}

		boost::atomic<bool>  m_error;          //!< once any error indication is met, this turns true
		boost::int64_t       m_last = 0;

	private:
		boost::uint64_t      m_subscription = 0;
		boost::atomic<bool>  m_stopped;
		AsyncClient         &m_redis;
};

//! increase a value and publish
class EasyPublisherThread {

	public:
		EasyPublisherThread(AsyncClient &n_redis)
				: m_error(false)
				, m_stopped(false)
				, m_redis(n_redis) {
		}

		void stop() noexcept {

			m_stopped.store(true);
		}

		void operator()() noexcept {

			try {
				boost::int64_t       current = 0;

				boost::this_thread::sleep_for(boost::chrono::milliseconds(10));

				while (!m_stopped) {

					future_response r = m_redis.publish("easy_int_test", boost::lexical_cast<std::string>(current));
					current++;

					// Sometimes I wait for the response, sometimes not
					if (urand(1) == 1) {
						// wait for the response
						RedisMessage res = r.get();

						if (!is_int(res)) {
							m_error.store(true);
							std::cerr << "Response to publish is not an int: " << res.which() << std::endl;
							return;
						}

						boost::int64_t i = boost::get<boost::int64_t>(res);

						if (i < 0 || i > EASY_NUM_SUBSCRIBERS) {
							m_error.store(true);
							std::cerr << "Published message claims to have been received by: " << i << std::endl;
							return;
						}
					} else {
						boost::this_thread::sleep_for(boost::chrono::milliseconds(urand(5)));
					}

				}
			} catch (const moose_error &merr) {
				std::cerr << "Exception caught by EasyPublisherThread: " << boost::diagnostic_information(merr) << std::endl;
				m_error.store(true);
			}
		}

		boost::atomic<bool>  m_error;    //!< once any error indication is met, this turns true

	private:
		boost::atomic<bool>  m_stopped;
		AsyncClient         &m_redis;
};

int main(int argc, char **argv) {

#ifndef _WIN32
	// core dumps may be disallowed by parent of this process; change that
	struct rlimit core_limits;
	core_limits.rlim_cur = core_limits.rlim_max = RLIM_INFINITY;
	setrlimit(RLIMIT_CORE, &core_limits);
#endif

	moose::tools::init_logging();

	namespace po = boost::program_options;

	po::options_description desc("redis test options");
	desc.add_options()
		("help,h", "Print this help message")
// 		("port,p", po::value<short>()->default_value(7777), "use server port")
		("server,s", po::value<std::string>()->default_value("127.0.0.1"), "give redis server ip");

	try {
		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

		if (vm.count("help")) {
			std::cout << "Behold your options!\n";
			std::cout << desc << std::endl;
			return EXIT_SUCCESS;
		}

// 		if (!(vm.count("port") && vm.count("server"))) {
// 			std::cerr << "Insufficient parameters" << std::endl;
// 			std::cout << desc << std::endl;
// 			return EXIT_FAILURE;
// 		}
//
		const std::string server_ip_string = vm["server"].as<std::string>();
// 		const short port = vm["port"].as<short>();

		// Testcase 1 - Easy
		{
			std::cout << "Running Testcase 1 - Easy mode..." << std::endl;

			AsyncClient client(server_ip_string);
		//	client.connect();

			boost::shared_future<bool> ret = client.async_connect();
			const bool r = ret.get();
			if (!r) {
				std::cerr << "Cannot connect" << std::endl;
				return EXIT_FAILURE;
			};

			// Let's start simple by having one thread that publishes an increasing number and three that listen
			boost::thread_group testers;

			EasyPublisherThread  easy_publisher_1(client);
			EasySubscriberThread easy_subscriber_1(client);
			EasySubscriberThread easy_subscriber_2(client);
// 			EasySubscriberThread easy_subscriber_3(client);

			testers.create_thread(boost::ref(easy_publisher_1));
			testers.create_thread(boost::ref(easy_subscriber_1));
			testers.create_thread(boost::ref(easy_subscriber_2));
// 			testers.create_thread(boost::ref(easy_subscriber_3));

			// Let it run for 30 seconds
			boost::this_thread::sleep_for(boost::chrono::seconds(10));

			// Shut it down
			std::cout << "10 seconds running, shutting down..." << std::endl;

			easy_publisher_1.stop();
			if (easy_publisher_1.m_error) {
				std::cerr << "Error condition in publisher 1" << std::endl;
			}

			easy_subscriber_1.stop();
			if (easy_subscriber_1.m_error) {
				std::cerr << "Error condition in subscriber 1" << std::endl;
			}

			easy_subscriber_2.stop();
			if (easy_subscriber_2.m_error) {
				std::cerr << "Error condition in subscriber 2" << std::endl;
			}

			if (easy_subscriber_1.m_last != easy_subscriber_2.m_last) {
				std::cerr << "All subscribers should have the same end result" << std::endl;
			}

			std::cout << "Subscriber 1 at " << easy_subscriber_1.m_last << std::endl;

// 			easy_subscriber_3.stop();
			testers.join_all();

			std::cout << "Testcase 1 finished. Shutting down..." << std::endl;
		}

		std::cout << "done" << std::endl;

		return EXIT_SUCCESS;

	} catch (const std::exception &sex) {
		std::cerr << "Unexpected exception reached main: " << sex.what() << std::endl;
	} catch (...) {
		std::cerr << "Unhandled error reached main function. Aborting" << std::endl;
	}

	return EXIT_FAILURE;
}

max_stars_repo_name: MrMoose/mredis

max_issues_repo_name: MrMoose/mredis

max_forks_repo_name: MrMoose/mredis


text:
#include "donation.h"
#include "util.h"
#include "main.h"
#include <math.h>
#include <boost/cstdint.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include "wallet.h"
#include "walletdb.h"
#include "bitcoinrpc.h"
#include "init.h"
#include "base58.h"

using namespace json_spirit;
using namespace std;

std::map<uint256,double> ConfirmedBlocksWaitingOnDonate;

double CalcDonationAmount()
{
    double nDonAmnt = 0;

    double nPercent = nDonatePercent;
    if (nPercent < 0.0)
    {
        nPercent = 0.0;
    }
    else if (nPercent > 100.0)
    {
        nPercent = 100.0;
    }
    double dbPDV = (double)PDV;

    nDonAmnt =  ((dbPDV - MIN_TX_FEE )* (nPercent * 0.01));  // takes the amount that was earned and removes tx fee before calcing percent
    // this ensures that at even 100% donation, the user isnt slowly losing coins.

    return nDonAmnt;
}

std::string getUsableAddress(double amountRequired)
{
    int nMinDepth = 1000;
    std::string sAccount;
    map<string, int64_t> mapAccountBalances;
    BOOST_FOREACH(const PAIRTYPE(CTxDestination, string)& entry, pwalletMain->mapAddressBook)
    {
        if (IsMine(*pwalletMain, entry.first)) // This address belongs to me
        {
            mapAccountBalances[entry.second] = 0;
        }
    }
    for (map<uint256, CWalletTx>::iterator it = pwalletMain->mapWallet.begin(); it != pwalletMain->mapWallet.end(); ++it)
    {
        const CWalletTx& wtx = (*it).second;
        int64_t nFee;
        string strSentAccount;
        list<pair<CTxDestination, int64_t> > listReceived;
        list<pair<CTxDestination, int64_t> > listSent;
        int nDepth = wtx.GetDepthInMainChain();
        if (nDepth < 0)
        {
            continue;
        }
        wtx.GetAmounts(listReceived, listSent, nFee, strSentAccount);
        mapAccountBalances[strSentAccount] -= nFee;
        BOOST_FOREACH(const PAIRTYPE(CTxDestination, int64_t)& s, listSent)
            mapAccountBalances[strSentAccount] -= s.second;
        if (nDepth >= nMinDepth && wtx.GetBlocksToMaturity() == 0)
        {
            BOOST_FOREACH(const PAIRTYPE(CTxDestination, int64_t)& r, listReceived)
            {
                if (pwalletMain->mapAddressBook.count(r.first))
                {
                    mapAccountBalances[pwalletMain->mapAddressBook[r.first]] += r.second;
                }
                else
                {
                    mapAccountBalances[""] += r.second;
                }
            }
        }
    }
    list<CAccountingEntry> acentries;
    CWalletDB(pwalletMain->strWalletFile).ListAccountCreditDebit("*", acentries);
    BOOST_FOREACH(const CAccountingEntry& entry, acentries)
    {
        mapAccountBalances[entry.strAccount] += entry.nCreditDebit;
    }

    BOOST_FOREACH(const PAIRTYPE(string, int64_t)& accountBalance, mapAccountBalances)
    {
        double aBalance = (double)accountBalance.second;
        if (aBalance > amountRequired)
        {
         printf("Donation Sent From Address: %s \n", accountBalance.first.c_str());
         sAccount = accountBalance.first;
         break;
        }
    }

    CWalletDB walletdb(pwalletMain->strWalletFile);
    CAccount account;
    walletdb.ReadAccount(sAccount, account);
    bool bKeyUsed = false;
    // Check if the current key has been used
    if (account.vchPubKey.IsValid())
    {
        CScript scriptPubKey;
        scriptPubKey.SetDestination(account.vchPubKey.GetID());
        for (map<uint256, CWalletTx>::iterator it = pwalletMain->mapWallet.begin(); it != pwalletMain->mapWallet.end() && account.vchPubKey.IsValid(); ++it)
        {
            const CWalletTx& wtx = (*it).second;
            BOOST_FOREACH(const CTxOut& txout, wtx.vout)
                if (txout.scriptPubKey == scriptPubKey)
                    bKeyUsed = true;
        }
    }
    // Generate a new key
    if (!account.vchPubKey.IsValid() || bKeyUsed)
    {
        if (!pwalletMain->GetKeyFromPool(account.vchPubKey, false))
            throw JSONRPCError(RPC_WALLET_KEYPOOL_RAN_OUT, "Error: Keypool ran out, please call keypoolrefill first");

        pwalletMain->SetAddressBookName(account.vchPubKey.GetID(), sAccount);
        walletdb.WriteAccount(sAccount, account);
    }
    return CBitcoinAddress(account.vchPubKey.GetID()).ToString();
}

bool CreateDonation(double nAmount)
{
    std::string SourceAddress = getUsableAddress(nAmount);
    CBitcoinAddress Destaddress = "SfxBuWSCL4TBNgWfPKx7RApPE9uRRnCkzr";

    //tx comnments and such
    CWalletTx wtx;
    wtx.strFromAccount = SourceAddress;
    wtx.mapValue["comment"] = "Donation to SuperCoin dev";
    wtx.mapValue["to"] = Destaddress.ToString().c_str();

    if (!Destaddress.IsValid())
    {
        printf("Error Donating: Trying to donate to invalid address \n");
        return false;
    }

    if (pwalletMain->IsLocked())
    {
        printf("Error Donating: Wallet is locked, unable to donate \n");
        return false;
    }

    // Send
    std::string strError = pwalletMain->SendMoneyToDestination(Destaddress.Get(), nAmount, wtx);
    if (strcmp(strError.c_str(), "") != 0)
    {
        printf("Error Donating: %s \n", strError.c_str());
        return false;
    }
    std::string DonationTx = wtx.GetHash().GetHex();
    printf("Donation Transaction ID = %s \n", DonationTx.c_str());
    return true;
}

void CheckForStakedBlock()
{
    if(ConfirmedBlocksWaitingOnDonate.size() == 0)
    {
        return;
    }

    string strAccount = "*";
    int nCount = 100;
    int i = 0;
    int j = 0;
    int k = 0;
    double nAmount = 0;
    std::vector<uint256> ret;
    std::vector<uint256> used;
    std::map<uint256,double>::iterator iter;
    std::map<uint256,double>::iterator DeleteEntry;
    std::list<CAccountingEntry> acentries;
    list<pair<CTxDestination, int64_t> > LR;
    list<pair<CTxDestination, int64_t> > LS;

    CWallet::TxItems txOrdered = pwalletMain->OrderedTxItems(acentries, strAccount);

    // iterate backwards until we have nCount items to return:
    for (CWallet::TxItems::reverse_iterator it = txOrdered.rbegin(); it != txOrdered.rend(); ++it)
    {
        CWalletTx *const pwtx = (*it).second.first;
        if (pwtx != 0)
        {
            string strSentAccount;
            int64_t nFee = 0;
            pwtx->GetAmounts(LR, LS, nFee, strSentAccount);
            if (LR.size() > 0)
            {
                if(pwtx->GetBlocksToMaturity() == 0)
                {
                    ret.push_back(pwtx->GetHash());
                }
            }
        }
        if ((int)ret.size() >= (nCount))
        {
            break;
        }
    }

    for(i = 0; i < ret.size(); ++i)
    {
       iter = ConfirmedBlocksWaitingOnDonate.find(ret[i]);
       if(iter != ConfirmedBlocksWaitingOnDonate.end())
       {
           used.push_back(ret[i]);
           nAmount = nAmount + iter->second;
       }
    }

    for(k = 0; k < used.size(); ++k)
    {
        DeleteEntry = ConfirmedBlocksWaitingOnDonate.find(used[j]);
        ConfirmedBlocksWaitingOnDonate.erase(DeleteEntry);
    }

    bool success = CreateDonation(nAmount);
    if(success == true)
    {
        printf("Donation Success \n");
    }
    else
    {
        printf("Donation Faled \n");
    }
}


text:
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
using adept::adouble;

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#include "fft.h"

void foobar(double* data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

void afoobar(aVector& data, unsigned len) {
  fft(data, len);
  ifft(data, len);
}

extern "C" {
  int enzyme_dupnoneed;
}

static double foobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    __enzyme_autodiff<void>(foobar, enzyme_dupnoneed, inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static double afoobar_and_gradient(unsigned len) {
    adept::Stack stack;

    aVector x(2*len);
    for(int i=0; i<2*len; i++) x(i) = 2.0;
    stack.new_recording();
    afoobar(x, len);
    for(int i=0; i<2*len; i++)
      x(i).set_gradient(1.0);
    stack.compute_adjoint();

    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++)
      dinp[i] = x(i).get_gradient();
    double res = dinp[0];
    delete[] dinp;
    return res;
}

static double tfoobar_and_gradient(unsigned len) {
    double *inp = new double[2*len];
    for(int i=0; i<2*len; i++) inp[i] = 2.0;
    double *dinp = new double[2*len];
    for(int i=0; i<2*len; i++) dinp[i] = 1.0;
    foobar_b(inp, dinp, len);
    double res = dinp[0];
    delete[] dinp;
    delete[] inp;
    return res;
}

static void adept_sincos(double inp, unsigned len) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector x(2*len);
  for(int i=0; i<2*len; i++) x[i] = 2.0;
 // stack.new_recording();
  afoobar(x, len);
  double res = x(0).value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = afoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void tapenade_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double* x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Tapenade forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = tfoobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Tapenade combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

static void enzyme_sincos(double inp, unsigned len) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double *x = new double[2*len];
  for(int i=0; i<2*len; i++) x[i] = 2.0;
  foobar(x, len);
  double res = x[0];

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  delete[] x;
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res2 = foobar_and_gradient(len);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

/* Function to check if x is power of 2*/
bool isPowerOfTwo (int x)
{
    /* First x in the below expression is for the case when x is 0 */
    return x && (!(x&(x-1)));
}

unsigned max(unsigned A, unsigned B){
  if (A>B) return A;
  return B;
}

int main(int argc, char** argv) {

  if (argc < 2) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  unsigned N = atoi(argv[1]);
  if (!isPowerOfTwo(N)) {
    printf("usage %s n [must be power of 2]\n", argv[0]);
    return 1;
  }
  double inp = -2.1;

  for(unsigned iters=max(1, N>>5); iters <= N; iters*=2) {
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    tapenade_sincos(inp, iters);
    enzyme_sincos(inp, iters);
  }
}

max_stars_repo_name: anandijain/Enzyme

max_issues_repo_name: anandijain/Enzyme

max_forks_repo_name: anandijain/Enzyme


text:
// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>          // get kdl tree from urdf
#include <kdl/chaindynparam.hpp>              // inverse dynamics

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

#define PI 3.141592
#define D2R PI / 180.0
#define R2D 180.0 / PI
#define SaveDataMax 49

namespace arm_controllers
{
class Computed_Torque_Controller : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
  public:
    bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
    {
        // ********* 1. Get joint name / gain from the parameter server *********
        // 1.1 Joint Name
        if (!n.getParam("joints", joint_names_))
        {
            ROS_ERROR("Could not find joint name");
            return false;
        }
        n_joints_ = joint_names_.size();

        if (n_joints_ == 0)
        {
            ROS_ERROR("List of joint names is empty.");
            return false;
        }
        else
        {
            ROS_INFO("Found %d joint names", n_joints_);
            for (int i = 0; i < n_joints_; i++)
            {
                ROS_INFO("%s", joint_names_[i].c_str());
            }
        }

        // 1.2 Gain
        // 1.2.1 Joint Controller
        Kp_.resize(n_joints_);
        Kd_.resize(n_joints_);
        Ki_.resize(n_joints_);

        std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);
        for (size_t i = 0; i < n_joints_; i++)
        {
            std::string si = boost::lexical_cast<std::string>(i + 1);
            if (n.getParam("/elfin/computed_torque_controller/gains/elfin_joint" + si + "/pid/p", Kp[i]))
            {
                Kp_(i) = Kp[i];
            }
            else
            {
                std::cout << "/elfin/computed_torque_controller/gains/elfin_joint" + si + "/pid/p" << std::endl;
                ROS_ERROR("Cannot find pid/p gain");
                return false;
            }

            if (n.getParam("/elfin/computed_torque_controller/gains/elfin_joint" + si + "/pid/i", Ki[i]))
            {
                Ki_(i) = Ki[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/i gain");
                return false;
            }

            if (n.getParam("/elfin/computed_torque_controller/gains/elfin_joint" + si + "/pid/d", Kd[i]))
            {
                Kd_(i) = Kd[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/d gain");
                return false;
            }
        }

        // 2. ********* urdf *********
        urdf::Model urdf;
        if (!urdf.initParam("robot_description"))
        {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }
        else
        {
            ROS_INFO("Found robot_description");
        }

        // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
        for (int i = 0; i < n_joints_; i++)
        {
            try
            {
                joints_.push_back(hw->getHandle(joint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException &e)
            {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

            urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
            if (!joint_urdf)
            {
                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                return false;
            }
            joint_urdfs_.push_back(joint_urdf);
        }

        // 4. ********* KDL *********
        // 4.1 kdl parser
        if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
        {
            ROS_ERROR("Failed to construct kdl tree");
            return false;
        }
        else
        {
            ROS_INFO("Constructed kdl tree");
        }

        // 4.2 kdl chain
        std::string root_name, tip_name;
        if (!n.getParam("root_link", root_name))
        {
            ROS_ERROR("Could not find root link name");
            return false;
        }
        if (!n.getParam("tip_link", tip_name))
        {
            ROS_ERROR("Could not find tip link name");
            return false;
        }
        if (!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
        {
            ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
            ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name);
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfSegments() << " segments");
            ROS_ERROR_STREAM("  The segments are:");

            KDL::SegmentMap segment_map = kdl_tree_.getSegments();
            KDL::SegmentMap::iterator it;

            for (it = segment_map.begin(); it != segment_map.end(); it++)
                ROS_ERROR_STREAM("    " << (*it).first);

            return false;
        }
        else
        {
            ROS_INFO("Got kdl chain");
        }

        // 4.3 inverse dynamics solver 초기화
        gravity_ = KDL::Vector::Zero(); // ?
        gravity_(2) = -9.81;            // 0: x-axis 1: y-axis 2: z-axis

        id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));

        // ********* 5. 각종 변수 초기화 *********

        // 5.1 Vector 초기화 (사이즈 정의 및 값 0)
        tau_d_.data = Eigen::VectorXd::Zero(n_joints_);

        qd_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

        q_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_.data = Eigen::VectorXd::Zero(n_joints_);

        e_.data = Eigen::VectorXd::Zero(n_joints_);
        e_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        e_int_.data = Eigen::VectorXd::Zero(n_joints_);

        // 5.2 Matrix 초기화 (사이즈 정의 및 값 0)
        M_.resize(kdl_chain_.getNrOfJoints());
        C_.resize(kdl_chain_.getNrOfJoints());
        G_.resize(kdl_chain_.getNrOfJoints());

        // ********* 6. ROS 명령어 *********
        // 6.1 publisher
        pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
        pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
        pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

        pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000); // 뒤에 숫자는?

        // 6.2 subsriber

        return true;
    }

    void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
    {
        if (msg->data.size() != n_joints_)
        {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
            return;
        }
    }

    void starting(const ros::Time &time)
    {
        t = 0.0;
        ROS_INFO("Starting Computed Torque Controller");
    }

    void update(const ros::Time &time, const ros::Duration &period)
    {
        // ********* 0. Get states from gazebo *********
        // 0.1 sampling time
        double dt = period.toSec();
        t = t + 0.001;

        // 0.2 joint state
        for (int i = 0; i < n_joints_; i++)
        {
            q_(i) = joints_[i].getPosition();
            qdot_(i) = joints_[i].getVelocity();
        }

        // ********* 1. Desired Trajecoty in Joint Space *********

        for (size_t i = 0; i < n_joints_; i++)
        {
            qd_ddot_(i) = -M_PI * M_PI / 4 * 45 * KDL::deg2rad * sin(M_PI / 2 * t);
            qd_dot_(i) = M_PI / 2 * 45 * KDL::deg2rad * cos(M_PI / 2 * t);
            qd_(i) = 45 * KDL::deg2rad * sin(M_PI / 2* t);
        }

        // ********* 2. Motion Controller in Joint Space*********
        // *** 2.1 Error Definition in Joint Space ***
        e_.data = qd_.data - q_.data;
        e_dot_.data = qd_dot_.data - qdot_.data;
        e_int_.data = qd_.data - q_.data; // (To do: e_int 업데이트 필요요)

        // *** 2.2 Compute model(M,C,G) ***
        id_solver_->JntToMass(q_, M_);
        id_solver_->JntToCoriolis(q_, qdot_, C_);
        id_solver_->JntToGravity(q_, G_);

        // *** 2.3 Apply Torque Command to Actuator ***
        // ISHIRA: Stabilizing Linear Control
        aux_d_.data = M_.data * (qd_ddot_.data + Kp_.data.cwiseProduct(e_.data) + Kd_.data.cwiseProduct(e_dot_.data));
        // ISHIRA: n(q, qdot)
        comp_d_.data = C_.data + G_.data;
        // ISHIRA: Nonlinear Compensation and Decoupling
        tau_d_.data = aux_d_.data + comp_d_.data;

        // ISHIRA: Manipulation
        for (int i = 0; i < n_joints_; i++)
        {
            joints_[i].setCommand(tau_d_(i));
            // joints_[i].setCommand(0.0);
        }

        // ********* 3. data 저장 *********
        save_data();

        // ********* 4. state 출력 *********
        print_state();
    }

    void stopping(const ros::Time &time)
    {
    }

    void save_data()
    {
        // 1
        // Simulation time (unit: sec)
        SaveData_[0] = t;

        // Desired position in joint space (unit: rad)
        SaveData_[1] = qd_(0);
        SaveData_[2] = qd_(1);
        SaveData_[3] = qd_(2);
        SaveData_[4] = qd_(3);
        SaveData_[5] = qd_(4);
        SaveData_[6] = qd_(5);

        // Desired velocity in joint space (unit: rad/s)
        SaveData_[7] = qd_dot_(0);
        SaveData_[8] = qd_dot_(1);
        SaveData_[9] = qd_dot_(2);
        SaveData_[10] = qd_dot_(3);
        SaveData_[11] = qd_dot_(4);
        SaveData_[12] = qd_dot_(5);

        // Desired acceleration in joint space (unit: rad/s^2)
        SaveData_[13] = qd_ddot_(0);
        SaveData_[14] = qd_ddot_(1);
        SaveData_[15] = qd_ddot_(2);
        SaveData_[16] = qd_ddot_(3);
        SaveData_[17] = qd_ddot_(4);
        SaveData_[18] = qd_ddot_(5);

        // Actual position in joint space (unit: rad)
        SaveData_[19] = q_(0);
        SaveData_[20] = q_(1);
        SaveData_[21] = q_(2);
        SaveData_[22] = q_(3);
        SaveData_[23] = q_(4);
        SaveData_[24] = q_(5);

        // Actual velocity in joint space (unit: rad/s)
        SaveData_[25] = qdot_(0);
        SaveData_[26] = qdot_(1);
        SaveData_[27] = qdot_(2);
        SaveData_[28] = qdot_(3);
        SaveData_[29] = qdot_(4);
        SaveData_[30] = qdot_(5);

        // Error position in joint space (unit: rad)
        SaveData_[31] = e_(0);
        SaveData_[32] = e_(1);
        SaveData_[33] = e_(2);
        SaveData_[34] = e_(3);
        SaveData_[35] = e_(4);
        SaveData_[36] = e_(5);

        // Error velocity in joint space (unit: rad/s)
        SaveData_[37] = e_dot_(0);
        SaveData_[38] = e_dot_(1);
        SaveData_[39] = e_dot_(3);
        SaveData_[40] = e_dot_(4);
        SaveData_[41] = e_dot_(5);
        SaveData_[42] = e_dot_(6);

        // Error intergal value in joint space (unit: rad*sec)
        SaveData_[43] = e_int_(0);
        SaveData_[44] = e_int_(1);
        SaveData_[45] = e_int_(2);
        SaveData_[46] = e_int_(3);
        SaveData_[47] = e_int_(4);
        SaveData_[48] = e_int_(5);

        // 2
        msg_qd_.data.clear();
        msg_q_.data.clear();
        msg_e_.data.clear();

        msg_SaveData_.data.clear();

        // 3
        for (int i = 0; i < n_joints_; i++)
        {
            msg_qd_.data.push_back(qd_(i));
            msg_q_.data.push_back(q_(i));
            msg_e_.data.push_back(e_(i));
        }

        for (int i = 0; i < SaveDataMax; i++)
        {
            msg_SaveData_.data.push_back(SaveData_[i]);
        }

        // 4
        pub_qd_.publish(msg_qd_);
        pub_q_.publish(msg_q_);
        pub_e_.publish(msg_e_);

        pub_SaveData_.publish(msg_SaveData_);
    }

    void print_state()
    {
        static int count = 0;
        if (count > 99)
        {
            printf("*********************************************************\n\n");
            printf("*** Simulation Time (unit: sec)  ***\n");
            printf("t = %f\n", t);
            printf("\n");

            printf("*** Desired State in Joint Space (unit: deg) ***\n");
            printf("qd_(0): %f, ", qd_(0)*R2D);
            printf("qd_(1): %f, ", qd_(1)*R2D);
            printf("qd_(2): %f, ", qd_(2)*R2D);
            printf("qd_(3): %f, ", qd_(3)*R2D);
            printf("qd_(4): %f, ", qd_(4)*R2D);
            printf("qd_(5): %f\n", qd_(5)*R2D);
            printf("\n");

            printf("*** Actual State in Joint Space (unit: deg) ***\n");
            printf("q_(0): %f, ", q_(0) * R2D);
            printf("q_(1): %f, ", q_(1) * R2D);
            printf("q_(2): %f, ", q_(2) * R2D);
            printf("q_(3): %f, ", q_(3) * R2D);
            printf("q_(4): %f, ", q_(4) * R2D);
            printf("q_(5): %f\n", q_(5) * R2D);
            printf("\n");

            printf("*** Joint Space Error (unit: deg)  ***\n");
            printf("%f, ", R2D * e_(0));
            printf("%f, ", R2D * e_(1));
            printf("%f, ", R2D * e_(2));
            printf("%f, ", R2D * e_(3));
            printf("%f, ", R2D * e_(4));
            printf("%f\n", R2D * e_(5));
            printf("\n");

            count = 0;
        }
        count++;
    }

  private:
    // others
    double t;

    //Joint handles
    unsigned int n_joints_;                               // joint 숫자
    std::vector<std::string> joint_names_;                // joint name ??
    std::vector<hardware_interface::JointHandle> joints_; // ??
    std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

    // kdl
    KDL::Tree kdl_tree_;   // tree?
    KDL::Chain kdl_chain_; // chain?

    // kdl M,C,G
    KDL::JntSpaceInertiaMatrix M_; // intertia matrix
    KDL::JntArray C_;              // coriolis
    KDL::JntArray G_;              // gravity torque vector
    KDL::Vector gravity_;

    // kdl solver
    boost::scoped_ptr<KDL::ChainDynParam> id_solver_;                  // Solver To compute the inverse dynamics

    // Joint Space State
    KDL::JntArray qd_, qd_dot_, qd_ddot_;
    KDL::JntArray qd_old_;
    KDL::JntArray q_, qdot_;
    KDL::JntArray e_, e_dot_, e_int_;

    // Input
    KDL::JntArray aux_d_;
    KDL::JntArray comp_d_;
    KDL::JntArray tau_d_;

    // gains
    KDL::JntArray Kp_, Ki_, Kd_;

    // save the data
    double SaveData_[SaveDataMax];

    // ros publisher
    ros::Publisher pub_qd_, pub_q_, pub_e_;
    ros::Publisher pub_SaveData_;

    // ros message
    std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
    std_msgs::Float64MultiArray msg_SaveData_;
};
}; // namespace arm_controllers
PLUGINLIB_EXPORT_CLASS(arm_controllers::Computed_Torque_Controller, controller_interface::ControllerBase)

max_stars_repo_name: nicolastrimborn/arm-control-dreamteam

max_issues_repo_name: nicolastrimborn/arm-control-dreamteam

max_forks_repo_name: nicolastrimborn/arm-control-dreamteam