<commit_before>// Copyright (C) 2013-2014 Thalmic Labs Inc.
// Confidential and not for redistribution. See LICENSE.txt.
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <string>
#include <stdint.h>

// The only file that needs to be included to use the Myo C++ SDK is myo.hpp.
#include <myo/myo.hpp>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <myo_ros/GestureStamped.h>
#include <myo_ros/Vibration.h>
#include <myo_ros/StatusStamped.h>
#include <std_msgs/Bool.h>

// This class serves as a ROS namespace wrapper and allows
// to publish individual Myo's data in a separate namespace.
class MyoRos {
  ros::NodeHandlePtr pnode_;
  myo::Myo* myo_;
  size_t id_;
  std::string frame_id_;
  std::string fixed_frame_id_;
  std::string ns_;
  // Information that will be going out
  geometry_msgs::QuaternionStamped msg_rotation_;
  myo_ros::GestureStamped msg_gesture_;
  myo_ros::StatusStamped msg_status_;
  geometry_msgs::Vector3Stamped msg_gyro_;
  geometry_msgs::Vector3Stamped msg_accel_;

  ros::Publisher pub_rotation_;
  ros::Publisher pub_gesture_;
  ros::Publisher pub_gyro_;
  ros::Publisher pub_accel_;
  ros::Publisher pub_status_;
  ros::Subscriber sub_vibration_;
  ros::Subscriber sub_unlock_override_;

  tf::TransformBroadcaster br_;
public:

  MyoRos(ros::NodeHandlePtr& parent, myo::Myo* myo, size_t id): myo_(myo), id_(id), fixed_frame_id_("/world")
  {
    ros::NodeHandlePtr pnode_root = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    if (pnode_root->hasParam("static_myo_ids"))
    {
      XmlRpc::XmlRpcValue v;
      pnode_root->param("static_myo_ids", v, v);
      for (int i = 0; i < v.size(); i++)
      {
        if (std::string(v[i]) == myo->getMacAddress())
        {
          id_ = id = i + 1;
          break;
        }
      }
    }

    ns_ = std::string("myo") + std::to_string(id_);
    frame_id_ = std::string("myo") + std::to_string(id_) + std::string("_frame");
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle(*parent, ns_));
    // Subscribe & publish setup
    pub_rotation_ = pnode_->advertise<geometry_msgs::QuaternionStamped>("rotation", 100);
    pub_gesture_ = pnode_->advertise<myo_ros::GestureStamped>("gesture", 100);
    pub_gyro_ = pnode_->advertise<geometry_msgs::Vector3Stamped>("gyro", 100);
    pub_accel_ = pnode_->advertise<geometry_msgs::Vector3Stamped>("accel", 100);
    pub_status_ = pnode_->advertise<myo_ros::StatusStamped>("status", 100);

    sub_vibration_ = pnode_->subscribe<myo_ros::Vibration>("vibration", 100, boost::bind(&MyoRos::vibrationCallback, this, _1));
    sub_unlock_override_ = pnode_->subscribe<std_msgs::Bool>("unlock_override", 100, boost::bind(&MyoRos::unlockCallback, this, _1));

    ROS_INFO("Paired Myo (%s) with ID: %s", myo->getName().c_str(), ns_.c_str());
  }

  std::string getNs() const
  {
    return ns_;
  }

  // Creates vibrations
  void vibrationCallback(const myo_ros::VibrationConstPtr& vibration)
  {
    if (myo_)
    {
      myo_->vibrate(static_cast<myo::Myo::VibrationType>(vibration->vibration));
    }
  }

  // Sets the unlock timing
  void unlockCallback(const std_msgs::BoolConstPtr& unlock)
  {
    if (myo_)
    {
      if (unlock->data == true)
      {
        myo_->unlock(myo::Myo::unlockHold);
      }
      else
      {
        myo_->unlock(myo::Myo::unlockTimed);
      }
    }
  }

  void publishRotation(geometry_msgs::QuaternionStamped& msg)
  {
    tf::Transform myo_tf;
    tf::Quaternion tf_quat;

//    myo_tf.setOrigin(tf::Vector3(0.0, 0.0, 1.5));
    tf::quaternionMsgToTF(msg.quaternion, tf_quat);
    myo_tf.setRotation(tf_quat);

    // Myo returns data in global inertial frame, e.g. /world
    msg.header.frame_id = fixed_frame_id_;

    pub_rotation_.publish(msg);
    br_.sendTransform(tf::StampedTransform(myo_tf, msg.header.stamp, fixed_frame_id_, frame_id_));

  }

  void publishGesture(myo_ros::GestureStamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_gesture_.publish(msg);
  }

  void publishGyro(geometry_msgs::Vector3Stamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_gyro_.publish(msg);
  }

  void publishAccel(geometry_msgs::Vector3Stamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_accel_.publish(msg);
  }

  void publishStatus(myo_ros::StatusStamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_status_.publish(msg);
  }
};

// Classes that inherit from myo::DeviceListener can be used to receive events from Myo devices. DeviceListener
// provides several virtual functions for handling different kinds of events. If you do not override an event, the
// default behavior is to do nothing.
class DataCollector : public myo::DeviceListener
{
  typedef boost::shared_ptr<MyoRos> MyoRosPtr;

  int64_t time_offset_us_;

  size_t myo_counter_;
  std::map<myo::Myo*, MyoRosPtr> myo_map_;

  ros::NodeHandlePtr node_;

public:
  DataCollector(ros::NodeHandlePtr& nh): time_offset_us_(0), myo_counter_(0)
  {
    node_ = nh;
  }

  void addMyo(myo::Myo* myo)
  {
    MyoRosPtr mt;

    // If new Myo found assign new ID
    if (myo_map_.find(myo) == myo_map_.end())
    {
      myo_map_[myo] = MyoRosPtr(new MyoRos(node_, myo, ++myo_counter_));
    }
  }

  void removeMyo(myo::Myo* myo)
  {
    myo_map_.erase(myo);
  }

  inline ros::Time myoToRosTime(uint64_t timestamp) {
    // Myo timestamp is in microseconds
    uint64_t new_ts = timestamp + time_offset_us_;
    return ros::Time(new_ts / 1000000, (new_ts % 1000000) * 1000);
  }

  void onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
  {
    if (time_offset_us_ == 0)
    {
      time_offset_us_ = ros::Time::now().toNSec() / 1000 - timestamp; // time difference between ROS and Myo in microseconds
    }

    this->addMyo(myo);
  }

  void onUnpair(myo::Myo* myo, uint64_t timestamp)
  {
    time_offset_us_ = 0;

    this->removeMyo(myo);
  }

  // onConnect() is called when a paired Myo is connected
  void onConnect(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
  {
    ROS_INFO("Connected to %s", myo_map_[myo]->getNs().c_str());
  }

  // onDisconnect() is called when a paired Myo is disconnected
  void onDisconnect(myo::Myo* myo, uint64_t timestamp)
  {
    ROS_INFO("Disconnected from %s", myo_map_[myo]->getNs().c_str());
  }

  // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
  // as a unit quaternion.
  void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
  {
    geometry_msgs::QuaternionStamped msg;

    // We have to fill in all data here, except for frame id
    msg.header.stamp = myoToRosTime(timestamp);
    msg.quaternion.w = quat.w();
    msg.quaternion.x = quat.x();
    msg.quaternion.y = quat.y();
    msg.quaternion.z = quat.z();

    // Publish rotation and tf frame
    myo_map_[myo]->publishRotation(msg);
  }

  // onPose() is called whenever the Myo detects that the person wearing it has changed their pose, for example,
  // making a fist, or not making a fist anymore.
  void onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
  {
    myo_ros::GestureStamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.gesture.gesture = pose.type();

    myo_map_[myo]->publishGesture(msg);
  }

  // onGyroscopeData() called when a paired Myo has provided new gyroscope data in units of deg/s
  void onGyroscopeData(myo::Myo *myo, uint64_t timestamp, const myo::Vector3<float> &gyro)
  {
    geometry_msgs::Vector3Stamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.vector.x = gyro.x();
    msg.vector.y = gyro.y();
    msg.vector.z = gyro.z();

    myo_map_[myo]->publishGyro(msg);
  }

  // onAccelerometerData() called when a paired Myo has provided new accelerometer data in units of g.
  void onAccelerometerData(myo::Myo *myo, uint64_t timestamp, const myo::Vector3<float> &accel)
  {
    geometry_msgs::Vector3Stamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.vector.x = accel.x();
    msg.vector.y = accel.y();
    msg.vector.z = accel.z();

    myo_map_[myo]->publishAccel(msg);
}

  // onArmSync() is called whenever Myo has recognized a Sync Gesture after someone has put it on their
  // arm. This lets Myo know which arm it's on and which way it's facing.
  void onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm, myo::XDirection xDirection)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.status.sync = true;
    msg.status.unlock = false;
    msg.status.arm = static_cast<int8_t>(arm);
    msg.status.direction = static_cast<int8_t>(xDirection);

    myo_map_[myo]->publishStatus(msg);
  }

  // onArmUnsync() is called whenever Myo has detected that it was moved from a stable position on a person's arm after
  // it recognized the arm. Typically this happens when someone takes Myo off of their arm, but it can also happen
  // when Myo is moved around on the arm.
  void onArmUnsync(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.status.sync = false;
    msg.status.unlock = false;
    msg.status.arm = static_cast<int8_t>(myo::armUnknown);
    msg.status.direction = static_cast<int8_t>(myo::xDirectionUnknown);

    myo_map_[myo]->publishStatus(msg);
  }

  // onUnlock() is called whenever Myo has become unlocked, and will start delivering pose events.
  void onUnlock(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.status.unlock = true;

    myo_map_[myo]->publishStatus(msg);
  }

  // onLock() is called whenever Myo has become locked. No pose events will be sent until the Myo is unlocked again.
  void onLock(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myoToRosTime(timestamp);
    msg.status.unlock = false;

    myo_map_[myo]->publishStatus(msg);
  }
};

int main(int argc, char** argv)
{
  // We catch any exceptions that might occur below -- see the catch statement for more details.
  try
  {
    // Initialize ROS
    ros::init(argc, argv, "myo_ros_osx");

    // First, we create a Hub. The Hub provides access to one or more Myos.
    myo::Hub hub("org.ros.myo_ros_osx");

//    ROS_INFO("Attempting to find Myo...");
//
//    // Next, we try to find a Myo (any Myo) that's nearby and connect to it. waitForAnyMyo() takes a timeout
//    // value in milliseconds. In this case we will try to find a Myo for 10 seconds, and if that fails, the function
//    // will return a null pointer.
//    my_myo = hub.waitForMyo(10000);
//
//    // If waitForMyo() returned a null pointer, we failed to find a Myo, so exit with an error message.
//    if (!my_myo)
//    {
//      ROS_FATAL("Unable to find Myo!");
//      return -1;
//    }
//
//    // We've found a Myo, let's output its MAC address.
//    ROS_INFO("Found Myo");

    ros::NodeHandlePtr nh = ros::NodeHandlePtr(new ros::NodeHandle());

    // Next we construct an instance of our DeviceListener, so that we can register it with the Hub.
    DataCollector collector(nh);

    // Hub::addListener() takes the address of any object whose class inherits from DeviceListener, and will cause
    // Hub::run() to send events to all registered device listeners.
    hub.addListener(&collector);

    // Finally we enter our main loop.
    while (ros::ok())
    {
      // In each iteration of our main loop, we run the Myo event loop for a set number of milliseconds.
      // In this case, we wish to update our display 20 times a second, so we run for 1000/20 milliseconds.
      hub.run(1000 / 20);

      // Spin ROS
      ros::spinOnce();
    }

    // If a standard exception occurred, we print out its message and exit.
  }
  catch (const std::exception& e)
  {
    std::cerr << "Error: " << e.what() << std::endl;
    std::cerr << "Press enter to continue.";
    std::cin.ignore();
    return 1;
  }
}
<commit_msg>Fix: use individual time offset for each Myo<commit_after>// Copyright (C) 2013-2014 Thalmic Labs Inc.
// Confidential and not for redistribution. See LICENSE.txt.
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <string>
#include <stdint.h>

// The only file that needs to be included to use the Myo C++ SDK is myo.hpp.
#include <myo/myo.hpp>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <myo_ros/GestureStamped.h>
#include <myo_ros/Vibration.h>
#include <myo_ros/StatusStamped.h>
#include <std_msgs/Bool.h>

