<commit_before>#include <stdio.h>
#include <iostream>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"
#include "lcmtypes/bot_core.hpp"
#include <boost/shared_ptr.hpp>

#include <model-client/model-client.hpp>

#include "kdl/tree.hpp"
#include "kdl_parser/kdl_parser.hpp"
#include "forward_kinematics/treefksolverposfull_recursive.hpp"

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <ConciseArgs>

using namespace Eigen;
using namespace std;

class StatePub
{
public:
  StatePub(boost::shared_ptr<lcm::LCM> &_lcm, bool _ground_truth_mode);
  ~StatePub() {}
  boost::shared_ptr<lcm::LCM> _lcm;
  boost::shared_ptr<ModelClient> model_;

  void handleRobotStateMsg(const lcm::ReceiveBuffer* rbuf, const std::string& chan, const drc::robot_state_t * TRUE_state_msg);
  void handlePoseHeadMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);
  void triggerHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);

private:
  void outputNoSensing(const drc::robot_state_t * TRUE_state_msg,
        KDL::Frame  T_body_head  );
  void outputSensing(const drc::robot_state_t * TRUE_state_msg,
        KDL::Frame  T_head_body  );

  void sendPose(KDL::Frame pose, int64_t utime, std::string channel);
  void sendPose(drc::position_3d_t &origin, int64_t utime, std::string channel);

  boost::shared_ptr<KDL::TreeFkSolverPosFull_recursive> fksolver_;
  KDL::Frame  _T_world_head;
  bool _initPoseHead;
  bool _ground_truth_mode;

  /// used for Trigger based message demand:
  drc::robot_state_t _last_est_state;
  void sendTriggerOutput();
};

StatePub::StatePub(boost::shared_ptr<lcm::LCM> &_lcm, bool _ground_truth_mode):
    _lcm(_lcm),_initPoseHead(false), _ground_truth_mode(_ground_truth_mode){

  _T_world_head = KDL::Frame::Identity();
  // Get URDF string - this is instead of doing a URDF handler
  // These seems to need to be before the other subscribers otherwise
  // handleRobotStateMsg will be handled before we get the client
  model_ = boost::shared_ptr<ModelClient>(new ModelClient(_lcm->getUnderlyingLCM(), 0));

  _lcm->subscribe("TRUE_ROBOT_STATE", &StatePub::handleRobotStateMsg, this); //
  _lcm->subscribe("POSE_HEAD",&StatePub::handlePoseHeadMsg,this);
  _lcm->subscribe("TRIGGER",&StatePub::triggerHandler,this);

  // Parse KDL tree
  KDL::Tree tree;
  if (!kdl_parser::treeFromString(  model_->getURDFString() ,tree)){
    std::cerr << "ERROR: Failed to extract kdl tree from xml robot description" << std::endl;
    return;
  }
  fksolver_ = boost::shared_ptr<KDL::TreeFkSolverPosFull_recursive>(new KDL::TreeFkSolverPosFull_recursive(tree));

  _last_est_state.utime = 0; // used to indicate no message recieved yet
}

void StatePub::sendPose(drc::position_3d_t &origin, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime = utime;
  pose_msg.pos[0] = origin.translation.x;
  pose_msg.pos[1] = origin.translation.y;
  pose_msg.pos[2] = origin.translation.z;
  pose_msg.orientation[0] = origin.rotation.w;
  pose_msg.orientation[1] = origin.rotation.x;
  pose_msg.orientation[2] = origin.rotation.y;
  pose_msg.orientation[3] = origin.rotation.z;
  _lcm->publish(channel, &pose_msg);
}

void StatePub::sendPose(KDL::Frame pose, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime = utime;
  pose_msg.pos[0] = pose.p[0];
  pose_msg.pos[1] = pose.p[1];
  pose_msg.pos[2] = pose.p[2];
  pose.M.GetQuaternion(pose_msg.orientation[1], pose_msg.orientation[2],
                              pose_msg.orientation[3], pose_msg.orientation[0]);
  _lcm->publish(channel, &pose_msg);
}

void StatePub::outputNoSensing(const drc::robot_state_t * TRUE_state_msg,
  KDL::Frame  T_body_head){
  drc::robot_state_t msgout;
  msgout= *TRUE_state_msg;
  _lcm->publish("EST_ROBOT_STATE", &msgout);
  sendPose(msgout.origin_position, msgout.utime, "POSE_BODY");

  // Infer the Robot's head position from the ground truth root world pose
  KDL::Frame T_world_body, T_world_head;
  T_world_body.p[0]= msgout.origin_position.translation.x;
  T_world_body.p[1]= msgout.origin_position.translation.y;
  T_world_body.p[2]= msgout.origin_position.translation.z;
  T_world_body.M =  KDL::Rotation::Quaternion(msgout.origin_position.rotation.x, msgout.origin_position.rotation.y,
                                                msgout.origin_position.rotation.z, msgout.origin_position.rotation.w);
  T_world_head = T_world_body * T_body_head;
  sendPose(T_world_head, msgout.utime, "POSE_HEAD");

  // Keep this for the trigger:
  _last_est_state = msgout;
}

void StatePub::outputSensing(const drc::robot_state_t * TRUE_state_msg,
  KDL::Frame  T_head_body){
  if (!_initPoseHead){
    std::cout <<"Haven't received the first POSE_HEAD yet.\nRefusing to output EST_ROBOT_STATE ["<< TRUE_state_msg->utime <<"]\n";
    return;
  }

  // Determine world position of root link using head position and relative transform
  KDL::Frame  T_world_body;
  T_world_body  = _T_world_head*T_head_body;
  drc::position_3d_t body_origin;
  body_origin.translation.x = T_world_body.p[0];
  body_origin.translation.y = T_world_body.p[1];
  body_origin.translation.z = T_world_body.p[2];
  T_world_body.M.GetQuaternion(body_origin.rotation.x,body_origin.rotation.y,body_origin.rotation.z,body_origin.rotation.w);

  // EST is TRUE with sensor estimated position
  drc::robot_state_t msgout;
  msgout = *TRUE_state_msg;
  msgout.origin_position = body_origin;
  _lcm->publish("EST_ROBOT_STATE", &msgout);

  // Publish robot's root link position as a curtesy - prob no necessary:
  sendPose(body_origin, msgout.utime, "POSE_BODY");

  // Keep this for the trigger:
  _last_est_state = msgout;
}

void StatePub::handleRobotStateMsg(const lcm::ReceiveBuffer* rbuf,
  const std::string& chan, const drc::robot_state_t * TRUE_state_msg){

  /* This prints out the joint ordering - so they can be copied to drc_robot.cfg and kept in sync:
  cout << "[";
  for (size_t i=0; i < TRUE_state_msg->num_joints; i++){
    cout << "\"" << TRUE_state_msg->joint_name[i]  << "\"" ;
    if (i < TRUE_state_msg->num_joints - 1)
      cout << ", ";

    if ( (i+1)%7==0)
      cout << "\n";
  }
  cout << "];\n";
  */

  // call a routine that calculates the transforms the joint_state_t* TRUE_state_msg.
  map<string, double> jointpos_in;
  for (int i=0; i< TRUE_state_msg->num_joints; i++) //cast to uint to suppress compiler warning
    jointpos_in.insert(make_pair(TRUE_state_msg->joint_name[i], TRUE_state_msg->joint_position[i]));
  map<string, KDL::Frame > cartpos_out;

  // Calculate forward position kinematics
  bool kinematics_status;
  bool flatten_tree=true; // determines the absolute transforms with respect to robot origin.
  //Otherwise returns relative transforms between joints.

  kinematics_status =  fksolver_->JntToCart(jointpos_in,cartpos_out,flatten_tree);
  if(kinematics_status>=0){
    // cout << "Success!" <<endl;
  }else{
    cerr << "Error: could not calculate forward kinematics!" <<endl;
    return;
  }

  // PRINTS THE VISUAL PROPERTIES OF ALL LINKS THAT HAVE A VISUAL ELEMENT DEFINED IN THE URDF FILE
  map<string, KDL::Frame>::const_iterator transform_it;

  KDL::Frame  T_body_head;
  transform_it=cartpos_out.find("head");
  T_body_head = KDL::Frame::Identity();
  if(transform_it!=cartpos_out.end()){// fk cart pos exists
    T_body_head= transform_it->second;
  }else{
    std::cout<< "fk position does not exist" <<std::endl;
  }

  KDL::Frame T_head_body;
  T_head_body = T_body_head.Inverse();
  bot_core::rigid_transform_t tf;
  tf.utime = TRUE_state_msg->utime;
  tf.trans[0] = T_head_body.p[0];
  tf.trans[1] = T_head_body.p[1];
  tf.trans[2] = T_head_body.p[2];
  T_head_body.M.GetQuaternion(tf.quat[1], tf.quat[2], tf.quat[3], tf.quat[0]);
  _lcm->publish("HEAD_TO_BODY", &tf);

  if(_ground_truth_mode  ){ // formerly est_robot_state_no_sensing
    outputNoSensing(TRUE_state_msg, T_body_head);
  }else{
    outputSensing(TRUE_state_msg, T_head_body);
  }
}

void StatePub::handlePoseHeadMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg)  {
  _initPoseHead=true;
  _T_world_head.p[0]= msg->pos[0];
  _T_world_head.p[1]= msg->pos[1];
  _T_world_head.p[2]= msg->pos[2];
  _T_world_head.M =  KDL::Rotation::Quaternion(msg->orientation[1], msg->orientation[2], msg->orientation[3], msg->orientation[0]);
  // std::cout<< "head x,y,z in world frame:" <<_T_world_head.p[0] <<" , " <<_T_world_head.p[1] <<" , "<< _T_world_head.p[2] <<std::endl;
};

// This function can also be used to publish on
// a regular basis e.g. if set to publish at 1Hz
void StatePub::sendTriggerOutput(){

  if (_last_est_state.utime==0){     return;   } // if no msg recieved yet then ignore output command
  drc::minimal_robot_state_t msgout;
  msgout.utime = _last_est_state.utime;
  msgout.origin_position = _last_est_state.origin_position;
  msgout.num_joints = _last_est_state.num_joints;
  msgout.joint_position = _last_est_state.joint_position;
  _lcm->publish("EST_ROBOT_STATE_MINIMAL", &msgout);
  cout << "Sending Triggered Message\n";
}

void StatePub::triggerHandler(const lcm::ReceiveBuffer* rbuf,
                    const std::string& channel, const  bot_core::pose_t* msg){
  sendTriggerOutput();
}

int main (int argc, char ** argv){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool ground_truth_mode=false;
  parser.add(ground_truth_mode, "g", "ground_truth_mode", "Use Ground Truth");
  parser.parse();
  cout << "ground_truth_mode is: " << ground_truth_mode << "\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good())
    return 1;

  StatePub app(lcm, ground_truth_mode);
  cout << "StatePub ready"<< endl;
  while(0 == lcm->handle());
  return 0;
}
<commit_msg>integrating with data requests from matt<commit_after>#include <stdio.h>
#include <iostream>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"
#include "lcmtypes/bot_core.hpp"
#include <boost/shared_ptr.hpp>

#include <model-client/model-client.hpp>

#include "kdl/tree.hpp"
#include "kdl_parser/kdl_parser.hpp"
#include "forward_kinematics/treefksolverposfull_recursive.hpp"

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <ConciseArgs>

using namespace Eigen;
using namespace std;

class StatePub
{
public:
  StatePub(boost::shared_ptr<lcm::LCM> &_lcm, bool _ground_truth_mode);
  ~StatePub() {}
  boost::shared_ptr<lcm::LCM> _lcm;
  boost::shared_ptr<ModelClient> model_;

  void handleRobotStateMsg(const lcm::ReceiveBuffer* rbuf, const std::string& chan, const drc::robot_state_t * TRUE_state_msg);
  void handlePoseHeadMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);
  void triggerHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::data_request_t* msg);

