text:
/*
 * Copyright 2013 Open Source Robotics Foundation
 *
 * 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 <boost/thread/mutex.hpp>

#include <ros/ros.h>
#include <AtlasInterface.h>
#include <AtlasInterfaceTypes.h>
#include <AtlasUtility.h>

#include <atlas_msgs/AtlasCommand.h>
#include <atlas_msgs/AtlasState.h>

// This is not the cleanest thing to do, but it does avoid code duplication in
// the command message callback.
#define ROS_MSG_VECTOR_SIZE_CHECK(v) \
      if ((v).size() != Atlas::NUM_JOINTS)\
      {\
        ROS_ERROR("AtlasCommand message contains a different number (%ld) of "\
                  "elements in " #v " than expected (%d)",\
                  (v).size(),\
                  Atlas::NUM_JOINTS);\
        this->last_atlas_cmd_msg_valid = false;\
        return true;\
      }

// Uncomment and recompile to do basic testing without a robot available
//#define NOROBOT

class AtlasInterfaceBridge
{
  private:
    AtlasInterface the_atlas;
    AtlasUtility u_atlas;
    ros::Publisher atlas_state_pub;
    ros::Subscriber atlas_cmd_sub;
    ros::NodeHandle node;
    boost::mutex msg_lock;
    atlas_msgs::AtlasCommand last_atlas_cmd_msg;
    atlas_msgs::AtlasState last_atlas_state_msg;
    bool last_atlas_cmd_msg_valid;

    void atlasCommandCallback(const atlas_msgs::AtlasCommandConstPtr& msg);

  public:
    AtlasInterfaceBridge() : last_atlas_cmd_msg_valid(false) {}
    ~AtlasInterfaceBridge() {}
    // Initialize the connection to the robot.  Must be called first.
    bool start(int argc, char** argv);
    // Shut down the connection to the robot.
    bool stop();
    // Do one iteration of the interaction loop with the robot.
    // Returns:
    //  true if the update was successful or at least survivable (keep going)
    //  false if a significant error occurred (time to stop)
    bool update();
};

bool AtlasInterfaceBridge::start(int argc, char** argv)
{
  ROS_INFO("Opening connection to robot.");
  bool result;
#if ! defined (NOROBOT)
  result = this->u_atlas.start_control_session(argc, argv);
#else
  result = true;
#endif

  this->atlas_cmd_sub = this->node.subscribe("atlas/atlas_command", 1,
    &AtlasInterfaceBridge::atlasCommandCallback, this);
  this->atlas_state_pub =
    this->node.advertise<atlas_msgs::AtlasState>("atlas/atlas_state", 1);
  this->last_atlas_state_msg.position.resize(Atlas::NUM_JOINTS);
  this->last_atlas_state_msg.velocity.resize(Atlas::NUM_JOINTS);
  this->last_atlas_state_msg.effort.resize(Atlas::NUM_JOINTS);
  // TODO: also allocate space for the gains

  //
  // Set position control joint gains.
  //
  this->u_atlas.set_position_control_gains();

  return result;
}

bool AtlasInterfaceBridge::stop()
{
  ROS_INFO("Closing connection to robot.");
  if (this->atlas_cmd_sub)
    this->atlas_cmd_sub.shutdown();
  if (this->atlas_state_pub)
    this->atlas_state_pub.shutdown();
  bool result;
#if ! defined (NOROBOT)
  result = this->u_atlas.stop_control_session();
#else
  result = true;
#endif
  return result;
}

bool
AtlasInterfaceBridge::update()
{
  //
  // Read any available data from the robot.  Use the AtlasUtility function
  //  recv_data_from_robot().  It will wait (up to the specified time)
  //  for data from the robot, and then read all pending data.
  //
  // wait up to 1 second for data
#if ! defined (NOROBOT)
  double timeout = 1.0;
  bool data_received = this->u_atlas.recv_data_from_robot(timeout);

  if (!data_received)
  {
    ROS_WARN("Did not receive data from robot within %f second(s)", timeout);
    // TODO: return false here?  I don't know how serious this condition is.
    return true;
  }
#endif

  // Fill out and publish the received data
  // TODO: if this step is too slow, then try_lock and copy the data for
  // publication from another thread.
  for (unsigned int i = 0; i < Atlas::NUM_JOINTS; ++i)
  {
    this->last_atlas_state_msg.position[i] = s_data_from_robot.j[i].q;
    this->last_atlas_state_msg.velocity[i] = s_data_from_robot.j[i].qd;
    this->last_atlas_state_msg.effort[i] = s_data_from_robot.j[i].f;
    // TODO: also fill in other arrays
  }

  // Atlas gives time in microseconds since the epoch.
  this->last_atlas_state_msg.header.stamp.fromSec(
    s_data_from_robot.timestamp / 1e6);
  this->atlas_state_pub.publish(this->last_atlas_state_msg);

  //
  // Check for faults.  Don't exit if we hit one; wait for stop state.
  //
  this->u_atlas.check_robot_faults();

  //
  // Check for unexpected change to stop.
  //
  if (s_data_from_robot.run_state == RUN_STATE_STOP)
  {
    ROS_WARN("Robot control unexpectedly interrupted by stop state.");

    this->u_atlas.handle_robot_stop();
    return false;
  }

  // Try to fill out the robot command packet from the latest ROS command
  // message.
  bool data_to_send = false;
  {
    // Try to lock on the shared copy of the incoming command, but don't wait
    // for it.
    boost::mutex::scoped_try_lock lock(this->msg_lock);
    if (lock.owns_lock() && this->last_atlas_cmd_msg_valid)
    {
      // Sanity checks; the check macro will return out of this function if
      // there's a mismatch in size.
      ROS_MSG_VECTOR_SIZE_CHECK(this->last_atlas_cmd_msg.position);
      ROS_MSG_VECTOR_SIZE_CHECK(this->last_atlas_cmd_msg.velocity);
      ROS_MSG_VECTOR_SIZE_CHECK(this->last_atlas_cmd_msg.effort);
      // TODO: also check the arrays of gains

      // Here I'm assuming that the ordering of joints dictated by the ROS
      // message atlas_msgs/AtlasState agrees with the ordering of joints
      // assumed by the Atlas API.
      for (unsigned int i = 0; i < Atlas::NUM_JOINTS; ++i)
      {
        s_data_to_robot.j[i].q_d = this->last_atlas_cmd_msg.position[i];
        s_data_to_robot.j[i].qd_d = this->last_atlas_cmd_msg.velocity[i];
        s_data_to_robot.j[i].f_d = this->last_atlas_cmd_msg.effort[i];

        // TODO: also apply the gains in the message
      }

      // Atlas wants time in milliseconds since the epoch.
      s_data_to_robot.timestamp =
        this->last_atlas_cmd_msg.header.stamp.toSec() * 1e3;

      data_to_send = true;
    }

  }

  //
  // If we succeeded in filling out the packet, send control data to robot.
  //
  if (data_to_send)
  {
    ROS_INFO("Sending control data to robot.");
#if ! defined (NOROBOT)
    AtlasErrorCode ec;
    ec = this->the_atlas.send_control_data_to_robot(s_data_to_robot,
      &s_sent_control_seq_id);
    this->u_atlas.check_error_code(ec, "send_control_data_to_robot()");
#endif
  }

  return true;
}

void
AtlasInterfaceBridge::atlasCommandCallback(
  const atlas_msgs::AtlasCommandConstPtr& msg)
{
  // Lock and copy the incoming command to where the main loop will pick it up
  boost::mutex::scoped_lock lock(this->msg_lock);
  ROS_INFO("Received a message");
  this->last_atlas_cmd_msg = *msg;
  this->last_atlas_cmd_msg_valid = true;
}

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "atlas_interface_bridge");

  AtlasInterfaceBridge aib;

  if (!aib.start(argc, argv))
  {
    ROS_ERROR("Failed to start Atlas robot.");
    exit(1);
  }

  // Service message callbacks in another thread
  ros::AsyncSpinner spinner(1);
  spinner.start();

  ROS_INFO("Entering update loop");
  while (ros::ok() && aib.update());
  ROS_INFO("Exited update loop");

  spinner.stop();

  if (!aib.stop())
  {
    ROS_ERROR("Failed to stop Atlas robot.");
    exit(1);
  }

  return 0;
}


text:
#include "CorePch.h"
#include <rtp++/rfc3261/SipClientTcpConnection.h>
#include <boost/algorithm/string.hpp>
#include <boost/asio/error.hpp>
#include <boost/asio/read.hpp>
#include <boost/asio/read_until.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/asio/write.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/regex.hpp>
#include <cpputil/Conversion.h>
#include <cpputil/OBitStream.h>
#include <rtp++/RtpTime.h>
#include <rtp++/rfc2326/RtspUtil.h>

#define DEBUG_SIP

namespace rtp_plus_plus
{
namespace rfc3261
{

using boost::asio::ip::tcp;

const unsigned DEFAULT_SIP_BUFFER_SIZE = 512;

SipClientTcpConnectionPtr SipClientTcpConnection::create( boost::asio::io_service& io_service, const std::string& sIp, uint16_t uiPort )
{
  return boost::make_shared<SipClientTcpConnection>( boost::ref(io_service), sIp, uiPort );
}

SipClientTcpConnection::SipClientTcpConnection( boost::asio::io_service& io_service, const std::string& sIp, uint16_t uiPort )
  :m_ioService(io_service),
    m_socket(io_service),
    m_eState(CONNECTION_READY),
    m_sIpOrHostName(sIp),
    m_uiPort(uiPort),
    m_incomingMessageBuffer(new unsigned char[DEFAULT_SIP_BUFFER_SIZE], DEFAULT_SIP_BUFFER_SIZE),
    m_uiMessageLength(0),
    m_uiMessageBodyLength(0)
{
  VLOG(10) << "SipClientTcpConnection " << sIp << ":" << uiPort;
}

SipClientTcpConnection::~SipClientTcpConnection()
{
  VLOG(10) << "~SipClientTcpConnection " << m_sIpOrHostName << ":" << m_uiPort;
  doClose();
}

boost::system::error_code SipClientTcpConnection::connect()
{
  switch (m_eState)
  {
    case CONNECTION_READY:
    {
      tcp::resolver resolver(m_socket.get_io_service());
      tcp::resolver::query query(m_sIpOrHostName, ::toString(m_uiPort));
      tcp::resolver::iterator iterator = resolver.resolve(query);

      tcp::endpoint endpoint = *iterator;
      m_socket.async_connect(endpoint, boost::bind(&SipClientTcpConnection::handleConnect, shared_from_this(), boost::asio::placeholders::error, ++iterator));
      m_eState = CONNECTION_CONNECTING;
      return boost::system::error_code();
    }
    default:
    {
      LOG(WARNING) << "Connection in invalid state for connect";
      return boost::system::error_code(boost::system::errc::invalid_argument, boost::system::get_generic_category());
    }
  }
}

void SipClientTcpConnection::write(const std::string& sSipMessage)
{
  switch (m_eState)
  {
    case CONNECTION_CONNECTED:
    {
      boost::mutex::scoped_lock l(m_lock);
      bool bBusyWriting = !m_qSipRequests.empty();
      m_qSipRequests.push_back(sSipMessage);

      if (!bBusyWriting)
      {
        m_sMessage = m_qSipRequests.front();

    #ifdef DEBUG_SIP
        VLOG(2) << "[UAS->UAC]:\r\n" << m_sMessage;
    #endif

        boost::asio::async_write(m_socket, boost::asio::buffer(m_sMessage),
          boost::bind(&SipClientTcpConnection::handleWrite, shared_from_this(),
          boost::asio::placeholders::error,
          boost::asio::placeholders::bytes_transferred));
      }
      break;
    }
    case CONNECTION_READY:
    {
      // store message for later delivery, but start connect
      {
        boost::mutex::scoped_lock l(m_lock);
        m_qSipRequests.push_back(sSipMessage);
      }
      boost::system::error_code ec = connect();
      if (ec)
      {
        LOG(WARNING) << "Failed to connect";
      }
      break;
    }
    case CONNECTION_CONNECTING:
    {
      boost::mutex::scoped_lock l(m_lock);
      m_qSipRequests.push_back(sSipMessage);
      break;
    }
    case CONNECTION_CLOSED:
    case CONNECTION_ERROR:
    default:
    {
      LOG(WARNING) << "Connection is in invalid state for write";
      break;
    }
  }
}

void SipClientTcpConnection::handleConnect( const boost::system::error_code& error, tcp::resolver::iterator endpointIterator )
{
  if (!error)
  {
    m_eState = CONNECTION_CONNECTED;
    boost::regex regex("\r\n\r\n");
    // Start read for server commands, responses and media data
    boost::asio::async_read_until( m_socket, m_streamBuffer, regex,
                                   boost::bind(&SipClientTcpConnection::handleRead,
                                               shared_from_this(),  boost::asio::placeholders::error,
                                               boost::asio::placeholders::bytes_transferred));

    if (!m_qSipRequests.empty())
    {
      m_sMessage = m_qSipRequests.front();

  #ifdef DEBUG_SIP
      VLOG(2) << "[UAS->UAC]:\r\n" << m_sMessage;
  #endif

      boost::asio::async_write(m_socket, boost::asio::buffer(m_sMessage),
        boost::bind(&SipClientTcpConnection::handleWrite, shared_from_this(),
        boost::asio::placeholders::error,
        boost::asio::placeholders::bytes_transferred));
    }
  }
  else
  {
    if (error == boost::asio::error::invalid_argument)
    {
      if (endpointIterator == tcp::resolver::iterator())
      {
        // Failed to connect
        LOG(WARNING) << "Failed to connect using endpoint! No more endpoints.";
        m_eState = CONNECTION_ERROR;
        // call error handler?
        if (m_connectErrorHandler)
          m_connectErrorHandler(error, shared_from_this());
      }
      else
      {
        VLOG(2) << "Failed to connect using endpoint! Trying next endpoint.";
        tcp::endpoint endpoint = *endpointIterator;
        m_socket.async_connect(endpoint, boost::bind(&SipClientTcpConnection::handleConnect, shared_from_this(), boost::asio::placeholders::error, ++endpointIterator) );
      }
    }
    else
    {
      VLOG(5) << "Failed to connect: " << error.message();
      m_eState = CONNECTION_ERROR;
      if (m_connectErrorHandler)
        m_connectErrorHandler(error, shared_from_this());
      // can't connect to endpoint
      // handleClose();
    }
  }
}

void SipClientTcpConnection::close()
{
  m_ioService.post(boost::bind(&SipClientTcpConnection::doClose, shared_from_this()));
}

void SipClientTcpConnection::doClose()
{
  if (m_socket.is_open())
  {
    boost::system::error_code ec;
    VLOG(15) << this << " shutting down socket";
    m_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both, ec);
    if (ec)
      VLOG(5) << this << " shutting down socket: " << ec.message();
    VLOG(15) << this << " closing socket";
    m_socket.close(ec);
    if (ec)
      VLOG(5) << this << " closed socket: " << ec.message();
    else
      VLOG(15) << this << " closed socket";
    m_eState = CONNECTION_CLOSED;
  }
  else
  {
    VLOG(15) << this << " socket closed";
  }
}

void SipClientTcpConnection::handleClose()
{
#ifdef DEBUG_SIP
  VLOG(2) << this << " closing connection";
#endif
  // Notify about closing of connection
  m_closeHandler( shared_from_this() );

  // Close socket
  close();
}

void SipClientTcpConnection::handleRead( const boost::system::error_code& error, std::size_t bytesTransferred )
{
  VLOG(15) << "handleRead " << bytesTransferred;

  if (!error )
  {
    std::istream is(&m_streamBuffer);
    size_t size = m_streamBuffer.size();

    // check if buffer is big enough
    if (m_incomingMessageBuffer.getSize() < bytesTransferred + 1)
    {
      m_incomingMessageBuffer.setData(new uint8_t[bytesTransferred + 1], bytesTransferred + 1);
    }

    // store message length so we don't overwrite it later
    m_uiMessageLength = bytesTransferred;

    m_incomingMessageBuffer[bytesTransferred] = '\0';
    is.read((char*)&m_incomingMessageBuffer[0], m_uiMessageLength);

    // HACK: for now do scan f for Content-Length header
    // Content-Length: 376
    std::string sContent((char*)m_incomingMessageBuffer.data(), bytesTransferred);
    // TODO: refactor getContentLength
    if (rfc2326::getContentLength(sContent, m_uiMessageBodyLength) && m_uiMessageBodyLength > 0)
    {
      uint32_t uiTotalMessageSize = m_uiMessageLength + m_uiMessageBodyLength + 1;
      if (m_incomingMessageBuffer.getSize() < uiTotalMessageSize)
      {
        uint8_t* pNewBuffer = new uint8_t[uiTotalMessageSize];
        memcpy((char*)pNewBuffer, &m_incomingMessageBuffer[0], m_uiMessageLength);
        m_incomingMessageBuffer.setData(pNewBuffer, uiTotalMessageSize);
      }

      // Note that there might be more data in the stream buffer than bytesTransferred
      size_t overflow = size - bytesTransferred;

      // Build up response string
      // TODO: create message big enough for SIP message + message body
      //      m_messageBuffer = MessageBuffer_t( new char[uiContentLength] );
      size_t to_read = m_uiMessageBodyLength - overflow;

      boost::asio::async_read(m_socket,
                              m_streamBuffer, boost::asio::transfer_at_least(to_read),
                              boost::bind(&SipClientTcpConnection::handleReadBody, shared_from_this(),
                                          boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
    }
    else
    {
#ifdef DEBUG_SIP
      VLOG(2) << "[UAC->UAS]:\r\n" << m_incomingMessageBuffer.data();
#endif

      // No message body
      if (m_readCompletionHandler)
      {
        std::string sSipMessage((char*)&m_incomingMessageBuffer[0], bytesTransferred);
        m_readCompletionHandler(error, sSipMessage, shared_from_this());
      }

      /// Create task for reading next message
      boost::asio::async_read_until( m_socket,
                                      m_streamBuffer,
                                      boost::regex("\r\n\r\n"),
                                      boost::bind(&SipClientTcpConnection::handleRead, shared_from_this(),  boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)
                                      );
    }
  }
  else
  {
    // high verbosity level to let calling class control logging
    if (error != boost::asio::error::eof && error != boost::asio::error::operation_aborted)
      VLOG(15) << "Error in read: " << error.message();

    // update state to error here: the read handler might try to use the same connection!
    m_eState = CONNECTION_ERROR;
    if (m_readCompletionHandler)
    {
      m_readCompletionHandler(error, "", shared_from_this());
    }
    // Check if connection has been closed
    handleClose();
  }
}

void SipClientTcpConnection::handleReadBody( const boost::system::error_code& error, std::size_t bytesTransferred )
{
  if (!error)
  {
    // TODO: check if Content-Length == bytesTransferred?
    std::istream is(&m_streamBuffer);
    size_t size = m_streamBuffer.size();
    assert(size >= m_uiMessageBodyLength);
    VLOG(15) << "Read body: bytesTransferred: " << bytesTransferred << " size: " << size << " m_uiMessageBodyLength: " << m_uiMessageBodyLength ;
    is.read((char*)&m_incomingMessageBuffer[m_uiMessageLength], m_uiMessageBodyLength);

    uint32_t uiTotalMessageSize = m_uiMessageLength + m_uiMessageBodyLength;

    std::string sSipMessage((char*)&m_incomingMessageBuffer[0], uiTotalMessageSize);

#ifdef DEBUG_SIP
    VLOG(2) << "[UAC->UAS MB] " << sSipMessage;
#endif

    m_readCompletionHandler(error, sSipMessage, shared_from_this());

    /// Create task for reading next message
    boost::asio::async_read_until( m_socket,
      m_streamBuffer,
      boost::regex("\r\n\r\n"),
      boost::bind(&SipClientTcpConnection::handleRead, shared_from_this(),  boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)
      );
  }
  else
  {
    LOG(WARNING) << "Error in read body: " << error.message();

    // update state to error here: the read handler might try to use the same connection!
    m_eState = CONNECTION_ERROR;
    if (m_readCompletionHandler)
    {
      m_readCompletionHandler(error, "", shared_from_this());
    }
    handleClose();
  }
}

void SipClientTcpConnection::handleWrite( const boost::system::error_code& error, size_t bytes_transferred)
{
  if (!error)
  {
    // Remove request from queue
    std::string sSipRequest = m_qSipRequests.front();
    m_qSipRequests.pop_front();

    if (m_writeCompletionHandler)
    {
      m_writeCompletionHandler(error, sSipRequest, shared_from_this());
    }

    if ( !m_qSipRequests.empty() )
    {
      m_sMessage = m_qSipRequests.front();
#ifdef DEBUG_SIP
      VLOG(2) << "[UAS->UAC] " << m_sMessage;
#endif

      boost::asio::async_write(m_socket, boost::asio::buffer(m_sMessage),
        boost::bind(&SipClientTcpConnection::handleWrite, shared_from_this(),
        boost::asio::placeholders::error,
        boost::asio::placeholders::bytes_transferred));
    }
  }
  else
  {
    LOG(WARNING) << "Error on write: " << error.message();
    // update state to error here: the read handler might try to use the same connection!
    m_eState = CONNECTION_ERROR;
    if (m_writeCompletionHandler)
    {
      // Remove request from queue
      std::string sSipRequest = m_qSipRequests.front();
      m_qSipRequests.pop_front();
      m_writeCompletionHandler(error, sSipRequest, shared_from_this());
    }
    // Check if connection has been closed
    handleClose();
  }
}

} // rfc3261
} // rtp_plus_plus

ext: cpp


text:
/*
 * Stream-Processing Architecture for Real-time Cyber-physical Security (SPARCS)
 */