// This class serves as a ROS namespace wrapper and allows
// to publish individual Myo's data in a separate namespace.
class MyoRos {
  ros::NodeHandlePtr pnode_;
  myo::Myo* myo_;
  size_t id_;
  std::string frame_id_;
  std::string fixed_frame_id_;
  std::string ns_;

  uint64_t time_offset_us_;

  // Information that will be going out
  geometry_msgs::QuaternionStamped msg_rotation_;
  myo_ros::GestureStamped msg_gesture_;
  myo_ros::StatusStamped msg_status_;
  geometry_msgs::Vector3Stamped msg_gyro_;
  geometry_msgs::Vector3Stamped msg_accel_;

  ros::Publisher pub_rotation_;
  ros::Publisher pub_gesture_;
  ros::Publisher pub_gyro_;
  ros::Publisher pub_accel_;
  ros::Publisher pub_status_;
  ros::Subscriber sub_vibration_;
  ros::Subscriber sub_unlock_override_;

  tf::TransformBroadcaster br_;
public:

  MyoRos(ros::NodeHandlePtr& parent, myo::Myo* myo, size_t id, uint64_t time_offset_us)
    : myo_(myo), id_(id), fixed_frame_id_("/world"), time_offset_us_(time_offset_us)
  {
    ros::NodeHandlePtr pnode_root = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    if (pnode_root->hasParam("static_myo_ids"))
    {
      XmlRpc::XmlRpcValue v;
      pnode_root->param("static_myo_ids", v, v);
      for (int i = 0; i < v.size(); i++)
      {
        if (std::string(v[i]) == myo->getMacAddress())
        {
          id_ = id = i + 1;
          break;
        }
      }
    }

    ns_ = std::string("myo") + std::to_string(id_);
    frame_id_ = std::string("myo") + std::to_string(id_) + std::string("_frame");
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle(*parent, ns_));
    // Subscribe & publish setup
    pub_rotation_ = pnode_->advertise<geometry_msgs::QuaternionStamped>("rotation", 100);
    pub_gesture_ = pnode_->advertise<myo_ros::GestureStamped>("gesture", 100);
    pub_gyro_ = pnode_->advertise<geometry_msgs::Vector3Stamped>("gyro", 100);
    pub_accel_ = pnode_->advertise<geometry_msgs::Vector3Stamped>("accel", 100);
    pub_status_ = pnode_->advertise<myo_ros::StatusStamped>("status", 100);

    sub_vibration_ = pnode_->subscribe<myo_ros::Vibration>("vibration", 100, boost::bind(&MyoRos::vibrationCallback, this, _1));
    sub_unlock_override_ = pnode_->subscribe<std_msgs::Bool>("unlock_override", 100, boost::bind(&MyoRos::unlockCallback, this, _1));

    ROS_INFO("Paired Myo (%s) with ID: %s. The time offset is: %lli", myo->getName().c_str(), ns_.c_str(), time_offset_us_);
  }

  inline ros::Time myoToRosTime(uint64_t timestamp) {
    // Myo timestamp is in microseconds
    uint64_t new_ts = timestamp + time_offset_us_;
    return ros::Time(new_ts / 1000000, (new_ts % 1000000) * 1000);
  }

  std::string getNs() const
  {
    return ns_;
  }

  // Creates vibrations
  void vibrationCallback(const myo_ros::VibrationConstPtr& vibration)
  {
    if (myo_)
    {
      myo_->vibrate(static_cast<myo::Myo::VibrationType>(vibration->vibration));
    }
  }

  // Sets the unlock timing
  void unlockCallback(const std_msgs::BoolConstPtr& unlock)
  {
    if (myo_)
    {
      if (unlock->data == true)
      {
        myo_->unlock(myo::Myo::unlockHold);
      }
      else
      {
        myo_->unlock(myo::Myo::unlockTimed);
      }
    }
  }

  void publishRotation(geometry_msgs::QuaternionStamped& msg)
  {
    tf::Transform myo_tf;
    tf::Quaternion tf_quat;

//    myo_tf.setOrigin(tf::Vector3(0.0, 0.0, 1.5));
    tf::quaternionMsgToTF(msg.quaternion, tf_quat);
    myo_tf.setRotation(tf_quat);

    // Myo returns data in global inertial frame, e.g. /world
    msg.header.frame_id = fixed_frame_id_;

    pub_rotation_.publish(msg);
    br_.sendTransform(tf::StampedTransform(myo_tf, msg.header.stamp, fixed_frame_id_, frame_id_));

  }

  void publishGesture(myo_ros::GestureStamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_gesture_.publish(msg);
  }

  void publishGyro(geometry_msgs::Vector3Stamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_gyro_.publish(msg);
  }

  void publishAccel(geometry_msgs::Vector3Stamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_accel_.publish(msg);
  }

  void publishStatus(myo_ros::StatusStamped& msg)
  {
    msg.header.frame_id = fixed_frame_id_;

    pub_status_.publish(msg);
  }
};

// Classes that inherit from myo::DeviceListener can be used to receive events from Myo devices. DeviceListener
// provides several virtual functions for handling different kinds of events. If you do not override an event, the
// default behavior is to do nothing.
class DataCollector : public myo::DeviceListener
{
  typedef boost::shared_ptr<MyoRos> MyoRosPtr;

  size_t myo_counter_;
  std::map<myo::Myo*, MyoRosPtr> myo_map_;

  ros::NodeHandlePtr node_;

public:
  DataCollector(ros::NodeHandlePtr& nh): myo_counter_(0)
  {
    node_ = nh;
  }

  void addMyo(myo::Myo* myo, int64_t time_offset_us)
  {
    MyoRosPtr mt;

    // If new Myo found assign new ID
    if (myo_map_.find(myo) == myo_map_.end())
    {
      myo_map_[myo] = MyoRosPtr(new MyoRos(node_, myo, ++myo_counter_, time_offset_us));
    }
  }

  void removeMyo(myo::Myo* myo)
  {
    myo_map_.erase(myo);
  }

  void onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
  {
    // time difference between ROS and Myo in microseconds
    int64_t time_offset_us = ros::Time::now().toNSec() / 1000 - timestamp;

    this->addMyo(myo, time_offset_us);
  }

  void onUnpair(myo::Myo* myo, uint64_t timestamp)
  {
    this->removeMyo(myo);
  }

  // onConnect() is called when a paired Myo is connected
  void onConnect(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion firmwareVersion)
  {
    ROS_INFO("Connected to %s", myo_map_[myo]->getNs().c_str());
  }

  // onDisconnect() is called when a paired Myo is disconnected
  void onDisconnect(myo::Myo* myo, uint64_t timestamp)
  {
    ROS_INFO("Disconnected from %s", myo_map_[myo]->getNs().c_str());
  }

  // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
  // as a unit quaternion.
  void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
  {
    geometry_msgs::QuaternionStamped msg;

    // We have to fill in all data here, except for frame id
    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.quaternion.w = quat.w();
    msg.quaternion.x = quat.x();
    msg.quaternion.y = quat.y();
    msg.quaternion.z = quat.z();

    // Publish rotation and tf frame
    myo_map_[myo]->publishRotation(msg);
  }

  // onPose() is called whenever the Myo detects that the person wearing it has changed their pose, for example,
  // making a fist, or not making a fist anymore.
  void onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
  {
    myo_ros::GestureStamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.gesture.gesture = pose.type();

    myo_map_[myo]->publishGesture(msg);
  }

  // onGyroscopeData() called when a paired Myo has provided new gyroscope data in units of deg/s
  void onGyroscopeData(myo::Myo *myo, uint64_t timestamp, const myo::Vector3<float> &gyro)
  {
    geometry_msgs::Vector3Stamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.vector.x = gyro.x();
    msg.vector.y = gyro.y();
    msg.vector.z = gyro.z();

    myo_map_[myo]->publishGyro(msg);
  }

  // onAccelerometerData() called when a paired Myo has provided new accelerometer data in units of g.
  void onAccelerometerData(myo::Myo *myo, uint64_t timestamp, const myo::Vector3<float> &accel)
  {
    geometry_msgs::Vector3Stamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.vector.x = accel.x();
    msg.vector.y = accel.y();
    msg.vector.z = accel.z();

    myo_map_[myo]->publishAccel(msg);
}

  // onArmSync() is called whenever Myo has recognized a Sync Gesture after someone has put it on their
  // arm. This lets Myo know which arm it's on and which way it's facing.
  void onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm, myo::XDirection xDirection)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.status.sync = true;
    msg.status.unlock = false;
    msg.status.arm = static_cast<int8_t>(arm);
    msg.status.direction = static_cast<int8_t>(xDirection);

    myo_map_[myo]->publishStatus(msg);
  }

  // onArmUnsync() is called whenever Myo has detected that it was moved from a stable position on a person's arm after
  // it recognized the arm. Typically this happens when someone takes Myo off of their arm, but it can also happen
  // when Myo is moved around on the arm.
  void onArmUnsync(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.status.sync = false;
    msg.status.unlock = false;
    msg.status.arm = static_cast<int8_t>(myo::armUnknown);
    msg.status.direction = static_cast<int8_t>(myo::xDirectionUnknown);

    myo_map_[myo]->publishStatus(msg);
  }

  // onUnlock() is called whenever Myo has become unlocked, and will start delivering pose events.
  void onUnlock(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.status.unlock = true;

    myo_map_[myo]->publishStatus(msg);
  }

  // onLock() is called whenever Myo has become locked. No pose events will be sent until the Myo is unlocked again.
  void onLock(myo::Myo* myo, uint64_t timestamp)
  {
    myo_ros::StatusStamped msg;

    msg.header.stamp = myo_map_[myo]->myoToRosTime(timestamp);
    msg.status.unlock = false;

    myo_map_[myo]->publishStatus(msg);
  }
};

int main(int argc, char** argv)
{
  // We catch any exceptions that might occur below -- see the catch statement for more details.
  try
  {
    // Initialize ROS
    ros::init(argc, argv, "myo_ros_osx");

    // First, we create a Hub. The Hub provides access to one or more Myos.
    myo::Hub hub("org.ros.myo_ros_osx");

//    ROS_INFO("Attempting to find Myo...");
//
//    // Next, we try to find a Myo (any Myo) that's nearby and connect to it. waitForAnyMyo() takes a timeout
//    // value in milliseconds. In this case we will try to find a Myo for 10 seconds, and if that fails, the function
//    // will return a null pointer.
//    my_myo = hub.waitForMyo(10000);
//
//    // If waitForMyo() returned a null pointer, we failed to find a Myo, so exit with an error message.
//    if (!my_myo)
//    {
//      ROS_FATAL("Unable to find Myo!");
//      return -1;
//    }
//
//    // We've found a Myo, let's output its MAC address.
//    ROS_INFO("Found Myo");

    ros::NodeHandlePtr nh = ros::NodeHandlePtr(new ros::NodeHandle());

    // Next we construct an instance of our DeviceListener, so that we can register it with the Hub.
    DataCollector collector(nh);

    // Hub::addListener() takes the address of any object whose class inherits from DeviceListener, and will cause
    // Hub::run() to send events to all registered device listeners.
    hub.addListener(&collector);

    // Finally we enter our main loop.
    while (ros::ok())
    {
      // In each iteration of our main loop, we run the Myo event loop for a set number of milliseconds.
      // In this case, we wish to update our display 20 times a second, so we run for 1000/20 milliseconds.
      hub.run(1000 / 20);

      // Spin ROS
      ros::spinOnce();
    }

    // If a standard exception occurred, we print out its message and exit.
  }
  catch (const std::exception& e)
  {
    std::cerr << "Error: " << e.what() << std::endl;
    std::cerr << "Press enter to continue.";
    std::cin.ignore();
    return 1;
  }
}
<|endoftext|>


<commit_before>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

// bzflag common header
#include "common.h"

// interface header
#include "Address.h"

// system headers
#include <string.h>
#include <string>
#include <sys/types.h>
#if !defined(_WIN32)
#include <unistd.h>
#include <setjmp.h>
#else
#include <winsock2.h>
#include <ws2tcpip.h>
#endif

// common implementation headers
#include "bzsignal.h"
#include "ErrorHandler.h"

// local implementation headers
#include "Pack.h"