private:
  void outputNoSensing(const drc::robot_state_t * TRUE_state_msg,
        KDL::Frame  T_body_head  );
  void outputSensing(const drc::robot_state_t * TRUE_state_msg,
        KDL::Frame  T_head_body  );

  void sendPose(KDL::Frame pose, int64_t utime, std::string channel);
  void sendPose(drc::position_3d_t &origin, int64_t utime, std::string channel);

  boost::shared_ptr<KDL::TreeFkSolverPosFull_recursive> fksolver_;
  KDL::Frame  _T_world_head;
  bool _initPoseHead;
  bool _ground_truth_mode;

  /// used for Trigger based message demand:
  drc::robot_state_t _last_est_state;
  void sendTriggerOutput();
};

StatePub::StatePub(boost::shared_ptr<lcm::LCM> &_lcm, bool _ground_truth_mode):
    _lcm(_lcm),_initPoseHead(false), _ground_truth_mode(_ground_truth_mode){

  _T_world_head = KDL::Frame::Identity();
  // Get URDF string - this is instead of doing a URDF handler
  // These seems to need to be before the other subscribers otherwise
  // handleRobotStateMsg will be handled before we get the client
  model_ = boost::shared_ptr<ModelClient>(new ModelClient(_lcm->getUnderlyingLCM(), 0));

  _lcm->subscribe("TRUE_ROBOT_STATE", &StatePub::handleRobotStateMsg, this); //
  _lcm->subscribe("POSE_HEAD",&StatePub::handlePoseHeadMsg,this);
  _lcm->subscribe("TRIGGER_STATE",&StatePub::triggerHandler,this);

  // Parse KDL tree
  KDL::Tree tree;
  if (!kdl_parser::treeFromString(  model_->getURDFString() ,tree)){
    std::cerr << "ERROR: Failed to extract kdl tree from xml robot description" << std::endl;
    return;
  }
  fksolver_ = boost::shared_ptr<KDL::TreeFkSolverPosFull_recursive>(new KDL::TreeFkSolverPosFull_recursive(tree));

  _last_est_state.utime = 0; // used to indicate no message recieved yet
}

void StatePub::sendPose(drc::position_3d_t &origin, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime = utime;
  pose_msg.pos[0] = origin.translation.x;
  pose_msg.pos[1] = origin.translation.y;
  pose_msg.pos[2] = origin.translation.z;
  pose_msg.orientation[0] = origin.rotation.w;
  pose_msg.orientation[1] = origin.rotation.x;
  pose_msg.orientation[2] = origin.rotation.y;
  pose_msg.orientation[3] = origin.rotation.z;
  _lcm->publish(channel, &pose_msg);
}

void StatePub::sendPose(KDL::Frame pose, int64_t utime, std::string channel){
  bot_core::pose_t pose_msg;
  pose_msg.utime = utime;
  pose_msg.pos[0] = pose.p[0];
  pose_msg.pos[1] = pose.p[1];
  pose_msg.pos[2] = pose.p[2];
  pose.M.GetQuaternion(pose_msg.orientation[1], pose_msg.orientation[2],
                              pose_msg.orientation[3], pose_msg.orientation[0]);
  _lcm->publish(channel, &pose_msg);
}

void StatePub::outputNoSensing(const drc::robot_state_t * TRUE_state_msg,
  KDL::Frame  T_body_head){
  drc::robot_state_t msgout;
  msgout= *TRUE_state_msg;
  _lcm->publish("EST_ROBOT_STATE", &msgout);
  sendPose(msgout.origin_position, msgout.utime, "POSE_BODY");

  // Infer the Robot's head position from the ground truth root world pose
  KDL::Frame T_world_body, T_world_head;
  T_world_body.p[0]= msgout.origin_position.translation.x;
  T_world_body.p[1]= msgout.origin_position.translation.y;
  T_world_body.p[2]= msgout.origin_position.translation.z;
  T_world_body.M =  KDL::Rotation::Quaternion(msgout.origin_position.rotation.x, msgout.origin_position.rotation.y,
                                                msgout.origin_position.rotation.z, msgout.origin_position.rotation.w);
  T_world_head = T_world_body * T_body_head;
  sendPose(T_world_head, msgout.utime, "POSE_HEAD");

  // Keep this for the trigger:
  _last_est_state = msgout;
}

void StatePub::outputSensing(const drc::robot_state_t * TRUE_state_msg,
  KDL::Frame  T_head_body){
  if (!_initPoseHead){
    std::cout <<"Haven't received the first POSE_HEAD yet.\nRefusing to output EST_ROBOT_STATE ["<< TRUE_state_msg->utime <<"]\n";
    return;
  }

  // Determine world position of root link using head position and relative transform
  KDL::Frame  T_world_body;
  T_world_body  = _T_world_head*T_head_body;
  drc::position_3d_t body_origin;
  body_origin.translation.x = T_world_body.p[0];
  body_origin.translation.y = T_world_body.p[1];
  body_origin.translation.z = T_world_body.p[2];
  T_world_body.M.GetQuaternion(body_origin.rotation.x,body_origin.rotation.y,body_origin.rotation.z,body_origin.rotation.w);

  // EST is TRUE with sensor estimated position
  drc::robot_state_t msgout;
  msgout = *TRUE_state_msg;
  msgout.origin_position = body_origin;
  _lcm->publish("EST_ROBOT_STATE", &msgout);

  // Publish robot's root link position as a curtesy - prob no necessary:
  sendPose(body_origin, msgout.utime, "POSE_BODY");

  // Keep this for the trigger:
  _last_est_state = msgout;
}

void StatePub::handleRobotStateMsg(const lcm::ReceiveBuffer* rbuf,
  const std::string& chan, const drc::robot_state_t * TRUE_state_msg){

  /* This prints out the joint ordering - so they can be copied to drc_robot.cfg and kept in sync:
  cout << "[";
  for (size_t i=0; i < TRUE_state_msg->num_joints; i++){
    cout << "\"" << TRUE_state_msg->joint_name[i]  << "\"" ;
    if (i < TRUE_state_msg->num_joints - 1)
      cout << ", ";

    if ( (i+1)%7==0)
      cout << "\n";
  }
  cout << "];\n";
  */

  // call a routine that calculates the transforms the joint_state_t* TRUE_state_msg.
  map<string, double> jointpos_in;
  for (int i=0; i< TRUE_state_msg->num_joints; i++) //cast to uint to suppress compiler warning
    jointpos_in.insert(make_pair(TRUE_state_msg->joint_name[i], TRUE_state_msg->joint_position[i]));
  map<string, KDL::Frame > cartpos_out;

  // Calculate forward position kinematics
  bool kinematics_status;
  bool flatten_tree=true; // determines the absolute transforms with respect to robot origin.
  //Otherwise returns relative transforms between joints.

  kinematics_status =  fksolver_->JntToCart(jointpos_in,cartpos_out,flatten_tree);
  if(kinematics_status>=0){
    // cout << "Success!" <<endl;
  }else{
    cerr << "Error: could not calculate forward kinematics!" <<endl;
    return;
  }

  // PRINTS THE VISUAL PROPERTIES OF ALL LINKS THAT HAVE A VISUAL ELEMENT DEFINED IN THE URDF FILE
  map<string, KDL::Frame>::const_iterator transform_it;

  KDL::Frame  T_body_head;
  transform_it=cartpos_out.find("head");
  T_body_head = KDL::Frame::Identity();
  if(transform_it!=cartpos_out.end()){// fk cart pos exists
    T_body_head= transform_it->second;
  }else{
    std::cout<< "fk position does not exist" <<std::endl;
  }

  KDL::Frame T_head_body;
  T_head_body = T_body_head.Inverse();
  bot_core::rigid_transform_t tf;
  tf.utime = TRUE_state_msg->utime;
  tf.trans[0] = T_head_body.p[0];
  tf.trans[1] = T_head_body.p[1];
  tf.trans[2] = T_head_body.p[2];
  T_head_body.M.GetQuaternion(tf.quat[1], tf.quat[2], tf.quat[3], tf.quat[0]);
  _lcm->publish("HEAD_TO_BODY", &tf);

  if(_ground_truth_mode  ){ // formerly est_robot_state_no_sensing
    outputNoSensing(TRUE_state_msg, T_body_head);
  }else{
    outputSensing(TRUE_state_msg, T_head_body);
  }
}

void StatePub::handlePoseHeadMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg)  {
  _initPoseHead=true;
  _T_world_head.p[0]= msg->pos[0];
  _T_world_head.p[1]= msg->pos[1];
  _T_world_head.p[2]= msg->pos[2];
  _T_world_head.M =  KDL::Rotation::Quaternion(msg->orientation[1], msg->orientation[2], msg->orientation[3], msg->orientation[0]);
  // std::cout<< "head x,y,z in world frame:" <<_T_world_head.p[0] <<" , " <<_T_world_head.p[1] <<" , "<< _T_world_head.p[2] <<std::endl;
};

// This function can also be used to publish on
// a regular basis e.g. if set to publish at 1Hz
void StatePub::sendTriggerOutput(){

  if (_last_est_state.utime==0){     return;   } // if no msg recieved yet then ignore output command
  drc::minimal_robot_state_t msgout;
  msgout.utime = _last_est_state.utime;
  msgout.origin_position = _last_est_state.origin_position;
  msgout.num_joints = _last_est_state.num_joints;
  msgout.joint_position = _last_est_state.joint_position;
  _lcm->publish("EST_ROBOT_STATE_MINIMAL", &msgout);
  cout << "Sending Triggered Message\n";
}

void StatePub::triggerHandler(const lcm::ReceiveBuffer* rbuf,
                    const std::string& channel, const  drc::data_request_t* msg){
  sendTriggerOutput();
}

int main (int argc, char ** argv){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool ground_truth_mode=false;
  parser.add(ground_truth_mode, "g", "ground_truth_mode", "Use Ground Truth");
  parser.parse();
  cout << "ground_truth_mode is: " << ground_truth_mode << "\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good())
    return 1;

  StatePub app(lcm, ground_truth_mode);
  cout << "StatePub ready"<< endl;
  while(0 == lcm->handle());
  return 0;
}
<|endoftext|>


<commit_before>#include "property_diversity_reranker.h"
#include "group_manager.h"

#include <mining-manager/group-label-logger/GroupLabelLogger.h>
#include <util/ustring/UString.h>
#include <util/ClockTimer.h>

#include <list>
#include <algorithm>
#include <glog/logging.h>

NS_FACETED_BEGIN

bool CompareSecond(const std::pair<size_t, count_t>& r1, const std::pair<size_t, count_t>& r2)
{
    return (r1.second > r2.second);
}

PropertyDiversityReranker::PropertyDiversityReranker(
    const GroupManager* groupManager,
    const std::string& diversityProperty,
    const std::string& boostingProperty
)
    :groupManager_(groupManager)
    ,diversityProperty_(diversityProperty)
    ,boostingProperty_(boostingProperty)
    ,groupLabelLogger_(NULL)
{
}

PropertyDiversityReranker::~PropertyDiversityReranker()
{
}

