text:
/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_rbdyn/Robot.h>
#include <mc_rbdyn/RobotModule.h>
#include <mc_rbdyn/Robots.h>
#include <mc_rbdyn/SCHAddon.h>
#include <mc_rbdyn/Surface.h>
#include <mc_rbdyn/surface_utils.h>
#include <mc_rtc/logging.h>

#include <mc_rbdyn_urdf/urdf.h>

#include <RBDyn/CoM.h>
#include <RBDyn/EulerIntegration.h>
#include <RBDyn/FA.h>
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>

#include <boost/filesystem.hpp>
namespace bfs = boost::filesystem;

#include <tuple>

namespace
{

using bound_t = std::vector<std::vector<double>>;
using bounds_t = std::tuple<bound_t, bound_t, bound_t, bound_t, bound_t, bound_t>;
using rm_bounds_t = mc_rbdyn::RobotModule::bounds_t;
using rm_bound_t = rm_bounds_t::value_type;

/** Generate bounds compatible with the given MultiBody
 *
 * If bounds is provided, use the values provided to build the bounds.
 *
 * Otherwise, default bounds are returned.
 */
bounds_t bounds(const rbd::MultiBody & mb, const rm_bounds_t & bounds)
{
  using jt_method = int (rbd::Joint::*)() const;
  auto fill_bound = [&mb](const std::string & name, const rm_bound_t & bound_in, jt_method def_size, double def_value,
                          double ff_def_value) {
    bound_t res;
    res.reserve(static_cast<size_t>(mb.nrJoints()));
    for(const auto & j : mb.joints())
    {
      res.emplace_back(((j).*(def_size))(), j.type() == rbd::Joint::Free ? ff_def_value : def_value);
      if(bound_in.count(j.name()))
      {
        const auto & b_ref = bound_in.at(j.name());
        auto & b = res.back();
        if(b_ref.size() != b.size())
        {
          LOG_ERROR_AND_THROW(std::runtime_error, name << " provided bound size (" << b_ref.size() << ")"
                                                       << " different from expected size (" << b.size() << ")"
                                                       << " for joint " << j.name())
        }
        res.back() = bound_in.at(j.name());
      }
    }
    return res;
  };
  return std::make_tuple(fill_bound("lower position", bounds.at(0), &rbd::Joint::params, -INFINITY, -INFINITY),
                         fill_bound("upper position", bounds.at(1), &rbd::Joint::params, INFINITY, INFINITY),
                         fill_bound("lower velocity", bounds.at(2), &rbd::Joint::dof, -INFINITY, -INFINITY),
                         fill_bound("upper velocity", bounds.at(3), &rbd::Joint::dof, INFINITY, INFINITY),
                         fill_bound("lower torque", bounds.at(4), &rbd::Joint::dof, -INFINITY, 0),
                         fill_bound("upper torque", bounds.at(5), &rbd::Joint::dof, INFINITY, 0));
}

template<typename schT, typename mapT>
void loadSCH(const mc_rbdyn::Robot & robot,
             const std::map<std::string, std::pair<std::string, std::string>> & urls,
             schT * (*sch_load_fn)(const std::string &),
             mapT & data_)
{
  for(const auto & cH : urls)
  {
    const std::string & cHName = cH.first;
    const std::string & parent = cH.second.first;
    const std::string & cHURL = cH.second.second;
    if(robot.hasBody(parent))
    {
      auto poly = std::shared_ptr<schT>(sch_load_fn(cHURL));
      sch::mc_rbdyn::transform(*poly, robot.bodyPosW()[robot.bodyIndexByName(parent)]);
      data_[cHName] = {parent, poly};
    }
  }
}

template<typename mapT>
void fixSCH(const mc_rbdyn::Robot & robot, mapT & data_, const std::map<std::string, sva::PTransformd> & tfs)
{
  for(const auto & d : data_)
  {
    const auto & pos = robot.bodyPosW(d.second.first);
    if(tfs.count(d.first))
    {
      sch::mc_rbdyn::transform(*d.second.second, tfs.at(d.first) * pos);
    }
    else
    {
      sch::mc_rbdyn::transform(*d.second.second, pos);
    }
  }
}

} // namespace