#if defined(sun)
  // Solaris...
  extern "C" int inet_aton(const char *, struct in_addr *);
#endif

//
// Address
//

#if !defined( GUSI_20 ) && !defined( WIN32 )
  Address			Address::localAddress("");
#endif

Address::Address()
{
  InAddr tempAddr;

  memset(&tempAddr, 0, sizeof(tempAddr));
  tempAddr.s_addr = htonl(INADDR_ANY);
  addr.push_back(tempAddr);
}

Address::Address(const std::string& name)
{
  Address a = getHostAddress(name);
  addr.push_back(a.addr[0]);
}

Address::Address(const Address& address) : addr(address.addr)
{
  // do nothing
}

Address::Address(const InAddr& _addr)
{
  addr.push_back(_addr);
}

Address::Address(const struct sockaddr_in& _addr)
{
  addr.push_back(_addr.sin_addr);
}

Address::~Address()
{
  // do nothing
}

Address&		Address::operator=(const Address& address)
{
  addr.clear();
  addr.push_back(address.addr[0]);
  return *this;
}

Address::operator InAddr() const
{
  return addr[0];
}

bool			Address::operator==(const Address& address) const
{
  return addr[0].s_addr == address.addr[0].s_addr;
}

bool			Address::operator!=(const Address& address) const
{
  return addr[0].s_addr != address.addr[0].s_addr;
}

bool			Address::isAny() const
{
  return addr[0].s_addr == htonl(INADDR_ANY);
}

std::string		Address::getDotNotation() const
{
  return std::string(inet_ntoa(addr[0]));
}

uint8_t		        Address::getIPVersion() const {
  return 4;
}

#if !defined(_WIN32)
static jmp_buf alarmEnv;
static void		onAlarm(int)
{
  // jump back to setjmp.  this handles the race condition where we
  // set the alarm but gethostbyname() wouldn't have been called
  // until after the alarm goes off (resulting in an indeterminate
  // wait).
  longjmp(alarmEnv, 1);
}
#endif

Address			Address::getHostAddress(const std::string hname)
{
  Address a;
  InAddr tempAddr;
  int j;

  struct hostent* hent;
  if (hname == "") {				// local address
    char hostname[MAXHOSTNAMELEN+1];
    if (gethostname(hostname, sizeof(hostname)) >= 0)
      hent = gethostbyname(hostname);
    else
      return a;
  }
  else if (inet_aton(hname.c_str(), &tempAddr) != 0) {
    a.addr.clear();
    a.addr.push_back(tempAddr);
    return a;
  }
  else {				// non-local address
#if !defined(_WIN32)
    // set alarm to avoid waiting too long
    SIG_PF oldAlarm = bzSignal(SIGALRM, SIG_PF(onAlarm));
    if (oldAlarm != SIG_ERR) {
      if (setjmp(alarmEnv) != 0) {
	// alarm went off
	hent = NULL;
	printError("Looking up host name: timeout");
	return a;
      }

      // wait up to this many seconds
      alarm(8);
    }
#endif

    hent = gethostbyname(hname.c_str());

#if !defined(_WIN32)
    if (oldAlarm != SIG_ERR) {
      alarm(0);
      bzSignal(SIGALRM, oldAlarm);
    }
#endif
  }

  if (!hent) {
    herror("Looking up host name");
    return a;
  }

  a.addr.clear();
  for (j=0; hent->h_addr_list[j] != NULL; j++){
    ::memcpy(&tempAddr, hent->h_addr_list[j], sizeof(tempAddr));
    a.addr.push_back(tempAddr);
  }
  return a;
}

std::string		Address::getHostByAddress(InAddr addr)
{
#if !defined(_WIN32)
  // set alarm to avoid waiting too long
  SIG_PF oldAlarm = bzSignal(SIGALRM, SIG_PF(onAlarm));
  if (oldAlarm != SIG_ERR) {
    if (setjmp(alarmEnv) != 0) {
      // alarm went off
      return std::string(inet_ntoa(addr));
    }

    // wait up to this many seconds
    alarm(8);
  }
#endif

  int addrLen = sizeof(addr);
  struct hostent* hent = gethostbyaddr((char*)&addr, addrLen, AF_INET);

#if !defined(_WIN32)
  if (oldAlarm != SIG_ERR) {
    alarm(0);
    bzSignal(SIGALRM, oldAlarm);
  }
#endif

  if (!hent) {
    // can't lookup name -- return in standard dot notation
    return std::string(inet_ntoa(addr));
  }
  return std::string(hent->h_name);
}

const std::string Address::getHostName(const std::string hostname) // const
{
  char myname[MAXHOSTNAMELEN+1];
  std::string name = hostname;
  if (name.length() <= 0) {
    if (gethostname(myname, sizeof(myname)) >= 0)
      name = std::string(myname);
  }
  if (name.length() <= 0) {
    return std::string("");
  }
  struct hostent* hent = gethostbyname(name.c_str());
  if (!hent) {
    return std::string("");
  }
  return std::string(hent->h_name);
}

void*			Address::pack(void* _buf) const
{
  unsigned char* buf = (unsigned char*)_buf;
  buf = (unsigned char*)nboPackUByte(_buf, 4);
  // everything in InAddr  is already in network byte order
  int32_t hostaddr = int32_t(addr[0].s_addr);
  ::memcpy(buf, &hostaddr, sizeof(int32_t));	buf += sizeof(int32_t);
  return (void*)buf;
}

void*			Address::unpack(void* _buf)
{
  unsigned char* buf = (unsigned char*)_buf;
  InAddr tempAddr;
  // FIXME - should actually parse the first byte to see if it's IPv4 or
  // IPv6
  ++buf;
  // everything in InAddr should be stored in network byte order
  int32_t hostaddr;
  ::memcpy(&hostaddr, buf, sizeof(int32_t));	buf += sizeof(int32_t);
  tempAddr.s_addr = u_long(hostaddr);
  addr.clear();
  addr.push_back(tempAddr);
  return (void*)buf;
}

//
// ServerId
//

void*			ServerId::pack(void* _buf) const
{
  // everything in PlayerId is already in network byte order
  unsigned char* buf = (unsigned char*)_buf;
  int32_t hostaddr = int32_t(serverHost.s_addr);
  ::memcpy(buf, &hostaddr, sizeof(int32_t));	buf += sizeof(int32_t);
  ::memcpy(buf, &port, sizeof(int16_t));	buf += sizeof(int16_t);
  ::memcpy(buf, &number, sizeof(int16_t));	buf += sizeof(int16_t);
  return (void*)buf;
}

void*			ServerId::unpack(void* _buf)
{
  // everything in ServerId should be stored in network byte order
  unsigned char* buf = (unsigned char*)_buf;
  int32_t hostaddr;
  ::memcpy(&hostaddr, buf, sizeof(int32_t));	buf += sizeof(int32_t);
  ::memcpy(&port, buf, sizeof(int16_t));	buf += sizeof(int16_t);
  ::memcpy(&number, buf, sizeof(int16_t));	buf += sizeof(int16_t);
  serverHost.s_addr = u_long(hostaddr);
  return (void*)buf;
}

bool			ServerId::operator==(const ServerId& id) const
{
  return serverHost.s_addr == id.serverHost.s_addr &&
			port == id.port && number == id.number;
}