#include <mutex>
#include <map>
#include <ctime>
#include <boost/thread/condition.hpp>
#include <boost/any.hpp>

#include "serviceCommandQueueTemplate.h"
#include "serviceInstanceApi.h"
#include "upmuStructKeyValueListProtobuf.pb.h"

namespace serviceCommon {
using namespace boost;

#include "serviceCommonStats.h"

/* these need to be static */
/* label storage */
static std::string lbl_serviceStartTime(LBL_SERVICE_START_TIME);
static std::string lbl_numDataBufSeen(LBL_NUM_DATA_BUF_SEEN);
static std::string lbl_numDataBufAccepted(LBL_NUM_DATA_BUF_ACCEPTED);
static std::string lbl_numDataBufRejected(LBL_NUM_DATA_BUF_REJECTED);

/* value storage **/
static unsigned int val_serviceStartTime = 0;
static unsigned int val_numDataBufSeen = 0;
static unsigned int val_numDataBufAccepted = 0;
static unsigned int val_numDataBufRejected = 0;

void createServiceCommonStats(serviceInstanceApi * servApi) {
    (servApi->serviceCmnStatsMutex).lock();
    /* first the labels */
    std::map<unsigned int, std::string *> * lblMap =
        new std::map<unsigned int, std::string *>();
    lblMap->insert(std::make_pair(INDX_UINT_SERVICE_START_TIME, &lbl_serviceStartTime));
    lblMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_SEEN, &lbl_numDataBufSeen));
    lblMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_ACCEPTED, &lbl_numDataBufAccepted));
    lblMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_REJECTED, &lbl_numDataBufRejected));

    /* next the values */
    std::map<unsigned int, boost::any> * valMap =
        new std::map<unsigned int, boost::any>();
    std::time_t result = std::time(nullptr);
    val_serviceStartTime = (unsigned int)result;
    valMap->insert(std::make_pair(INDX_UINT_SERVICE_START_TIME, &val_serviceStartTime));
    valMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_SEEN, &val_numDataBufSeen));
    valMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_ACCEPTED, &val_numDataBufAccepted));
    valMap->insert(std::make_pair(INDX_UINT_NUM_DATA_BUF_REJECTED, &val_numDataBufRejected));

    /* init any values */
    val_serviceStartTime = (uint32_t)time(NULL);
    val_numDataBufSeen = 0;
    val_numDataBufAccepted = 0;
    val_numDataBufRejected = 0;

    servApi->serviceCmnStatsLabels = lblMap;
    servApi->serviceCmnStatsValues = valMap;
    (servApi->serviceCmnStatsMutex).unlock();
}