void PropertyDiversityReranker::rerank(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList,
    const std::string& query
)
{
    bool isBoostLabel = false;
    const PropValueTable* pvTable = NULL;
    PropValueTable::pvid_t labelId = 0;
    izenelib::util::ClockTimer timer;

    if(groupManager_ && groupLabelLogger_ && !boostingProperty_.empty())
    {
        std::vector<PropValueTable::pvid_t> pvIdVec;
        std::vector<int> freqVec;
        groupLabelLogger_->getFreqLabel(query, 1, pvIdVec, freqVec);

        if(!pvIdVec.empty() && (labelId = pvIdVec[0]))
        {
            pvTable = groupManager_->getPropValueTable(boostingProperty_);
            if (pvTable)
            {
                isBoostLabel = true;

                const izenelib::util::UString& labelUStr = pvTable->propValueStr(labelId);
                std::string boostLabel;
                labelUStr.convertString(boostLabel, izenelib::util::UString::UTF_8);

                LOG(INFO) << "boosting property: " << boostingProperty_
                        << ", label: " << boostLabel
                        << ", doc num: " << docIdList.size();
            }
            else
            {
                LOG(ERROR) << "in PropertyDiversityReranker: group index file is not loaded for property " << boostingProperty_;
            }
        }
        else
        {
            LOG(INFO) << "no boosting label for property " << boostingProperty_;
        }
    }

    if (isBoostLabel)
    {
        std::size_t numDoc = docIdList.size();

        std::vector<unsigned int> boostingDocIdList;
        std::vector<float> boostingScoreList;
        boostingDocIdList.reserve(numDoc);;
        boostingScoreList.reserve(numDoc);

        std::vector<unsigned int> leftDocIdList;
        std::vector<float> leftScoreList;
        leftDocIdList.reserve(numDoc);;
        leftScoreList.reserve(numDoc);

        for(std::size_t i = 0; i < numDoc; ++i)
        {
            docid_t docId = docIdList[i];

            if (pvTable->testDoc(docId, labelId))
            {
                boostingDocIdList.push_back(docId);
                boostingScoreList.push_back(rankScoreList[i]);
            }
            else
            {
                leftDocIdList.push_back(docId);
                leftScoreList.push_back(rankScoreList[i]);
            }
        }
        if(!boostingDocIdList.empty())
        {
            rerankImpl_(boostingDocIdList, boostingScoreList);
        }
        if(!leftDocIdList.empty())
        {
            rerankImpl_(leftDocIdList, leftScoreList);
        }
        boostingDocIdList.resize(numDoc);
        boostingScoreList.resize(numDoc);
        std::copy_backward(leftDocIdList.begin(), leftDocIdList.end(), boostingDocIdList.end());
        std::copy_backward(leftScoreList.begin(), leftScoreList.end(), boostingScoreList.end());
        std::swap(docIdList, boostingDocIdList);
        std::swap(rankScoreList, boostingScoreList);
    }
    else
    {
        rerankImpl_(docIdList, rankScoreList);
    }

    LOG(INFO) << "PropertyDiversityReranker::rerank() costs " << timer.elapsed() << " seconds";
}

void PropertyDiversityReranker::rerankImpl_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    rerankDiversity_(docIdList, rankScoreList);
    rerankCTR_(docIdList, rankScoreList);
}

void PropertyDiversityReranker::rerankDiversity_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    typedef std::list<std::pair<unsigned int,float> > DocIdList;
    typedef std::map<PropValueTable::pvid_t, DocIdList> DocIdMap;

    const PropValueTable* pvTable = NULL;
    if (groupManager_ && !diversityProperty_.empty())
    {
        pvTable = groupManager_->getPropValueTable(diversityProperty_);
        if (!pvTable)
        {
            LOG(ERROR) << "in PropertyDiversityReranker: group index file is not loaded for group property " << diversityProperty_;
            return;
        }
    }
    else
    {
        return;
    }

    LOG(INFO) << "diversity property: " << diversityProperty_
              << ", doc num: " << docIdList.size();

    const PropValueTable::ValueIdTable& idTable = pvTable->valueIdTable();
    std::size_t numDoc = docIdList.size();
    std::vector<unsigned int> newDocIdList;
    std::vector<float> newScoreList;
    newDocIdList.reserve(numDoc);
    newScoreList.reserve(numDoc);

    DocIdMap docIdMap;
    DocIdList missDocs;
    for (std::size_t i = 0; i < numDoc; ++i)
    {
        docid_t docId = docIdList[i];

        if (docId < idTable.size() && idTable[docId].empty() == false)
        {
            // use 1st group value
            PropValueTable::pvid_t pvId = idTable[docId][0];
            docIdMap[pvId].push_back(std::make_pair(docId, rankScoreList[i]));
        }
        else
        {
            missDocs.push_back(std::make_pair(docId, rankScoreList[i]));
        }
    }

    // single property or empty
    if(docIdMap.size() > 1)
        return;

    do{
        DocIdMap::iterator mapIt = docIdMap.begin();
        while(mapIt != docIdMap.end())
        {
            if(mapIt->second.empty())
            {
                docIdMap.erase(mapIt++);
            }
            else
            {
                std::pair<unsigned int, float> element = mapIt->second.front();
                mapIt->second.pop_front();
                newDocIdList.push_back(element.first);
                newScoreList.push_back(element.second);
                ++mapIt;
            }
        }
    }while(!docIdMap.empty());

    for(DocIdList::iterator missIt = missDocs.begin(); missIt != missDocs.end(); ++missIt)
    {
        newDocIdList.push_back(missIt->first);
        newScoreList.push_back(missIt->second);
    }

    std::swap(docIdList, newDocIdList);
    std::swap(rankScoreList, newScoreList);
}

void PropertyDiversityReranker::rerankCTR_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    std::vector<std::pair<size_t, count_t> > posClickCountList;
    bool hasClickCount = false;
    if (ctrManager_)
    {
        hasClickCount = ctrManager_->getClickCountListByDocIdList(docIdList, posClickCountList);
    }

    if (!hasClickCount)
        return;

    LOG(INFO) << "CTR rerank, doc num: " << docIdList.size();

    // sort by click-count
    std::sort(posClickCountList.begin(), posClickCountList.end(), CompareSecond);

    std::size_t numDoc = docIdList.size();
    std::vector<unsigned int> newDocIdList(numDoc);
    std::vector<float> newScoreList(numDoc);

    size_t i = 0;
    for (std::vector<std::pair<size_t, count_t> >::iterator it = posClickCountList.begin();
        it != posClickCountList.end(); ++it)
    {
        size_t pos = it->first;
        newDocIdList[i] = docIdList[pos];
        newScoreList[i] = rankScoreList[pos];
        ++i;
    }

    std::swap(docIdList, newDocIdList);
    std::swap(rankScoreList, newScoreList);
}

NS_FACETED_END

<commit_msg>in PropertyDiversityReranker::rerankCTR_(), use std::stable_sort() instead of std::sort() to preserve rank order if their click-counts are equal.<commit_after>#include "property_diversity_reranker.h"
#include "group_manager.h"

#include <mining-manager/group-label-logger/GroupLabelLogger.h>
#include <util/ustring/UString.h>
#include <util/ClockTimer.h>

#include <list>
#include <algorithm>
#include <glog/logging.h>

NS_FACETED_BEGIN

bool CompareSecond(const std::pair<size_t, count_t>& r1, const std::pair<size_t, count_t>& r2)
{
    return (r1.second > r2.second);
}

PropertyDiversityReranker::PropertyDiversityReranker(
    const GroupManager* groupManager,
    const std::string& diversityProperty,
    const std::string& boostingProperty
)
    :groupManager_(groupManager)
    ,diversityProperty_(diversityProperty)
    ,boostingProperty_(boostingProperty)
    ,groupLabelLogger_(NULL)
{
}

PropertyDiversityReranker::~PropertyDiversityReranker()
{
}

void PropertyDiversityReranker::rerank(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList,
    const std::string& query
)
{
    bool isBoostLabel = false;
    const PropValueTable* pvTable = NULL;
    PropValueTable::pvid_t labelId = 0;
    izenelib::util::ClockTimer timer;

    if(groupManager_ && groupLabelLogger_ && !boostingProperty_.empty())
    {
        std::vector<PropValueTable::pvid_t> pvIdVec;
        std::vector<int> freqVec;
        groupLabelLogger_->getFreqLabel(query, 1, pvIdVec, freqVec);

        if(!pvIdVec.empty() && (labelId = pvIdVec[0]))
        {
            pvTable = groupManager_->getPropValueTable(boostingProperty_);
            if (pvTable)
            {
                isBoostLabel = true;

                const izenelib::util::UString& labelUStr = pvTable->propValueStr(labelId);
                std::string boostLabel;
                labelUStr.convertString(boostLabel, izenelib::util::UString::UTF_8);

                LOG(INFO) << "boosting property: " << boostingProperty_
                        << ", label: " << boostLabel
                        << ", doc num: " << docIdList.size();
            }
            else
            {
                LOG(ERROR) << "in PropertyDiversityReranker: group index file is not loaded for property " << boostingProperty_;
            }
        }
        else
        {
            LOG(INFO) << "no boosting label for property " << boostingProperty_;
        }
    }

    if (isBoostLabel)
    {
        std::size_t numDoc = docIdList.size();

        std::vector<unsigned int> boostingDocIdList;
        std::vector<float> boostingScoreList;
        boostingDocIdList.reserve(numDoc);;
        boostingScoreList.reserve(numDoc);

        std::vector<unsigned int> leftDocIdList;
        std::vector<float> leftScoreList;
        leftDocIdList.reserve(numDoc);;
        leftScoreList.reserve(numDoc);

        for(std::size_t i = 0; i < numDoc; ++i)
        {
            docid_t docId = docIdList[i];

            if (pvTable->testDoc(docId, labelId))
            {
                boostingDocIdList.push_back(docId);
                boostingScoreList.push_back(rankScoreList[i]);
            }
            else
            {
                leftDocIdList.push_back(docId);
                leftScoreList.push_back(rankScoreList[i]);
            }
        }
        if(!boostingDocIdList.empty())
        {
            rerankImpl_(boostingDocIdList, boostingScoreList);
        }
        if(!leftDocIdList.empty())
        {
            rerankImpl_(leftDocIdList, leftScoreList);
        }
        boostingDocIdList.resize(numDoc);
        boostingScoreList.resize(numDoc);
        std::copy_backward(leftDocIdList.begin(), leftDocIdList.end(), boostingDocIdList.end());
        std::copy_backward(leftScoreList.begin(), leftScoreList.end(), boostingScoreList.end());
        std::swap(docIdList, boostingDocIdList);
        std::swap(rankScoreList, boostingScoreList);
    }
    else
    {
        rerankImpl_(docIdList, rankScoreList);
    }

    LOG(INFO) << "PropertyDiversityReranker::rerank() costs " << timer.elapsed() << " seconds";
}

void PropertyDiversityReranker::rerankImpl_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    rerankDiversity_(docIdList, rankScoreList);
    rerankCTR_(docIdList, rankScoreList);
}

void PropertyDiversityReranker::rerankDiversity_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    typedef std::list<std::pair<unsigned int,float> > DocIdList;
    typedef std::map<PropValueTable::pvid_t, DocIdList> DocIdMap;

    const PropValueTable* pvTable = NULL;
    if (groupManager_ && !diversityProperty_.empty())
    {
        pvTable = groupManager_->getPropValueTable(diversityProperty_);
        if (!pvTable)
        {
            LOG(ERROR) << "in PropertyDiversityReranker: group index file is not loaded for group property " << diversityProperty_;
            return;
        }
    }
    else
    {
        return;
    }

    LOG(INFO) << "diversity property: " << diversityProperty_
              << ", doc num: " << docIdList.size();

    const PropValueTable::ValueIdTable& idTable = pvTable->valueIdTable();
    std::size_t numDoc = docIdList.size();
    std::vector<unsigned int> newDocIdList;
    std::vector<float> newScoreList;
    newDocIdList.reserve(numDoc);
    newScoreList.reserve(numDoc);

    DocIdMap docIdMap;
    DocIdList missDocs;
    for (std::size_t i = 0; i < numDoc; ++i)
    {
        docid_t docId = docIdList[i];

        if (docId < idTable.size() && idTable[docId].empty() == false)
        {
            // use 1st group value
            PropValueTable::pvid_t pvId = idTable[docId][0];
            docIdMap[pvId].push_back(std::make_pair(docId, rankScoreList[i]));
        }
        else
        {
            missDocs.push_back(std::make_pair(docId, rankScoreList[i]));
        }
    }

    // single property or empty
    if(docIdMap.size() > 1)
        return;

    do{
        DocIdMap::iterator mapIt = docIdMap.begin();
        while(mapIt != docIdMap.end())
        {
            if(mapIt->second.empty())
            {
                docIdMap.erase(mapIt++);
            }
            else
            {
                std::pair<unsigned int, float> element = mapIt->second.front();
                mapIt->second.pop_front();
                newDocIdList.push_back(element.first);
                newScoreList.push_back(element.second);
                ++mapIt;
            }
        }
    }while(!docIdMap.empty());

    for(DocIdList::iterator missIt = missDocs.begin(); missIt != missDocs.end(); ++missIt)
    {
        newDocIdList.push_back(missIt->first);
        newScoreList.push_back(missIt->second);
    }

    std::swap(docIdList, newDocIdList);
    std::swap(rankScoreList, newScoreList);
}