bool			ServerId::operator!=(const ServerId& id) const
{
  return serverHost.s_addr != id.serverHost.s_addr ||
			port != id.port || number != id.number;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<commit_msg>try to fix winsock debaucle<commit_after>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

// bzflag common header
#include "common.h"

// interface header
#include "Address.h"

// system headers
#include <string.h>
#include <string>
#include <sys/types.h>
#if !defined(_WIN32)
#include <unistd.h>
#include <setjmp.h>
#endif

// common implementation headers
#include "bzsignal.h"
#include "ErrorHandler.h"

// local implementation headers
#include "Pack.h"

#if defined(sun)
  // Solaris...
  extern "C" int inet_aton(const char *, struct in_addr *);
#endif

//
// Address
//

#if !defined( GUSI_20 ) && !defined( WIN32 )
  Address			Address::localAddress("");
#endif

Address::Address()
{
  InAddr tempAddr;

  memset(&tempAddr, 0, sizeof(tempAddr));
  tempAddr.s_addr = htonl(INADDR_ANY);
  addr.push_back(tempAddr);
}

Address::Address(const std::string& name)
{
  Address a = getHostAddress(name);
  addr.push_back(a.addr[0]);
}

Address::Address(const Address& address) : addr(address.addr)
{
  // do nothing
}

Address::Address(const InAddr& _addr)
{
  addr.push_back(_addr);
}

Address::Address(const struct sockaddr_in& _addr)
{
  addr.push_back(_addr.sin_addr);
}

Address::~Address()
{
  // do nothing
}

Address&		Address::operator=(const Address& address)
{
  addr.clear();
  addr.push_back(address.addr[0]);
  return *this;
}

Address::operator InAddr() const
{
  return addr[0];
}

bool			Address::operator==(const Address& address) const
{
  return addr[0].s_addr == address.addr[0].s_addr;
}

bool			Address::operator!=(const Address& address) const
{
  return addr[0].s_addr != address.addr[0].s_addr;
}

bool			Address::isAny() const
{
  return addr[0].s_addr == htonl(INADDR_ANY);
}

std::string		Address::getDotNotation() const
{
  return std::string(inet_ntoa(addr[0]));
}

uint8_t		        Address::getIPVersion() const {
  return 4;
}

#if !defined(_WIN32)
static jmp_buf alarmEnv;
static void		onAlarm(int)
{
  // jump back to setjmp.  this handles the race condition where we
  // set the alarm but gethostbyname() wouldn't have been called
  // until after the alarm goes off (resulting in an indeterminate
  // wait).
  longjmp(alarmEnv, 1);
}
#endif

Address			Address::getHostAddress(const std::string hname)
{
  Address a;
  InAddr tempAddr;
  int j;

  struct hostent* hent;
  if (hname == "") {				// local address
    char hostname[MAXHOSTNAMELEN+1];
    if (gethostname(hostname, sizeof(hostname)) >= 0)
      hent = gethostbyname(hostname);
    else
      return a;
  }
  else if (inet_aton(hname.c_str(), &tempAddr) != 0) {
    a.addr.clear();
    a.addr.push_back(tempAddr);
    return a;
  }
  else {				// non-local address
#if !defined(_WIN32)
    // set alarm to avoid waiting too long
    SIG_PF oldAlarm = bzSignal(SIGALRM, SIG_PF(onAlarm));
    if (oldAlarm != SIG_ERR) {
      if (setjmp(alarmEnv) != 0) {
	// alarm went off
	hent = NULL;
	printError("Looking up host name: timeout");
	return a;
      }

      // wait up to this many seconds
      alarm(8);
    }
#endif

    hent = gethostbyname(hname.c_str());

#if !defined(_WIN32)
    if (oldAlarm != SIG_ERR) {
      alarm(0);
      bzSignal(SIGALRM, oldAlarm);
    }
#endif
  }

  if (!hent) {
    herror("Looking up host name");
    return a;
  }

  a.addr.clear();
  for (j=0; hent->h_addr_list[j] != NULL; j++){
    ::memcpy(&tempAddr, hent->h_addr_list[j], sizeof(tempAddr));
    a.addr.push_back(tempAddr);
  }
  return a;
}

std::string		Address::getHostByAddress(InAddr addr)
{
#if !defined(_WIN32)
  // set alarm to avoid waiting too long
  SIG_PF oldAlarm = bzSignal(SIGALRM, SIG_PF(onAlarm));
  if (oldAlarm != SIG_ERR) {
    if (setjmp(alarmEnv) != 0) {
      // alarm went off
      return std::string(inet_ntoa(addr));
    }

    // wait up to this many seconds
    alarm(8);
  }
#endif

  int addrLen = sizeof(addr);
  struct hostent* hent = gethostbyaddr((char*)&addr, addrLen, AF_INET);

#if !defined(_WIN32)
  if (oldAlarm != SIG_ERR) {
    alarm(0);
    bzSignal(SIGALRM, oldAlarm);
  }
#endif

  if (!hent) {
    // can't lookup name -- return in standard dot notation
    return std::string(inet_ntoa(addr));
  }
  return std::string(hent->h_name);
}

const std::string Address::getHostName(const std::string hostname) // const
{
  char myname[MAXHOSTNAMELEN+1];
  std::string name = hostname;
  if (name.length() <= 0) {
    if (gethostname(myname, sizeof(myname)) >= 0)
      name = std::string(myname);
  }
  if (name.length() <= 0) {
    return std::string("");
  }
  struct hostent* hent = gethostbyname(name.c_str());
  if (!hent) {
    return std::string("");
  }
  return std::string(hent->h_name);
}

void*			Address::pack(void* _buf) const
{
  unsigned char* buf = (unsigned char*)_buf;
  buf = (unsigned char*)nboPackUByte(_buf, 4);
  // everything in InAddr  is already in network byte order
  int32_t hostaddr = int32_t(addr[0].s_addr);
  ::memcpy(buf, &hostaddr, sizeof(int32_t));	buf += sizeof(int32_t);
  return (void*)buf;
}

void*			Address::unpack(void* _buf)
{
  unsigned char* buf = (unsigned char*)_buf;
  InAddr tempAddr;
  // FIXME - should actually parse the first byte to see if it's IPv4 or
  // IPv6
  ++buf;
  // everything in InAddr should be stored in network byte order
  int32_t hostaddr;
  ::memcpy(&hostaddr, buf, sizeof(int32_t));	buf += sizeof(int32_t);
  tempAddr.s_addr = u_long(hostaddr);
  addr.clear();
  addr.push_back(tempAddr);
  return (void*)buf;
}

//
// ServerId
//

void*			ServerId::pack(void* _buf) const
{
  // everything in PlayerId is already in network byte order
  unsigned char* buf = (unsigned char*)_buf;
  int32_t hostaddr = int32_t(serverHost.s_addr);
  ::memcpy(buf, &hostaddr, sizeof(int32_t));	buf += sizeof(int32_t);
  ::memcpy(buf, &port, sizeof(int16_t));	buf += sizeof(int16_t);
  ::memcpy(buf, &number, sizeof(int16_t));	buf += sizeof(int16_t);
  return (void*)buf;
}

void*			ServerId::unpack(void* _buf)
{
  // everything in ServerId should be stored in network byte order
  unsigned char* buf = (unsigned char*)_buf;
  int32_t hostaddr;
  ::memcpy(&hostaddr, buf, sizeof(int32_t));	buf += sizeof(int32_t);
  ::memcpy(&port, buf, sizeof(int16_t));	buf += sizeof(int16_t);
  ::memcpy(&number, buf, sizeof(int16_t));	buf += sizeof(int16_t);
  serverHost.s_addr = u_long(hostaddr);
  return (void*)buf;
}

bool			ServerId::operator==(const ServerId& id) const
{
  return serverHost.s_addr == id.serverHost.s_addr &&
			port == id.port && number == id.number;
}

bool			ServerId::operator!=(const ServerId& id) const
{
  return serverHost.s_addr != id.serverHost.s_addr ||
			port != id.port || number != id.number;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8

<|endoftext|>


<commit_before>#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <asio.hpp>

#include <cstring>
#include <cstdlib>
#include <iostream>
#include <vector>

#include "antenna.hxx"
#include "tuner.hxx"
#include "io.hxx"
#include "packet_processor.hxx"
#include "abuhops.hxx"

using namespace std;

namespace abuhops {
  #define MIN_TIME_BETWEEN_WHOAMI 1024
  #define MAX_TIME_BETWEEN_WHOAMI 4096
  #define MIN_TIME_BETWEEN_PING 16384
  #define MAX_TIME_BETWEEN_PING 32768
  #define TIME_BETWEEN_CONNECT 1024
  #define MAX_CONNECT_ATTEMPTS 16

  #define CONNECT 0
  #define PING 1
  #define PROXY 2
  #define POST 3
  #define LIST 4
  #define SIGN 5
  #define BYE 6

  #define YOUARE 0
  #define PONG 1
  #define FROMOTHER 2
  #define ADVERT 3
  #define SIGNATURE 5

  #define IPV4PORT 12545
  #define IPV6PORT 12546
  #define SERVER "ABENDSTERN.SERVEGAME.COM."

  #define HMAC_SIZE 32
  #define MAX_NAME_LENGTH 128

  #ifdef DEBUG
  #define debug(x) cout << "abuhops: " << x << endl;
  #else
  #define debug(x)
  #endif

  static unsigned timeUntilPing = 0;

  static bool isConnected4 = false,  isConnected6 = false, isConnecting = true;
  static unsigned timeUntilConnectXmit = 0;
  static byte connectPacket[1+4+4+HMAC_SIZE+MAX_NAME_LENGTH+1];
  static unsigned connectPacketSize;
  static unsigned connectionAttempts = 0;

  static bool knowIpv4Address = false, knowIpv6Address = false;

  static Antenna::endpoint server4, server6;
  static bool hasv4 = false, hasv6 = false;

  static void (*listCallback)(void*, const byte*, unsigned) = NULL;
  static void* listCallbackUserdata = NULL;

  static void sendConnectPacket();
  static void processPacket(bool v6, const byte* data, unsigned len);

  void connect(unsigned id, const char* name,
               unsigned timestamp, const char* hmac) {
    static bool hasInit = false;
    debug(">> CONNECT");
    if (!hasInit) {
      hasInit = true;

      try {
        //Look server IP address up
        asio::io_service svc;
        asio::ip::udp::resolver resolver(svc);
        asio::ip::udp::resolver::query query(SERVER, "0");
        asio::ip::udp::resolver::iterator it(resolver.resolve(query)), end;
        while (it != end && (!hasv4 || !hasv6)) {
          asio::ip::address addr(it++->endpoint().address());
          if (addr.is_v4() && !hasv4) {
            hasv4 = true;
            server4 = Antenna::endpoint(addr, IPV4PORT);
            cout << "Resolved " << SERVER << " ipv4 to " << addr.to_string()
                 << endl;
          } else if (addr.is_v6() && !hasv6) {
            hasv6 = true;
            server6 = Antenna::endpoint(addr, IPV6PORT);
            cout << "Resolved " << SERVER << " ipv6 to " << addr.to_string()
                 << endl;
          }
        }
      } catch (const asio::system_error& err) {
        cerr << "Error resolving " << SERVER << ": " << err.what() << endl;
      }

      //Even if we did get a result for a certain protocol, we can't use it
      //if it is not available
      hasv4 &= antenna.hasV4();
      hasv6 &= antenna.hasV6();

      ensureRegistered();
    }

    //Set initial conditions
    isConnected4 = isConnected6 = false;
    isConnecting = true;
    timeUntilConnectXmit = 0;
    connectionAttempts = 0;

    //Write the connection packet
    byte* pack = connectPacket;
    *pack++ = CONNECT;
    io::write(pack, id);
    io::write(pack, timestamp);
    //Convert HMAC from hex
    for (unsigned i = 0; i < HMAC_SIZE; ++i) {
#define HEXIT(x) (x >= '0' && x <= '9'? x-'0' : \
                  x >= 'a' && x <= 'f'? x-'a'+0xa : x-'A'+0xA)
      *pack++ = (HEXIT(hmac[2*i])<<4) | HEXIT(hmac[2*i+1]);
#undef HEXIT
    }
    //Copy name into packet.
    strncpy((char*)pack, name, MAX_NAME_LENGTH);
    pack[MAX_NAME_LENGTH] = 0;

    //Set packet size
    connectPacketSize = (pack - connectPacket) + strlen(name)+1;

    //Done, send the first packet immediately
    sendConnectPacket();
  }

  static void sendConnectPacket() {
    debug(">> CONNECT (send)");
    if (hasv4 && !isConnected4)
      antenna.send(server4, connectPacket, connectPacketSize);
    if (hasv6 && !isConnected6)
      antenna.send(server6, connectPacket, connectPacketSize);

    ++connectionAttempts;
    if (connectionAttempts > MAX_CONNECT_ATTEMPTS) {
      //Give up, the server probably is not reachable on one or both protocols
      hasv4 &= isConnected4;
      hasv6 &= isConnected6;
      isConnecting = false;
    }

    //If we have not given up or connected yet, try again in a little bit
    timeUntilConnectXmit = TIME_BETWEEN_CONNECT;
  }

  void ensureRegistered() {
    class PP: public PacketProcessor {
    public:
      bool v6;
      PP(bool v) : v6(v) {}
      virtual void process(const Antenna::endpoint&, Antenna*, Tuner*,
                           const byte* data, unsigned len) {
        processPacket(v6, data, len);
      }
    } static ppv4(false), ppv6(true);

    if (antenna.tuner) {
      if (hasv4)
        antenna.tuner->connect(server4, &ppv4);
      if (hasv6)
        antenna.tuner->connect(server6, &ppv6);
    }
  }

  void bye() {
    debug(">> BYE");
    byte pack = BYE;
    if (hasv4)
      antenna.send(server4, &pack, 1);
    if (hasv6)
      antenna.send(server6, &pack, 1);

    isConnecting = isConnected4 = isConnected6 = false;
  }

  void post(const byte* dat, unsigned len) {
    debug(">> POST");
    vector<byte> pack(1 + len);
    pack[0] = POST;
    memcpy(&pack[1], dat, len);

    if (isConnected4)
      antenna.send(server4, &pack[0], pack.size());
    if (isConnected6)
      antenna.send(server6, &pack[0], pack.size());
  }

  void list(void (*callback)(void*, const byte*, unsigned),
            void* userdata) {
    debug(">> LIST");
    listCallback = callback;
    listCallbackUserdata = userdata;

    byte pack = LIST;
    if (isConnected4)
      antenna.send(server4, &pack, 1);
    if (isConnected6)
      antenna.send(server6, &pack, 1);
  }

  void stopList() {
    listCallback = NULL;
    listCallbackUserdata = NULL;
  }

  void proxy(const Antenna::endpoint& dst,
             const byte* payload, unsigned len) {
    debug(">> PROXY");
    vector<byte> pack(1 + (dst.address().is_v4()? 4 : 2*8) + 2 + len);
    pack[0] = PROXY;
    byte* dat = &pack[1];
    if (dst.address().is_v4()) {
      asio::ip::address_v4::bytes_type b(dst.address().to_v4().to_bytes());
      memcpy(dat, &b[0], 4);
      dat += 4;
    } else {
      asio::ip::address_v6::bytes_type b(dst.address().to_v6().to_bytes());
      io::a6fromlbo(dat, &b[0]);
      dat += 16;
    }
    io::write(dat, dst.port());
    memcpy(dat, payload, len);

    if (dst.address().is_v4() && isConnected4)
      antenna.send(server4, &pack[0], pack.size());
    else if (dst.address().is_v6() && isConnected6)
      antenna.send(server6, &pack[0], pack.size());
    else
      cerr << "warn: Attempt to proxy via unavailable IP version." << endl;
  }

  bool ready() {
    return (knowIpv4Address && isConnected4) ||
           (knowIpv6Address && isConnected6);
  }

  void update(unsigned et) {
    if (isConnecting) {
      //Move to non-connecting state if both IP versions we support are
      //connected
      if ((!hasv4 || isConnected4) && (!hasv6 || isConnected6))
        isConnecting = true;
      //Otherwise, retransmit connect packet if necessary
      else if (timeUntilConnectXmit <= et)
        sendConnectPacket();
      else
        timeUntilConnectXmit -= et;
    } else if (isConnected4 || isConnected6) {
      //Send PING if needed
      if (timeUntilPing < et) {
        bool whoAmI = (hasv4 && !knowIpv4Address) ||
                      (hasv6 && !knowIpv6Address);
        byte pack[2] = { PING, (byte)whoAmI };
        if (hasv4)
          antenna.send(server4, pack, 2);
        if (hasv6)
          antenna.send(server6, pack, 2);

        unsigned lower = whoAmI? MIN_TIME_BETWEEN_WHOAMI:MIN_TIME_BETWEEN_PING;
        unsigned upper = whoAmI? MAX_TIME_BETWEEN_WHOAMI:MAX_TIME_BETWEEN_PING;
        timeUntilPing = lower + rand()%(upper-lower);
      } else {
        timeUntilPing -= et;
      }
    }
  }

  static void processYouAre(bool, const byte*, unsigned);
  static void processPong(bool, const byte*, unsigned);
  static void processFromOther(bool, const byte*, unsigned);
  static void processAdvert(bool, const byte*, unsigned);
  static void (*const packetTypes[256])(bool, const byte*, unsigned) = {
    processYouAre,
    processPong,
    processFromOther,
    processAdvert,
    NULL,
    /* processSignature, */
    /* rest of array is NULL implicitly */
  };

  static void processPacket(bool v6, const byte* dat, unsigned len) {
    if (packetTypes[dat[0]])
      packetTypes[dat[0]](v6, dat+1, len-1);
    else {
#ifdef DEBUG
      cerr << "WARN: Dropping unknown abuhops packet type " << (unsigned)dat[0]
           << endl;
#endif
    }
  }

  static void processYouAre(bool v6, const byte* dat, unsigned len) {
    debug("<< YOU-ARE");
    if (v6) {
      GlobalID& gid(*antenna.getGlobalID6());
      if (len != 8*2 + 2) {
#ifdef DEBUG
        cerr << "WARN: Dropping IPv6 YOU-ARE packet of length " << len << endl;
#endif
      } else {
        for (unsigned i = 0; i < 8; ++i)
          io::read(dat, gid.ia6[i]);
        io::read(dat, gid.iport);

        knowIpv6Address = true;
        isConnected6 = true;

        cout << "Our IPv6 GlobalID is " << gid.toString() << endl;
      }
    } else {
      GlobalID& gid(*antenna.getGlobalID4());
      if (len != 4 + 2) {
#ifdef DEBUG
        cerr << "WARN: Dropping IPv4 YOU-ARE packet of length " << len << endl;
#endif
      } else {
        memcpy(gid.ia4, dat, 4);
        dat += 4;
        io::read(dat, gid.iport);

        knowIpv4Address = true;
        isConnected4 = true;

        cout << "Our IPv4 GlobalID is " << gid.toString() << endl;
      }
    }
  }

  static void processPong(bool v6, const byte* dat, unsigned len) {
    //Nothing to do
    debug("<< PONG");
  }

  static void processFromOther(bool v6, const byte* dat, unsigned len) {
    debug("<< FROM-OTHER");
    if (!len) {
#ifdef DEBUG
      cerr << "WARN: Dropping FROM-OTHER with empty payload." << endl;
#endif
      return;
    }

    //The default endpoint is used to tell the NetworkGame that it must get the
    //Internet IP address from the STX itself
    Antenna::endpoint defaultEndpoint;
    if (antenna.tuner)
      antenna.tuner->receivePacket(defaultEndpoint, &antenna, dat, len);
  }

  static const char requiredAdvertHeader[] = "Abendspiel";
  #define ADVERT_IPV_OFF (sizeof(requiredAdvertHeader)-1 + 4 + 1)
  static void processAdvert(bool v6, const byte* dat, unsigned len) {
    debug("<< ADVERT");
    Antenna::endpoint defaultEndpoint;
    if (len < sizeof(requiredAdvertHeader) + ADVERT_IPV_OFF ||
        memcmp(dat, requiredAdvertHeader, sizeof(requiredAdvertHeader)-1)) {
#ifdef DEBUG
      cerr << "WARN: Dropping non-Abendspiel ADVERT" << endl;
#endif
      return;
    }

    //Ensure that the reported IP version of the advert matches the Abuhops
    //realm it is comming from
    if ((unsigned)v6 != dat[ADVERT_IPV_OFF]) {
#ifdef DEBUG
      cerr << "WARN: Dropping ADVERT for wrong IP version" << endl;
#endif
      return;
    }

    if (antenna.tuner)
      antenna.tuner->receivePacket(defaultEndpoint, &antenna, dat, len);
  }
}
<commit_msg>Comment out useless warning message in Abuhops client.<commit_after>#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <asio.hpp>

#include <cstring>
#include <cstdlib>
#include <iostream>
#include <vector>

#include "antenna.hxx"
#include "tuner.hxx"
#include "io.hxx"
#include "packet_processor.hxx"
#include "abuhops.hxx"

using namespace std;

namespace abuhops {
  #define MIN_TIME_BETWEEN_WHOAMI 1024
  #define MAX_TIME_BETWEEN_WHOAMI 4096
  #define MIN_TIME_BETWEEN_PING 16384
  #define MAX_TIME_BETWEEN_PING 32768
  #define TIME_BETWEEN_CONNECT 1024
  #define MAX_CONNECT_ATTEMPTS 16

  #define CONNECT 0
  #define PING 1
  #define PROXY 2
  #define POST 3
  #define LIST 4
  #define SIGN 5
  #define BYE 6

  #define YOUARE 0
  #define PONG 1
  #define FROMOTHER 2
  #define ADVERT 3
  #define SIGNATURE 5

  #define IPV4PORT 12545
  #define IPV6PORT 12546
  #define SERVER "ABENDSTERN.SERVEGAME.COM."

  #define HMAC_SIZE 32
  #define MAX_NAME_LENGTH 128

  #ifdef DEBUG
  #define debug(x) cout << "abuhops: " << x << endl;
  #else
  #define debug(x)
  #endif

  static unsigned timeUntilPing = 0;

  static bool isConnected4 = false,  isConnected6 = false, isConnecting = true;
  static unsigned timeUntilConnectXmit = 0;
  static byte connectPacket[1+4+4+HMAC_SIZE+MAX_NAME_LENGTH+1];
  static unsigned connectPacketSize;
  static unsigned connectionAttempts = 0;

  static bool knowIpv4Address = false, knowIpv6Address = false;

  static Antenna::endpoint server4, server6;
  static bool hasv4 = false, hasv6 = false;

  static void (*listCallback)(void*, const byte*, unsigned) = NULL;
  static void* listCallbackUserdata = NULL;

  static void sendConnectPacket();
  static void processPacket(bool v6, const byte* data, unsigned len);

  void connect(unsigned id, const char* name,
               unsigned timestamp, const char* hmac) {
    static bool hasInit = false;
    debug(">> CONNECT");
    if (!hasInit) {
      hasInit = true;

      try {
        //Look server IP address up
        asio::io_service svc;
        asio::ip::udp::resolver resolver(svc);
        asio::ip::udp::resolver::query query(SERVER, "0");
        asio::ip::udp::resolver::iterator it(resolver.resolve(query)), end;
        while (it != end && (!hasv4 || !hasv6)) {
          asio::ip::address addr(it++->endpoint().address());
          if (addr.is_v4() && !hasv4) {
            hasv4 = true;
            server4 = Antenna::endpoint(addr, IPV4PORT);
            cout << "Resolved " << SERVER << " ipv4 to " << addr.to_string()
                 << endl;
          } else if (addr.is_v6() && !hasv6) {
            hasv6 = true;
            server6 = Antenna::endpoint(addr, IPV6PORT);
            cout << "Resolved " << SERVER << " ipv6 to " << addr.to_string()
                 << endl;
          }
        }
      } catch (const asio::system_error& err) {
        cerr << "Error resolving " << SERVER << ": " << err.what() << endl;
      }

      //Even if we did get a result for a certain protocol, we can't use it
      //if it is not available
      hasv4 &= antenna.hasV4();
      hasv6 &= antenna.hasV6();

      ensureRegistered();
    }

    //Set initial conditions
    isConnected4 = isConnected6 = false;
    isConnecting = true;
    timeUntilConnectXmit = 0;
    connectionAttempts = 0;

    //Write the connection packet
    byte* pack = connectPacket;
    *pack++ = CONNECT;
    io::write(pack, id);
    io::write(pack, timestamp);
    //Convert HMAC from hex
    for (unsigned i = 0; i < HMAC_SIZE; ++i) {
#define HEXIT(x) (x >= '0' && x <= '9'? x-'0' : \
                  x >= 'a' && x <= 'f'? x-'a'+0xa : x-'A'+0xA)
      *pack++ = (HEXIT(hmac[2*i])<<4) | HEXIT(hmac[2*i+1]);
#undef HEXIT
    }
    //Copy name into packet.
    strncpy((char*)pack, name, MAX_NAME_LENGTH);
    pack[MAX_NAME_LENGTH] = 0;

    //Set packet size
    connectPacketSize = (pack - connectPacket) + strlen(name)+1;

    //Done, send the first packet immediately
    sendConnectPacket();
  }

  static void sendConnectPacket() {
    debug(">> CONNECT (send)");
    if (hasv4 && !isConnected4)
      antenna.send(server4, connectPacket, connectPacketSize);
    if (hasv6 && !isConnected6)
      antenna.send(server6, connectPacket, connectPacketSize);

    ++connectionAttempts;
    if (connectionAttempts > MAX_CONNECT_ATTEMPTS) {
      //Give up, the server probably is not reachable on one or both protocols
      hasv4 &= isConnected4;
      hasv6 &= isConnected6;
      isConnecting = false;
    }

    //If we have not given up or connected yet, try again in a little bit
    timeUntilConnectXmit = TIME_BETWEEN_CONNECT;
  }

  void ensureRegistered() {
    class PP: public PacketProcessor {
    public:
      bool v6;
      PP(bool v) : v6(v) {}
      virtual void process(const Antenna::endpoint&, Antenna*, Tuner*,
                           const byte* data, unsigned len) {
        processPacket(v6, data, len);
      }
    } static ppv4(false), ppv6(true);

    if (antenna.tuner) {
      if (hasv4)
        antenna.tuner->connect(server4, &ppv4);
      if (hasv6)
        antenna.tuner->connect(server6, &ppv6);
    }
  }

  void bye() {
    debug(">> BYE");
    byte pack = BYE;
    if (hasv4)
      antenna.send(server4, &pack, 1);
    if (hasv6)
      antenna.send(server6, &pack, 1);

    isConnecting = isConnected4 = isConnected6 = false;
  }

  void post(const byte* dat, unsigned len) {
    debug(">> POST");
    vector<byte> pack(1 + len);
    pack[0] = POST;
    memcpy(&pack[1], dat, len);

    if (isConnected4)
      antenna.send(server4, &pack[0], pack.size());
    if (isConnected6)
      antenna.send(server6, &pack[0], pack.size());
  }

  void list(void (*callback)(void*, const byte*, unsigned),
            void* userdata) {
    debug(">> LIST");
    listCallback = callback;
    listCallbackUserdata = userdata;

    byte pack = LIST;
    if (isConnected4)
      antenna.send(server4, &pack, 1);
    if (isConnected6)
      antenna.send(server6, &pack, 1);
  }

  void stopList() {
    listCallback = NULL;
    listCallbackUserdata = NULL;
  }

  void proxy(const Antenna::endpoint& dst,
             const byte* payload, unsigned len) {
    debug(">> PROXY");
    vector<byte> pack(1 + (dst.address().is_v4()? 4 : 2*8) + 2 + len);
    pack[0] = PROXY;
    byte* dat = &pack[1];
    if (dst.address().is_v4()) {
      asio::ip::address_v4::bytes_type b(dst.address().to_v4().to_bytes());
      memcpy(dat, &b[0], 4);
      dat += 4;
    } else {
      asio::ip::address_v6::bytes_type b(dst.address().to_v6().to_bytes());
      io::a6fromlbo(dat, &b[0]);
      dat += 16;
    }
    io::write(dat, dst.port());
    memcpy(dat, payload, len);

    if (dst.address().is_v4() && isConnected4)
      antenna.send(server4, &pack[0], pack.size());
    else if (dst.address().is_v6() && isConnected6)
      antenna.send(server6, &pack[0], pack.size());
    else
      cerr << "warn: Attempt to proxy via unavailable IP version." << endl;
  }

  bool ready() {
    return (knowIpv4Address && isConnected4) ||
           (knowIpv6Address && isConnected6);
  }

  void update(unsigned et) {
    if (isConnecting) {
      //Move to non-connecting state if both IP versions we support are
      //connected
      if ((!hasv4 || isConnected4) && (!hasv6 || isConnected6))
        isConnecting = true;
      //Otherwise, retransmit connect packet if necessary
      else if (timeUntilConnectXmit <= et)
        sendConnectPacket();
      else
        timeUntilConnectXmit -= et;
    } else if (isConnected4 || isConnected6) {
      //Send PING if needed
      if (timeUntilPing < et) {
        bool whoAmI = (hasv4 && !knowIpv4Address) ||
                      (hasv6 && !knowIpv6Address);
        byte pack[2] = { PING, (byte)whoAmI };
        if (hasv4)
          antenna.send(server4, pack, 2);
        if (hasv6)
          antenna.send(server6, pack, 2);

        unsigned lower = whoAmI? MIN_TIME_BETWEEN_WHOAMI:MIN_TIME_BETWEEN_PING;
        unsigned upper = whoAmI? MAX_TIME_BETWEEN_WHOAMI:MAX_TIME_BETWEEN_PING;
        timeUntilPing = lower + rand()%(upper-lower);
      } else {
        timeUntilPing -= et;
      }
    }
  }

  static void processYouAre(bool, const byte*, unsigned);
  static void processPong(bool, const byte*, unsigned);
  static void processFromOther(bool, const byte*, unsigned);
  static void processAdvert(bool, const byte*, unsigned);
  static void (*const packetTypes[256])(bool, const byte*, unsigned) = {
    processYouAre,
    processPong,
    processFromOther,
    processAdvert,
    NULL,
    /* processSignature, */
    /* rest of array is NULL implicitly */
  };

  static void processPacket(bool v6, const byte* dat, unsigned len) {
    if (packetTypes[dat[0]])
      packetTypes[dat[0]](v6, dat+1, len-1);
    else {
#ifdef DEBUG
      cerr << "WARN: Dropping unknown abuhops packet type " << (unsigned)dat[0]
           << endl;
#endif
    }
  }

  static void processYouAre(bool v6, const byte* dat, unsigned len) {
    debug("<< YOU-ARE");
    if (v6) {
      GlobalID& gid(*antenna.getGlobalID6());
      if (len != 8*2 + 2) {
#ifdef DEBUG
        cerr << "WARN: Dropping IPv6 YOU-ARE packet of length " << len << endl;
#endif
      } else {
        for (unsigned i = 0; i < 8; ++i)
          io::read(dat, gid.ia6[i]);
        io::read(dat, gid.iport);

        knowIpv6Address = true;
        isConnected6 = true;

        cout << "Our IPv6 GlobalID is " << gid.toString() << endl;
      }
    } else {
      GlobalID& gid(*antenna.getGlobalID4());
      if (len != 4 + 2) {
#ifdef DEBUG
        cerr << "WARN: Dropping IPv4 YOU-ARE packet of length " << len << endl;
#endif
      } else {
        memcpy(gid.ia4, dat, 4);
        dat += 4;
        io::read(dat, gid.iport);

        knowIpv4Address = true;
        isConnected4 = true;

        cout << "Our IPv4 GlobalID is " << gid.toString() << endl;
      }
    }
  }

  static void processPong(bool v6, const byte* dat, unsigned len) {
    //Nothing to do
    debug("<< PONG");
  }

  static void processFromOther(bool v6, const byte* dat, unsigned len) {
    debug("<< FROM-OTHER");
    if (!len) {
#ifdef DEBUG
      cerr << "WARN: Dropping FROM-OTHER with empty payload." << endl;
#endif
      return;
    }

    //The default endpoint is used to tell the NetworkGame that it must get the
    //Internet IP address from the STX itself
    Antenna::endpoint defaultEndpoint;
    if (antenna.tuner)
      antenna.tuner->receivePacket(defaultEndpoint, &antenna, dat, len);
  }

  static const char requiredAdvertHeader[] = "Abendspiel";
  #define ADVERT_IPV_OFF (sizeof(requiredAdvertHeader)-1 + 4 + 1)
  static void processAdvert(bool v6, const byte* dat, unsigned len) {
    debug("<< ADVERT");
    Antenna::endpoint defaultEndpoint;
    if (len < sizeof(requiredAdvertHeader) + ADVERT_IPV_OFF ||
        memcmp(dat, requiredAdvertHeader, sizeof(requiredAdvertHeader)-1)) {
#ifdef DEBUG
      cerr << "WARN: Dropping non-Abendspiel ADVERT" << endl;
#endif
      return;
    }

    //Ensure that the reported IP version of the advert matches the Abuhops
    //realm it is comming from
    if ((unsigned)v6 != dat[ADVERT_IPV_OFF]) {
      /*
       * This warning is meaningless, since the Abendstern abuhops client
       * always posts to both realms.
#ifdef DEBUG
      cerr << "WARN: Dropping ADVERT for wrong IP version" << endl;
#endif
      */
      return;
    }

    if (antenna.tuner)
      antenna.tuner->receivePacket(defaultEndpoint, &antenna, dat, len);
  }
}
<|endoftext|>


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