void destroyServiceCommonStats(serviceInstanceApi * servApi) {
    servApi->serviceCmnStatsMutex.lock();
    /* first the labels */
    delete servApi->serviceCmnStatsLabels;

    /* next the values */
    delete servApi->serviceCmnStatsValues;

    servApi->serviceCmnStatsLabels = nullptr;
    servApi->serviceCmnStatsValues = nullptr;
    (servApi->serviceCmnStatsMutex).unlock();
}

void clearServiceCommonStats(serviceInstanceApi * servApi) {
    (servApi->serviceCmnStatsMutex).lock();

    /* init any values */
    val_numDataBufSeen = 0;
    val_numDataBufAccepted = 0;
    val_numDataBufRejected = 0;

    (servApi->serviceCmnStatsMutex).unlock();
}

void populateServiceCommonProtobufStatsList(serviceInstanceApi * servApi,
        KeyValueList * list) {

    (servApi->serviceCmnStatsMutex).lock();
    list->set_category("Service_Common_Stats");

    for(int i = BASE_INDX_STR_COMMON_STATS;
            i < BASE_INDX_STR_COMMON_STATS + NUM_INDX_STR_COMMON_STATS;
            i++) {
            KeyValue * pair = list->add_element();
            pair->set_key((*(servApi->serviceSpecStatsLabels))[i]->c_str());
            pair->set_stringval(any_cast<std::string *>
                ((*(servApi->serviceSpecStatsValues))[i])->c_str());
    }

    for(int i = BASE_INDX_UINT_COMMON_STATS;
            i < BASE_INDX_UINT_COMMON_STATS + NUM_INDX_UINT_COMMON_STATS;
            i++) {
            KeyValue * pair = list->add_element();
            pair->set_key((*(servApi->serviceSpecStatsLabels))[i]->c_str());
            pair->set_uintegerval(*(any_cast<uint32_t *>
                ((*(servApi->serviceSpecStatsValues))[i])));
    }

    for(int i = BASE_INDX_INT_COMMON_STATS;
            i < BASE_INDX_INT_COMMON_STATS + NUM_INDX_INT_COMMON_STATS;
            i++) {
            KeyValue * pair = list->add_element();
            pair->set_key((*(servApi->serviceSpecStatsLabels))[i]->c_str());
            pair->set_integerval(*(any_cast<int32_t *>
                ((*(servApi->serviceSpecStatsValues))[i])));
    }

    for(int i = BASE_INDX_FLT_COMMON_STATS;
            i < BASE_INDX_FLT_COMMON_STATS + NUM_INDX_FLT_COMMON_STATS;
            i++) {
            KeyValue * pair = list->add_element();
            pair->set_key((*(servApi->serviceSpecStatsLabels))[i]->c_str());
            pair->set_floatval(*(any_cast<float *>
                ((*(servApi->serviceSpecStatsValues))[i])));
    }

    (servApi->serviceCmnStatsMutex).unlock();
}
}