namespace mc_rbdyn
{

// We can safely ignore those since they are due to different index types and
// our index never go near unsafe territories
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#ifdef __clang__
#  pragma clang diagnostic ignored "-Wshorten-64-to-32"
#endif

Robot::Robot(Robots & robots,
             unsigned int robots_idx,
             bool loadFiles,
             const sva::PTransformd * base,
             const std::string & bName)
: robots_(&robots), robots_idx_(robots_idx)
{
  const auto & module_ = module();

  if(base)
  {
    std::string baseName = bName.empty() ? mb().body(0).name() : bName;
    mb() = mbg().makeMultiBody(baseName, mb().joint(0).type() == rbd::Joint::Fixed, *base);
    mbc() = rbd::MultiBodyConfig(mb());
  }

  mbc().gravity = Eigen::Vector3d{0, 0, 9.81};
  mbc().zero(mb());
  {
    auto initQ = mbc().q;
    const auto & stance = module_.stance();
    for(size_t i = 0; i < mb().joints().size(); ++i)
    {
      const auto & j = mb().joint(static_cast<int>(i));
      if(stance.count(j.name()))
      {
        const auto & jQ = stance.at(j.name());
        if(initQ[i].size() != jQ.size())
        {
          LOG_ERROR_AND_THROW(std::runtime_error, "Missmatch between RobotModule stance for joint "
                                                      << j.name() << std::endl
                                                      << "Stance provides " << jQ.size() << " values but should be "
                                                      << initQ[i].size())
        }
        initQ[i] = jQ;
      }
    }
    if(initQ[0].size())
    {
      const auto & attitude = module_.default_attitude();
      initQ[0] = {std::begin(attitude), std::end(attitude)};
    }
    mbc().q = initQ;
    forwardKinematics();
  }

  name_ = module_.name;

  bodyTransforms_.resize(mb().bodies().size());
  const auto & bbts =
      base ? mbg().bodiesBaseTransform(mb().body(0).name(), *base) : mbg().bodiesBaseTransform(mb().body(0).name());
  for(size_t i = 0; i < mb().bodies().size(); ++i)
  {
    const auto & b = mb().body(static_cast<int>(i));
    bodyTransforms_[i] = bbts.at(b.name());
  }

  if(module_.bounds().size() != 6)
  {
    LOG_ERROR_AND_THROW(std::invalid_argument, "The bounds of robotmodule '"
                                                   << module_.name << "' have a size of " << module_.bounds().size()
                                                   << " instead of 6 (ql, qu, vl, vu, tl, tu).")
  }
  std::tie(ql_, qu_, vl_, vu_, tl_, tu_) = bounds(mb(), module_.bounds());

  if(loadFiles)
  {
    loadSCH(*this, module_.convexHull(), &sch::mc_rbdyn::Polyhedron, convexes_);
  }

  for(const auto & b : mb().bodies())
  {
    collisionTransforms_[b.name()] = sva::PTransformd::Identity();
  }
  for(const auto & p : module_.collisionTransforms())
  {
    collisionTransforms_[p.first] = p.second;
  }
  fixCollisionTransforms();

  if(loadFiles)
  {
    if(bfs::exists(module_.rsdf_dir))
    {
      loadRSDFFromDir(module_.rsdf_dir);
    }
    else if(module_.rsdf_dir.size())
    {
      LOG_ERROR("RSDF directory (" << module_.rsdf_dir << ") specified by RobotModule for " << module_.name
                                   << " does not exist")
    }
  }

  forceSensors_ = module_.forceSensors();
  for(auto & fs : forceSensors_)
  {
    bfs::path calib_file = bfs::path(module_.calib_dir) / std::string("calib_data." + fs.name());
    if(bfs::exists(calib_file))
    {
      fs.loadCalibrator(calib_file.string(), mbc().gravity);
    }
  }
  for(size_t i = 0; i < forceSensors_.size(); ++i)
  {
    const auto & fs = forceSensors_[i];
    forceSensorsIndex_[fs.name()] = i;
    bodyForceSensors_[fs.parentBody()] = i;
  }

  stance_ = module_.stance();

  bodySensors_ = module_.bodySensors();
  // Add a single default sensor if no sensor on the robot
  if(bodySensors_.size() == 0)
  {
    bodySensors_.emplace_back();
  }
  for(size_t i = 0; i < bodySensors_.size(); ++i)
  {
    const auto & bS = bodySensors_[i];
    bodySensorsIndex_[bS.name()] = i;
    bodyBodySensors_[bS.parentBody()] = i;
  }

  refJointOrder_ = module_.ref_joint_order();
  refJointIndexToMBCIndex_.resize(refJointOrder_.size());
  for(size_t i = 0; i < refJointOrder_.size(); ++i)
  {
    const auto & jN = refJointOrder_[i];
    if(hasJoint(jN))
    {
      auto jIndex = mb().jointIndexByName(jN);
      refJointIndexToMBCIndex_[i] = mb().joint(jIndex).dof() != 0 ? jIndex : -1;
    }
    else
    {
      refJointIndexToMBCIndex_[i] = -1;
    }
  }

  springs_ = module_.springs();
  flexibility_ = module_.flexibility();

  zmp_ = Eigen::Vector3d::Zero();
}

std::string Robot::name() const
{
  return name_;
}

void Robot::name(const std::string & name)
{
  name_ = name;
}

const RobotModule & Robot::module() const
{
  return robots_->robotModule(robots_idx_);
}

BodySensor & Robot::bodySensor()
{
  return bodySensors_[0];
}

const BodySensor & Robot::bodySensor() const
{
  return bodySensors_[0];
}

bool Robot::hasBodySensor(const std::string & name) const
{
  return bodySensorsIndex_.count(name) != 0;
}

bool Robot::bodyHasBodySensor(const std::string & body) const
{
  return bodyBodySensors_.count(body) != 0;
}

BodySensor & Robot::bodySensor(const std::string & name)
{
  return const_cast<BodySensor &>(static_cast<const Robot *>(this)->bodySensor(name));
}

const BodySensor & Robot::bodySensor(const std::string & name) const
{
  return bodySensors_[bodySensorsIndex_.at(name)];
}

BodySensor & Robot::bodyBodySensor(const std::string & body)
{
  return const_cast<BodySensor &>(static_cast<const Robot *>(this)->bodyBodySensor(body));
}

const BodySensor & Robot::bodyBodySensor(const std::string & body) const
{
  return bodySensors_[bodyBodySensors_.at(body)];
}

BodySensorVector & Robot::bodySensors()
{
  return bodySensors_;
}

const BodySensorVector & Robot::bodySensors() const
{
  return bodySensors_;
}

bool Robot::hasJoint(const std::string & name) const
{
  return mb().jointIndexByName().count(name) != 0;
}

bool Robot::hasBody(const std::string & name) const
{
  return mb().bodyIndexByName().count(name) != 0;
}

unsigned int Robot::jointIndexByName(const std::string & name) const
{
  return mb().jointIndexByName().at(name);
}

int Robot::jointIndexInMBC(size_t jointIndex) const
{
  return refJointIndexToMBCIndex_.at(jointIndex);
}

unsigned int Robot::bodyIndexByName(const std::string & name) const
{
  return mb().bodyIndexByName().at(name);
}

rbd::MultiBody & Robot::mb()
{
  return robots_->mbs_[robots_idx_];
}
const rbd::MultiBody & Robot::mb() const
{
  return robots_->mbs_[robots_idx_];
}

rbd::MultiBodyConfig & Robot::mbc()
{
  return robots_->mbcs_[robots_idx_];
}
const rbd::MultiBodyConfig & Robot::mbc() const
{
  return robots_->mbcs_[robots_idx_];
}

rbd::MultiBodyGraph & Robot::mbg()
{
  return robots_->mbgs_[robots_idx_];
}
const rbd::MultiBodyGraph & Robot::mbg() const
{
  return robots_->mbgs_[robots_idx_];
}

const std::vector<std::vector<double>> & Robot::q() const
{
  return mbc().q;
}
const std::vector<std::vector<double>> & Robot::alpha() const
{
  return mbc().alpha;
}
const std::vector<std::vector<double>> & Robot::alphaD() const
{
  return mbc().alphaD;
}
const std::vector<std::vector<double>> & Robot::jointTorque() const
{
  return mbc().jointTorque;
}
const std::vector<sva::PTransformd> & Robot::bodyPosW() const
{
  return mbc().bodyPosW;
}
const std::vector<sva::MotionVecd> & Robot::bodyVelW() const
{
  return mbc().bodyVelW;
}
const std::vector<sva::MotionVecd> & Robot::bodyVelB() const
{
  return mbc().bodyVelB;
}
const std::vector<sva::MotionVecd> & Robot::bodyAccB() const
{
  return mbc().bodyAccB;
}
std::vector<std::vector<double>> & Robot::q()
{
  return mbc().q;
}
std::vector<std::vector<double>> & Robot::alpha()
{
  return mbc().alpha;
}
std::vector<std::vector<double>> & Robot::alphaD()
{
  return mbc().alphaD;
}
std::vector<std::vector<double>> & Robot::jointTorque()
{
  return mbc().jointTorque;
}
std::vector<sva::PTransformd> & Robot::bodyPosW()
{
  return mbc().bodyPosW;
}
std::vector<sva::MotionVecd> & Robot::bodyVelW()
{
  return mbc().bodyVelW;
}
std::vector<sva::MotionVecd> & Robot::bodyVelB()
{
  return mbc().bodyVelB;
}
std::vector<sva::MotionVecd> & Robot::bodyAccB()
{
  return mbc().bodyAccB;
}

const sva::PTransformd & Robot::bodyPosW(const std::string & name) const
{
  return bodyPosW()[bodyIndexByName(name)];
}

sva::PTransformd Robot::X_b1_b2(const std::string & b1, const std::string & b2) const
{
  return bodyPosW()[bodyIndexByName(b2)] * bodyPosW()[bodyIndexByName(b1)].inv();
}

const sva::MotionVecd & Robot::bodyVelW(const std::string & name) const
{
  return bodyVelW()[bodyIndexByName(name)];
}

const sva::MotionVecd & Robot::bodyVelB(const std::string & name) const
{
  return bodyVelB()[bodyIndexByName(name)];
}

const sva::MotionVecd & Robot::bodyAccB(const std::string & name) const
{
  return bodyAccB()[bodyIndexByName(name)];
}

Eigen::Vector3d Robot::com() const
{
  return rbd::computeCoM(mb(), mbc());
}
Eigen::Vector3d Robot::comVelocity() const
{
  return rbd::computeCoMVelocity(mb(), mbc());
}
Eigen::Vector3d Robot::comAcceleration() const
{
  return rbd::computeCoMAcceleration(mb(), mbc());
}

sva::ForceVecd Robot::bodyWrench(const std::string & bodyName) const
{
  const auto & fs = bodyForceSensor(bodyName);
  sva::ForceVecd w_fsactual = fs.wrenchWithoutGravity(*this);
  sva::PTransformd X_fsactual_surf = fs.X_fsactual_parent();
  return X_fsactual_surf.dualMul(w_fsactual);
}

sva::ForceVecd Robot::surfaceWrench(const std::string & surfaceName) const
{
  const auto & bodyName = surface(surfaceName).bodyName();
  const auto & fs = bodyForceSensor(bodyName);
  sva::ForceVecd w_fsactual = fs.wrenchWithoutGravity(*this);
  sva::PTransformd X_fsactual_surf = surface(surfaceName).X_b_s() * fs.X_fsactual_parent();
  return X_fsactual_surf.dualMul(w_fsactual);
}

Eigen::Vector2d Robot::cop(const std::string & surfaceName, double min_pressure) const
{
  const sva::ForceVecd w_surf = surfaceWrench(surfaceName);
  const double pressure = w_surf.force()(2);
  if(pressure < min_pressure)
  {
    return Eigen::Vector2d::Zero();
  }
  const Eigen::Vector3d & tau_surf = w_surf.couple();
  return Eigen::Vector2d(-tau_surf(1) / pressure, +tau_surf(0) / pressure);
}

Eigen::Vector3d Robot::copW(const std::string & surfaceName, double min_pressure) const
{
  Eigen::Vector3d cop_s;
  cop_s << cop(surfaceName, min_pressure), 0.;
  const sva::PTransformd X_0_s = surfacePose(surfaceName);
  return X_0_s.translation() + X_0_s.rotation().transpose() * cop_s;
}

Eigen::Vector3d Robot::zmp(const std::vector<std::string> & sensorsName,
                           const Eigen::Vector3d & plane_p,
                           const Eigen::Vector3d & plane_n,
                           double forceThreshold) const
{
  sva::ForceVecd measuredWrench{Eigen::Vector6d::Zero()};
  for(const auto & sensorName : sensorsName)
  {
    const auto & sensor = forceSensor(sensorName);
    if(sensor.force().norm() > forceThreshold)
    {
      measuredWrench += sensor.worldWrenchWithoutGravity(*this);
    }
  }

  const Eigen::Vector3d & force = measuredWrench.force();
  const Eigen::Vector3d & moment_0 = measuredWrench.couple();

  Eigen::Vector3d moment_p = moment_0 - plane_p.cross(force);
  double floorn_dot_force = plane_n.dot(force);
  // Prevent potential division by zero
  if(floorn_dot_force < 1.)
  {
    LOG_ERROR_AND_THROW(std::runtime_error, "ZMP cannot be computed, projected force too small " << floorn_dot_force);
  }
  Eigen::Vector3d zmp = plane_p + plane_n.cross(moment_p) / floorn_dot_force;
  return zmp;
}

const std::vector<std::vector<double>> & Robot::ql() const
{
  return ql_;
}
const std::vector<std::vector<double>> & Robot::qu() const
{
  return qu_;
}
const std::vector<std::vector<double>> & Robot::vl() const
{
  return vl_;
}
const std::vector<std::vector<double>> & Robot::vu() const
{
  return vu_;
}
const std::vector<std::vector<double>> & Robot::tl() const
{
  return tl_;
}
const std::vector<std::vector<double>> & Robot::tu() const
{
  return tu_;
}
std::vector<std::vector<double>> & Robot::ql()
{
  return ql_;
}
std::vector<std::vector<double>> & Robot::qu()
{
  return qu_;
}
std::vector<std::vector<double>> & Robot::vl()
{
  return vl_;
}
std::vector<std::vector<double>> & Robot::vu()
{
  return vu_;
}
std::vector<std::vector<double>> & Robot::tl()
{
  return tl_;
}
std::vector<std::vector<double>> & Robot::tu()
{
  return tu_;
}

const std::vector<Flexibility> & Robot::flexibility() const
{
  return flexibility_;
}

std::vector<Flexibility> & Robot::flexibility()
{
  return flexibility_;
}

const std::vector<double> & Robot::encoderValues() const
{
  return encoderValues_;
}

void Robot::encoderValues(const std::vector<double> & encoderValues)
{
  encoderValues_ = encoderValues;
}

const std::vector<double> & Robot::encoderVelocities() const
{
  return encoderVelocities_;
}

void Robot::encoderVelocities(const std::vector<double> & encoderVelocities)
{
  encoderVelocities_ = encoderVelocities;
}

const std::vector<double> & Robot::flexibilityValues() const
{
  return flexibilityValues_;
}

void Robot::flexibilityValues(const std::vector<double> & flexibilityValues)
{
  flexibilityValues_ = flexibilityValues;
}

const std::vector<double> & Robot::jointTorques() const
{
  return jointTorques_;
}

void Robot::jointTorques(const std::vector<double> & jointTorques)
{
  jointTorques_ = jointTorques;
}

const std::vector<std::string> & Robot::refJointOrder() const
{
  return refJointOrder_;
}

bool Robot::hasForceSensor(const std::string & name) const
{
  return forceSensorsIndex_.count(name) != 0;
}

bool Robot::bodyHasForceSensor(const std::string & body) const
{
  return bodyForceSensors_.count(body) != 0;
}

ForceSensor & Robot::forceSensor(const std::string & name)
{
  return const_cast<ForceSensor &>(static_cast<const Robot *>(this)->forceSensor(name));
}

const ForceSensor & Robot::forceSensor(const std::string & name) const
{
  return forceSensors_[forceSensorsIndex_.at(name)];
}

ForceSensor & Robot::bodyForceSensor(const std::string & body)
{
  return const_cast<ForceSensor &>(static_cast<const Robot *>(this)->bodyForceSensor(body));
}

const ForceSensor & Robot::bodyForceSensor(const std::string & body) const
{
  return forceSensors_.at(bodyForceSensors_.at(body));
}

bool Robot::hasSurface(const std::string & surface) const
{
  return surfaces_.count(surface) != 0;
}

std::vector<ForceSensor> & Robot::forceSensors()
{
  return forceSensors_;
}

const std::vector<ForceSensor> & Robot::forceSensors() const
{
  return forceSensors_;
}

mc_rbdyn::Surface & Robot::surface(const std::string & sName)
{
  return const_cast<mc_rbdyn::Surface &>(static_cast<const Robot *>(this)->surface(sName));
}

sva::PTransformd Robot::surfacePose(const std::string & sName) const
{
  return surface(sName).X_0_s(*this);
}

const mc_rbdyn::Surface & Robot::surface(const std::string & sName) const
{
  if(!hasSurface(sName))
  {
    LOG_ERROR_AND_THROW(std::runtime_error, "No surface named " << sName << " found in robot " << this->name())
  }
  return *(surfaces_.at(sName));
}

const std::map<std::string, SurfacePtr> & Robot::surfaces() const
{
  return surfaces_;
}

std::vector<std::string> Robot::availableSurfaces() const
{
  std::vector<std::string> ret;
  ret.reserve(surfaces_.size());
  for(const auto & s : surfaces_)
  {
    ret.push_back(s.first);
  }
  return ret;
}

bool Robot::hasConvex(const std::string & name) const
{
  return convexes_.count(name);
}

Robot::convex_pair_t & Robot::convex(const std::string & cName)
{
  return const_cast<Robot::convex_pair_t &>(static_cast<const Robot *>(this)->convex(cName));
}
const Robot::convex_pair_t & Robot::convex(const std::string & cName) const
{
  if(convexes_.count(cName) == 0)
  {
    LOG_ERROR_AND_THROW(std::runtime_error,
                        "No convex named " << cName << " found in this robot (" << this->name_ << ")")
  }
  return convexes_.at(cName);
}

void Robot::addConvex(const std::string & cName,
                      const std::string & body,
                      Robot::S_ObjectPtr convex,
                      const sva::PTransformd & X_b_c)
{
  if(convexes_.count(cName))
  {
    LOG_ERROR("Attempted to add a convex named " << cName << " that already exists in " << name())
    return;
  }
  convexes_[cName] = {body, convex};
  collisionTransforms_[cName] = X_b_c;
  sch::mc_rbdyn::transform(*convex, X_b_c * bodyPosW(body));
}

void Robot::removeConvex(const std::string & cName)
{
  if(convexes_.count(cName))
  {
    convexes_.erase(cName);
    collisionTransforms_.erase(cName);
  }
}

const sva::PTransformd & Robot::bodyTransform(const std::string & bName) const
{
  if(!hasBody(bName))
  {
    LOG_ERROR_AND_THROW(std::runtime_error, "No body transform with name " << bName << " found in this robot")
  }
  return bodyTransforms_[bodyIndexByName(bName)];
}

const sva::PTransformd & Robot::bodyTransform(int bodyIndex) const
{
  return bodyTransforms_[bodyIndex];
}

const std::vector<sva::PTransformd> & Robot::bodyTransforms() const
{
  return bodyTransforms_;
}

const sva::PTransformd & Robot::collisionTransform(const std::string & cName) const
{
  if(collisionTransforms_.count(cName) == 0)
  {
    LOG_ERROR_AND_THROW(std::runtime_error, "No collision transform with name " << cName << " found in this robot")
  }
  return collisionTransforms_.at(cName);
}

void Robot::fixSurfaces()
{
  for(auto & s : surfaces_)
  {
    const sva::PTransformd & trans = bodyTransform(s.second->bodyName());
    s.second->X_b_s(s.second->X_b_s() * trans);
  }
}

void Robot::fixCollisionTransforms()
{
  for(auto & ct : collisionTransforms_)
  {
    // assert(bodyTransforms_.size(ct.first));
    const auto & trans = bodyTransform(ct.first);
    ct.second = ct.second * trans;
  }
}

void Robot::loadRSDFFromDir(const std::string & surfaceDir)
{
  std::vector<SurfacePtr> surfacesIn = readRSDFFromDir(surfaceDir);
  for(const auto & sp : surfacesIn)
  {
    /* Check coherence of surface with mb */
    if(hasBody(sp->bodyName()))
    {
      surfaces_[sp->name()] = sp;
    }
    else
    {
      LOG_WARNING("Loaded surface " << sp->name() << " attached to body " << sp->bodyName()
                                    << " from RSDF but the robot " << name()
                                    << " has no such body, discard this surface to avoid future problems...")
    }
  }
  fixSurfaces();
}

std::map<std::string, std::vector<double>> Robot::stance() const
{
  return stance_;
}

unsigned int mc_rbdyn::Robot::robotIndex() const
{
  return robots_idx_;
}

void Robot::forwardKinematics()
{
  rbd::forwardKinematics(mb(), mbc());
}
void Robot::forwardKinematics(rbd::MultiBodyConfig & mbc) const
{
  rbd::forwardKinematics(mb(), mbc);
}

void Robot::forwardVelocity()
{
  rbd::forwardVelocity(mb(), mbc());
}
void Robot::forwardVelocity(rbd::MultiBodyConfig & mbc) const
{
  rbd::forwardVelocity(mb(), mbc);
}

void Robot::forwardAcceleration(const sva::MotionVecd & A_0)
{
  rbd::forwardAcceleration(mb(), mbc(), A_0);
}
void Robot::forwardAcceleration(rbd::MultiBodyConfig & mbc, const sva::MotionVecd & A_0) const
{
  rbd::forwardAcceleration(mb(), mbc, A_0);
}

void mc_rbdyn::Robot::eulerIntegration(double step)
{
  rbd::eulerIntegration(mb(), mbc(), step);
}

void mc_rbdyn::Robot::eulerIntegration(rbd::MultiBodyConfig & mbc, double step) const
{
  rbd::eulerIntegration(mb(), mbc, step);
}

const sva::PTransformd & Robot::posW() const
{
  return bodyPosW().at(0);
}

void Robot::posW(const sva::PTransformd & pt)
{
  if(mb().joint(0).type() == rbd::Joint::Type::Free)
  {
    sva::Quaterniond rotation{pt.rotation().transpose()};
    rotation.normalize();
    q()[0] = {rotation.w(),         rotation.x(),         rotation.y(),        rotation.z(),
              pt.translation().x(), pt.translation().y(), pt.translation().z()};
    forwardKinematics();
  }
  else if(mb().joint(0).type() == rbd::Joint::Type::Fixed)
  {
    mb().transform(0, pt);
    forwardKinematics();
    fixSCH(*this, this->convexes_, this->collisionTransforms_);
  }
  else
  {
    LOG_ERROR_AND_THROW(std::logic_error,
                        "The root pose can only be changed for robots with a free flyer or a fixed joint as joint(0)");
  }
}

void Robot::velW(const sva::MotionVecd & vel)
{
  if(mb().joint(0).type() == rbd::Joint::Type::Free)
  {
    auto vB = sva::PTransformd(mbc().bodyPosW[0].rotation()) * vel;
    alpha()[0][0] = vB.angular().x();
    alpha()[0][1] = vB.angular().y();
    alpha()[0][2] = vB.angular().z();
    alpha()[0][3] = vB.linear().x();
    alpha()[0][4] = vB.linear().y();
    alpha()[0][5] = vB.linear().z();
    forwardVelocity();
  }
  else
  {
    LOG_WARNING("You cannot set the base velocity on a fixed-base robot")
  }
}

const sva::MotionVecd & Robot::velW() const
{
  return bodyVelW().at(0);
}

void Robot::copy(Robots & robots, unsigned int robots_idx, const Base & base) const
{
  robots.robots_.emplace_back(Robot(robots, robots_idx, false, &base.X_0_s, base.baseName));
  auto & robot = robots.robots_.back();
  for(const auto & s : surfaces_)
  {
    robot.surfaces_[s.first] = s.second->copy();
  }
  robot.fixSurfaces();
  for(const auto & cH : convexes_)
  {
    // FIXME Should implement sch::S_Object::clone in sch-core but this should be good enough for now
    sch::S_Polyhedron * poly = dynamic_cast<sch::S_Polyhedron *>(cH.second.second.get());
    if(poly)
    {
      robot.convexes_[cH.first] = {cH.second.first, std::make_shared<sch::S_Polyhedron>(*poly)};
    }
    else
    {
      LOG_WARNING("Could not copy the convex "
                  << cH.first << " as it's not an sch::S_Polyhedron object, send complaint to mc_rtc maintainers...")
    }
  }
  fixSCH(robot, robot.convexes_, robot.collisionTransforms_);
}

void Robot::copy(Robots & robots, unsigned int robots_idx) const
{
  robots.robots_.emplace_back(Robot(robots, robots_idx, false));
  auto & robot = robots.robots_.back();
  for(const auto & s : surfaces_)
  {
    robot.surfaces_[s.first] = s.second->copy();
  }
}

mc_rbdyn::Surface & Robot::copySurface(const std::string & sName, const std::string & name)
{
  if(hasSurface(name))
  {
    LOG_ERROR_AND_THROW(std::runtime_error,
                        name << " already exists within this robot. Cannot overwrite an existing surface")
  }
  const Surface & surf = surface(sName);
  SurfacePtr nSurf = surf.copy();
  nSurf->name(name);
  surfaces_[name] = nSurf;
  return *nSurf;
}

void mc_rbdyn::Robot::addSurface(SurfacePtr surface, bool doNotReplace)
{
  if(!hasBody(surface->bodyName()))
  {
    LOG_WARNING("Surface " << surface->name() << " attached to body " << surface->bodyName() << " but the robot "
                           << name() << " has no such body.")
    return;
  }
  if(hasSurface(surface->name()) && doNotReplace)
  {
    LOG_WARNING("Surface " << surface->name() << " already exists for the robot " << name() << ".")
    return;
  }
  surfaces_[surface->name()] = std::move(surface);
}

#pragma GCC diagnostic pop

double mc_rbdyn::Robot::mass() const
{
  double mass = 0.;
  for(const auto & b : mb().bodies())
  {
    mass += b.inertia().mass();
  }
  return mass;
}

void mc_rbdyn::Robot::zmpTarget(const Eigen::Vector3d & zmp)
{
  zmp_ = zmp;
}

const Eigen::Vector3d & mc_rbdyn::Robot::zmpTarget() const
{
  return zmp_;
}

} // namespace mc_rbdyn