void PropertyDiversityReranker::rerankCTR_(
    std::vector<unsigned int>& docIdList,
    std::vector<float>& rankScoreList
)
{
    std::vector<std::pair<size_t, count_t> > posClickCountList;
    bool hasClickCount = false;
    if (ctrManager_)
    {
        hasClickCount = ctrManager_->getClickCountListByDocIdList(docIdList, posClickCountList);
    }

    if (!hasClickCount)
        return;

    LOG(INFO) << "CTR rerank, doc num: " << docIdList.size();

    // sort by click-count
    std::stable_sort(posClickCountList.begin(), posClickCountList.end(), CompareSecond);

    std::size_t numDoc = docIdList.size();
    std::vector<unsigned int> newDocIdList(numDoc);
    std::vector<float> newScoreList(numDoc);

    size_t i = 0;
    for (std::vector<std::pair<size_t, count_t> >::iterator it = posClickCountList.begin();
        it != posClickCountList.end(); ++it)
    {
        size_t pos = it->first;
        newDocIdList[i] = docIdList[pos];
        newScoreList[i] = rankScoreList[pos];
        ++i;
    }

    std::swap(docIdList, newDocIdList);
    std::swap(rankScoreList, newScoreList);
}

NS_FACETED_END

<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
// Name:      vi.cpp
// Purpose:   Implementation of class wxExSTC vi mode
// Author:    Anton van Wezenbeek
// Created:   2009-11-21
// RCS-ID:    $Id$
// Copyright: (c) 2009 Anton van Wezenbeek
////////////////////////////////////////////////////////////////////////////////

#include <wx/textdlg.h>
#include <wx/tokenzr.h>
#include <wx/extension/vi.h>
#include <wx/extension/stc.h>

#if wxUSE_GUI

wxExVi::wxExVi(wxExSTC* stc)
  : m_STC(stc)
  , m_InsertMode(true)
{
  ResetInsertMode();
}

void wxExVi::Delete(
  const wxString& begin_address,
  const wxString& end_address)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Cut();
}

void wxExVi::LineEditor(const wxString& command)
{
  if (command.empty())
  {
    // Do nothing.
  }
  if (command == "$")
  {
    m_STC->DocumentEnd();
  }
  else if (command == ".=")
  {
    m_STC->CallTipShow(
      m_STC->GetCurrentPos(),
      wxString::Format("%d", m_STC->GetCurrentLine() + 1));
  }
  else if (command.IsNumber())
  {
    m_STC->GotoLine(atoi(command.c_str()) - 1);
  }
  else if (command.StartsWith("w"))
  {
    m_STC->FileSave();
  }
  else
  {
    // [address] m destination
    // [address] s [/pattern/replacement/] [options] [count]
    const wxString begin_address = command.BeforeFirst(',');
    wxString rest = command.AfterFirst(',');

    wxStringTokenizer tkz(rest, "dmsy");
    const wxString end_address = tkz.GetNextToken();
    const wxChar cmd = tkz.GetLastDelimiter();

    rest = tkz.GetString();

    switch (cmd)
    {
    case 'd':
      Delete(begin_address, end_address);
      break;
    case 'm':
      Move(begin_address, end_address, rest);
      break;
    case 's':
      {
      wxStringTokenizer tkz(rest, "/");

      tkz.GetNextToken(); // skip empty token
      const wxString pattern = tkz.GetNextToken();
      const wxString replacement = tkz.GetNextToken();

      Substitute(begin_address, end_address, pattern, replacement);
      }
      break;
    case 'y':
      Yank(begin_address, end_address);
      break;
    }
  }
}

void wxExVi::Move(
  const wxString& begin_address,
  const wxString& end_address,
  const wxString& destination)
{
  const int dest_line = ToLineNumber(destination);

  if (dest_line == 0)
  {
    return;
  }

  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Cut();
  m_STC->GotoLine(dest_line);
  m_STC->Paste();
}

void wxExVi::OnKey(wxKeyEvent& event)
{
  if(
    !event.ShiftDown() &&
    !event.ControlDown())
  {
    m_Command += event.GetUnicodeKey();
  }

  int repeat = atoi(m_Command.c_str());

  if (repeat == 0)
  {
    repeat++;
  }

  bool handled_command = true;

  // Handle multichar commands.
  if (m_Command.EndsWith("CW"))
  {
    for (int i = 0; i < repeat; i++) m_STC->WordRightExtend();
    SetInsertMode();
  }
  else if (m_Command.EndsWith("DD"))
  {
    const int line = m_STC->LineFromPosition(m_STC->GetCurrentPos());
    const int start = m_STC->PositionFromLine(line);
    const int end = m_STC->GetLineEndPosition(line + repeat - 1);
    m_STC->SetSelectionStart(start);
    m_STC->SetSelectionEnd(end);
    m_STC->Delete();
  }
  else if (m_Command.EndsWith("DW"))
  {
    for (int i = 0; i < repeat; i++) m_STC->DelWordRight();
  }
  else if (m_Command.Matches("*F?"))
  {
    for (int i = 0; i < repeat; i++) m_STC->FindNext(m_Command.Last());
  }
  else if (m_Command.EndsWith("YY"))
  {
    const int line = m_STC->LineFromPosition(m_STC->GetCurrentPos());
    const int start = m_STC->PositionFromLine(line);
    const int end = m_STC->GetLineEndPosition(line + repeat - 1);
    m_STC->CopyRange(start, end);
  }
  else
  {
    if (!event.ShiftDown() && !event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case '0':
          if (m_Command.length() == 1)
          {
            m_STC->Home();
          }
          else
          {
            handled_command = false;
          }
          break;
        case 'A': SetInsertMode(); m_STC->CharRight(); break;
        case 'B': for (int i = 0; i < repeat; i++) m_STC->WordLeft(); break;
        case 'G': m_STC->DocumentStart(); break;
        case 'H': for (int i = 0; i < repeat; i++) m_STC->CharLeft(); break;
        case 'I': SetInsertMode(); break;
        case 'J': for (int i = 0; i < repeat; i++) m_STC->LineDown(); break;
        case 'K': for (int i = 0; i < repeat; i++) m_STC->LineUp(); break;
        case 'L': for (int i = 0; i < repeat; i++) m_STC->CharRight(); break;
        case 'N':
          for (int i = 0; i < repeat; i++)
            m_STC->FindNext(m_STC->GetSearchText());
          break;
        case 'P':
          {
          const int pos = m_STC->GetCurrentPos();
          m_STC->LineDown();
          m_STC->Home();
          m_STC->Paste();
          m_STC->GotoPos(pos);
          }
          break;
        case 'W': for (int i = 0; i < repeat; i++) m_STC->WordRight(); break;
        case 'U': m_STC->Undo(); break;
        case 'X': m_STC->DeleteBack(); break;

        case '/':
          {
            wxTextEntryDialog dlg(
              m_STC,
              "/",
              "vi",
              m_STC->GetSearchText());

            if (dlg.ShowModal() == wxID_OK)
            {
              m_STC->FindNext(dlg.GetValue());
            }
          }
          break;

        // Repeat last text changing command.
        case '.':
          break;

        case '[':
        case ']':
          {
          const int brace_match = m_STC->BraceMatch(m_STC->GetCurrentPos());
          if (brace_match != wxSTC_INVALID_POSITION)
          {
            m_STC->GotoPos(brace_match);
          }
          }
          break;

        default:
          handled_command = false;
      }
    }
    else if (event.ShiftDown() && !event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case 'A': SetInsertMode(); m_STC->LineEnd(); break;
        case 'D': m_STC->DelLineRight(); break;
        case 'G':
          if (repeat > 1)
          {
            m_STC->GotoLine(repeat - 1);
          }
          else
          {
            m_STC->DocumentEnd();
          }
          break;
        case 'N':
          for (int i = 0; i < repeat; i++)
            m_STC->FindNext(m_STC->GetSearchText(), false);
          break;
        case 'P':
          {
          m_STC->LineUp();
          m_STC->Home();
          m_STC->Paste();
          }
          break;
        // Reverse case current char.
        case '1': // TODO: Should be ~, that does not work
          {
            wxString text(m_STC->GetTextRange(
              m_STC->GetCurrentPos(),
              m_STC->GetCurrentPos() + 1));

            wxIslower(text[0]) ? text.UpperCase(): text.LowerCase();

            m_STC->wxStyledTextCtrl::Replace(
              m_STC->GetCurrentPos(),
              m_STC->GetCurrentPos() + 1,
              text);

            m_STC->CharRight();
          }
          break;

        case '4': m_STC->LineEnd(); break; // $
        case '[': m_STC->ParaUp(); break; // {
        case ']': m_STC->ParaDown(); break; // }

        case ';': // :
          {
            wxTextEntryDialog dlg(m_STC, ":", "vi");

            if (dlg.ShowModal() == wxID_OK)
            {
              LineEditor(dlg.GetValue());
            }
          }
          break;

        case '/':
          {
            wxTextEntryDialog dlg(
              m_STC,
              "?",
              "vi",
              m_STC->GetSearchText());

            if (dlg.ShowModal() == wxID_OK)
            {
              m_STC->FindNext(dlg.GetValue(), false);
            }
          }
          break;

        default:
          handled_command = false;
      }
    }
    else if (event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case 'B': for (int i = 0; i < repeat; i++) m_STC->PageUp(); break;
        case 'E': for (int i = 0; i < repeat; i++) m_STC->LineScrollUp(); break;
        case 'F': for (int i = 0; i < repeat; i++) m_STC->PageDown(); break;
        case 'Y': for (int i = 0; i < repeat; i++) m_STC->LineScrollDown(); break;
        default:
          handled_command = false;
      }
    }
    else
    {
      wxFAIL;
    }
  }

  if (handled_command)
  {
    m_Command.clear();
  }
}

void wxExVi::ResetInsertMode()
{
  if (m_InsertMode)
  {
    m_InsertMode = false;
  }
}

void wxExVi::SetInsertMode()
{
  m_InsertMode = true;
}

bool wxExVi::SetSelection(
  const wxString& begin_address,
  const wxString& end_address)
{
  const int begin_line = ToLineNumber(begin_address);
  const int end_line = ToLineNumber(end_address);

  if (begin_line == 0 || end_line == 0)
  {
    return false;
  }

  m_STC->SetSelectionStart(m_STC->PositionFromLine(begin_line));
  m_STC->SetSelectionEnd(m_STC->GetLineEndPosition(end_line));

  return true;
}

void wxExVi::Substitute(
  const wxString& begin_address,
  const wxString& end_address,
  const wxString& pattern,
  const wxString& replacement)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->ReplaceAll(pattern, replacement, true);
}

int wxExVi::ToLineNumber(const wxString& address) const
{
  wxString filtered_address(address);
  int dot = 0;

  if (filtered_address.Contains("."))
  {
    dot = m_STC->GetCurrentLine();
    filtered_address.Replace(".", "");
    if (!filtered_address.IsNumber()) return 0;
  }

  return dot + atoi(filtered_address.c_str());
}

void wxExVi::Yank(
  const wxString& begin_address,
  const wxString& end_address)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Copy();
}

#endif // wxUSE_GUI
<commit_msg>fix for negative numbers<commit_after>////////////////////////////////////////////////////////////////////////////////
// Name:      vi.cpp
// Purpose:   Implementation of class wxExSTC vi mode
// Author:    Anton van Wezenbeek
// Created:   2009-11-21
// RCS-ID:    $Id$
// Copyright: (c) 2009 Anton van Wezenbeek
////////////////////////////////////////////////////////////////////////////////

#include <wx/textdlg.h>
#include <wx/tokenzr.h>
#include <wx/extension/vi.h>
#include <wx/extension/stc.h>

#if wxUSE_GUI

wxExVi::wxExVi(wxExSTC* stc)
  : m_STC(stc)
  , m_InsertMode(true)
{
  ResetInsertMode();
}

void wxExVi::Delete(
  const wxString& begin_address,
  const wxString& end_address)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Cut();
}