max_stars_repo_name: lbnl-cybersecurity/sparcs

max_issues_repo_name: lbnl-cybersecurity/sparcs

max_forks_repo_name: lbnl-cybersecurity/sparcs


text:
#include <Eigen/Dense>
#include <Eigen/LU>

#include "apriltag_mit/AprilTags/GrayModel.h"

namespace AprilTags {

void GrayModel::AddBlackObs(float x, float y, float v) {
  black_model_.AddObservation(x, y, v);
}

void GrayModel::AddWhiteObs(float x, float y, float v) {
  white_model_.AddObservation(x, y, v);
}

void GrayModel::Fit() {
  black_model_.Fit();
  white_model_.Fit();
}

float GrayModel::CalcThreshold(float x, float y) const {
  return (black_model_.Predict(x, y) + white_model_.Predict(x, y)) / 2;
}

IntensityModel::IntensityModel()
    : A_(), c_(), b_(), num_obs_(0), dirty_(false) {
  A_.setZero();
  c_.setZero();
  b_.setZero();
}

void IntensityModel::AddObservation(float x, float y, float v) {
  float xy = x * y;

  // update only upper-right elements. A'A is symmetric,
  // we'll fill the other elements in later.
  A_(0, 0) += x * x;
  A_(0, 1) += x * y;
  A_(0, 2) += x * xy;
  A_(0, 3) += x;
  A_(1, 1) += y * y;
  A_(1, 2) += y * xy;
  A_(1, 3) += y;
  A_(2, 2) += xy * xy;
  A_(2, 3) += xy;
  A_(3, 3) += 1;

  b_[0] += x * v;
  b_[1] += y * v;
  b_[2] += xy * v;
  b_[3] += v;

  num_obs_++;
  dirty_ = true;
}

float IntensityModel::Predict(float x, float y) const {
  return c_[0] * x + c_[1] * y + c_[2] * x * y + c_[3];
}

void IntensityModel::Fit() {
  // we really only need 4 linearly independent observations to fit our answer,
  // but we'll be very sensitive to noise if we don't have an over-determined
  // system. Thus, require at least 6 observations (or we'll use a constant
  // model below).

  if (num_obs_ >= 6) {
    // make symmetric
    for (int i = 0; i < 4; ++i) {
      for (int j = i + 1; j < 4; ++j) {
        A_(j, i) = A_(i, j);
      }
    }

    bool invertible;
    double det_unused;
    Eigen::Matrix4d Ainv;
    A_.computeInverseAndDetWithCheck(Ainv, det_unused, invertible);
    if (invertible) {
      c_ = Ainv * b_;
      return;
    }
  }

  // If we get here, either nobs < 6 or the matrix inverse generated
  // an underflow, so use a constant model.
  c_.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness
  c_[3] = b_[3] / num_obs_;
}

bool IsOnOuterBorder(int x, int y, int l, bool black_corner) {
  if (black_corner) {
    // Black corners will be excluded
    const bool on_corner = (x == -1 && y == -1) || (x == -1 && y == l) ||
                           (x == l && y == -1) || (x == l && y == l);
    return (y == -1 || y == l || x == -1 || x == l) && !on_corner;
  } else {
    return (y == -1 || y == l || x == -1 || x == l);
  }
}

bool IsOnInnerBorder(int x, int y, int l) {
  return (y == 0 || y == (l - 1) || x == 0 || x == (l - 1));
}

bool IsInsideInnerBorder(int x, int y, int l) {
  return (y >= 1 && y < (l - 1) && x >= 1 && x < (l - 1));
}

} // namespace AprilTags