text:
/**
 * @file newproblem_test.cc
 * @brief NPDE homework NewProblem code
 * @author Oliver Rietmann, Erick Schulz
 * @date 01.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "../newproblem.h"

namespace NewProblem::test {

TEST(NewProblem, dummyFunction) {
  double x = 0.0;
  int n = 0;

  Eigen::Vector2d v = NewProblem::dummyFunction(x, n);

  Eigen::Vector2d v_ref = {1.0, 1.0};

  double tol = 1.0e-8;
  ASSERT_NEAR(0.0, (v - v_ref).lpNorm<Eigen::Infinity>(), tol);
}

}  // namespace NewProblem::test

ext: cc

max_stars_repo_name: padomu/NPDECODES

max_issues_repo_name: padomu/NPDECODES

max_forks_repo_name: padomu/NPDECODES


text:
//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - arg/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/arithmetic/include/functions/arg.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/atan2.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

NT2_TEST_CASE_TPL ( arg_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                         cT;
  typedef native<T ,ext_t>                        vT;
  typedef native<cT ,ext_t>                      vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;

  // specific values tests
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Inf<vT>(), nt2::Zero<vT>())), nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Minf<vT>(), nt2::Zero<vT>())),nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Nan<vT>(), nt2::Zero<vT>())), nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::One<vT>(), nt2::Zero<vT>())), nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Zero<vT>(), nt2::Zero<vT>())),nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Zero<vT>(), nt2::One<vT>())),  nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::One<vT>(), nt2::Zero<vT>())),  nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Two<vT>(), nt2::Two<vT>())),   nt2::Pio_2<vT>()/nt2::Two<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Two<vT>(),nt2::Mtwo<vT>())),  -nt2::Pio_2<vT>()/nt2::Two<vT>());

  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Inf<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Minf<vciT>())) , -nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Nan<vciT>()))  , nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::One<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Zero<vciT>())) , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Two<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mone<vciT>()))  , -nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mzero<vciT>())) , -nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mtwo<vciT>()))  , -nt2::Pio_2<vT>());

  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Inf<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Minf<vdT>())) , nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Nan<vdT>()))  , nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::One<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Zero<vdT>())) , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Two<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mone<vdT>())) ,nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mzero<vdT>())),nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mtwo<vdT>())) ,nt2::Pi<vT>());

} // end of test for floating_

ext: cpp


text:
// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/utility.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

using boost::mp11::mp_eval_if;
using boost::mp11::mp_eval_if_q;
using boost::mp11::mp_identity_t;
using boost::mp11::mp_valid;
using boost::mp11::mp_quote;

template<class C, class... A> using eval_if = mp_eval_if<C, void, mp_identity_t, A...>;

int main()
{
    BOOST_TEST_TRAIT_TRUE((mp_valid<eval_if, std::true_type>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<eval_if, std::true_type, void>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<eval_if, std::true_type, void, void>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<eval_if, std::true_type, void, void, void>));

    BOOST_TEST_TRAIT_FALSE((mp_valid<eval_if, std::false_type>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<eval_if, std::false_type, void>));
    BOOST_TEST_TRAIT_FALSE((mp_valid<eval_if, std::false_type, void, void>));
    BOOST_TEST_TRAIT_FALSE((mp_valid<eval_if, std::false_type, void, void, void>));

    using Qi = mp_quote<mp_identity_t>;

    BOOST_TEST_TRAIT_TRUE((mp_valid<mp_eval_if_q, std::true_type, void, Qi>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<mp_eval_if_q, std::true_type, void, Qi, void>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<mp_eval_if_q, std::true_type, void, Qi, void, void>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<mp_eval_if_q, std::true_type, void, Qi, void, void, void>));

    BOOST_TEST_TRAIT_FALSE((mp_valid<mp_eval_if_q, std::false_type, void, Qi>));
    BOOST_TEST_TRAIT_TRUE((mp_valid<mp_eval_if_q, std::false_type, void, Qi, void>));
    BOOST_TEST_TRAIT_FALSE((mp_valid<mp_eval_if_q, std::false_type, void, Qi, void, void>));
    BOOST_TEST_TRAIT_FALSE((mp_valid<mp_eval_if_q, std::false_type, void, Qi, void, void, void>));

    return boost::report_errors();
}


text:
// Copyright (c) 2021 Fetullah Atas, Norwegian University of Life Sciences
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Equations and logic parts of this code was based on ;
// https://github.com/MPC-Berkeley/genesis_path_follower
// Also refer to;
// https://github.com/MPC-Berkeley/barc/wiki/Car-Model

#ifndef VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_
#define VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_

#include <casadi/casadi.hpp>

#include <vector>
#include <memory>
#include <chrono>
#include <Eigen/Eigen>
#include <vox_nav_control/common.hpp>

namespace vox_nav_control
{
  namespace mpc_controller_casadi
  {

/**
 * @brief CASADI based MOdel Predcitive Control for Ackermann Vehicle
 *
 */
    class MPCControllerCasadiCore
    {
    public:
      /**
       * @brief structure for slack variables, these are used to relax input rate constraints
       *
       */
      struct SlackVars
      {
        double sl_acc;
        double sl_df;
        SlackVars()
        : sl_acc(0.0),
          sl_df(0.0) {}
      };

      /**
       * @brief result is passed as this struct
       *
       */
      struct SolutionResult
      {
        // whether the found solution was optimal
        bool is_optimal;
        // Execution ime took by solver in milliseconds
        int solve_time_ms;
        // computed control input as result of optimal control
        vox_nav_control::common::ControlInput control_input;
        // actual states that are going to be achieved in the time horizon
        std::vector<vox_nav_control::common::States> actual_computed_states;
        SolutionResult()
        : is_optimal(false),
          solve_time_ms(0)
        {}
      };

      /**
       * @brief Construct a new MPCControllerCasadiCore object
       *
       * @param params
       */
      MPCControllerCasadiCore(vox_nav_control::common::Parameters params);

      /**
       * @brief Destroy the MPCControllerCasadiCore object
       *
       */
      ~MPCControllerCasadiCore();

      /**
       * @brief state dynamcs constraints, control input
       * constraints and boundries are defined through this function
       *
       */
      void addConstraints();

      /**
       * @brief objective function and cost is defined with this function
       *
       */
      void addCost();

      /**
       * @brief update the current states
       *
       * @param curr_states
       */
      void updateCurrentStates(vox_nav_control::common::States curr_states);

      /**
       * @brief update current refernce states in horizon
       *
       * @param reference_states
       */
      void updateReferences(std::vector<vox_nav_control::common::States> reference_states);

      /**
       * @brief update previous control input
       *
       * @param previous_control_input
       */
      void updatePreviousControlInput(vox_nav_control::common::ControlInput previous_control_input);

      /**
       * @brief initialize slack variables, mostly just equalizing them to zero
       *
       * @param initial_slack_vars
       */
      void initializeSlackVars(std::vector<SlackVars> initial_slack_vars);

      /**
       * @brief  initialize actual states, mostly just equalizing them to zero.
       * These cannot be updated
       * they are variables of optimal control problem itself
       *
       * @param initial_actual_states
       */
      void initializeActualStates(
        std::vector<vox_nav_control::common::States> initial_actual_states);

      /**
     * @brief  initialize control inputs, mostly just equalizing them to zero. These cannot be updated
     *  they are variables of optimal control problem itself
     *
     * @param initial_actual_control_inputs
     */
      void initializeActualControlInputs(
        std::vector<vox_nav_control::common::ControlInput> initial_actual_control_inputs);

      /**
       * @brief solve the actual optiml control problem
       *
       * @return SolutionResult, includes resulting control inputs
       * as well as optimal control variables; computed actual states
       */
      SolutionResult solve(const std::vector<vox_nav_control::common::Ellipsoid> & obstacles);

    private:
      std::shared_ptr<casadi::Opti> opti_;
      // used to slice casadi matrixes
      casadi::Slice slice_all_;
      casadi::Matrix<double> Q;
      casadi::Matrix<double> R;

      casadi::MX u_prev_; // previous input: [u_{acc, -1}, u_{df, -1}]
      casadi::MX z_curr_; // current state:  [x_0, y_0, psi_0, v_0]

      // reference traj that we would like to follow
      // First index corresponds to our desired state at timestep k+1:
      // i.e. z_ref_[0,:] = z_{desired, 1}.
      // second index selects the state element from [x_k, y_k, psi_k, v_k].
      casadi::MX z_ref_;
      casadi::MX x_ref_;
      casadi::MX y_ref_;
      casadi::MX psi_ref_;
      casadi::MX v_ref_;

      // Actual trajectory we will follow given the optimal solution.
      // First index is the timestep k, i.e. self.z_dv[0,:] is z_0.
      // It has N+1 timesteps since we go from z_0, ..., z_N.
      // Second index is the state element, as detailed below.
      casadi::MX z_dv_;
      casadi::MX x_dv_;
      casadi::MX y_dv_;
      casadi::MX psi_dv_;
      casadi::MX v_dv_;

      // Control inputs used to achieve z_dv according to dynamics.
      // First index is the timestep k, i.e. self.u_dv[0,:] is u_0.
      // Second index is the input element as detailed below.
      casadi::MX u_dv_;
      casadi::MX acc_dv_;
      casadi::MX df_dv_;

      // Slack variables used to relax input rate constraints.
      casadi::MX sl_dv_;
      casadi::MX sl_acc_dv_;
      casadi::MX sl_df_dv_;

      // this will be updated through the constructor.
      vox_nav_control::common::Parameters params_;

      casadi::MX z_static_obs_;
      casadi::MX x_static_obs_;
      casadi::MX y_static_obs_;
      casadi::MX a_static_obs_;
      casadi::MX b_static_obs_;

      casadi::MX z_dynamic_obs_;
      casadi::MX x_dynamic_obs_;
      casadi::MX y_dynamic_obs_;
      casadi::MX a_dynamic_obs_;
      casadi::MX b_dynamic_obs_;
      casadi::MX yaw_dynamic_obs_;

    };
  } // namespace mpc_controller_casadi
}  // namespace vox_nav_control