void wxExVi::LineEditor(const wxString& command)
{
  if (command.empty())
  {
    // Do nothing.
  }
  if (command == "$")
  {
    m_STC->DocumentEnd();
  }
  else if (command == ".=")
  {
    m_STC->CallTipShow(
      m_STC->GetCurrentPos(),
      wxString::Format("%d", m_STC->GetCurrentLine() + 1));
  }
  else if (command.IsNumber())
  {
    m_STC->GotoLine(atoi(command.c_str()) - 1);
  }
  else if (command.StartsWith("w"))
  {
    m_STC->FileSave();
  }
  else
  {
    // [address] m destination
    // [address] s [/pattern/replacement/] [options] [count]
    const wxString begin_address = command.BeforeFirst(',');
    wxString rest = command.AfterFirst(',');

    wxStringTokenizer tkz(rest, "dmsy");
    const wxString end_address = tkz.GetNextToken();
    const wxChar cmd = tkz.GetLastDelimiter();

    rest = tkz.GetString();

    switch (cmd)
    {
    case 'd':
      Delete(begin_address, end_address);
      break;
    case 'm':
      Move(begin_address, end_address, rest);
      break;
    case 's':
      {
      wxStringTokenizer tkz(rest, "/");

      tkz.GetNextToken(); // skip empty token
      const wxString pattern = tkz.GetNextToken();
      const wxString replacement = tkz.GetNextToken();

      Substitute(begin_address, end_address, pattern, replacement);
      }
      break;
    case 'y':
      Yank(begin_address, end_address);
      break;
    }
  }
}

void wxExVi::Move(
  const wxString& begin_address,
  const wxString& end_address,
  const wxString& destination)
{
  const int dest_line = ToLineNumber(destination);

  if (dest_line == 0)
  {
    return;
  }

  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Cut();
  m_STC->GotoLine(dest_line);
  m_STC->Paste();
}

void wxExVi::OnKey(wxKeyEvent& event)
{
  if(
    !event.ShiftDown() &&
    !event.ControlDown())
  {
    m_Command += event.GetUnicodeKey();
  }

  int repeat = atoi(m_Command.c_str());

  if (repeat == 0)
  {
    repeat++;
  }

  bool handled_command = true;

  // Handle multichar commands.
  if (m_Command.EndsWith("CW"))
  {
    for (int i = 0; i < repeat; i++) m_STC->WordRightExtend();
    SetInsertMode();
  }
  else if (m_Command.EndsWith("DD"))
  {
    const int line = m_STC->LineFromPosition(m_STC->GetCurrentPos());
    const int start = m_STC->PositionFromLine(line);
    const int end = m_STC->GetLineEndPosition(line + repeat - 1);
    m_STC->SetSelectionStart(start);
    m_STC->SetSelectionEnd(end);
    m_STC->Delete();
  }
  else if (m_Command.EndsWith("DW"))
  {
    for (int i = 0; i < repeat; i++) m_STC->DelWordRight();
  }
  else if (m_Command.Matches("*F?"))
  {
    for (int i = 0; i < repeat; i++) m_STC->FindNext(m_Command.Last());
  }
  else if (m_Command.EndsWith("YY"))
  {
    const int line = m_STC->LineFromPosition(m_STC->GetCurrentPos());
    const int start = m_STC->PositionFromLine(line);
    const int end = m_STC->GetLineEndPosition(line + repeat - 1);
    m_STC->CopyRange(start, end);
  }
  else
  {
    if (!event.ShiftDown() && !event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case '0':
          if (m_Command.length() == 1)
          {
            m_STC->Home();
          }
          else
          {
            handled_command = false;
          }
          break;
        case 'A': SetInsertMode(); m_STC->CharRight(); break;
        case 'B': for (int i = 0; i < repeat; i++) m_STC->WordLeft(); break;
        case 'G': m_STC->DocumentStart(); break;
        case 'H': for (int i = 0; i < repeat; i++) m_STC->CharLeft(); break;
        case 'I': SetInsertMode(); break;
        case 'J': for (int i = 0; i < repeat; i++) m_STC->LineDown(); break;
        case 'K': for (int i = 0; i < repeat; i++) m_STC->LineUp(); break;
        case 'L': for (int i = 0; i < repeat; i++) m_STC->CharRight(); break;
        case 'N':
          for (int i = 0; i < repeat; i++)
            m_STC->FindNext(m_STC->GetSearchText());
          break;
        case 'P':
          {
          const int pos = m_STC->GetCurrentPos();
          m_STC->LineDown();
          m_STC->Home();
          m_STC->Paste();
          m_STC->GotoPos(pos);
          }
          break;
        case 'W': for (int i = 0; i < repeat; i++) m_STC->WordRight(); break;
        case 'U': m_STC->Undo(); break;
        case 'X': m_STC->DeleteBack(); break;

        case '/':
          {
            wxTextEntryDialog dlg(
              m_STC,
              "/",
              "vi",
              m_STC->GetSearchText());

            if (dlg.ShowModal() == wxID_OK)
            {
              m_STC->FindNext(dlg.GetValue());
            }
          }
          break;

        // Repeat last text changing command.
        case '.':
          break;

        case '[':
        case ']':
          {
          const int brace_match = m_STC->BraceMatch(m_STC->GetCurrentPos());
          if (brace_match != wxSTC_INVALID_POSITION)
          {
            m_STC->GotoPos(brace_match);
          }
          }
          break;

        default:
          handled_command = false;
      }
    }
    else if (event.ShiftDown() && !event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case 'A': SetInsertMode(); m_STC->LineEnd(); break;
        case 'D': m_STC->DelLineRight(); break;
        case 'G':
          if (repeat > 1)
          {
            m_STC->GotoLine(repeat - 1);
          }
          else
          {
            m_STC->DocumentEnd();
          }
          break;
        case 'N':
          for (int i = 0; i < repeat; i++)
            m_STC->FindNext(m_STC->GetSearchText(), false);
          break;
        case 'P':
          {
          m_STC->LineUp();
          m_STC->Home();
          m_STC->Paste();
          }
          break;
        // Reverse case current char.
        case '1': // TODO: Should be ~, that does not work
          {
            wxString text(m_STC->GetTextRange(
              m_STC->GetCurrentPos(),
              m_STC->GetCurrentPos() + 1));

            wxIslower(text[0]) ? text.UpperCase(): text.LowerCase();

            m_STC->wxStyledTextCtrl::Replace(
              m_STC->GetCurrentPos(),
              m_STC->GetCurrentPos() + 1,
              text);

            m_STC->CharRight();
          }
          break;

        case '4': m_STC->LineEnd(); break; // $
        case '[': m_STC->ParaUp(); break; // {
        case ']': m_STC->ParaDown(); break; // }

        case ';': // :
          {
            wxTextEntryDialog dlg(m_STC, ":", "vi");

            if (dlg.ShowModal() == wxID_OK)
            {
              LineEditor(dlg.GetValue());
            }
          }
          break;

        case '/':
          {
            wxTextEntryDialog dlg(
              m_STC,
              "?",
              "vi",
              m_STC->GetSearchText());

            if (dlg.ShowModal() == wxID_OK)
            {
              m_STC->FindNext(dlg.GetValue(), false);
            }
          }
          break;

        default:
          handled_command = false;
      }
    }
    else if (event.ControlDown())
    {
      switch (event.GetKeyCode())
      {
        case 'B': for (int i = 0; i < repeat; i++) m_STC->PageUp(); break;
        case 'E': for (int i = 0; i < repeat; i++) m_STC->LineScrollUp(); break;
        case 'F': for (int i = 0; i < repeat; i++) m_STC->PageDown(); break;
        case 'Y': for (int i = 0; i < repeat; i++) m_STC->LineScrollDown(); break;
        default:
          handled_command = false;
      }
    }
    else
    {
      wxFAIL;
    }
  }

  if (handled_command)
  {
    m_Command.clear();
  }
}

void wxExVi::ResetInsertMode()
{
  if (m_InsertMode)
  {
    m_InsertMode = false;
  }
}

void wxExVi::SetInsertMode()
{
  m_InsertMode = true;
}

bool wxExVi::SetSelection(
  const wxString& begin_address,
  const wxString& end_address)
{
  const int begin_line = ToLineNumber(begin_address);
  const int end_line = ToLineNumber(end_address);

  if (begin_line == 0 || end_line == 0)
  {
    return false;
  }

  m_STC->SetSelectionStart(m_STC->PositionFromLine(begin_line));
  m_STC->SetSelectionEnd(m_STC->GetLineEndPosition(end_line));

  return true;
}

void wxExVi::Substitute(
  const wxString& begin_address,
  const wxString& end_address,
  const wxString& pattern,
  const wxString& replacement)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->ReplaceAll(pattern, replacement, true);
}

int wxExVi::ToLineNumber(const wxString& address) const
{
  wxString filtered_address(address);
  int dot = 0;

  if (filtered_address.Contains("."))
  {
    dot = m_STC->GetCurrentLine();
    filtered_address.Replace(".", "");
    if (!filtered_address.IsNumber()) return 0;
  }

  const int line_no = dot + atoi(filtered_address.c_str());

  if (line_no < 0)
  {
    return 0;
  }

  return line_no;
}

void wxExVi::Yank(
  const wxString& begin_address,
  const wxString& end_address)
{
  if (!SetSelection(begin_address, end_address))
  {
    return;
  }

  m_STC->Copy();
}

#endif // wxUSE_GUI
<|endoftext|>


<commit_before>//* This file is part of the MOOSE framework
//* https://www.mooseframework.org
//*
//* All rights reserved, see COPYRIGHT for full restrictions
//* https://github.com/idaholab/moose/blob/master/COPYRIGHT
//*
//* Licensed under LGPL 2.1, please see LICENSE for details
//* https://www.gnu.org/licenses/lgpl-2.1.html

#include "GlobalDisplacementAux.h"
#include "GlobalStrainUserObjectInterface.h"

// MOOSE includes
#include "Assembly.h"
#include "SystemBase.h"
#include "RankTwoTensor.h"

registerMooseObject("TensorMechanicsApp", GlobalDisplacementAux);

template <>
InputParameters
validParams<GlobalDisplacementAux>()
{
  InputParameters params = validParams<AuxKernel>();
  params.addClassDescription(
      "AuxKernel to visualize the displacements generated by the global strain tensor");
  params.addCoupledVar("scalar_global_strain",
                       "Scalar variable providing global strain components");
  params.addCoupledVar("displacements", "The name of the displacement variables");
  params.addRequiredParam<unsigned int>("component",
                                        "The displacement component to consider for this kernel");
  params.addParam<bool>(
      "output_global_displacement", false, "Option to output global displacement only");
  params.addRequiredParam<UserObjectName>("global_strain_uo",
                                          "The name of the GlobalStrainUserObject");

  return params;
}

GlobalDisplacementAux::GlobalDisplacementAux(const InputParameters & parameters)
  : AuxKernel(parameters),
    _scalar_global_strain(coupledScalarValue("scalar_global_strain")),
    _component(getParam<unsigned int>("component")),
    _output_global_disp(getParam<bool>("output_global_displacement")),
    _pst(getUserObject<GlobalStrainUserObjectInterface>("global_strain_uo")),
    _periodic_dir(_pst.getPeriodicDirections()),
    _dim(_mesh.dimension()),
    _ndisp(coupledComponents("displacements")),
    _disp(_ndisp)
{
  if (!isNodal())
    paramError("variable", "GlobalDisplacementAux must be used on a nodal auxiliary variable");

  if (_component >= _dim)
    paramError("component",
               "The component ",
               _component,
               " does not exist for ",
               _dim,
               " dimensional problems");

  for (unsigned int i = 0; i < _ndisp; ++i)
    _disp[i] = &coupledValue("displacements", i);
}