ext: cc


text:
/*
 * Copyright (c) 2010-2012 frankee zhou (frankee.zhou at gmail dot com)
 *
 * Distributed under 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 <cetty/config/ConfigDescriptor.h>

#include <cstdarg>
#include <cstdlib>
#include <boost/assert.hpp>
#include <cetty/util/StringUtil.h>
#include <cetty/config/ConfigObject.h>

namespace cetty {
namespace config {

ConfigObjectDescriptor::ConfigObjectDescriptor(ConfigObject* defaultInstance,
        int count,
        ConstConfigFieldDescriptorPtr descriptor,
        ...) {
    BOOST_ASSERT(defaultInstance);

    defaultInstance_ = defaultInstance;

    if (count <= 0 || !descriptor) {
        return;
    }

    va_list valist;
    va_start(valist, descriptor);

    fields_.push_back(descriptor);
    fieldMap_.insert(std::make_pair(descriptor->name, descriptor));

    for (int i = 1; i < count; ++i) {
        ConstConfigFieldDescriptorPtr field
            = va_arg(valist, ConstConfigFieldDescriptorPtr);

        if (!field) {
            break;
        }

        fields_.push_back(field);
        fieldMap_.insert(std::make_pair(field->name, field));
    }

    va_end(valist);
}

ConfigObjectDescriptor::~ConfigObjectDescriptor() {
    std::size_t j = fields_.size();

    for (std::size_t i = 0; i < j; ++i) {
        ConstConfigFieldDescriptorPtr& field = fields_[i];

        if (field) {
            delete field;
            field = NULL;
        }
    }

    if (defaultInstance_) {
        delete defaultInstance_;
    }
}

const std::string& ConfigObjectDescriptor::className() const {
    return defaultInstance_->className();
}

std::string ConfigFieldDescriptor::toString() const {
    std::string tmp;
    StringUtil::printf(&tmp, "field: %s(%s)", name.c_str(), className.c_str());
    return tmp;
}

}
}


text:
/*
 * Search.cpp
 *
 *  Created on: 16 Nov 2015
 *      Author: hieu
 */