#endif  // VOX_NAV_CONTROL__MPC_CONTROLLER__MPC_CONTROLLER_CORE_HPP_

ext: hpp


text:
/***************************************************************************************************************************
 * landpad_det.cpp
 * Author: Jario
 * Update Time: 2020.1.12
 *
 * 说明: 降落目标识别程序，降落板的尺寸为60cmX60cm
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/landpad_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_landpad_det
***************************************************************************************************************************/

#include <time.h>
#include <fstream>
#include <iostream>
#include <math.h>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// ros头文件
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <ros/package.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

// opencv头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

// topic 头文件
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/Message.h>

#include "message_utils.h"

using namespace std;
using namespace cv;

double threshold_error=0.4;

//---------------------------variables---------------------------------------
//------------ROS TOPIC---------
//【订阅】无人机位置
ros::Subscriber drone_pose_sub;
//【订阅】小车位置
ros::Subscriber vehicle_pose_sub;
//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【订阅】输入开关量
ros::Subscriber switch_subscriber;
//【发布】无人机和小车相对位置
ros::Publisher position_pub;
//【发布】识别后的图像
image_transport::Publisher landpad_pub;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

//-------------VISION-----------
Mat img;
prometheus_msgs::DetectionInfo pose_now;

//-------------TIME-------------
ros::Time begin_time;
float photo_time;
double calculation_time;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 无人机位姿message
geometry_msgs::Pose pos_drone_optitrack;
Eigen::Vector3d euler_drone_optitrack;
Eigen::Quaterniond q_drone;
// 小车位姿message
geometry_msgs::Pose pos_vehicle_optitrack;
Eigen::Vector3d euler_vehicle_optitrack;
Eigen::Quaterniond q_vehicle;