Real
GlobalDisplacementAux::computeValue()
{
  RankTwoTensor strain;
  strain.fillFromScalarVariable(_scalar_global_strain);

  for (unsigned int dir = 0; dir < _dim; ++dir)
    if (!_periodic_dir(dir))
      for (unsigned int var = 0; var < _ndisp; ++var)
        strain(dir, var) = 0.0;

  const RealVectorValue & global_disp = strain * (*_current_node);

  if (_output_global_disp)
    return global_disp(_component);
  else
    return global_disp(_component) + (*_disp[_component])[_qp];
}
<commit_msg>Default GlobalDisplacementAux to PRE_DISPLACE (#12374)<commit_after>//* This file is part of the MOOSE framework
//* https://www.mooseframework.org
//*
//* All rights reserved, see COPYRIGHT for full restrictions
//* https://github.com/idaholab/moose/blob/master/COPYRIGHT
//*
//* Licensed under LGPL 2.1, please see LICENSE for details
//* https://www.gnu.org/licenses/lgpl-2.1.html

#include "GlobalDisplacementAux.h"
#include "GlobalStrainUserObjectInterface.h"

// MOOSE includes
#include "Assembly.h"
#include "SystemBase.h"
#include "RankTwoTensor.h"

registerMooseObject("TensorMechanicsApp", GlobalDisplacementAux);

template <>
InputParameters
validParams<GlobalDisplacementAux>()
{
  InputParameters params = validParams<AuxKernel>();
  params.addClassDescription(
      "AuxKernel to visualize the displacements generated by the global strain tensor");
  params.addCoupledVar("scalar_global_strain",
                       "Scalar variable providing global strain components");
  params.addCoupledVar("displacements", "The name of the displacement variables");
  params.addRequiredParam<unsigned int>("component",
                                        "The displacement component to consider for this kernel");
  params.addParam<bool>(
      "output_global_displacement", false, "Option to output global displacement only");
  params.addRequiredParam<UserObjectName>("global_strain_uo",
                                          "The name of the GlobalStrainUserObject");

  // Default this object to get executed before the displaced mesh update.
  // This way the AuxVars set by this object can be used as mesh displacements.
  params.set<ExecFlagEnum>("execute_on") = EXEC_PRE_DISPLACE;

  return params;
}

GlobalDisplacementAux::GlobalDisplacementAux(const InputParameters & parameters)
  : AuxKernel(parameters),
    _scalar_global_strain(coupledScalarValue("scalar_global_strain")),
    _component(getParam<unsigned int>("component")),
    _output_global_disp(getParam<bool>("output_global_displacement")),
    _pst(getUserObject<GlobalStrainUserObjectInterface>("global_strain_uo")),
    _periodic_dir(_pst.getPeriodicDirections()),
    _dim(_mesh.dimension()),
    _ndisp(coupledComponents("displacements")),
    _disp(_ndisp)
{
  if (!isNodal())
    paramError("variable", "GlobalDisplacementAux must be used on a nodal auxiliary variable");

  if (_component >= _dim)
    paramError("component",
               "The component ",
               _component,
               " does not exist for ",
               _dim,
               " dimensional problems");

  for (unsigned int i = 0; i < _ndisp; ++i)
    _disp[i] = &coupledValue("displacements", i);
}

Real
GlobalDisplacementAux::computeValue()
{
  RankTwoTensor strain;
  strain.fillFromScalarVariable(_scalar_global_strain);

  for (unsigned int dir = 0; dir < _dim; ++dir)
    if (!_periodic_dir(dir))
      for (unsigned int var = 0; var < _ndisp; ++var)
        strain(dir, var) = 0.0;

  const RealVectorValue & global_disp = strain * (*_current_node);

  if (_output_global_disp)
    return global_disp(_component);
  else
    return global_disp(_component) + (*_disp[_component])[_qp];
}
<|endoftext|>


<commit_before>/*********************************************************************
 *
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, Christian Rose
 *                      Team Hector,
 *                      Technische Universität Darmstadt
 *  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 Technische Universität Darmstadt nor the names of
 *     its contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/

#include <vigir_atlas_sim_ros_control/atlas_sim_ros_control.h>

#include <controller_manager/controller_manager.h>
#include <std_msgs/String.h>
#include <atlas_msgs/AtlasState.h>

namespace Atlas_Sim_Ros_Control
{

  void Atlas_Sim_Ros_Control::atlasStateCb(const atlas_msgs::AtlasState::ConstPtr &_as)
  {
    // We don´t know how our joint state looks like till we received the first instance of it, so we initialize the hardware interface here
    if (joint_state_.position.size() == 0){
      atlas_state_ = *_as;

      for(size_t i = 0; i < atlas_state_.position.size(); ++i){
        joint_state_.position.push_back(atlas_state_.position[i]);
        joint_state_.velocity.push_back(atlas_state_.velocity[i]);
        joint_state_.effort.push_back(atlas_state_.effort[i]);
        joint_state_.name.push_back(joint_names_[i]);
      }

      for (size_t i = 0; i < joint_state_.position.size(); ++i){
        hardware_interface::JointStateHandle state_handle(joint_names_[i], &joint_state_.position[i], &joint_state_.velocity[i], &joint_state_.velocity[i]);
        joint_state_interface_.registerHandle(state_handle);

        hardware_interface::JointHandle pos_handle(joint_state_interface_.getHandle(joint_names_[i]), &jointcommands.position[i]);
        position_joint_interface_.registerHandle(pos_handle);

        jointcommands.position[i] = joint_state_.position[i];

      }

      registerInterface(&joint_state_interface_);
      registerInterface(&position_joint_interface_);
    }else{

      atlas_state_ = *_as;

      for(size_t i = 0; i < atlas_state_.position.size(); ++i){
        joint_state_.position[i] = (atlas_state_.position[i]);
        joint_state_.velocity[i] = (atlas_state_.velocity[i]);
        joint_state_.effort[i] = (atlas_state_.effort[i]);
      }

    }

  }

Atlas_Sim_Ros_Control::Atlas_Sim_Ros_Control()
{
  ros::NodeHandle* rosnode = new ros::NodeHandle();

  rosnode->setCallbackQueue(&subscriber_queue_);

  ros::Time last_ros_time_;
  bool wait = true;
  while (wait)
  {
    last_ros_time_ = ros::Time::now();
    if (last_ros_time_.toSec() > 0)
      wait = false;
  }

  // must match those inside AtlasPlugin
  jointcommands.name.push_back("atlas::back_bkx");
  jointcommands.name.push_back("atlas::back_bky");
  jointcommands.name.push_back("atlas::back_bkz");
  jointcommands.name.push_back("atlas::neck_ry");
  jointcommands.name.push_back("atlas::l_leg_hpz");
  jointcommands.name.push_back("atlas::l_leg_hpx");
  jointcommands.name.push_back("atlas::l_leg_hpy");
  jointcommands.name.push_back("atlas::l_leg_kny");
  jointcommands.name.push_back("atlas::l_leg_aky");
  jointcommands.name.push_back("atlas::l_leg_akx");
  jointcommands.name.push_back("atlas::r_leg_hpz");
  jointcommands.name.push_back("atlas::r_leg_hpx");
  jointcommands.name.push_back("atlas::r_leg_hpy");
  jointcommands.name.push_back("atlas::r_leg_kny");
  jointcommands.name.push_back("atlas::r_leg_aky");
  jointcommands.name.push_back("atlas::r_leg_akx");
  jointcommands.name.push_back("atlas::l_arm_shy");
  jointcommands.name.push_back("atlas::l_arm_shx");
  jointcommands.name.push_back("atlas::l_arm_ely");
  jointcommands.name.push_back("atlas::l_arm_elx");
  jointcommands.name.push_back("atlas::l_arm_wry");
  jointcommands.name.push_back("atlas::l_arm_wrx");
  jointcommands.name.push_back("atlas::r_arm_shy");
  jointcommands.name.push_back("atlas::r_arm_shx");
  jointcommands.name.push_back("atlas::r_arm_ely");
  jointcommands.name.push_back("atlas::r_arm_elx");
  jointcommands.name.push_back("atlas::r_arm_wry");
  jointcommands.name.push_back("atlas::r_arm_wrx");

  unsigned int n = jointcommands.name.size();
  jointcommands.position.resize(n);
  jointcommands.velocity.resize(n);
  jointcommands.effort.resize(n);
  jointcommands.kp_position.resize(n);
  jointcommands.ki_position.resize(n);
  jointcommands.kd_position.resize(n);
  jointcommands.kp_velocity.resize(n);
  jointcommands.i_effort_min.resize(n);
  jointcommands.i_effort_max.resize(n);

  for (unsigned int i = 0; i < n; i++)
  {
    std::vector<std::string> pieces;
    boost::split(pieces, jointcommands.name[i], boost::is_any_of(":"));

    std::string param_string = "/atlas_controller/gains/" + pieces[2] + "/p";
    //std::cout << param_string << "\n";
    joint_names_.push_back(pieces[2]);

    rosnode->getParam(param_string,
      jointcommands.kp_position[i]);
    //jointcommands.kp_position[i] = 1000.0;

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i",
      jointcommands.ki_position[i]);

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/d",
      jointcommands.kd_position[i]);

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i_clamp",
      jointcommands.i_effort_min[i]);
    jointcommands.i_effort_min[i] = -jointcommands.i_effort_min[i];

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i_clamp",
      jointcommands.i_effort_max[i]);

    jointcommands.velocity[i]     = 0;
    jointcommands.effort[i]       = 0;
    jointcommands.kp_velocity[i]  = 0;
  }

  // ros topic subscribtions
  ros::SubscribeOptions jointStatesSo =
    ros::SubscribeOptions::create<atlas_msgs::AtlasState>(
    "/atlas/atlas_state", 1,boost::bind(&Atlas_Sim_Ros_Control::atlasStateCb, this, _1),
    ros::VoidPtr(), rosnode->getCallbackQueue());

  // Because TCP causes bursty communication with high jitter,
  // declare a preference on UDP connections for receiving
  // joint states, which we want to get at a high rate.
  // Note that we'll still accept TCP connections for this topic
  // (e.g., from rospy nodes, which don't support UDP);
  // we just prefer UDP.
  jointStatesSo.transport_hints = ros::TransportHints().unreliable();

  subJointStates_ = rosnode->subscribe(jointStatesSo);
  // ros::Subscriber subJointStates =
  //   rosnode->subscribe("/atlas/joint_states", 1000, SetJointStates);

  pub_joint_commands_ =
    rosnode->advertise<osrf_msgs::JointCommands>(
    "/atlas/joint_commands", 1, true);

  subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
  subscriber_spinner_->start();

}

void Atlas_Sim_Ros_Control::cleanup()
{
    subscriber_spinner_->stop();
}

void Atlas_Sim_Ros_Control::read(ros::Time time, ros::Duration period)
{
  //Normally we would read joint angles from hardware here, but we get those via ROS topic as we use Gazebo
}

void Atlas_Sim_Ros_Control::write(ros::Time time, ros::Duration period)
{
  jointcommands.header.stamp = time;
  pub_joint_commands_.publish(jointcommands);
}

}

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

try{
        ROS_INFO("starting");
        ros::init(argc, argv, "atlas_sim_ros_control");

        Atlas_Sim_Ros_Control::Atlas_Sim_Ros_Control atlas_sim_ros_control_interface;

        sleep(2);

        // Publish Atlas user mode
        ros::NodeHandle nh;

        ros::Publisher pub_user_mode_ = nh.advertise<std_msgs::String>("/atlas/control_mode",1,true);
        std_msgs::String msg;
        std::stringstream ss;
        ss << "User";
        msg.data = ss.str();
        pub_user_mode_.publish(msg);

        controller_manager::ControllerManager cm(&atlas_sim_ros_control_interface);

        ros::AsyncSpinner spinner(4);
        spinner.start();

        ros::Rate loop_rate(50);

        ros::Time last_time = ros::Time::now();

        while (ros::ok())
                {
                    //ROS_INFO("in main loop");
                    loop_rate.sleep();
                    ros::spinOnce();

                    ros::Time current_time = ros::Time::now();
                    ros::Duration elapsed_time = current_time - last_time;
                    last_time = current_time;

                    //ROS_INFO("before read");
                    atlas_sim_ros_control_interface.read(current_time, elapsed_time);
                    //ROS_INFO("after read");

                    //ROS_INFO("before cm.update");
                    cm.update(current_time, elapsed_time);
                    //ROS_INFO("after cm.update");

                    //ROS_INFO("before write");
                    atlas_sim_ros_control_interface.write(current_time, elapsed_time);
                    //ROS_INFO("after write");
                }

        atlas_sim_ros_control_interface.cleanup();
    }
    catch(...)
    {
        ROS_ERROR("Unhandled exception!");
        return -1;
    }

    return 0;
}
<commit_msg>-Increase rate (from 50Hz to 1000Hz) -Switch to receiving AtlasState<commit_after>/*********************************************************************
 *
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, Christian Rose
 *                      Team Hector,
 *                      Technische Universität Darmstadt
 *  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 Technische Universität Darmstadt nor the names of
 *     its contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/

#include <vigir_atlas_sim_ros_control/atlas_sim_ros_control.h>

#include <controller_manager/controller_manager.h>
#include <std_msgs/String.h>
#include <atlas_msgs/AtlasState.h>

namespace Atlas_Sim_Ros_Control
{

  void Atlas_Sim_Ros_Control::atlasStateCb(const atlas_msgs::AtlasState::ConstPtr &_as)
  {
    // We don´t know how our joint state looks like till we received the first instance of it, so we initialize the hardware interface here
    if (joint_state_.position.size() == 0){
      atlas_state_ = *_as;

      for(size_t i = 0; i < atlas_state_.position.size(); ++i){
        joint_state_.position.push_back(atlas_state_.position[i]);
        joint_state_.velocity.push_back(atlas_state_.velocity[i]);
        joint_state_.effort.push_back(atlas_state_.effort[i]);
        joint_state_.name.push_back(joint_names_[i]);
      }

      for (size_t i = 0; i < joint_state_.position.size(); ++i){
        hardware_interface::JointStateHandle state_handle(joint_names_[i], &joint_state_.position[i], &joint_state_.velocity[i], &joint_state_.velocity[i]);
        joint_state_interface_.registerHandle(state_handle);

        hardware_interface::JointHandle pos_handle(joint_state_interface_.getHandle(joint_names_[i]), &jointcommands.position[i]);
        position_joint_interface_.registerHandle(pos_handle);

        jointcommands.position[i] = joint_state_.position[i];

      }

      registerInterface(&joint_state_interface_);
      registerInterface(&position_joint_interface_);
    }else{

      atlas_state_ = *_as;

      for(size_t i = 0; i < atlas_state_.position.size(); ++i){
        joint_state_.position[i] = (atlas_state_.position[i]);
        joint_state_.velocity[i] = (atlas_state_.velocity[i]);
        joint_state_.effort[i] = (atlas_state_.effort[i]);
      }

    }

  }

Atlas_Sim_Ros_Control::Atlas_Sim_Ros_Control()
{
  ros::NodeHandle* rosnode = new ros::NodeHandle();

  rosnode->setCallbackQueue(&subscriber_queue_);

  ros::Time last_ros_time_;
  bool wait = true;
  while (wait)
  {
    last_ros_time_ = ros::Time::now();
    if (last_ros_time_.toSec() > 0)
      wait = false;
  }

  // must match those inside AtlasPlugin
  jointcommands.name.push_back("atlas::back_bkx");
  jointcommands.name.push_back("atlas::back_bky");
  jointcommands.name.push_back("atlas::back_bkz");
  jointcommands.name.push_back("atlas::neck_ry");
  jointcommands.name.push_back("atlas::l_leg_hpz");
  jointcommands.name.push_back("atlas::l_leg_hpx");
  jointcommands.name.push_back("atlas::l_leg_hpy");
  jointcommands.name.push_back("atlas::l_leg_kny");
  jointcommands.name.push_back("atlas::l_leg_aky");
  jointcommands.name.push_back("atlas::l_leg_akx");
  jointcommands.name.push_back("atlas::r_leg_hpz");
  jointcommands.name.push_back("atlas::r_leg_hpx");
  jointcommands.name.push_back("atlas::r_leg_hpy");
  jointcommands.name.push_back("atlas::r_leg_kny");
  jointcommands.name.push_back("atlas::r_leg_aky");
  jointcommands.name.push_back("atlas::r_leg_akx");
  jointcommands.name.push_back("atlas::l_arm_shy");
  jointcommands.name.push_back("atlas::l_arm_shx");
  jointcommands.name.push_back("atlas::l_arm_ely");
  jointcommands.name.push_back("atlas::l_arm_elx");
  jointcommands.name.push_back("atlas::l_arm_wry");
  jointcommands.name.push_back("atlas::l_arm_wrx");
  jointcommands.name.push_back("atlas::r_arm_shy");
  jointcommands.name.push_back("atlas::r_arm_shx");
  jointcommands.name.push_back("atlas::r_arm_ely");
  jointcommands.name.push_back("atlas::r_arm_elx");
  jointcommands.name.push_back("atlas::r_arm_wry");
  jointcommands.name.push_back("atlas::r_arm_wrx");

  unsigned int n = jointcommands.name.size();
  jointcommands.position.resize(n);
  jointcommands.velocity.resize(n);
  jointcommands.effort.resize(n);
  jointcommands.kp_position.resize(n);
  jointcommands.ki_position.resize(n);
  jointcommands.kd_position.resize(n);
  jointcommands.kp_velocity.resize(n);
  jointcommands.i_effort_min.resize(n);
  jointcommands.i_effort_max.resize(n);

  for (unsigned int i = 0; i < n; i++)
  {
    std::vector<std::string> pieces;
    boost::split(pieces, jointcommands.name[i], boost::is_any_of(":"));

    std::string param_string = "/atlas_controller/gains/" + pieces[2] + "/p";
    //std::cout << param_string << "\n";
    joint_names_.push_back(pieces[2]);

    rosnode->getParam(param_string,
      jointcommands.kp_position[i]);
    //jointcommands.kp_position[i] = 1000.0;

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i",
      jointcommands.ki_position[i]);

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/d",
      jointcommands.kd_position[i]);

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i_clamp",
      jointcommands.i_effort_min[i]);
    jointcommands.i_effort_min[i] = -jointcommands.i_effort_min[i];

    rosnode->getParam("/atlas_controller/gains/" + pieces[2] + "/i_clamp",
      jointcommands.i_effort_max[i]);

    jointcommands.velocity[i]     = 0;
    jointcommands.effort[i]       = 0;
    jointcommands.kp_velocity[i]  = 0;
  }

  // ros topic subscribtions
  ros::SubscribeOptions jointStatesSo =
    ros::SubscribeOptions::create<atlas_msgs::AtlasState>(
    "/atlas/atlas_state", 1,boost::bind(&Atlas_Sim_Ros_Control::atlasStateCb, this, _1),
    ros::VoidPtr(), rosnode->getCallbackQueue());

  // Because TCP causes bursty communication with high jitter,
  // declare a preference on UDP connections for receiving
  // joint states, which we want to get at a high rate.
  // Note that we'll still accept TCP connections for this topic
  // (e.g., from rospy nodes, which don't support UDP);
  // we just prefer UDP.
  jointStatesSo.transport_hints = ros::TransportHints().unreliable();

  subJointStates_ = rosnode->subscribe(jointStatesSo);
  // ros::Subscriber subJointStates =
  //   rosnode->subscribe("/atlas/joint_states", 1000, SetJointStates);

  pub_joint_commands_ =
    rosnode->advertise<osrf_msgs::JointCommands>(
    "/atlas/joint_commands", 1, true);

  subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
  subscriber_spinner_->start();

}

void Atlas_Sim_Ros_Control::cleanup()
{
    subscriber_spinner_->stop();
}

void Atlas_Sim_Ros_Control::read(ros::Time time, ros::Duration period)
{
  //Normally we would read joint angles from hardware here, but we get those via ROS topic as we use Gazebo
}

void Atlas_Sim_Ros_Control::write(ros::Time time, ros::Duration period)
{
  jointcommands.header.stamp = time;
  pub_joint_commands_.publish(jointcommands);
}

}

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

try{
        ROS_INFO("starting");
        ros::init(argc, argv, "atlas_sim_ros_control");

        Atlas_Sim_Ros_Control::Atlas_Sim_Ros_Control atlas_sim_ros_control_interface;

        sleep(2);

        // Publish Atlas user mode
        ros::NodeHandle nh;

        ros::Publisher pub_user_mode_ = nh.advertise<std_msgs::String>("/atlas/control_mode",1,true);
        std_msgs::String msg;
        std::stringstream ss;
        ss << "User";
        msg.data = ss.str();
        pub_user_mode_.publish(msg);

        controller_manager::ControllerManager cm(&atlas_sim_ros_control_interface);

        ros::AsyncSpinner spinner(2);
        spinner.start();

        ros::Rate loop_rate(1000);

        ros::Time last_time = ros::Time::now();

        while (ros::ok())
                {
                    //ROS_INFO("in main loop");
                    loop_rate.sleep();
                    //ros::spinOnce();

                    ros::Time current_time = ros::Time::now();
                    ros::Duration elapsed_time = current_time - last_time;
                    last_time = current_time;

                    //ROS_INFO("before read");
                    atlas_sim_ros_control_interface.read(current_time, elapsed_time);
                    //ROS_INFO("after read");

                    //ROS_INFO("before cm.update");
                    cm.update(current_time, elapsed_time);
                    //ROS_INFO("after cm.update");

                    //ROS_INFO("before write");
                    atlas_sim_ros_control_interface.write(current_time, elapsed_time);
                    //ROS_INFO("after write");
                }

        atlas_sim_ros_control_interface.cleanup();
    }
    catch(...)
    {
        ROS_ERROR("Unhandled exception!");
        return -1;
    }

    return 0;
}
<|endoftext|>


<commit_before>#include "core/cint.h"

#include <limits>

#include "core/assert.h"

namespace euphoria::core
{
    int
    c_sizet_to_int(size_t t)
    {
        ASSERTX(t <= static_cast<size_t>(std::numeric_limits<int>::max()), t);
        return static_cast<int>(t);
    }

    int
    c_unsigned_int_to_int(unsigned int i)
    {
        ASSERTX(i <= static_cast<unsigned int>(std::numeric_limits<int>::max()), i);
        return static_cast<int>(i);
    }

    unsigned int
    c_int_to_unsigned_int(int i)
    {
        ASSERTX(i >= 0, i);
        return static_cast<unsigned int>(i);
    }

    size_t
    c_int_to_sizet(int i)
    {
        ASSERTX(i >= 0, i);
        return static_cast<size_t>(i);
    }

    u8
    c_int_to_u8(unsigned int i)
    {
        ASSERTX(i >= 0, i);
        ASSERTX(i <255, i);
        return static_cast<u8>(i);
    }

    int
    c_u64_to_int(u64 u)
    {
        ASSERTX(u > static_cast<u64>(std::numeric_limits<int>::max()), u);
        return static_cast<int>(u);
    }
}

<commit_msg>fixed tests<commit_after>#include "core/cint.h"

#include <limits>

#include "core/assert.h"

namespace euphoria::core
{
    int
    c_sizet_to_int(size_t t)
    {
        ASSERTX(t <= static_cast<size_t>(std::numeric_limits<int>::max()), t);
        return static_cast<int>(t);
    }

    int
    c_unsigned_int_to_int(unsigned int i)
    {
        ASSERTX(i <= static_cast<unsigned int>(std::numeric_limits<int>::max()), i);
        return static_cast<int>(i);
    }

    unsigned int
    c_int_to_unsigned_int(int i)
    {
        ASSERTX(i >= 0, i);
        return static_cast<unsigned int>(i);
    }

    size_t
    c_int_to_sizet(int i)
    {
        ASSERTX(i >= 0, i);
        return static_cast<size_t>(i);
    }

    u8
    c_int_to_u8(unsigned int i)
    {
        ASSERTX(i >= 0, i);
        ASSERTX(i <255, i);
        return static_cast<u8>(i);
    }

    int
    c_u64_to_int(u64 u)
    {
        ASSERTX(u < static_cast<u64>(std::numeric_limits<int>::max()), u);
        return static_cast<int>(u);
    }
}

<|endoftext|>


<commit_before>/* -*- coding: utf-8; mode: c++; tab-width: 3; indent-tabs-mode: nil -*-

Copyright 2010, 2011, 2012, 2013
Raffaello D. Di Napoli

This file is part of Application-Building Components (henceforth referred to as ABC).

ABC 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.

ABC 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 ABC. If not, see
<http://www.gnu.org/licenses/>.
--------------------------------------------------------------------------------------------------*/

#ifndef ABC_TYPE_RAW_CDA_HXX
#define ABC_TYPE_RAW_CDA_HXX

#include <abc/core.hxx>
#ifdef ABC_CXX_PRAGMA_ONCE
   #pragma once
#endif

#include <type_traits>

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::void_cda

namespace abc {

/** Encapsulates raw constructors, destructors and assignment operators for a type. To be
instantiated via type_raw_cda.
*/
struct void_cda {

   /** Prototype of a function that copies items from one array to another.

   pDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   pSrc
      Pointer to the source array.
   ci
      Count of items to copy.
   */
   typedef void (* copy_fn)(void * pDst, void const * pSrc, size_t ci);

   /** Prototype of a function that compares two values for equality.

   p1
      Pointer to the first item.
   p2
      Pointer to the second item.
   return
      true if the items are equal, or false otherwise.
   */
   typedef bool (* equal_fn)(void const * p1, void const * p2);

   /** Prototype of a function that moves items from one array to another.

   pDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   pSrc
      Pointer to the source array.
   ci
      Count of items to move.
   */
   typedef void (* move_fn)(void * pDst, void * pSrc, size_t ci);

   /** Prototype of a function that destructs items in an array.

   p
      Pointer to the array.
   ci
      Count of items to destruct.
   */
   typedef void (* destr_fn)(void * p, size_t ci);

   /** Size of a variable of this type, in bytes. */
   size_t cb;
   /** Function to copy items from one array to another. */
   copy_fn copy_constr;
   /** Function to move items from one array to another. */
   move_fn move_constr;
   /** Function to destruct items in an array. */
   destr_fn destruct;
   /** Function to compare two items for equality. */
   equal_fn equal;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::typed_raw_cda

namespace abc {

/** DOC:3395 Move constructors and exceptions

In this section, “move constructor” will strictly refer to class::class(class &&).

All classes must provide move constructors and assignment operators if the copy constructor would
result in execution of exception-prone code (e.g. resource allocation).

Because move constructors are employed widely in container classes that need to provide strong
exception guarantee (fully transacted operation) even in case of moves, move constructors must not
throw exceptions. This requirement is relaxed for moves that involve two different classes, since
these will not be used by container classes.
*/

/** Defines a generic data type.
*/
template <typename T>
struct typed_raw_cda {

   /** Copies a range of items from one array to another, overwriting any existing contents in the
   destination.

   ptDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   ptSrc
      Pointer to the source array.
   ci
      Count of items to copy.
   */
   static void copy_constr(T * ptDst, T const * ptSrc, size_t ci) {
      if (std::has_trivial_copy_constructor<T>::value) {
         // No constructor, fastest copy possible.
         memory::copy(ptDst, ptSrc, ci);
      } else if (
#if defined(_GCC_VER) && _GCC_VER >= 40700
         std::is_nothrow_copy_constructible<T>::value
#else
         std::has_nothrow_copy_constructor<T>::value
#endif
      ) {
         // Not trivial, but it won’t throw either.
         for (T const * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
            ::new(ptDst) T(*ptSrc);
         }
      } else {
         // Exceptions can occur, so implement an all-or-nothing copy.
         T const * ptDstBegin(ptDst);
         try {
            for (T const * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
               ::new(ptDst) T(*ptSrc);
            }
         } catch (...) {
            // Undo (destruct) all the copies instantiated.
            while (--ptSrc >= ptDstBegin) {
               ptDst->~T();
            }
            throw;
         }
      }
   }

   /** Destructs a range of items in an array.

   pt
      Pointer to the array.
   ci
      Count of items to destruct.
   */
   static void destruct(T * pt, size_t ci) {
      if (!std::has_trivial_destructor<T>::value) {
         // The destructor is not a no-op.
         for (T * ptEnd(pt + ci); pt < ptEnd; ++pt) {
            pt->~T();
         }
      }
   }

   /** Compares two values for equality.

   pt1
      Pointer to the first item.
   pt2
      Pointer to the second item.
   return
      true if the items are equal, or false otherwise.
   */
   static bool equal(void const * pt1, void const * pt2) {
      return *static_cast<T const *>(pt1) == *static_cast<T const *>(pt2);
   }

   /** Moves a range of items from one array to another, overwriting any existing contents in the
   destination.

   ptDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   ptSrc
      Pointer to the source array.
   ci
      Count of items to move.
   */
   static void move_constr(T * ptDst, T * ptSrc, size_t ci) {
      for (T * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
         ::new(ptDst) T(std::move(*ptSrc));
      }
   }
};

// Remove const, but not volatile. Or maybe remove both?
template <typename T>
struct typed_raw_cda<T const> :
   public typed_raw_cda<T> {
};
template <typename T>
struct typed_raw_cda<T const volatile> :
   public typed_raw_cda<T volatile> {
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::type_raw_cda

namespace abc {

/** Returns a void_cda populated with the static methods from a typed_raw_cda.

TODO: comment signature.
*/
template <class T>
/*constexpr*/ void_cda const & type_raw_cda() {
   static void_cda const sc_vrcda = {
      sizeof(T),
      reinterpret_cast<void_cda:: copy_fn>(typed_raw_cda<T>::copy_constr),
      reinterpret_cast<void_cda:: move_fn>(typed_raw_cda<T>::move_constr),
      reinterpret_cast<void_cda::destr_fn>(typed_raw_cda<T>::destruct),
      reinterpret_cast<void_cda::equal_fn>(typed_raw_cda<T>::equal)
   };
   return sc_vrcda;
}

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////

#endif //ifndef ABC_TYPE_RAW_CDA_HXX

<commit_msg>Sort members<commit_after>/* -*- coding: utf-8; mode: c++; tab-width: 3; indent-tabs-mode: nil -*-

Copyright 2010, 2011, 2012, 2013
Raffaello D. Di Napoli

This file is part of Application-Building Components (henceforth referred to as ABC).

ABC 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.

ABC 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 ABC. If not, see
<http://www.gnu.org/licenses/>.
--------------------------------------------------------------------------------------------------*/

#ifndef ABC_TYPE_RAW_CDA_HXX
#define ABC_TYPE_RAW_CDA_HXX

#include <abc/core.hxx>
#ifdef ABC_CXX_PRAGMA_ONCE
   #pragma once
#endif

#include <type_traits>

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::void_cda

namespace abc {

/** Encapsulates raw constructors, destructors and assignment operators for a type. To be
instantiated via type_raw_cda.
*/
struct void_cda {

   /** Prototype of a function that copies items from one array to another.

   pDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   pSrc
      Pointer to the source array.
   ci
      Count of items to copy.
   */
   typedef void (* copy_fn)(void * pDst, void const * pSrc, size_t ci);

   /** Prototype of a function that destructs items in an array.

   p
      Pointer to the array.
   ci
      Count of items to destruct.
   */
   typedef void (* destr_fn)(void * p, size_t ci);

   /** Prototype of a function that compares two values for equality.

   p1
      Pointer to the first item.
   p2
      Pointer to the second item.
   return
      true if the items are equal, or false otherwise.
   */
   typedef bool (* equal_fn)(void const * p1, void const * p2);

   /** Prototype of a function that moves items from one array to another.

   pDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   pSrc
      Pointer to the source array.
   ci
      Count of items to move.
   */
   typedef void (* move_fn)(void * pDst, void * pSrc, size_t ci);

   /** Size of a variable of this type, in bytes. */
   size_t cb;
   /** Function to copy items from one array to another. */
   copy_fn copy_constr;
   /** Function to destruct items in an array. */
   destr_fn destruct;
   /** Function to compare two items for equality. */
   equal_fn equal;
   /** Function to move items from one array to another. */
   move_fn move_constr;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::typed_raw_cda

namespace abc {

/** DOC:3395 Move constructors and exceptions

In this section, “move constructor” will strictly refer to class::class(class &&).

All classes must provide move constructors and assignment operators if the copy constructor would
result in execution of exception-prone code (e.g. resource allocation).

Because move constructors are employed widely in container classes that need to provide strong
exception guarantee (fully transacted operation) even in case of moves, move constructors must not
throw exceptions. This requirement is relaxed for moves that involve two different classes, since
these will not be used by container classes.
*/

/** Defines a generic data type.
*/
template <typename T>
struct typed_raw_cda {

   /** Copies a range of items from one array to another, overwriting any existing contents in the
   destination.

   ptDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   ptSrc
      Pointer to the source array.
   ci
      Count of items to copy.
   */
   static void copy_constr(T * ptDst, T const * ptSrc, size_t ci) {
      if (std::has_trivial_copy_constructor<T>::value) {
         // No constructor, fastest copy possible.
         memory::copy(ptDst, ptSrc, ci);
      } else if (
#if defined(_GCC_VER) && _GCC_VER >= 40700
         std::is_nothrow_copy_constructible<T>::value
#else
         std::has_nothrow_copy_constructor<T>::value
#endif
      ) {
         // Not trivial, but it won’t throw either.
         for (T const * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
            ::new(ptDst) T(*ptSrc);
         }
      } else {
         // Exceptions can occur, so implement an all-or-nothing copy.
         T const * ptDstBegin(ptDst);
         try {
            for (T const * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
               ::new(ptDst) T(*ptSrc);
            }
         } catch (...) {
            // Undo (destruct) all the copies instantiated.
            while (--ptSrc >= ptDstBegin) {
               ptDst->~T();
            }
            throw;
         }
      }
   }

   /** Destructs a range of items in an array.

   pt
      Pointer to the array.
   ci
      Count of items to destruct.
   */
   static void destruct(T * pt, size_t ci) {
      if (!std::has_trivial_destructor<T>::value) {
         // The destructor is not a no-op.
         for (T * ptEnd(pt + ci); pt < ptEnd; ++pt) {
            pt->~T();
         }
      }
   }

   /** Compares two values for equality.

   pt1
      Pointer to the first item.
   pt2
      Pointer to the second item.
   return
      true if the items are equal, or false otherwise.
   */
   static bool equal(void const * pt1, void const * pt2) {
      return *static_cast<T const *>(pt1) == *static_cast<T const *>(pt2);
   }

   /** Moves a range of items from one array to another, overwriting any existing contents in the
   destination.

   ptDst
      Pointer to the destination array. The items are supposed to be uninitialized.
   ptSrc
      Pointer to the source array.
   ci
      Count of items to move.
   */
   static void move_constr(T * ptDst, T * ptSrc, size_t ci) {
      for (T * ptSrcEnd(ptSrc + ci); ptSrc < ptSrcEnd; ++ptSrc, ++ptDst) {
         ::new(ptDst) T(std::move(*ptSrc));
      }
   }
};

// Remove const, but not volatile. Or maybe remove both?
template <typename T>
struct typed_raw_cda<T const> :
   public typed_raw_cda<T> {
};
template <typename T>
struct typed_raw_cda<T const volatile> :
   public typed_raw_cda<T volatile> {
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::type_raw_cda

namespace abc {

/** Returns a void_cda populated with the static methods from a typed_raw_cda.

TODO: comment signature.
*/
template <class T>
/*constexpr*/ void_cda const & type_raw_cda() {
   static void_cda const sc_vrcda = {
      sizeof(T),
      reinterpret_cast<void_cda:: copy_fn>(typed_raw_cda<T>::copy_constr),
      reinterpret_cast<void_cda::destr_fn>(typed_raw_cda<T>::destruct),
      reinterpret_cast<void_cda::equal_fn>(typed_raw_cda<T>::equal),
      reinterpret_cast<void_cda:: move_fn>(typed_raw_cda<T>::move_constr)
   };
   return sc_vrcda;
}

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////

#endif //ifndef ABC_TYPE_RAW_CDA_HXX

<|endoftext|>


<commit_before>#include "log.hpp"

namespace aw
{
  Log::Log()
  {

  }

  Log::~Log()
  {
    mLogFile.close();
  }

  Log& Log::getInstance()
  {
    std::cout << "Hier";
    static Log log;
    return log;
  }

  void Log::setFilePath(const std::string &filePath)
  {
    mLogFile.open(filePath.c_str(), std::ios::app);
  }

  template<class T>
  void Log::write(const T &message)
  {
    std::time_t t = std::time(nullptr);

    std::cout << message << std::endl;
    if (mLogFile.is_open())
      mLogFile << std::put_time(std::localtime(&t), "%c: ") << message << std::endl;
  }
}<commit_msg>Fixed copy and paste error in log.cpp<commit_after>#include "log.hpp"

namespace aw
{
  Log::Log()
  {

  }

  Log::~Log()
  {
    mLogFile.close();
  }

  Log& Log::getInstance()
  {
    std::cout << "Hier";
    static Log log;
    return log;
  }

  void Log::setFilePath(const std::string &filePath)
  {
    mLogFile.open(filePath.c_str(), std::ios::app);
  }
}<|endoftext|>