#include <boost/foreach.hpp>
#include "Search.h"
#include "Stack.h"
#include "../Manager.h"
#include "../Hypothesis.h"
#include "../TrellisPath.h"
#include "../Sentence.h"
#include "../../TrellisPaths.h"
#include "../../InputPathsBase.h"
#include "../../InputPathBase.h"
#include "../../System.h"
#include "../../TranslationTask.h"
#include "../../legacy/Util2.h"
#include "../../PhraseBased/TargetPhrases.h"

using namespace std;

namespace Moses2
{

namespace NSCubePruningMiniStack
{

////////////////////////////////////////////////////////////////////////
Search::Search(Manager &mgr) :
  Moses2::Search(mgr), m_stack(mgr), m_cubeEdgeAlloc(mgr.GetPool())

  , m_queue(QueueItemOrderer(),
            std::vector<QueueItem*, MemPoolAllocator<QueueItem*> >(
              MemPoolAllocator<QueueItem*>(mgr.GetPool())))

  , m_seenPositions(
    MemPoolAllocator<CubeEdge::SeenPositionItem>(mgr.GetPool()))

  , m_queueItemRecycler(MemPoolAllocator<QueueItem*>(mgr.GetPool()))

{
}

Search::~Search()
{
}

void Search::Decode()
{
  const Sentence &sentence = static_cast<const Sentence&>(mgr.GetInput());

  // init cue edges
  m_cubeEdges.resize(sentence.GetSize() + 1);
  for (size_t i = 0; i < m_cubeEdges.size(); ++i) {
    m_cubeEdges[i] = new (mgr.GetPool().Allocate<CubeEdges>()) CubeEdges(
      m_cubeEdgeAlloc);
  }

  const Bitmap &initBitmap = mgr.GetBitmaps().GetInitialBitmap();
  Hypothesis *initHypo = Hypothesis::Create(mgr.GetSystemPool(), mgr);
  initHypo->Init(mgr, mgr.GetInputPaths().GetBlank(), mgr.GetInitPhrase(),
                 initBitmap);
  initHypo->EmptyHypothesisState(mgr.GetInput());
  //cerr << "initHypo=" << *initHypo << endl;

  m_stack.Add(initHypo, mgr.GetHypoRecycle(), mgr.arcLists);
  PostDecode(0);

  for (size_t stackInd = 1; stackInd < sentence.GetSize() + 1;
       ++stackInd) {
    //cerr << "stackInd=" << stackInd << endl;
    m_stack.Clear();
    Decode(stackInd);
    PostDecode(stackInd);

    //m_stack.DebugCounts();
  }

}

void Search::Decode(size_t stackInd)
{
  Recycler<HypothesisBase*> &hypoRecycler = mgr.GetHypoRecycle();

  // reuse queue from previous stack. Clear it first
  std::vector<QueueItem*, MemPoolAllocator<QueueItem*> > &container = Container(
        m_queue);
  //cerr << "container=" << container.size() << endl;
  BOOST_FOREACH(QueueItem *item, container) {
    // recycle unused hypos from queue
    Hypothesis *hypo = item->hypo;
    hypoRecycler.Recycle(hypo);

    // recycle queue item
    m_queueItemRecycler.push_back(item);
  }
  container.clear();

  m_seenPositions.clear();

  // add top hypo from every edge into queue
  CubeEdges &edges = *m_cubeEdges[stackInd];

  BOOST_FOREACH(CubeEdge *edge, edges) {
    //cerr << *edge << " ";
    edge->CreateFirst(mgr, m_queue, m_seenPositions, m_queueItemRecycler);
  }

  /*
  cerr << "edges: ";
  boost::unordered_set<const Bitmap*> uniqueBM;
  BOOST_FOREACH(CubeEdge *edge, edges) {
  uniqueBM.insert(&edge->newBitmap);
  //cerr << *edge << " ";
  }
  cerr << edges.size() << " " << uniqueBM.size();
  cerr << endl;
   */

  size_t pops = 0;
  while (!m_queue.empty() && pops < mgr.system.options.cube.pop_limit) {
    // get best hypo from queue, add to stack
    //cerr << "queue=" << queue.size() << endl;
    QueueItem *item = m_queue.top();
    m_queue.pop();

    CubeEdge *edge = item->edge;

    // add hypo to stack
    Hypothesis *hypo = item->hypo;

    if (mgr.system.options.cube.lazy_scoring) {
      hypo->EvaluateWhenApplied();
    }

    //cerr << "hypo=" << *hypo << " " << hypo->GetBitmap() << endl;
    m_stack.Add(hypo, hypoRecycler, mgr.arcLists);

    edge->CreateNext(mgr, item, m_queue, m_seenPositions, m_queueItemRecycler);

    ++pops;
  }

  // create hypo from every edge. Increase diversity
  if (mgr.system.options.cube.diversity) {
    while (!m_queue.empty()) {
      QueueItem *item = m_queue.top();
      m_queue.pop();

      if (item->hypoIndex == 0 && item->tpIndex == 0) {
        // add hypo to stack
        Hypothesis *hypo = item->hypo;
        //cerr << "hypo=" << *hypo << " " << hypo->GetBitmap() << endl;
        m_stack.Add(hypo, hypoRecycler, mgr.arcLists);
      }
    }
  }
}

void Search::PostDecode(size_t stackInd)
{
  MemPool &pool = mgr.GetPool();

  const InputPaths &paths = mgr.GetInputPaths();
  const Matrix<InputPath*> &pathMatrix = paths.GetMatrix();
  size_t inputSize = pathMatrix.GetRows();
  size_t numPaths = pathMatrix.GetCols();

  BOOST_FOREACH(const Stack::Coll::value_type &val, m_stack.GetColl()) {
    const Bitmap &hypoBitmap = *val.first.first;
    size_t firstGap = hypoBitmap.GetFirstGapPos();
    size_t hypoEndPos = val.first.second;

    Moses2::HypothesisColl &hypos = *val.second;

    //cerr << "key=" << hypoBitmap << " " << firstGap << " " << inputSize << endl;

    // create edges to next hypos from existing hypos
    for (size_t startPos = firstGap; startPos < inputSize; ++startPos) {
      for (size_t pathInd = 0; pathInd < numPaths; ++pathInd) {
        const InputPath *path = pathMatrix.GetValue(startPos, pathInd);

        if (path == NULL) {
          break;
        }
        if (path->GetNumRules() == 0) {
          continue;
        }

        const Range &pathRange = path->range;
        //cerr << "pathRange=" << pathRange << endl;
        if (!CanExtend(hypoBitmap, hypoEndPos, pathRange)) {
          continue;
        }

        const ReorderingConstraint &reorderingConstraint = mgr.GetInput().GetReorderingConstraint();
        if (!reorderingConstraint.Check(hypoBitmap, startPos, pathRange.GetEndPos())) {
          continue;
        }

        const Bitmap &newBitmap = mgr.GetBitmaps().GetBitmap(hypoBitmap, pathRange);
        size_t numWords = newBitmap.GetNumWordsCovered();

        CubeEdges &edges = *m_cubeEdges[numWords];

        // sort hypo for a particular bitmap and hypoEndPos
        const Hypotheses &sortedHypos = hypos.GetSortedAndPrunedHypos(mgr, mgr.arcLists);

        size_t numPt = mgr.system.mappings.size();
        for (size_t i = 0; i < numPt; ++i) {
          const TargetPhrases *tps = path->targetPhrases[i];
          if (tps && tps->GetSize()) {
            CubeEdge *edge = new (pool.Allocate<CubeEdge>()) CubeEdge(mgr, sortedHypos, *path, *tps, newBitmap);
            edges.push_back(edge);
          }
        }
      }
    }
  }
}

const Hypothesis *Search::GetBestHypo() const
{
  const Hypothesis *bestHypo = m_stack.GetBestHypo();
  return bestHypo;
}

void Search::AddInitialTrellisPaths(TrellisPaths<TrellisPath> &paths) const
{
  const Stack::Coll &coll = m_stack.GetColl();
  BOOST_FOREACH(const Stack::Coll::value_type &val, coll) {
    Moses2::HypothesisColl &hypos = *val.second;
    const Hypotheses &sortedHypos = hypos.GetSortedAndPrunedHypos(mgr, mgr.arcLists);

    BOOST_FOREACH(const HypothesisBase *hypoBase, sortedHypos) {
      const Hypothesis *hypo = static_cast<const Hypothesis*>(hypoBase);
      TrellisPath *path = new TrellisPath(hypo, mgr.arcLists);
      paths.Add(path);
    }
  }
}

}

}