// 保存的上次观测的位置 用于cluster算法使用
Eigen::Vector3d last_position;
bool bool_last_position = false;
// 接收消息，允许暂停检测
bool is_suspanded = false;
bool local_print = false;
bool message_print = true;

void printf_result();

//-----------------利用Euler角进行三次旋转得到无人机相对目标的位置------------------
void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
    double x1 = x;  // 将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
    double y1 = y;
    double rz = thetaz * CV_PI / 180;
    outx = cos(rz) * x1 - sin(rz) * y1;
    outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
    double x1 = x;
    double z1 = z;
    double ry = thetay * CV_PI / 180;
    outx = cos(ry) * x1 + sin(ry) * z1;
    outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
    double y1 = y;  // 将变量拷贝一次，保证&y == &y这种情况下也能计算正确
    double z1 = z;
    double rx = thetax * CV_PI / 180;
    outy = cos(rx) * y1 - sin(rx) * z1;
    outz = cos(rx) * z1 + sin(rx) * y1;
}
// 四元数转Euler
// q0 q1 q2 q3
// w x y z
void quaternion_2_euler(Eigen::Quaterniond quat, Eigen::Vector3d &angle)
{
    angle(0) = atan2(2.0 * (quat.z() * quat.y() + quat.w() * quat.x()), 1.0 - 2.0 * (quat.x() * quat.x() + quat.y() * quat.y()));
    angle(1) = asin(2.0 * (quat.y() * quat.w() - quat.z() * quat.x()));
    // angle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), -1.0 + 2.0 * (quat[0] * quat[0] + quat[1] * quat[1]));
    angle(2) = atan2(2.0 * (quat.z() * quat.w() + quat.x() * quat.y()), 1.0 - 2.0 * (quat.y() * quat.y() + quat.z() * quat.z()));
}

//--------------------------利用optitrack获取真值-------------------------------