#include "triton/common/logging.h"

#ifdef _WIN32
// suppress the min and max definitions in Windef.h.
#define NOMINMAX
#include <Windows.h>
#else
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#endif
#include <algorithm>
#include <iomanip>
#include <iostream>

namespace triton { namespace common {

Logger gLogger_;

Logger::Logger() : enables_{true, true, true}, vlevel_(0), format_(Format::kDEFAULT) {}

void
Logger::Log(const std::string& msg)
{
  const std::lock_guard<std::mutex> lock(mutex_);
  std::cerr << msg << std::endl;
}

void
Logger::Flush()
{
  std::cerr << std::flush;
}

const std::vector<char> LogMessage::level_name_{'E', 'W', 'I'};

LogMessage::LogMessage(const char* file, int line, uint32_t level)
{
  std::string path(file);
  size_t pos = path.rfind('/');
  if (pos != std::string::npos) {
    path = path.substr(pos + 1, std::string::npos);
  }

  // 'L' below is placeholder for showing log level
  switch (gLogger_.LogFormat())
  {
  case Logger::Format::kDEFAULT: {
    // LMMDD hh:mm:ss.ssssss
#ifdef _WIN32
    SYSTEMTIME system_time;
    GetSystemTime(&system_time);
    stream_ << level_name_[std::min(level, (uint32_t)Level::kINFO)]
            << std::setfill('0') << std::setw(2) << system_time.wMonth
            << std::setw(2) << system_time.wDay << " " << std::setw(2)
            << system_time.wHour << ':' << std::setw(2) << system_time.wMinute
            << ':' << std::setw(2) << system_time.wSecond << "." << std::setw(6)
            << system_time.wMilliseconds * 1000 << ' '
            << static_cast<uint32_t>(GetCurrentProcessId()) << ' ' << path << ':'
            << line << "] ";
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    struct tm tm_time;
    gmtime_r(((time_t*)&(tv.tv_sec)), &tm_time);
    stream_ << level_name_[std::min(level, (uint32_t)Level::kINFO)]
            << std::setfill('0') << std::setw(2) << (tm_time.tm_mon + 1)
            << std::setw(2) << tm_time.tm_mday << " " << std::setw(2)
            << tm_time.tm_hour << ':' << std::setw(2) << tm_time.tm_min << ':'
            << std::setw(2) << tm_time.tm_sec << "." << std::setw(6) << tv.tv_usec
            << ' ' << static_cast<uint32_t>(getpid()) << ' ' << path << ':'
            << line << "] ";
#endif
    break;
  }
  case Logger::Format::kISO8601: {
    // YYYY-MM-DDThh:mm:ssZ L
#ifdef _WIN32
    SYSTEMTIME system_time;
    GetSystemTime(&system_time);
    stream_ << system_time.wYear << '-'
            << std::setfill('0') << std::setw(2) << system_time.wMonth << '-'
            << std::setw(2) << system_time.wDay << 'T' << std::setw(2)
            << system_time.wHour << ':' << std::setw(2) << system_time.wMinute
            << ':' << std::setw(2) << system_time.wSecond << 'Z '
            << level_name_[std::min(level, (uint32_t)Level::kINFO)] << ' '
            << static_cast<uint32_t>(GetCurrentProcessId()) << ' ' << path << ':'
            << line << "] ";
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    struct tm tm_time;
    gmtime_r(((time_t*)&(tv.tv_sec)), &tm_time);
    stream_ << (tm_time.tm_year + 1900) << '-'
            << std::setfill('0') << std::setw(2) << (tm_time.tm_mon + 1) << '-'
            << std::setw(2) << tm_time.tm_mday << 'T' << std::setw(2)
            << tm_time.tm_hour << ':' << std::setw(2) << tm_time.tm_min << ':'
            << std::setw(2) << tm_time.tm_sec << ' '
            << level_name_[std::min(level, (uint32_t)Level::kINFO)] << ' '
            << static_cast<uint32_t>(getpid()) << ' ' << path << ':'
            << line << "] ";
#endif
    break;
  }
  }
}

LogMessage::~LogMessage()
{
  gLogger_.Log(stream_.str());
}

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

#include "triton/common/logging.h"

#ifdef _WIN32
// suppress the min and max definitions in Windef.h.
#define NOMINMAX
#include <Windows.h>
#else
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#endif
#include <algorithm>
#include <iomanip>
#include <iostream>

namespace triton { namespace common {

Logger gLogger_;

Logger::Logger() : enables_{true, true, true}, vlevel_(0), format_(Format::kDEFAULT) {}

void
Logger::Log(const std::string& msg)
{
  const std::lock_guard<std::mutex> lock(mutex_);
  std::cerr << msg << std::endl;
}

void
Logger::Flush()
{
  std::cerr << std::flush;
}

const std::vector<char> LogMessage::level_name_{'E', 'W', 'I'};

LogMessage::LogMessage(const char* file, int line, uint32_t level)
{
  std::string path(file);
  size_t pos = path.rfind('/');
  if (pos != std::string::npos) {
    path = path.substr(pos + 1, std::string::npos);
  }

  // 'L' below is placeholder for showing log level
  switch (gLogger_.LogFormat())
  {
  case Logger::Format::kDEFAULT: {
    // LMMDD hh:mm:ss.ssssss
#ifdef _WIN32
    SYSTEMTIME system_time;
    GetSystemTime(&system_time);
    stream_ << level_name_[std::min(level, (uint32_t)Level::kINFO)]
            << std::setfill('0') << std::setw(2) << system_time.wMonth
            << std::setw(2) << system_time.wDay << ' ' << std::setw(2)
            << system_time.wHour << ':' << std::setw(2) << system_time.wMinute
            << ':' << std::setw(2) << system_time.wSecond << '.' << std::setw(6)
            << system_time.wMilliseconds * 1000 << ' '
            << static_cast<uint32_t>(GetCurrentProcessId()) << ' ' << path << ':'
            << line << "] ";
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    struct tm tm_time;
    gmtime_r(((time_t*)&(tv.tv_sec)), &tm_time);
    stream_ << level_name_[std::min(level, (uint32_t)Level::kINFO)]
            << std::setfill('0') << std::setw(2) << (tm_time.tm_mon + 1)
            << std::setw(2) << tm_time.tm_mday << ' ' << std::setw(2)
            << tm_time.tm_hour << ':' << std::setw(2) << tm_time.tm_min << ':'
            << std::setw(2) << tm_time.tm_sec << '.' << std::setw(6) << tv.tv_usec
            << ' ' << static_cast<uint32_t>(getpid()) << ' ' << path << ':'
            << line << "] ";
#endif
    break;
  }
  case Logger::Format::kISO8601: {
    // YYYY-MM-DDThh:mm:ssZ L
#ifdef _WIN32
    SYSTEMTIME system_time;
    GetSystemTime(&system_time);
    stream_ << system_time.wYear << '-'
            << std::setfill('0') << std::setw(2) << system_time.wMonth << '-'
            << std::setw(2) << system_time.wDay << 'T' << std::setw(2)
            << system_time.wHour << ':' << std::setw(2) << system_time.wMinute
            << ':' << std::setw(2) << system_time.wSecond << "Z "
            << level_name_[std::min(level, (uint32_t)Level::kINFO)] << ' '
            << static_cast<uint32_t>(GetCurrentProcessId()) << ' ' << path << ':'
            << line << "] ";
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    struct tm tm_time;
    gmtime_r(((time_t*)&(tv.tv_sec)), &tm_time);
    stream_ << (tm_time.tm_year + 1900) << '-'
            << std::setfill('0') << std::setw(2) << (tm_time.tm_mon + 1) << '-'
            << std::setw(2) << tm_time.tm_mday << 'T' << std::setw(2)
            << tm_time.tm_hour << ':' << std::setw(2) << tm_time.tm_min << ':'
            << std::setw(2) << tm_time.tm_sec << "Z "
            << level_name_[std::min(level, (uint32_t)Level::kINFO)] << ' '
            << static_cast<uint32_t>(getpid()) << ' ' << path << ':'
            << line << "] ";
#endif
    break;
  }
  }
}

LogMessage::~LogMessage()
{
  gLogger_.Log(stream_.str());
}

}}  // namespace triton::common
<|endoftext|>


<commit_before>/*
  Copyright (C) 2014 Marcus Soll
  All rights reserved.

  You may use this file under the terms of BSD license as follows:

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Jolla Ltd nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR
  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "gene.h"
#include <QTime>
#include <cstdlib>
#include <QFile>
#include <QTextStream>

namespace {
float getRandomFloat()
{
    return static_cast<float> (qrand()) / (static_cast<float> (RAND_MAX/2));
}

float getRandomChancet()
{
    return static_cast<float> (qrand()) / (static_cast<float> (RAND_MAX));
}
}

Gene::Gene(float mutationRate):
    _inputToHidden1(),
    _hidden1ToHidden2(),
    _hidden2ToOutput(),
    _mutationRate(mutationRate),
    _score(0)
{
    for(int i = 0; i < 128*_sizeHidden; ++i)
    {
        _inputToHidden1.append(getRandomFloat());
    }
    for(int i = 0; i < _sizeHidden*_sizeHidden; ++i)
    {
        _hidden1ToHidden2.append(getRandomFloat());
    }
    for(int i = 0; i < 64*_sizeHidden; ++i)
    {
        _hidden2ToOutput.append(getRandomFloat());
    }
}

void Gene::mutate()
{
    for(int i = 0; i < 128*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _inputToHidden1[i] = getRandomFloat();
        }
    }
    for(int i = 0; i < _sizeHidden*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _hidden1ToHidden2[i] = getRandomFloat();
        }
    }
    for(int i = 0; i < 64*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _hidden2ToOutput[i] = getRandomFloat();
        }
    }
}

void Gene::saveScore(int score)
{
    _score = score;
}

void Gene::saveFiles(QString inputToHiddenPath, QString hiddenToHiddenPath, QString hiddenToOutputPath)
{
    QFile ith1(inputToHiddenPath);
    ith1.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream ith1Stream(&ith1);
    for(int i = 0; i < _sizeHidden*128; ++i)
    {
        ith1Stream << _inputToHidden1[i];
        ith1Stream << " ";
    }
    ith1.close();

    QFile h1th2(hiddenToHiddenPath);
    h1th2.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream h1th2Stream(&h1th2);
    for(int i = 0; i < _sizeHidden+_sizeHidden; ++i)
    {
        h1th2Stream << _hidden1ToHidden2[i];
        h1th2Stream << " ";
    }
    h1th2.close();

    QFile hto(hiddenToOutputPath);
    hto.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream htoStream(&hto);
    for(int i = 0; i < _sizeHidden*64; ++i)
    {
        htoStream << _hidden2ToOutput[i];
        htoStream << " ";
    }
    hto.close();
}

bool Gene::operator<(const Gene &other) const
{
    return _score > other._score;
}

int Gene::getScore()
{
    return _score;
}
<commit_msg>Allowing negative values<commit_after>/*
  Copyright (C) 2014 Marcus Soll
  All rights reserved.

  You may use this file under the terms of BSD license as follows:

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Jolla Ltd nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR
  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "gene.h"
#include <QTime>
#include <cstdlib>
#include <QFile>
#include <QTextStream>

namespace {
float getRandomFloat()
{
    return static_cast<float> (qrand()) / (static_cast<float> (RAND_MAX/4)) - 2.0;
}

float getRandomChancet()
{
    return static_cast<float> (qrand()) / (static_cast<float> (RAND_MAX));
}
}

Gene::Gene(float mutationRate):
    _inputToHidden1(),
    _hidden1ToHidden2(),
    _hidden2ToOutput(),
    _mutationRate(mutationRate),
    _score(0)
{
    for(int i = 0; i < 128*_sizeHidden; ++i)
    {
        _inputToHidden1.append(getRandomFloat());
    }
    for(int i = 0; i < _sizeHidden*_sizeHidden; ++i)
    {
        _hidden1ToHidden2.append(getRandomFloat());
    }
    for(int i = 0; i < 64*_sizeHidden; ++i)
    {
        _hidden2ToOutput.append(getRandomFloat());
    }
}

void Gene::mutate()
{
    for(int i = 0; i < 128*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _inputToHidden1[i] = getRandomFloat();
        }
    }
    for(int i = 0; i < _sizeHidden*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _hidden1ToHidden2[i] = getRandomFloat();
        }
    }
    for(int i = 0; i < 64*_sizeHidden; ++i)
    {
        if(getRandomChancet() < _mutationRate)
        {
            _hidden2ToOutput[i] = getRandomFloat();
        }
    }
}

void Gene::saveScore(int score)
{
    _score = score;
}

void Gene::saveFiles(QString inputToHiddenPath, QString hiddenToHiddenPath, QString hiddenToOutputPath)
{
    QFile ith1(inputToHiddenPath);
    ith1.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream ith1Stream(&ith1);
    for(int i = 0; i < _sizeHidden*128; ++i)
    {
        ith1Stream << _inputToHidden1[i];
        ith1Stream << " ";
    }
    ith1.close();

    QFile h1th2(hiddenToHiddenPath);
    h1th2.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream h1th2Stream(&h1th2);
    for(int i = 0; i < _sizeHidden+_sizeHidden; ++i)
    {
        h1th2Stream << _hidden1ToHidden2[i];
        h1th2Stream << " ";
    }
    h1th2.close();

    QFile hto(hiddenToOutputPath);
    hto.open(QIODevice::WriteOnly|QIODevice::Truncate);
    QTextStream htoStream(&hto);
    for(int i = 0; i < _sizeHidden*64; ++i)
    {
        htoStream << _hidden2ToOutput[i];
        htoStream << " ";
    }
    hto.close();
}

bool Gene::operator<(const Gene &other) const
{
    return _score > other._score;
}

int Gene::getScore()
{
    return _score;
}
<|endoftext|>


<commit_before>#include <cstdint>
#include <vector>
#include <limits>
#include <iostream>
#include <utility>
#include <sstream>
#include <memory>
#include <fstream>
#include <chrono>

#include "problem.h"
#include "full_model.h"
#include "cpu_optimizer.h"
#include "gpu_optimizer.h"
#include "problem_io.h"
#include "parse_utilities.h"
#include "print_utilities.h"
#include "simulator.h"
#include "evaluator.h"

void optimize(const std::string & problem_file, const std::string & policy_file, bool use_gpu = true) {
	// load problem
	problem p = load_problem(problem_file);
	// write problem to command line
	std::cout << p;
	// analyze problem
	full_model fm;
	fm.analyze(p);
	// optimize problem
	std::unique_ptr<optimizer> opt = 0;
	if (!use_gpu) {
		std::cout << "Optimizing on CPU" << std::endl;
		opt.reset( new cpu_optimizer<float>() );
	} else {
		std::cout << "Optimizing on GPU" << std::endl;
		opt.reset( new gpu_optimizer() );
	}
	opt->set_ignore_revenue(false);
	opt->set_always_allow_reject(false);
	opt->set_check_strict_convergence(true);
	opt->optimize(fm);
	// save policy
	save_policy(policy_file, opt->get_policy());
}

void evaluate(const std::string & problem_file, const std::string & policy_file) {
	problem p = load_problem(problem_file);
	std::vector<uint32_t> policy = load_policy(policy_file);
	full_model fm(p);
	evaluator evaluator;
	std::cout << "Evaluating policy..." << std::endl;
	std::chrono::high_resolution_clock::time_point start, end;
	start = std::chrono::high_resolution_clock::now();
	evaluator.evaluate(fm, policy);
	end = std::chrono::high_resolution_clock::now();
	long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
	std::cout << "Evaluator completed in " << ms << " ms" << std::endl;
	std::cout << "Evaluator cost: " << evaluator.get_average_cost() << std::endl;
	std::cout << "Evaluator revenue: " << evaluator.get_average_revenue() << std::endl;
	std::cout << "Evaluator lost: " << evaluator.get_average_loss() << std::endl;
}

void simulate(const std::string & problem_file, const std::string & policy_file) {
	problem p = load_problem(problem_file);
	std::vector<uint32_t> policy = load_policy(policy_file);
	full_model fm(p);
	simulator sim;
	std::cout << "Simulating policy..." << std::endl;
	std::chrono::high_resolution_clock::time_point start, end;
	start = std::chrono::high_resolution_clock::now();
	sim.simulate(fm, policy);
	end = std::chrono::high_resolution_clock::now();
	long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
	std::cout << "Simulator completed in " << ms << " ms" << std::endl;
	std::cout << "Simulator cost: " << sim.get_average_cost() << std::endl;
	std::cout << "Simulator revenue: " << sim.get_average_revenue() << std::endl;
	std::cout << "Simulator lost: " << sim.get_average_loss() << std::endl;
}

#ifdef _WIN32
#include <Windows.h>
#endif

#define ANVIK_RUN_INVALID_NUMBER_OF_ARGUMENTS 1
#define ANVIK_RUN_INVALID_COMMAND 2
#define ANVIK_RUN_ERROR 3

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

	if (argc != 4) {
		std::cerr << "Invalid number of arguments" << std::endl;
		return ANVIK_RUN_INVALID_NUMBER_OF_ARGUMENTS;
	}

	std::string cmd = argv[1];
	std::string problem_file = argv[2];
	std::string policy_file = argv[3];

	try {

		if (cmd == "optimize-cpu") {
			optimize(problem_file, policy_file, false);
		} else if (cmd == "optimize-gpu") {
			optimize(problem_file, policy_file, true);
		} else if (cmd == "evaluate") {
			evaluate(problem_file, policy_file);
		} else if (cmd == "simulate") {
			simulate(problem_file, policy_file);
		} else {
			std::cerr << "Invalid commmand '" << cmd << "'" << std::endl;
			return ANVIK_RUN_INVALID_COMMAND;
		}

		return 0;

	} catch (std::runtime_error & err) {
		std::cerr << err.what() << std::endl;
		return ANVIK_RUN_ERROR;
	}
}
<commit_msg>Command line messages<commit_after>#include <cstdint>
#include <vector>
#include <limits>
#include <iostream>
#include <utility>
#include <sstream>
#include <memory>
#include <fstream>
#include <chrono>

#include "problem.h"
#include "full_model.h"
#include "cpu_optimizer.h"
#include "gpu_optimizer.h"
#include "problem_io.h"
#include "parse_utilities.h"
#include "print_utilities.h"
#include "simulator.h"
#include "evaluator.h"

void optimize(const std::string & problem_file, const std::string & policy_file, bool use_gpu = true) {
	// load problem
	problem p = load_problem(problem_file);
	// write problem to command line
	std::cout << p;
	// analyze problem
	full_model fm;
	fm.analyze(p);
	// optimize problem
	std::unique_ptr<optimizer> opt = 0;
	if (!use_gpu) {
		std::cout << "Optimizing on CPU" << std::endl;
		opt.reset( new cpu_optimizer<float>() );
	} else {
		std::cout << "Optimizing on GPU" << std::endl;
		opt.reset( new gpu_optimizer() );
	}
	opt->set_ignore_revenue(false);
	opt->set_always_allow_reject(false);
	opt->set_check_strict_convergence(true);
	opt->optimize(fm);
	// save policy
	save_policy(policy_file, opt->get_policy());
}

void evaluate(const std::string & problem_file, const std::string & policy_file) {
	problem p = load_problem(problem_file);
	std::vector<uint32_t> policy = load_policy(policy_file);
	full_model fm(p);
	evaluator evaluator;
	std::cout << "Evaluating policy..." << std::endl;
	std::chrono::high_resolution_clock::time_point start, end;
	start = std::chrono::high_resolution_clock::now();
	evaluator.evaluate(fm, policy);
	end = std::chrono::high_resolution_clock::now();
	long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
	std::cout << "Evaluator completed in " << ms << " ms" << std::endl;
	std::cout << "Evaluator cost: " << evaluator.get_average_cost() << std::endl;
	std::cout << "Evaluator revenue: " << evaluator.get_average_revenue() << std::endl;
	std::cout << "Evaluator lost: " << evaluator.get_average_loss() << std::endl;
}

void simulate(const std::string & problem_file, const std::string & policy_file) {
	problem p = load_problem(problem_file);
	std::vector<uint32_t> policy = load_policy(policy_file);
	full_model fm(p);
	simulator sim;
	std::cout << "Simulating policy..." << std::endl;
	std::chrono::high_resolution_clock::time_point start, end;
	start = std::chrono::high_resolution_clock::now();
	sim.simulate(fm, policy);
	end = std::chrono::high_resolution_clock::now();
	long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
	std::cout << "Simulator completed in " << ms << " ms" << std::endl;
	std::cout << "Simulator cost: " << sim.get_average_cost() << std::endl;
	std::cout << "Simulator revenue: " << sim.get_average_revenue() << std::endl;
	std::cout << "Simulator lost: " << sim.get_average_loss() << std::endl;
}

#ifdef _WIN32
#include <Windows.h>
#endif

#define ANVIK_RUN_INVALID_NUMBER_OF_ARGUMENTS 1
#define ANVIK_RUN_INVALID_COMMAND 2
#define ANVIK_RUN_ERROR 3

void print_available_commands(std::ostream & dst) {
	dst << "Available commands: " << std::endl;
	dst << " - optimize-cpu		computes the optimal policy using the CPUs" << std::endl;
	dst << " - optimize-gpu     computes the optimal policy using the GPU" << std::endl;
	dst << " - evaluate			evaluates the expected profit under the given policy analytically" << std::endl;
	dst << " - simulate			evaluates the expected profit under the given policy through simulation" << std::endl;
}

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

	if (argc != 4) {
		std::cerr << "Invalid number of arguments" << std::endl;
		std::cerr << "Syntax: anvik-run command problem_file policy_file" << std::endl;
		print_available_commands(std::cerr);
		return ANVIK_RUN_INVALID_NUMBER_OF_ARGUMENTS;
	}

	std::string cmd = argv[1];
	std::string problem_file = argv[2];
	std::string policy_file = argv[3];

	try {

		if (cmd == "optimize-cpu") {
			optimize(problem_file, policy_file, false);
		} else if (cmd == "optimize-gpu") {
			optimize(problem_file, policy_file, true);
		} else if (cmd == "evaluate") {
			evaluate(problem_file, policy_file);
		} else if (cmd == "simulate") {
			simulate(problem_file, policy_file);
		} else {
			std::cerr << "Invalid commmand '" << cmd << "'" << std::endl;
			print_available_commands(std::cerr);
			return ANVIK_RUN_INVALID_COMMAND;
		}

		return 0;

	} catch (std::runtime_error & err) {
		std::cerr << err.what() << std::endl;
		return ANVIK_RUN_ERROR;
	}
}
<|endoftext|>


<commit_before>#include <algorithm>
#include <iostream>
#include <sstream>

#include "parsers/parse_error.hh"
#include "parsers/tina.hh"

namespace pnmc { namespace parsers {

namespace {

/*------------------------------------------------------------------------------------------------*/

unsigned int
value(std::string::const_iterator cit, std::string::const_iterator cend)
{
  std::string s(cit, cend);
  try
  {
    std::size_t pos;
    auto value = std::stoi(s, &pos);
    std::advance(cit, pos);
    if (std::distance(cit, cend) == 1)
    {
      switch (*cit)
      {
        case 'K': value *= 1000; break;
        case 'M': value *= 1000000; break;
        default: throw parse_error("Invalid suffix, got " + std::string(*cit, 1));
      }
    }
    else if (std::distance(cit, cend) > 1)
    {
      throw parse_error("Invalid suffix, got " + std::string(cit, cend));
    }
    return value;
  }
  catch (const std::invalid_argument&)
  {
    throw parse_error("Expected a value, got " + s);
  }
}

/*------------------------------------------------------------------------------------------------*/

std::pair<std::string, unsigned int>
place_valuation(const std::string& s)
{
  const auto star_cit = std::find(s.cbegin(), s.cend(), '*');
  if (star_cit == s.cend())
  {
    return std::make_pair(s, 1);
  }
  const auto valuation = value(star_cit + 1, s.cend());
  return std::make_pair(std::string(s.cbegin(), star_cit), valuation);
}

/*------------------------------------------------------------------------------------------------*/

unsigned int
marking(const std::string& s)
{
  if (*s.cbegin() == '(' and *std::prev(s.cend()) == ')')
  {
    return value(std::next(s.cbegin()), std::prev(s.cend()));
  }
  else
  {
    throw parse_error("Invalid marking format: " + s);
  }
}

} // namespace anonymous

/*------------------------------------------------------------------------------------------------*/

std::shared_ptr<pn::net>
tina(std::istream& in)
{
  // Known limitations:
  // - there's must be a transition or place per line
  // - labels are ignored

  std::shared_ptr<pn::net> net_ptr = std::make_shared<pn::net>();
  auto& net = *net_ptr;

  std::string line, s0, s1, s2, ignore;
  char peek;
  line.reserve(1024);

  while (std::getline(in, line))
  {
    std::istringstream ss(line);

    // Empty line or comment.
    if (((ss >> std::ws).peek() == std::char_traits<char>::to_int_type('#')) or not (ss >> s0))
    {
      continue;
    }

    // Net
    if (s0 == "net")
    {
      ss >> s0;
      net_ptr->name = s0;
    }

    // Transitions
    else if (s0 == "tr")
    {
      std::string place_id;
      unsigned int valuation;

      if (ss >> s0)
      {
        net.add_transition(s0);
      }
      else
      {
        throw parse_error("Invalid transition: " + line);
      }

      // Ignore label, if any.
      peek = (ss >> std::ws).peek();
      if (peek == std::char_traits<char>::to_int_type(':'))
      {
        ss >> ignore >> ignore; // ':' <label>
      }

      // Skip time interval, if any.
      // We don't check if the interval is well-formed...
      peek = (ss >> std::ws).peek();
      if (   peek == std::char_traits<char>::to_int_type('[')
          or peek == std::char_traits<char>::to_int_type(']'))
      {
        ss >> s1;
      }

      bool found_arrow = false;
      while (ss >> s1)
      {
        if (s1 == "->")
        {
          found_arrow = true;
          break;
        }
        std::tie(place_id, valuation) = place_valuation(s1);
        net.add_pre_place(s0, place_id, valuation);
      }

      if (not found_arrow)
      {
        throw parse_error("Invalid transition (missing '->'): " + line);
      }

      while (ss >> s1)
      {
        std::tie(place_id, valuation) = place_valuation(s1);
        net.add_post_place(s0, place_id, valuation);
      }
    }

    // Places
    else if (s0 == "pl")
    {
      if (ss >> s1)
      {
        // Ignore label, if any.
        peek = (ss >> std::ws).peek();
        if (peek == std::char_traits<char>::to_int_type(':'))
        {
          ss >> ignore >> ignore; // ':' <label>
        }

        if (ss >> s2)
        {
          net.add_place(s1, marking(s2));
        }
        else
        {
          net.add_place(s1, 0);
        }
      }
      else
      {
        throw parse_error("Invalid place: " + line);
      }
    }

    // Error.
    else
    {
      throw parse_error("Invalid line: " + line);
    }
  }

  return net_ptr;
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace pnmc::parsers
<commit_msg>Parse Tina’s nets time intervals.<commit_after>#include <algorithm>
#include <iostream>
#include <sstream>

#include "parsers/parse_error.hh"
#include "parsers/tina.hh"

namespace pnmc { namespace parsers {

namespace {

/*------------------------------------------------------------------------------------------------*/

unsigned int
value(std::string::const_iterator cit, std::string::const_iterator cend)
{
  std::string s(cit, cend);
  try
  {
    std::size_t pos;
    auto value = std::stoi(s, &pos);
    std::advance(cit, pos);
    if (std::distance(cit, cend) == 1)
    {
      switch (*cit)
      {
        case 'K': value *= 1000; break;
        case 'M': value *= 1000000; break;
        default: throw parse_error("Invalid suffix, got " + std::string(*cit, 1));
      }
    }
    else if (std::distance(cit, cend) > 1)
    {
      throw parse_error("Invalid suffix, got " + std::string(cit, cend));
    }
    return value;
  }
  catch (const std::invalid_argument&)
  {
    throw parse_error("Expected a value, got " + s);
  }
}

/*------------------------------------------------------------------------------------------------*/

std::pair<std::string, unsigned int>
place_valuation(const std::string& s)
{
  const auto star_cit = std::find(s.cbegin(), s.cend(), '*');
  if (star_cit == s.cend())
  {
    return std::make_pair(s, 1);
  }
  const auto valuation = value(star_cit + 1, s.cend());
  return std::make_pair(std::string(s.cbegin(), star_cit), valuation);
}

/*------------------------------------------------------------------------------------------------*/

unsigned int
marking(const std::string& s)
{
  if (*s.cbegin() == '(' and *std::prev(s.cend()) == ')')
  {
    return value(std::next(s.cbegin()), std::prev(s.cend()));
  }
  else
  {
    throw parse_error("Invalid marking format: " + s);
  }
}

} // namespace anonymous

/*------------------------------------------------------------------------------------------------*/

std::shared_ptr<pn::net>
tina(std::istream& in)
{
  // Known limitations:
  // - there's must be a transition or place per line
  // - labels are ignored
  // - time intervals are not completely validated (e.g. [0, 2a] is wrongly validated

  std::shared_ptr<pn::net> net_ptr = std::make_shared<pn::net>();
  auto& net = *net_ptr;

  std::string line, s0, s1, s2, ignore;
  char peek;
  line.reserve(1024);

  while (std::getline(in, line))
  {
    std::istringstream ss(line);

    // Empty line or comment.
    if (((ss >> std::ws).peek() == std::char_traits<char>::to_int_type('#')) or not (ss >> s0))
    {
      continue;
    }

    // Net
    if (s0 == "net")
    {
      ss >> s0;
      net_ptr->name = s0;
    }

    // Transitions
    else if (s0 == "tr")
    {
      std::string place_id;
      unsigned int valuation;

      if (ss >> s0)
      {
        net.add_transition(s0);
      }
      else
      {
        throw parse_error("Invalid transition: " + line);
      }

      // Ignore label, if any.
      peek = (ss >> std::ws).peek();
      if (peek == std::char_traits<char>::to_int_type(':'))
      {
        ss >> ignore >> ignore; // ':' <label>
      }

      // Time interval.
      peek = (ss >> std::ws).peek();
      if (   peek == std::char_traits<char>::to_int_type('[')
          or peek == std::char_traits<char>::to_int_type(']'))
      {
        char c = ss.get(); // '[' or ']';
        while (ss.good())
        {
          c = ss.get();
          if (c == '[' or c == ']')
          {
            break;
          }
          s1.push_back(c);
        }

        if (not ss.good())
        {
          throw parse_error("Unexpected end of file. Missing '[' or ']'?");
        }

        std::istringstream ss1(s1);
        std::getline(ss1, s2, ','); // split on ','
        unsigned int first, last;
        try
        {
          first = std::stoi(s2);
        }
        catch (const std::invalid_argument&)
        {
          throw parse_error("Expected a value, got: '" + s2 + "'");
        }
        ss1 >> s2;
        try
        {
          last = std::stoi(s2);
        }
        catch (const std::invalid_argument& e)
        {
          if (s2 == "w")
          {
            last = std::numeric_limits<unsigned int>::max();
          }
          else
          {
            throw parse_error("Expected a value, got: '" + s2 + "'");
          }
        }

        net_ptr->add_time_interval(s0, first, last);
      }

      bool found_arrow = false;
      while (ss >> s1)
      {
        if (s1 == "->")
        {
          found_arrow = true;
          break;
        }
        std::tie(place_id, valuation) = place_valuation(s1);
        net.add_pre_place(s0, place_id, valuation);
      }

      if (not found_arrow)
      {
        throw parse_error("Invalid transition (missing '->'): " + line);
      }

      while (ss >> s1)
      {
        std::tie(place_id, valuation) = place_valuation(s1);
        net.add_post_place(s0, place_id, valuation);
      }
    }

    // Places
    else if (s0 == "pl")
    {
      if (ss >> s1)
      {
        // Ignore label, if any.
        peek = (ss >> std::ws).peek();
        if (peek == std::char_traits<char>::to_int_type(':'))
        {
          ss >> ignore >> ignore; // ':' <label>
        }

        if (ss >> s2)
        {
          net.add_place(s1, marking(s2));
        }
        else
        {
          net.add_place(s1, 0);
        }
      }
      else
      {
        throw parse_error("Invalid place: " + line);
      }
    }

    // Error.
    else
    {
      throw parse_error("Invalid line: " + line);
    }
  }

  return net_ptr;
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace pnmc::parsers
<|endoftext|>