text:
#ifdef IS_TEST_NET
#include <blurt/chain/generic_custom_operation_interpreter.hpp>
#include <blurt/chain/account_object.hpp>

#include <boost/test/included/unit_test.hpp>

#include "../db_fixture/database_fixture.hpp"

using namespace blurt::chain;
using namespace blurt::chain::test;
/*
namespace blurt { namespace plugin_tests {

using namespace blurt::app;
using namespace blurt::chain;

struct test_a_operation : base_operation
{
   string account;

   void validate() { FC_ASSERT( account.size() ); }
};

struct test_b_operation : base_operation
{
   string account;

   void validate() { FC_ASSERT( account.size() ); }
};

typedef fc::static_variant<
   test_a_operation,
   test_b_operation >   test_op;

class test_plugin : public plugin
{
   public:
      test_plugin( application* app );

      std::string plugin_name()const override { return "TEST"; }

      std::shared_ptr< generic_custom_operation_interpreter< test_op > > _evaluator_registry;
};

BLURT_DEFINE_PLUGIN_EVALUATOR( test_plugin, test_a_operation, test_a );
BLURT_DEFINE_PLUGIN_EVALUATOR( test_plugin, test_b_operation, test_b );

void test_a_evaluator::do_apply( const test_a_operation& o )
{
   const auto& account = db().get_account( o.account );

   db().modify( account, [&]( account_object& a )
   {
      a.json_metadata = "a";
   });
}

void test_b_evaluator::do_apply( const test_b_operation& o )
{
   const auto& account = db().get_account( o.account );

   db().modify( account, [&]( account_object& a )
   {
      a.json_metadata = "b";
   });
}

test_plugin::test_plugin( application* app ) : plugin( app )
{
   _evaluator_registry = std::make_shared< generic_custom_operation_interpreter< test_op > >( database() );

   _evaluator_registry->register_evaluator< test_a_evaluator >( *this );
   _evaluator_registry->register_evaluator< test_b_evaluator >( *this );

   database().set_custom_operation_interpreter( plugin_name(), _evaluator_registry );
}

} } // blurt::plugin_tests

BLURT_DEFINE_PLUGIN( test, blurt::plugin_tests::test_plugin )

FC_REFLECT( blurt::plugin_tests::test_a_operation, (account) )
FC_REFLECT( blurt::plugin_tests::test_b_operation, (account) )

BLURT_DECLARE_OPERATION_TYPE( blurt::plugin_tests::test_op );
FC_REFLECT_TYPENAME( blurt::plugin_tests::test_op );
BLURT_DEFINE_OPERATION_TYPE( blurt::plugin_tests::test_op );
*/