// 获取系统时间
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[LandpadDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "landpad_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    // 发布调试消息
    msg_node_name = "/prometheus/message/landpad_det";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    position_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10);

    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/landpad_det", 10, switchCallback);

    // 接收图像的话题
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    // 发布ArUco检测结果的话题
    landpad_pub = it.advertise("/prometheus/camera/rgb/image_landpad_det", 1);

    sensor_msgs::ImagePtr msg_ellipse;

    std::string ros_path = ros::package::getPath("prometheus_detection");

    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);

    // 读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(camera_info);
    // 相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    // 相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double landpad_det_len = camera_config["landpad_det_len"].as<double>();
    // DEBUG
    // cout << fx << " " << fy << " " << cx << " " << cy << " " << k1 << " " << k2 << " ";

    //--------------------------相机参数赋值---------------------
    // 相机内参
    Mat camera_matrix;
    camera_matrix = cv::Mat(3,3,CV_64FC1,cv::Scalar::all(0));
    camera_matrix.ptr<double>(0)[0] = fx;
    camera_matrix.ptr<double>(0)[2] = cx;
    camera_matrix.ptr<double>(1)[1] = fy;
    camera_matrix.ptr<double>(1)[2] = cy;
    camera_matrix.ptr<double>(2)[2] = 1.0f;
    // 相机畸变参数k1 k2 p1 p2 k3
    Mat distortion_coefficients;
    distortion_coefficients = cv::Mat(5,1,CV_64FC1,cv::Scalar::all(0));
    distortion_coefficients.ptr<double>(0)[0] = k1;
    distortion_coefficients.ptr<double>(1)[0] = k2;
    distortion_coefficients.ptr<double>(2)[0] = p1;
    distortion_coefficients.ptr<double>(3)[0] = p2;
    distortion_coefficients.ptr<double>(4)[0] = k3;

    // ArUco Marker字典选择以及旋转向量和评议向量初始化
    Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(10);

    vector<double> rv(3), tv(3);
    cv::Mat rvec(rv), tvec(tv);
    // cv::VideoCapture capture(0);
    float last_x(0), last_y(0), last_z(0), last_yaw(0), last_az(0), last_ay(0), last_ax(0), last_qx(0), last_qy(0), last_qz(0), last_qw(0);
    bool switch_state = is_suspanded;

    // 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
    ros::Rate loopRate(20);
    ros::Rate loopRate_1Hz(1);
    //----------------------------------------主循环------------------------------------
    // const auto wait_duration = std::chrono::milliseconds(2000);
    while (ros::ok())
    {
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");

            // std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
            loopRate_1Hz.sleep();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            img = cam_image_copy.clone();
        }

        clock_t start = clock();

        //------------------调用ArUco Marker库对图像进行识别--------------
        // markerids存储每个识别到二维码的编号  markerCorners每个二维码对应的四个角点的像素坐标
        std::vector<int> markerids, markerids_deted;
        vector<vector<Point2f> > markerCorners, markerCorners_deted, rejectedCandidate;

        Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
        cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate);

        if (markerids_deted.size() > 0)
        {
            for (int tt=0; tt<markerids_deted.size(); tt++) {
                if (19 == markerids_deted[tt]) {
                    markerids.push_back(markerids_deted[tt]);
                    markerCorners.push_back(markerCorners_deted[tt]);
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (43 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (1 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (2 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (3 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
            if (markerids.size() == 0) {
                for (int tt=0; tt<markerids_deted.size(); tt++) {
                    if (4 == markerids_deted[tt]) {
                        markerids.push_back(markerids_deted[tt]);
                        markerCorners.push_back(markerCorners_deted[tt]);
                    }
                }
            }
        }

        //-------------------多于一个目标被识别到，进入算法-----------------
        if (markerids.size() > 0)
        {
            aruco::drawDetectedMarkers(img, markerCorners, markerids);

            std::vector<float> collected_tx, collected_ty, collected_tz;
            std::vector<float> collected_qx, collected_qy, collected_qz, collected_qw;

            int tt=0;
            vector< Vec3d > rvecs, tvecs;
            if (19 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.666667, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.666667*0.5f);
            }
            else if (43 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.066667, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.066667*0.5f);
            }
            else if (1 == markerids[tt] || 2 == markerids[tt] || 3 == markerids[tt] || 4 == markerids[tt])
            {
                vector<vector<Point2f> > markerCornersONE;
                markerCornersONE.push_back(markerCorners[tt]);
                aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len*0.133334, camera_matrix, distortion_coefficients, rvecs, tvecs);
                aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[0], tvecs[0],landpad_det_len*0.133334*0.5f);
            }

            cv::Mat rotation_matrix;
            cv::Rodrigues(rvecs[0], rotation_matrix);
            Eigen::Matrix3d rotation_matrix_eigen;
            cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
            Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
            q.normalize();

            /*if (43 != markerids[tt] && 19 != markerids[tt]) {
                static tf::TransformBroadcaster br;
                tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[0][0], tvecs[0][1], tvecs[0][2]));
                char obj_str[16];
                sprintf(obj_str, "object-%d", markerids[tt]);
                tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", obj_str);
                br.sendTransform(trans_world2camera);
            }*/

            std::vector<double> vec_t{tvecs[0][0], tvecs[0][1], tvecs[0][2]};
            cv::Mat vec_t_mat{vec_t};
            vec_t_mat = vec_t_mat;
            vec_t_mat.convertTo(vec_t_mat, CV_32FC1);
            // cout << "vec_t_mat.size():" << vec_t_mat.size() << endl;
            // cout << "vec_t_mat.type():" << vec_t_mat.type() <<endl;
            std::vector<double> id_to8_t(3);

            if (19 == markerids[tt] || 43 == markerids[tt])
            {
                id_to8_t[0] = 0.;
                id_to8_t[1] = 0.;
                id_to8_t[2] = 0.;
            }
            else if (1 == markerids[tt])
            {
                id_to8_t[0] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (2 == markerids[tt])
            {
                id_to8_t[0] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (3 == markerids[tt])
            {
                id_to8_t[0] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = -(landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }
            else if (4 == markerids[tt])
            {
                id_to8_t[0] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[1] = (landpad_det_len*0.666667 + landpad_det_len*0.133334) / 2.;
                id_to8_t[2] = 0.;
            }

            cv::Mat id_to8_t_mat{id_to8_t};
            id_to8_t_mat.convertTo(id_to8_t_mat, CV_32FC1);

            rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
            // cv::invert(rotation_matrix, rotation_matrix);
            cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;
            // cout << id_8_t << endl;

            float o_tx = id_8_t.at<float>(0);
            float o_ty = id_8_t.at<float>(1);
            float o_tz = id_8_t.at<float>(2);

            float o_qx = q.x();
            float o_qy = q.y();
            float o_qz = q.z();
            float o_qw = q.w();

            // 将解算的位置转化成旋转矩阵 并旋转计算无人机相对于目标的位置
            float r11 = rotation_matrix.ptr<float>(0)[0];
            float r12 = rotation_matrix.ptr<float>(0)[1];
            float r13 = rotation_matrix.ptr<float>(0)[2];
            float r21 = rotation_matrix.ptr<float>(1)[0];
            float r22 = rotation_matrix.ptr<float>(1)[1];
            float r23 = rotation_matrix.ptr<float>(1)[2];
            float r31 = rotation_matrix.ptr<float>(2)[0];
            float r32 = rotation_matrix.ptr<float>(2)[1];
            float r33 = rotation_matrix.ptr<float>(2)[2];

            // 计算欧拉角
            float thetaz = atan2(r21, r11) / CV_PI * 180;
            float thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
            float thetax = atan2(r32, r33) / CV_PI * 180;

            // C2W代表 相机坐标系转换到世界坐标系  W2C代表 世界坐标系转换到相机坐标系 Theta为欧拉角
            cv::Point3f Theta_C2W;
            cv::Point3f Theta_W2C;
            cv::Point3f Position_OcInW;

            Theta_C2W.z = thetaz;
            Theta_C2W.y = thetay;
            Theta_C2W.x = thetax;

            Theta_W2C.x = -1 * thetax;
            Theta_W2C.y = -1 * thetay;
            Theta_W2C.z = -1 * thetaz;

            Position_OcInW.x = id_8_t.at<float>(0);
            Position_OcInW.y = id_8_t.at<float>(1);
            Position_OcInW.z = id_8_t.at<float>(2);

            Eigen::Vector3d eulerVec;
            eulerVec(0) = (Theta_C2W.z) / 180 * CV_PI;
            double A1_yaw = eulerVec(0);

            // 将解算后的位置发给控制端
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = true;
            pose_now.frame = 0;
            pose_now.position[0] = o_tx;
            pose_now.position[1] = o_ty;
            pose_now.position[2] = o_tz;
            pose_now.attitude[0] = thetaz;
            pose_now.attitude[1] = thetay;
            pose_now.attitude[2] = thetax;
            pose_now.attitude_q[0] = o_qx;
            pose_now.attitude_q[1] = o_qy;
            pose_now.attitude_q[2] = o_qz;
            pose_now.attitude_q[3] = o_qw;
            pose_now.sight_angle[0] = atan(o_tx / o_tz);
            pose_now.sight_angle[1] = atan(o_ty / o_tz);
            pose_now.yaw_error = A1_yaw;

            last_x = pose_now.position[0];
            last_y = pose_now.position[1];
            last_z = pose_now.position[2];
            last_az = pose_now.attitude[0];
            last_ay = pose_now.attitude[1];
            last_ax = pose_now.attitude[2];
            last_qx = pose_now.attitude_q[0];
            last_qy = pose_now.attitude_q[1];
            last_qz = pose_now.attitude_q[2];
            last_qw = pose_now.attitude_q[3];
            last_yaw = pose_now.yaw_error;
        }
        else
        {
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = false;
            pose_now.frame = 0;
            pose_now.position[0] = last_x;
            pose_now.position[1] = last_y;
            pose_now.position[2] = last_z;
            pose_now.attitude[0] = last_az;
            pose_now.attitude[1] = last_ay;
            pose_now.attitude[2] = last_ax;
            pose_now.attitude_q[0] = last_qx;
            pose_now.attitude_q[1] = last_qy;
            pose_now.attitude_q[2] = last_qz;
            pose_now.attitude_q[3] = last_qw;
            pose_now.sight_angle[0] = atan(last_x / last_z);
            pose_now.sight_angle[1] = atan(last_y / last_z);
            pose_now.yaw_error = last_yaw;
        }
        position_pub.publish(pose_now);

        // 计算算法运行时间
        clock_t finish=clock();
        calculation_time=(finish-start)/1000;

        // 打印
        // printf_result();

        // 画出识别到的二维码
        // cv::aruco::drawDetectedMarkers(img, markerCorners, markerids);

        msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
        landpad_pub.publish(msg_ellipse);
        }

        // cv::imshow("test",img);
        ros::spinOnce();
        cv::waitKey(1);
        loopRate.sleep();
    }
}

void printf_result()
{
    // 固定的浮点显示
    cout.setf(ios::fixed);
    // setprecision(n) 设显示小数精度为n位
    cout<<setprecision(4);
    // 左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Landpad Detection<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    if(pose_now.detected)
    {
        cout << "is_detected: ture" <<endl;
    }else
    {
        cout << "is_detected: false" <<endl;
    }
    cout << "pos_target: [X Y Z] : " << pose_now.position[0]  << " [m] "<< pose_now.position[1] <<" [m] " << pose_now.position[2] << " [m] "<<endl;
    cout << "pos_target: [Yaw] :   " << pose_now.yaw_error/3.1415926 *180    << " [du] "<<endl;
    cout << "calculation_time =    " << time << " [ms] " << endl;
}


text:
//
// Copyright 2010 Ettus Research LLC
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//

#ifndef INCLUDED_UHD_USRP_DBOARD_BASE_HPP
#define INCLUDED_UHD_USRP_DBOARD_BASE_HPP

#include <uhd/config.hpp>
#include <uhd/property_tree.hpp>
#include <uhd/utils/pimpl.hpp>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <uhd/usrp/dboard_id.hpp>
#include <uhd/usrp/dboard_iface.hpp>

namespace uhd{ namespace usrp{

/*!
 * A daughter board dboard_base class for all dboards.
 * Only other dboard dboard_base classes should inherit this.
 */
class UHD_API dboard_base : boost::noncopyable{
public:
    typedef boost::shared_ptr<dboard_base> sptr;
    /*!
     * An opaque type for the dboard constructor args.
     * Derived classes should pass the args into the base class,
     * but should not deal with the internals of the args.
     */
    typedef void * ctor_args_t;

    //structors
    dboard_base(ctor_args_t);

protected:
    std::string get_subdev_name(void);
    dboard_iface::sptr get_iface(void);
    dboard_id_t get_rx_id(void);
    dboard_id_t get_tx_id(void);
    property_tree::sptr get_rx_subtree(void);
    property_tree::sptr get_tx_subtree(void);

private:
    UHD_PIMPL_DECL(impl) _impl;
};

/*!
 * A xcvr daughter board implements rx and tx methods
 * Sub classes for xcvr boards should inherit this.
 */
class UHD_API xcvr_dboard_base : public dboard_base{
public:
    /*!
     * Create a new xcvr dboard object, override in subclasses.
     */
    xcvr_dboard_base(ctor_args_t);
};

/*!
 * A rx daughter board only implements rx methods.
 * Sub classes for rx-only boards should inherit this.
 */
class UHD_API rx_dboard_base : public dboard_base{
public:
    /*!
     * Create a new rx dboard object, override in subclasses.
     */
    rx_dboard_base(ctor_args_t);
};

/*!
 * A tx daughter board only implements tx methods.
 * Sub classes for rx-only boards should inherit this.
 */
class UHD_API tx_dboard_base : public dboard_base{
public:
    /*!
     * Create a new rx dboard object, override in subclasses.
     */
    tx_dboard_base(ctor_args_t);
};

}} //namespace

#endif /* INCLUDED_UHD_USRP_DBOARD_BASE_HPP */

max_stars_repo_name: cityscapesc/specobs

max_issues_repo_name: cityscapesc/specobs

max_forks_repo_name: cityscapesc/specobs


text:
/*
  [auto_generated]
  boost/numeric/odeint/external/compute/compute.hpp

  [begin_description]
  includes all headers required for using odeint with Boost.Compute
  [end_description]

  Copyright 2009-2013 Karsten Ahnert
  Copyright 2009-2013 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_HPP_DEFINED

#include <boost/numeric/odeint/external/compute/compute_algebra.hpp>
#include <boost/numeric/odeint/external/compute/compute_operations.hpp>
#include <boost/numeric/odeint/external/compute/compute_algebra_dispatcher.hpp>
#include <boost/numeric/odeint/external/compute/compute_operations_dispatcher.hpp>
#include <boost/numeric/odeint/external/compute/compute_resize.hpp>

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_HPP_DEFINED

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
#include <igl/decimate.h>
#include <igl/qslim.h>
#include <Eigen/Core>
#include <iostream>
#include <set>

#ifdef MEX
#  include <mex.h>
#  include <igl/C_STR.h>
#  include <igl/matlab/mexErrMsgTxt.h>
#  undef assert
#  define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#endif

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  MatrixXd V,W;
  MatrixXi F,G;
  VectorXi J,I;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs>=3,"nrhs should be >= 3");
  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,F);
  mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
  mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
  mexErrMsgTxt(
    mxIsDouble(prhs[2]) && mxGetM(prhs[2])==1 && mxGetN(prhs[2])==1,
    "fraction to decimate should be scalar");
  double ratio = * mxGetPr(prhs[2]);
  mexErrMsgTxt((ratio>0 && ratio<1) || (ratio>0 && ratio<F.rows()) ,
    "Ratio should be in (0,1) or [1,#F)");
  const size_t max_m = ratio<1 ? ratio*F.rows() : ratio;

  enum DecimateMethod
  {
    DECIMATE_METHOD_NAIVE = 0,
    DECIMATE_METHOD_QSLIM = 1,
    NUM_DECIMATE_METHODS = 2
  } method = DECIMATE_METHOD_NAIVE;
  {
    int i = 3;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Method",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * type_name = mxArrayToString(prhs[++i]);
        if(strcmp("naive",type_name)==0)
        {
          method = DECIMATE_METHOD_NAIVE;
        }else if(strcmp("qslim",type_name)==0)
        {
          method = DECIMATE_METHOD_QSLIM;
        }else
        {
          mexErrMsgTxt(false,C_STR("Unknown method: "<<method));
        }
      }else
      {
        mexErrMsgTxt(false,C_STR("Unknown parameter: "<<name));
      }
      i++;
    }
  }

  switch(method)
  {
    case DECIMATE_METHOD_NAIVE:
      decimate(V,F,max_m,W,G,J,I);
      break;
    case DECIMATE_METHOD_QSLIM:
      qslim(V,F,max_m,W,G,J,I);
      break;
    default:
      mexErrMsgTxt(false,"Unkown method.");
      break;
  }

  switch(nlhs)
  {
    case 4:
      prepare_lhs_index(I,plhs+3);
    case 3:
      prepare_lhs_index(J,plhs+2);
    case 2:
      prepare_lhs_index(G,plhs+1);
    case 1:
      prepare_lhs_double(W,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


text:
///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2013 - 2016.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

/*!
  \file

 \brief This file is a partial reference implementation for the proposed
 "C++ binary fixed-point arithmetic" as specified in N3352.\n

 \sa http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2012/n3352.html \n

 In this file, we include subsidiary files that implement the
 proposed specified fixed-point types,
 and explain macros that may be optionally defined.

 \details
   There is optional support for certain variations of fixed_point
   using preprocessor definitions. Not all of these are supported
   at the moment. The potential options include:

   Is supported now  : \#define BOOST_FIXED_POINT_DISABLE_IOSTREAM\n
   Is supported now  : \#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION\n
   Not yet supported : \#define BOOST_FIXED_POINT_DISABLE_WIDE_INTEGER_MATH\n
   Not yet supported : \#define BOOST_FIXED_POINT_DISABLE_CPP11\n
   Is now supported  : \#define BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS\n
*/

#ifndef FIXED_POINT_2015_03_06_HPP_
  #define FIXED_POINT_2015_03_06_HPP_

  #ifdef BOOST_DOXYGEN_GENERATION
    // These items below document the function of optional macros,
    // but are processed only if the doxyfile or jamfile contains
    // <doxygen:param>PREDEFINED=BOOST_DOXYGEN_GENERATION

    /*! \def BOOST_FIXED_POINT_DISABLE_IOSTREAM
    \brief All I/O streaming is disabled, as is the inclusion of associated standard
    library headers. This option eliminates all I/O stream overhead,
    in particular for bare-metal microcontroller projects.
    Disabling I/O streaming requires simultaneous disabling of multiprecision,
    as shown below.
    */
    #define BOOST_FIXED_POINT_DISABLE_IOSTREAM

    /*! \def BOOST_FIXED_POINT_DISABLE_MULTIPRECISION
    \brief This option is defined to disable the use of
    Boost.Multiprecision for back-ends of the fixed-point classes.
    Multiprecision fixed-point numbers are not available if this
    option is set.
    (Implemented).
    */
    #define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

    /*! \def BOOST_FIXED_POINT_DISABLE_WIDE_INTEGER_MATH
    \brief This option is defined to avoid using the unsigned_large_type in the
    implementations of multiplication and division operations.
    This option is intended for systems with limited integer widths
    such as bare-metal microcontrollers.
    When used in combination with BOOST_FIXED_POINT_DISABLE_MULTIPRECISION,
    this option is intended to provide fixed-point representations
    with up to 64-bits (if 64-bit integer types are available)
    without requiring any multiprecision.
    (Not yet implemented).
    */
    #define BOOST_FIXED_POINT_DISABLE_WIDE_INTEGER_MATH

    /*! \def BOOST_FIXED_POINT_DISABLE_CPP11
    \brief This option enables support for a back-port to C++03.
    This option eliminates the use of all native C++11 language elements. This might send the
    wrong message about language technology, but could
    increase the range of potential target compilers, especially for embedded systems.
    (Implemented but not yet fully tested).
    */
    #define BOOST_FIXED_POINT_DISABLE_CPP11

    /*! \def BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS
    \brief When this option is defined, fixed-point uses multiple-precision backends
    (when required) based on GMP instead of Boost.Multiprecision's self-written
    binary floating-point backend.
    GMP backends use highly optimized assembler and advanced
    multiplication algorithms that are significantly faster for high digit counts
    in excess of a few hundred binary digits.
    \sa https://gmplib.org
    (Implemented but not yet fully tested).
    */
    #define BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS

  #endif // BOOST_DOXYGEN_GENERATION

  #include <boost/fixed_point/fixed_point_negatable.hpp>

#endif // FIXED_POINT_2015_03_06_HPP_


text:
/*
 * CVBoostConverter.hpp
 *
 *  Created on: Mar 20, 2014
 *      Author: Gregory Kramida
 *   Copyright: (c) 2014 Gregory Kramida
 *     License: MIT
 */

#ifndef CVBOOSTCONVERTER_HPP_
#define CVBOOSTCONVERTER_HPP_

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <numpy/ndarrayobject.h>
#include <opencv2/core/core.hpp>
#include <boost/python.hpp>
#include <cstdio>

namespace libpreviewer {

using namespace cv;

static PyObject* opencv_error = 0;

//===================    MACROS    =================================================================
#define ERRWRAP2(expr) \
try \
{ \
    PyAllowThreads allowThreads; \
    expr; \
} \
catch (const cv::Exception &e) \
{ \
    PyErr_SetString(opencv_error, e.what()); \
    return 0; \
}

//===================   ERROR HANDLING     =========================================================

static int failmsg(const char *fmt, ...);

//===================   THREADING     ==============================================================
class PyAllowThreads;
class PyEnsureGIL;

static size_t REFCOUNT_OFFSET = (size_t)&(((PyObject*)0)->ob_refcnt) +
    (0x12345678 != *(const size_t*)"\x78\x56\x34\x12\0\0\0\0\0")*sizeof(int);

static inline PyObject* pyObjectFromRefcount(const int* refcount)
{
    return (PyObject*)((size_t)refcount - REFCOUNT_OFFSET);
}

static inline int* refcountFromPyObject(const PyObject* obj)
{
    return (int*)((size_t)obj + REFCOUNT_OFFSET);
}

//===================   NUMPY ALLOCATOR FOR OPENCV     =============================================

class NumpyAllocator;

//===================   STANDALONE CONVERTER FUNCTIONS     =========================================

PyObject* fromMatToNDArray(const Mat& m);
Mat fromNDArrayToMat(PyObject* o);

//===================   BOOST CONVERTERS     =======================================================

struct matToNDArrayBoostConverter {
  static PyObject* convert(Mat const& m);
};

struct matFromNDArrayBoostConverter {

  matFromNDArrayBoostConverter();

  /// @brief Check if PyObject is an array and can be converted to OpenCV matrix.
  static void* convertible(PyObject* object);

  /// @brief Construct a Mat from an NDArray object.
  static void construct(PyObject* object,
      boost::python::converter::rvalue_from_python_stage1_data* data);
};
} // end namespace libpreviewer
#endif /* CVBOOSTCONVERTER_HPP_ */

max_stars_repo_name: Photonomie/previewer

max_issues_repo_name: Photonomie/previewer

max_forks_repo_name: Photonomie/previewer


text:
#pragma once

#include "boost_defs.hpp"

#include "dispatcher.hpp"
#include "logger.hpp"
#include "services.hpp"
#include <boost/signals2.hpp>

namespace krbn {
namespace monitor {
namespace service_monitor {
class monitor final : pqrs::dispatcher::extra::dispatcher_client {
public:
  // Signals (invoked from the shared dispatcher thread)

  boost::signals2::signal<void(std::shared_ptr<services>)> service_detected;
  boost::signals2::signal<void(std::shared_ptr<services>)> service_removed;

  // Methods

  monitor(const monitor&) = delete;

  monitor(CFDictionaryRef _Nonnull matching_dictionary) : matching_dictionary_(matching_dictionary),
                                                          notification_port_(nullptr),
                                                          matched_notification_(IO_OBJECT_NULL),
                                                          terminated_notification_(IO_OBJECT_NULL) {
    if (matching_dictionary_) {
      CFRetain(matching_dictionary_);
    }
  }

  virtual ~monitor(void) {
    detach_from_dispatcher([this] {
      stop();
    });

    if (matching_dictionary_) {
      CFRelease(matching_dictionary_);
    }
  }

  void async_start(void) {
    enqueue_to_dispatcher([this] {
      start();
    });
  }

  void async_stop(void) {
    enqueue_to_dispatcher([this] {
      stop();
    });
  }

  void async_invoke_service_detected(void) {
    enqueue_to_dispatcher([this] {
      if (matching_dictionary_) {
        CFRetain(matching_dictionary_);
        io_iterator_t it;
        auto kr = IOServiceGetMatchingServices(kIOMasterPortDefault, matching_dictionary_, &it);
        if (kr != KERN_SUCCESS) {
          logger::get_logger().error("IOServiceGetMatchingServices error: {1} @ {0}", __PRETTY_FUNCTION__, kr);
        } else {
          matched_callback(it);
          IOObjectRelease(it);
        }
      }
    });
  }

private:
  // This method is executed in the dispatcher thread.
  void start(void) {
    if (!notification_port_) {
      notification_port_ = IONotificationPortCreate(kIOMasterPortDefault);
      if (!notification_port_) {
        logger::get_logger().error("IONotificationPortCreate is failed @ {0}", __PRETTY_FUNCTION__);
        return;
      }

      if (auto loop_source = IONotificationPortGetRunLoopSource(notification_port_)) {
        CFRunLoopAddSource(CFRunLoopGetMain(), loop_source, kCFRunLoopCommonModes);
      } else {
        logger::get_logger().error("IONotificationPortGetRunLoopSource is failed @ {0}", __PRETTY_FUNCTION__);
      }
    }

    // kIOMatchedNotification

    if (!matched_notification_) {
      if (matching_dictionary_) {
        CFRetain(matching_dictionary_);
        auto kr = IOServiceAddMatchingNotification(notification_port_,
                                                   kIOMatchedNotification,
                                                   matching_dictionary_,
                                                   &(monitor::static_matched_callback),
                                                   static_cast<void*>(this),
                                                   &matched_notification_);
        if (kr != kIOReturnSuccess) {
          logger::get_logger().error("IOServiceAddMatchingNotification error: {1} @ {0}", __PRETTY_FUNCTION__, kr);
          CFRelease(matching_dictionary_);
        } else {
          matched_callback(matched_notification_);
        }
      }
    }

    // kIOTerminatedNotification

    if (!terminated_notification_) {
      if (matching_dictionary_) {
        CFRetain(matching_dictionary_);
        auto kr = IOServiceAddMatchingNotification(notification_port_,
                                                   kIOTerminatedNotification,
                                                   matching_dictionary_,
                                                   &(monitor::static_terminated_callback),
                                                   static_cast<void*>(this),
                                                   &terminated_notification_);
        if (kr != kIOReturnSuccess) {
          logger::get_logger().error("IOServiceAddMatchingNotification error: {1} @ {0}", __PRETTY_FUNCTION__, kr);
          CFRelease(matching_dictionary_);
        } else {
          terminated_callback(terminated_notification_);
        }
      }
    }
  }

  // This method is executed in the dispatcher thread.
  void stop(void) {
    if (matched_notification_) {
      IOObjectRelease(matched_notification_);
      matched_notification_ = IO_OBJECT_NULL;
    }

    if (terminated_notification_) {
      IOObjectRelease(terminated_notification_);
      terminated_notification_ = IO_OBJECT_NULL;
    }

    if (notification_port_) {
      if (auto loop_source = IONotificationPortGetRunLoopSource(notification_port_)) {
        CFRunLoopRemoveSource(CFRunLoopGetMain(), loop_source, kCFRunLoopCommonModes);
      }

      IONotificationPortDestroy(notification_port_);
      notification_port_ = nullptr;
    }
  }

  static void static_matched_callback(void* _Nonnull refcon, io_iterator_t iterator) {
    auto self = static_cast<monitor*>(refcon);
    if (!self) {
      return;
    }

    self->matched_callback(iterator);
  }

  void matched_callback(io_iterator_t iterator) {
    auto s = std::make_shared<services>(iterator);
    enqueue_to_dispatcher([this, s] {
      service_detected(s);
    });
  }

  static void static_terminated_callback(void* _Nonnull refcon, io_iterator_t iterator) {
    auto self = static_cast<monitor*>(refcon);
    if (!self) {
      return;
    }

    self->terminated_callback(iterator);
  }

  void terminated_callback(io_iterator_t iterator) {
    auto s = std::make_shared<services>(iterator);
    enqueue_to_dispatcher([this, s] {
      service_removed(s);
    });
  }

  CFDictionaryRef _Nonnull matching_dictionary_;

  IONotificationPortRef _Nullable notification_port_;
  io_iterator_t matched_notification_;
  io_iterator_t terminated_notification_;
};
} // namespace service_monitor
} // namespace monitor
} // namespace krbn

max_stars_repo_name: cyrusccy/Karabiner-Elements

max_issues_repo_name: cyrusccy/Karabiner-Elements

max_forks_repo_name: cyrusccy/Karabiner-Elements


text:
// vim: awa:sts=4:ts=4:sw=4:et:cin:fdm=manual:tw=120:ft=cpp
#include "common/include/special_files.h"

#include <algorithm>
#include <iterator>
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <catch2/catch.hpp>

using namespace mmotd::core::special_files;
using namespace std;
using namespace std::literals;

namespace mmotd::results::test {

CATCH_TEST_CASE("empty GetEnvironmentValue", "[SpecialFiles]") {
    CATCH_CHECK(GetEnvironmentValue(string{}) == string{});
}

CATCH_TEST_CASE("undefined GetEnvironmentValue", "[SpecialFiles]") {
    CATCH_CHECK(GetEnvironmentValue("NO_WAY_THIS_ENV_VAR_IS_DEFINED"s) == string{});
}

CATCH_TEST_CASE("empty ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_CHECK(ExpandEnvironmentVariables(string{}) == string{});
}

CATCH_TEST_CASE("undefined ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_SECTION("dollar-curlybrace-variable-curlybrace") {
        auto expanded = ExpandEnvironmentVariables("this is the ${NO_WAY_THIS_ENV_VAR_IS_DEFINED} string"s);
        CATCH_CHECK(expanded == "this is the  string"s);
    }
    CATCH_SECTION("dollar-variable") {
        auto expanded = ExpandEnvironmentVariables("this is the $NO_WAY_THIS_ENV_VAR_IS_DEFINED string"s);
        CATCH_CHECK(expanded == "this is the  string"s);
    }
}

CATCH_TEST_CASE("first in string ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_SECTION("dollar-curlybrace-variable-curlybrace") {
        CATCH_CHECK(!ExpandEnvironmentVariables("${USER}"s).empty());
    }
    CATCH_SECTION("dollar-variable") { CATCH_CHECK(!ExpandEnvironmentVariables("$USER"s).empty()); }
}

CATCH_TEST_CASE("middle in string ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_SECTION("dollar-curlybrace-variable-curlybrace") {
        auto expanded = ExpandEnvironmentVariables("/filling/${USER}/filling/"s);
        auto trimmed = boost::replace_all_copy(expanded, "/filling/"s, ""s);
        CATCH_CHECK(!trimmed.empty());
    }
    CATCH_SECTION("dollar-variable") {
        auto expanded = ExpandEnvironmentVariables("/filling/$USER/filling/"s);
        auto trimmed = boost::replace_all_copy(expanded, "/filling/"s, ""s);
        CATCH_CHECK(!trimmed.empty());
    }
}