#endif

max_stars_repo_name: Blurt-Blockchain/steem

max_issues_repo_name: Blurt-Blockchain/steem

max_forks_repo_name: Blurt-Blockchain/steem


text:
/*
 * Copyright (c) 2018-2022 curoky(cccuroky@gmail.com).
 *
 * This file is part of my-own-x.
 * See https://github.com/curoky/my-own-x for further info.
 *
 * 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 <boost/interprocess/creation_tags.hpp>          // for create_only, open_only
#include <boost/interprocess/interprocess_fwd.hpp>       // for managed_shared_memory
#include <boost/interprocess/managed_shared_memory.hpp>  // for basic_managed_shared_memory
#include <boost/interprocess/shared_memory_object.hpp>   // for shared_memory_object
#include <catch2/catch.hpp>  // for AssertionHandler, operator""_catch_sr, SourceLineInfo, StringRef, REQUIRE, TEST_CASE

TEST_CASE("BaseTest", "[ManagedSharedMemory]") {
  using boost::interprocess::create_only;
  using boost::interprocess::managed_shared_memory;
  using boost::interprocess::open_only;
  using boost::interprocess::shared_memory_object;

  // 保证内存文件被删除
  struct shm_remove {
    shm_remove() { shared_memory_object::remove("MySharedMemory"); }
    ~shm_remove() { shared_memory_object::remove("MySharedMemory"); }
  } remover;

  managed_shared_memory::handle_t handle;

  {
    // 创建一段共享内存的管理器
    // `MySharedMemory` 在/dev/shm/目录下，大小为64k
    managed_shared_memory segment(create_only, "MySharedMemory", 65536);

    // 获取未分配的大小
    managed_shared_memory::size_type free_memory = segment.get_free_memory();
    REQUIRE(free_memory == 65312);

    // 分配一块内存，大小1k
    void* shptr = segment.allocate(1024 /*bytes to allocate*/);
    REQUIRE(free_memory == 65312);

    // 即使共享内存映射到不同的地址空间，这个通过
    // 基地址获得的句柄能够标识共享内存段中的任意字节。
    // 这个handle可以在其它进程中使用
    handle = segment.get_handle_from_address(shptr);
  }

  {
    managed_shared_memory segment(open_only, "MySharedMemory");
    void* msg = segment.get_address_from_handle(handle);
    segment.deallocate(msg);
    REQUIRE(segment.get_free_memory() == 65312);
  }
}

max_stars_repo_name: curoky/my-own-x

max_issues_repo_name: curoky/my-own-x

max_forks_repo_name: curoky/my-own-x