CATCH_TEST_CASE("last in string ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_SECTION("dollar-curlybrace-variable-curlybrace") {
        auto expanded = ExpandEnvironmentVariables("/filling/${HOME}"s);
        auto trimmed = expanded.substr("/filling/"s.size());
        CATCH_CHECK(!trimmed.empty());
    }
    CATCH_SECTION("dollar-variable") {
        auto expanded = ExpandEnvironmentVariables("/filling/$HOME"s);
        auto trimmed = expanded.substr("/filling/"s.size());
        CATCH_CHECK(!trimmed.empty());
    }
}

CATCH_TEST_CASE("multiple in string ExpandEnvironmentVariables", "[SpecialFiles]") {
    CATCH_SECTION("dollar-curlybrace-variable-curlybrace") {
        auto expanded = ExpandEnvironmentVariables("${USER}/filling/${HOME}"s);
        auto trimmed = boost::replace_all_copy(expanded, "/filling/"s, ""s);
        CATCH_CHECK(!trimmed.empty());
    }
    CATCH_SECTION("dollar-variable") {
        auto expanded = ExpandEnvironmentVariables("$USER/filling/$HOME"s);
        auto trimmed = boost::replace_all_copy(expanded, "/filling/"s, ""s);
        CATCH_CHECK(!trimmed.empty());
    }
}

CATCH_TEST_CASE("no variables ExpandEnvironmentVariables", "[SpecialFiles]") {
    auto expanded = ExpandEnvironmentVariables("/a/b/data/dir.txt"s);
    CATCH_CHECK(expanded == "/a/b/data/dir.txt"s);
}

} // namespace mmotd::results::test

max_stars_repo_name: jasonivey/mmotd

max_issues_repo_name: jasonivey/mmotd

max_forks_repo_name: jasonivey/mmotd


text:
//<developer>
//    <name>linapex 曹一峰</name>
//    <email>linapex@163.com</email>
//    <wx>superexc</wx>
//    <qqgroup>128148617</qqgroup>
//    <url>https://jsq.ink</url>
//    <role>pku engineer</role>
//    <date>2019-03-16 19:44:15</date>
//</624457042765025280>

/*
 *@文件
 *@eos/license.txt中定义的版权
 **/

#pragma once

#include <boost/asio/thread_pool.hpp>
#include <boost/asio/post.hpp>
#include <future>
#include <memory>

namespace eosio { namespace chain {

//异步线程池并返回未来
   template<typename F>
   auto async_thread_pool( boost::asio::thread_pool& thread_pool, F&& f ) {
      auto task = std::make_shared<std::packaged_task<decltype( f() )()>>( std::forward<F>( f ) );
      boost::asio::post( thread_pool, [task]() { (*task)(); } );
      return task->get_future();
   }

} } //EOSIO：链

max_stars_repo_name: linapex/eos-chinese

max_issues_repo_name: linapex/eos-chinese

max_forks_repo_name: linapex/eos-chinese