<commit_before>#include <stdlib.h>
#include <iostream>
#include <math.h>
#include "diff_drive_robot.hpp"

#define PI 3.14159265359
#define K_RHO 0.5
#define K_DELTA -0.5
#define K_GAMMA 0.62
#define R_MERGE_START	1.0
#define R_MERGE_END	0.5

namespace diff_drive_robot {
DiffDriveRobot::DiffDriveRobot(double v_max, double w_max, double a_max, double alpha_max, double dt) :
	v_max_(v_max),
	w_max_(w_max),
	a_max_(a_max),
	alpha_max_(alpha_max),
  dt_(dt),
	goal_idx_(0),
	stop_mode_ (1)
  {};
void DiffDriveRobot::set_goals(std::vector<Goal> const &goals){
	//check if it's a new trajectory.
	// A better check is needed than the one below
	if (goals_.size()>0){
		if (hypot(goals.back().x - goals_.back().x,goals.back().y - goals_.back().y)> 0.5){
			goal_idx_ = 0; //reset goal counter
			goals_ = goals;
			stop_mode_ = 1;
			if (goals_.size()>1) { //chomp currently doesn't calculate heading for
														//trajectory points. It will be calculated here based
														//by forward propgating current headin	g.
				std::cout<<"end goal x = "<< goals_.back().x <<" , y = "<<goals_.back().y<<std::endl;
				goals_[0].theta = theta_;
				for (size_t ii=1; ii<goals_.size(); ++ii){
					goals_[ii].theta = atan2(goals_[ii].y-goals[ii-1].y,
																		goals_[ii].x-goals[ii-1].x);
				}
			}
		}
	}
	else { //goal.size() == 0
		goals_= goals;
		stop_mode_ = 1;
	}
}
void DiffDriveRobot::spin(){
	if (goals_.size()==0) return;
	if (stop_mode_){
		stop();
		return;
	}

  diff_drive_robot::Goal smoothed_goal, current_goal, next_goal;
  current_goal = goals_[goal_idx_];
	if ((distance_to_goal(current_goal)<R_MERGE_END) && (goal_idx_ < goals_.size() - 1)){
		goal_idx_++;
		current_goal = goals_[goal_idx_];
		std::cout<<"Moving to goal"<<goal_idx_<<std::endl;
	}
	std::cout<<"Distance to goal = "<<distance_to_goal(current_goal)<<std::endl;
  if (goal_idx_<goals_.size()-1)
    next_goal = goals_[goal_idx_+1];
  else
    next_goal = goals_[goal_idx_];
	//smooth transition between consecutive goals.

	if ((distance_to_goal(current_goal)<= R_MERGE_START) &&
					(distance_to_goal(current_goal)>= R_MERGE_END)){
						double d = distance_to_goal(current_goal);
						smoothed_goal.x = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.x +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.x ;
						smoothed_goal.y = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.y +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.y ;
						smoothed_goal.theta = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.theta +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.theta;
					}
	else 	smoothed_goal = current_goal;	//for all other cases

  double dx = smoothed_goal.x-x_;
  double dy = smoothed_goal.y-y_;
  double eps = atan2(dy, dx);
  double rho = hypot(dx, dy);
  double gamma = wrap_angle(eps - theta_);
  double delta = wrap_angle(smoothed_goal.theta - gamma - theta_);
	std::cout<<"rho = "<<rho<<", gamma = "<<gamma*180.0/PI<<", delta = "<<delta*180.0/PI<<std::endl;
  double v_desired = K_RHO * rho;
  double w_desired = K_GAMMA * gamma + K_DELTA * delta;
	std::cout<<"v_desired = "<<v_desired<< ", w_desired = "<<w_desired*180.0/PI;
  // check velocity limits
  if (fabs(v_desired)>v_max_) v_desired = copysign(v_max_, v_desired);
  if (fabs(w_desired)>w_max_) w_desired = copysign(w_max_, w_desired);
  // check accelration limits
  if (fabs(v_desired-v_)/dt_ > a_max_)
    v_desired = v_ + copysign(a_max_*dt_, v_desired-v_);
  if (fabs(w_desired-w_)/dt_ > alpha_max_)
    w_desired = w_ + copysign(alpha_max_*dt_, w_desired-w_);
  w_ = w_desired;
	v_ = v_desired *(1.0 -0.25*fabs(w_)/w_max_);
  std::cout<<", v = "<<v_<<", w = "<<w_*180.0/PI<<std::endl;
}
void DiffDriveRobot::set_pose(double x, double y, double theta){
  x_ = x;
  y_ = y;
  theta_ = wrap_angle(theta);
}
double DiffDriveRobot::distance_to_goal(Goal const &goal) const{
	return hypot(goal.x - x_, goal.y - y_);
}

Goal DiffDriveRobot::get_goal(size_t idx) const{
		//TODO: add some index check to make sure we are not out of bounds
		return goals_[idx];
}

void DiffDriveRobot::stop() {
	double v_desired = 0;
  double w_desired = 0;
  // check velocity limits
  if (fabs(v_desired)>v_max_) v_desired = copysign(v_max_, v_desired);
  if (fabs(w_desired)>w_max_) w_desired = copysign(w_max_, w_desired);
  // check accelration limits
  if (fabs(v_desired-v_)/dt_ > a_max_)
    v_desired = v_ + copysign(a_max_*dt_, v_desired-v_);
  if (fabs(w_desired-w_)/dt_ > alpha_max_)
    w_desired = w_ + copysign(alpha_max_*dt_, w_desired-w_);
  v_ = v_desired;
  w_ = w_desired;
	if (v_==0 && w_ == 0) stop_mode_ = 0;
}

double wrap_angle(double angle){
    // It's assumed that angle is between -2*pi and 2*pi, the returned
    // value will be between -pi and pi
    if (angle>PI) return -2*PI + angle;
    if (angle<-PI) return 2*PI + angle;
    return angle;
}
}
<commit_msg>fixed index out of bounds for get_goal when called before set_goals is called<commit_after>#include <stdlib.h>
#include <iostream>
#include <math.h>
#include "diff_drive_robot.hpp"

#define PI 3.14159265359
#define K_RHO 0.5
#define K_DELTA -0.5
#define K_GAMMA 0.62
#define R_MERGE_START	1.0
#define R_MERGE_END	0.5

namespace diff_drive_robot {
DiffDriveRobot::DiffDriveRobot(double v_max, double w_max, double a_max, double alpha_max, double dt) :
	v_max_(v_max),
	w_max_(w_max),
	a_max_(a_max),
	alpha_max_(alpha_max),
  dt_(dt),
	goal_idx_(0),
	stop_mode_ (1)
  {};
void DiffDriveRobot::set_goals(std::vector<Goal> const &goals){
	//check if it's a new trajectory.
	// A better check is needed than the one below
	if (goals_.size()>0){
		if (hypot(goals.back().x - goals_.back().x,goals.back().y - goals_.back().y)> 0.5){
			goal_idx_ = 0; //reset goal counter
			goals_ = goals;
			stop_mode_ = 1;
			if (goals_.size()>1) { //chomp currently doesn't calculate heading for
														//trajectory points. It will be calculated here based
														//by forward propgating current headin	g.
				std::cout<<"end goal x = "<< goals_.back().x <<" , y = "<<goals_.back().y<<std::endl;
				goals_[0].theta = theta_;
				for (size_t ii=1; ii<goals_.size(); ++ii){
					goals_[ii].theta = atan2(goals_[ii].y-goals[ii-1].y,
																		goals_[ii].x-goals[ii-1].x);
				}
			}
		}
	}
	else { //goal.size() == 0
		goals_= goals;
		stop_mode_ = 1;
	}
}
void DiffDriveRobot::spin(){
	if (goals_.size()==0) return;
	if (stop_mode_){
		stop();
		return;
	}

  diff_drive_robot::Goal smoothed_goal, current_goal, next_goal;
  current_goal = goals_[goal_idx_];
	if ((distance_to_goal(current_goal)<R_MERGE_END) && (goal_idx_ < goals_.size() - 1)){
		goal_idx_++;
		current_goal = goals_[goal_idx_];
		std::cout<<"Moving to goal"<<goal_idx_<<std::endl;
	}
	std::cout<<"Distance to goal = "<<distance_to_goal(current_goal)<<std::endl;
  if (goal_idx_<goals_.size()-1)
    next_goal = goals_[goal_idx_+1];
  else
    next_goal = goals_[goal_idx_];
	//smooth transition between consecutive goals.

	if ((distance_to_goal(current_goal)<= R_MERGE_START) &&
					(distance_to_goal(current_goal)>= R_MERGE_END)){
						double d = distance_to_goal(current_goal);
						smoothed_goal.x = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.x +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.x ;
						smoothed_goal.y = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.y +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.y ;
						smoothed_goal.theta = ((d-R_MERGE_END)/R_MERGE_END)*current_goal.theta +
																((R_MERGE_START-d)/R_MERGE_END)*next_goal.theta;
					}
	else 	smoothed_goal = current_goal;	//for all other cases

  double dx = smoothed_goal.x-x_;
  double dy = smoothed_goal.y-y_;
  double eps = atan2(dy, dx);
  double rho = hypot(dx, dy);
  double gamma = wrap_angle(eps - theta_);
  double delta = wrap_angle(smoothed_goal.theta - gamma - theta_);
	std::cout<<"rho = "<<rho<<", gamma = "<<gamma*180.0/PI<<", delta = "<<delta*180.0/PI<<std::endl;
  double v_desired = K_RHO * rho;
  double w_desired = K_GAMMA * gamma + K_DELTA * delta;
	std::cout<<"v_desired = "<<v_desired<< ", w_desired = "<<w_desired*180.0/PI;
  // check velocity limits
  if (fabs(v_desired)>v_max_) v_desired = copysign(v_max_, v_desired);
  if (fabs(w_desired)>w_max_) w_desired = copysign(w_max_, w_desired);
  // check accelration limits
  if (fabs(v_desired-v_)/dt_ > a_max_)
    v_desired = v_ + copysign(a_max_*dt_, v_desired-v_);
  if (fabs(w_desired-w_)/dt_ > alpha_max_)
    w_desired = w_ + copysign(alpha_max_*dt_, w_desired-w_);
  w_ = w_desired;
	v_ = v_desired *(1.0 -0.25*fabs(w_)/w_max_);
  std::cout<<", v = "<<v_<<", w = "<<w_*180.0/PI<<std::endl;
}
void DiffDriveRobot::set_pose(double x, double y, double theta){
  x_ = x;
  y_ = y;
  theta_ = wrap_angle(theta);
}
double DiffDriveRobot::distance_to_goal(Goal const &goal) const{
	return hypot(goal.x - x_, goal.y - y_);
}

Goal DiffDriveRobot::get_goal(size_t idx) const{
		if (idx>= goals_.size()){
			Goal dummy;
			return dummy;
		}
		return goals_[idx];
}

void DiffDriveRobot::stop() {
	double v_desired = 0;
  double w_desired = 0;
  // check velocity limits
  if (fabs(v_desired)>v_max_) v_desired = copysign(v_max_, v_desired);
  if (fabs(w_desired)>w_max_) w_desired = copysign(w_max_, w_desired);
  // check accelration limits
  if (fabs(v_desired-v_)/dt_ > a_max_)
    v_desired = v_ + copysign(a_max_*dt_, v_desired-v_);
  if (fabs(w_desired-w_)/dt_ > alpha_max_)
    w_desired = w_ + copysign(alpha_max_*dt_, w_desired-w_);
  v_ = v_desired;
  w_ = w_desired;
	if (v_==0 && w_ == 0) stop_mode_ = 0;
}

double wrap_angle(double angle){
    // It's assumed that angle is between -2*pi and 2*pi, the returned
    // value will be between -pi and pi
    if (angle>PI) return -2*PI + angle;
    if (angle<-PI) return 2*PI + angle;
    return angle;
}
}
<|endoftext|>


<commit_before>// Copyright (c) 2009-2012 Bitcoin Developers
// Copyright (c) 2011-2012 Litecoin Developers
// Copyright (c) 2013 albocoin Developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "init.h" // for pwalletMain
#include "bitcoinrpc.h"
#include "ui_interface.h"
#include "base58.h"

#include <boost/lexical_cast.hpp>

#define printf OutputDebugStringF

using namespace json_spirit;
using namespace std;

class CTxDump
{
public:
    CBlockIndex *pindex;
    int64 nValue;
    bool fSpent;
    CWalletTx* ptx;
    int nOut;
    CTxDump(CWalletTx* ptx = NULL, int nOut = -1)
    {
        pindex = NULL;
        nValue = 0;
        fSpent = false;
        this->ptx = ptx;
        this->nOut = nOut;
    }
};

Value importprivkey(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2)
        throw runtime_error(
            "importprivkey <dogecoinprivkey> [label]\n"
            "Adds a private key (as returned by dumpprivkey) to your wallet.");

    string strSecret = params[0].get_str();
    string strLabel = "";
    if (params.size() > 1)
        strLabel = params[1].get_str();
    CBitcoinSecret vchSecret;
    bool fGood = vchSecret.SetString(strSecret);

    if (!fGood) throw JSONRPCError(-5,"Invalid private key");

    CKey key;
    bool fCompressed;
    CSecret secret = vchSecret.GetSecret(fCompressed);
    key.SetSecret(secret, fCompressed);
    CKeyID vchAddress = key.GetPubKey().GetID();
    {
        LOCK2(cs_main, pwalletMain->cs_wallet);

        pwalletMain->MarkDirty();
        pwalletMain->SetAddressBookName(vchAddress, strLabel);

        if (!pwalletMain->AddKey(key))
            throw JSONRPCError(-4,"Error adding key to wallet");

        pwalletMain->ScanForWalletTransactions(pindexGenesisBlock, true);
        pwalletMain->ReacceptWalletTransactions();
    }

    return Value::null;
}

Value dumpprivkey(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
        "dumpprivkey <dogecoinaddress>\n"
        "Reveals the private key corresponding to <dogecoinaddress>.");

    string strAddress = params[0].get_str();
    CBitcoinAddress address;
    if (!address.SetString(strAddress))
    throw JSONRPCError(-5, "Invalid albocoin address");
    CKeyID keyID;
    if (!address.GetKeyID(keyID))
        throw JSONRPCError(-3, "Address does not refer to a key");
    CSecret vchSecret;
    bool fCompressed;
    if (!pwalletMain->GetSecret(keyID, vchSecret, fCompressed))
        throw JSONRPCError(-4,"Private key for address " + strAddress + " is not known");
    return CBitcoinSecret(vchSecret, fCompressed).ToString();
}
<commit_msg>Update rpcdump.cpp<commit_after>// Copyright (c) 2009-2012 Bitcoin Developers
// Copyright (c) 2011-2012 Litecoin Developers
// Copyright (c) 2013 albocoin Developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "init.h" // for pwalletMain
#include "bitcoinrpc.h"
#include "ui_interface.h"
#include "base58.h"

#include <boost/lexical_cast.hpp>

#define printf OutputDebugStringF

using namespace json_spirit;
using namespace std;

class CTxDump
{
public:
    CBlockIndex *pindex;
    int64 nValue;
    bool fSpent;
    CWalletTx* ptx;
    int nOut;
    CTxDump(CWalletTx* ptx = NULL, int nOut = -1)
    {
        pindex = NULL;
        nValue = 0;
        fSpent = false;
        this->ptx = ptx;
        this->nOut = nOut;
    }
};

Value importprivkey(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2)
        throw runtime_error(
            "importprivkey <dogecoinprivkey> [label]\n"
            "Adds a private key (as returned by dumpprivkey) to your wallet.");

    string strSecret = params[0].get_str();
    string strLabel = "";
    if (params.size() > 1)
        strLabel = params[1].get_str();
    CBitcoinSecret vchSecret;
    bool fGood = vchSecret.SetString(strSecret);

    if (!fGood) throw JSONRPCError(-5,"Invalid private key");

    CKey key;
    bool fCompressed;
    CSecret secret = vchSecret.GetSecret(fCompressed);
    key.SetSecret(secret, fCompressed);
    CKeyID vchAddress = key.GetPubKey().GetID();
    {
        LOCK2(cs_main, pwalletMain->cs_wallet);

        pwalletMain->MarkDirty();
        pwalletMain->SetAddressBookName(vchAddress, strLabel);

        if (!pwalletMain->AddKey(key))
            throw JSONRPCError(-4,"Error adding key to wallet");

        pwalletMain->ScanForWalletTransactions(pindexGenesisBlock, true);
        pwalletMain->ReacceptWalletTransactions();
    }

    return Value::null;
}

Value dumpprivkey(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
        "dumpprivkey <albocoinaddress>\n"
        "Reveals the private key corresponding to <albocoinaddress>.");

    string strAddress = params[0].get_str();
    CBitcoinAddress address;
    if (!address.SetString(strAddress))
    throw JSONRPCError(-5, "Invalid albocoin address");
    CKeyID keyID;
    if (!address.GetKeyID(keyID))
        throw JSONRPCError(-3, "Address does not refer to a key");
    CSecret vchSecret;
    bool fCompressed;
    if (!pwalletMain->GetSecret(keyID, vchSecret, fCompressed))
        throw JSONRPCError(-4,"Private key for address " + strAddress + " is not known");
    return CBitcoinSecret(vchSecret, fCompressed).ToString();
}
<|endoftext|>


<commit_before>#include "stdafx.h"

#include "Player.h"
#include "ETypes.h"
#include <ATF/global.hpp>

namespace GameServer
{
    namespace Fixes
    {
        void CPlayer::load()
        {
            auto& core = ATF::CATFCore::get_instance();
            core.set_hook(&ATF::CPlayer::CalcPvP, &CPlayer::CalcPvP);
            core.set_hook(&ATF::CPlayer::CalPvpTempCash, &CPlayer::CalPvpTempCash);
            core.set_hook(&ATF::CPlayer::pc_MakeTrapRequest, &CPlayer::pc_MakeTrapRequest);
            core.set_hook(&ATF::CPlayer::pc_MakeTowerRequest, &CPlayer::pc_MakeTowerRequest);
            core.set_hook(&ATF::CPlayer::pc_GestureRequest, &CPlayer::pc_GestureRequest);
            core.set_hook(&ATF::CPlayer::pc_GuildManageRequest, &CPlayer::pc_GuildManageRequest);
            core.set_hook(&ATF::CPlayer::pc_CharacterRenameCheck, &CPlayer::pc_CharacterRenameCheck);
            core.set_hook(&ATF::CPlayer::pc_GotoBasePortalRequest, &CPlayer::pc_GotoBasePortalRequest);
        }

        void CPlayer::unload()
        {
            auto& core = ATF::CATFCore::get_instance();
            core.unset_hook(&ATF::CPlayer::CalcPvP);
            core.unset_hook(&ATF::CPlayer::CalPvpTempCash);
            core.unset_hook(&ATF::CPlayer::pc_MakeTrapRequest);
            core.unset_hook(&ATF::CPlayer::pc_MakeTowerRequest);
            core.unset_hook(&ATF::CPlayer::pc_GestureRequest);
            core.unset_hook(&ATF::CPlayer::pc_GuildManageRequest);
            core.unset_hook(&ATF::CPlayer::pc_CharacterRenameCheck);
            core.unset_hook(&ATF::CPlayer::pc_GotoBasePortalRequest);
        }

        void CPlayer::loop()
        {
        }

        ModuleVersion_t CPlayer::get_version()
        {
            return ATF::usVersion;
        }

        ModuleName_t CPlayer::get_name()
        {
            static const ModuleName_t name = "fix_Player";
            return name;
        }

        void CPlayer::configure(
            const rapidjson::Value & nodeConfig)
        {
            UNREFERENCED_PARAMETER(nodeConfig);
        }

        bool WINAPIV CPlayer::Load(
            ATF::CPlayer * pObj,
            ATF::CUserDB * pUser,
            bool bFirstStart,
            ATF::info::CPlayerLoad366_ptr next)
        {
            bool bResult = next(pObj, pUser, bFirstStart);
            if (bResult && !pObj->m_Param.m_pGuild)
            {
                auto dwDestroyerSerial = ATF::global::g_HolySys->GetDestroyerSerial();
                if (pObj->m_Param.GetCharSerial() != dwDestroyerSerial)
                {
                    pObj->SetLastAttBuff(false);
                }
            }
            return bResult;
        }

        void WINAPIV CPlayer::CalcPvP(
            ATF::CPlayer * pObj,
            ATF::CPlayer * pDier,
            char byKillerObjID,
            ATF::info::CPlayerCalcPvP74_ptr next)
        {
            if (pObj->m_Param.GetRaceCode() == pDier->m_Param.GetRaceCode())
            {
                return;
            }

            next(pObj, pDier, byKillerObjID);
        }

        void WINAPIV CPlayer::CalPvpTempCash(
            ATF::CPlayer * pObj,
            ATF::CPlayer * pDier,
            char byKillerObjID,
            ATF::info::CPlayerCalPvpTempCash52_ptr next)
        {
            if (pObj->m_Param.GetRaceCode() == pDier->m_Param.GetRaceCode())
                return;

            next(pObj, pDier, byKillerObjID);
        }

        void WINAPIV CPlayer::pc_MakeTrapRequest(
            ATF::CPlayer * pObj,
            uint16_t wSkillIndex,
            uint16_t wTrapItemSerial,
            float * pfPos,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MakeTrapRequest1783_ptr next)
        {
            do
            {
                if (pObj->IsMapLoading())
                    break;

                auto pTrapItem = pObj->m_Param.m_dbInven.GetPtrFromSerial(wTrapItemSerial);
                if (!pTrapItem)
                    break;

                if (pTrapItem->m_byTableCode != (BYTE)e_code_item_table::tbl_code_trap)
                    break;

                if (!pObj->IsEffectableEquip(pTrapItem))
                    break;

                next(pObj, wSkillIndex, wTrapItemSerial, pfPos, pConsumeSerial);
            } while (false);
        }

        void WINAPIV CPlayer::pc_MakeTowerRequest(
            ATF::CPlayer * pObj,
            uint16_t wSkillIndex,
            uint16_t wTowerItemSerial,
            char byMaterialNum,
            ATF::_make_tower_request_clzo::__material * pMaterial,
            float * pfPos,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MakeTowerRequest1781_ptr next)
        {
            do
            {
                if (pObj->IsMapLoading())
                    break;

                auto pTrapItem = pObj->m_Param.m_dbInven.GetPtrFromSerial(wTowerItemSerial);
                if (!pTrapItem)
                    break;

                if (pTrapItem->m_byTableCode != (BYTE)e_code_item_table::tbl_code_tower)
                    break;

                if (!pObj->IsEffectableEquip(pTrapItem))
                    break;

                next(pObj, wSkillIndex, wTowerItemSerial, byMaterialNum, pMaterial, pfPos, pConsumeSerial);
            } while (false);
        }

        void WINAPIV CPlayer::pc_GestureRequest(
            ATF::CPlayer * pObj,
            char byGestureType,
            ATF::info::CPlayerpc_GestureRequest1719_ptr next)
        {
            if (pObj->IsMineMode())
                return;

            if (pObj->IsActingSiegeMode() || pObj->IsSiegeMode())
                return;

            if (pObj->IsRidingUnit())
                return;

            next(pObj, byGestureType);
        }

        void WINAPIV CPlayer::pc_GuildManageRequest(
            ATF::CPlayer * pObj,
            char byType,
            unsigned int dwDst,
            unsigned int dwObj1,
            unsigned int dwObj2,
            unsigned int dwObj3,
            ATF::info::CPlayerpc_GuildManageRequest1745_ptr next)
        {
            if (!pObj->m_Param.m_pGuild)
            {
                pObj->SendMsg_GuildManageResult(202);
                return;
            }

            next(pObj, byType, dwDst, dwObj1, dwObj2, dwObj3);
        }

        void WINAPIV CPlayer::pc_MovePortal(
            ATF::CPlayer * pObj,
            int nPortalIndex,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MovePortal1795_ptr next)
        {
            if (pObj->Is_Battle_Mode())
            {
                float pNewPos[3]{ 0.0f };
                char byMapIndx = -1;
                char byPortalIndx = -1;

                pObj->SendMsg_MovePortal(4, byMapIndx, byPortalIndx, pNewPos, true);
                return;
            }

            next(pObj, nPortalIndex, pConsumeSerial);;
        }

        char WINAPIV CPlayer::pc_CharacterRenameCheck(
            ATF::CPlayer * pObj,
            char * strCharacterName,
            ATF::info::CPlayerpc_CharacterRenameCheck1629_ptr next)
        {
            if (ATF::global::IsSQLValidString(strCharacterName))
                return next(pObj, strCharacterName);

            return 6;
        }

        void WINAPIV CPlayer::pc_GotoBasePortalRequest(
            ATF::CPlayer * pObj,
            unsigned __int16 wItemSerial,
            ATF::info::CPlayerpc_GotoBasePortalRequest1725_ptr next)
        {
            if (pObj->Is_Battle_Mode() && pObj->m_byUserDgr == 0)
            {
                pObj->SendMsg_GotoBasePortalResult(8);
                return;
            }

            next(pObj, wItemSerial);
        }
    }
}<commit_msg>Enabled fix for infinity chip hunter buff<commit_after>#include "stdafx.h"

#include "Player.h"
#include "ETypes.h"
#include <ATF/global.hpp>

namespace GameServer
{
    namespace Fixes
    {
        void CPlayer::load()
        {
            auto& core = ATF::CATFCore::get_instance();
            core.set_hook(&ATF::CPlayer::Load, &CPlayer::Load);
            core.set_hook(&ATF::CPlayer::CalcPvP, &CPlayer::CalcPvP);
            core.set_hook(&ATF::CPlayer::CalPvpTempCash, &CPlayer::CalPvpTempCash);
            core.set_hook(&ATF::CPlayer::pc_MakeTrapRequest, &CPlayer::pc_MakeTrapRequest);
            core.set_hook(&ATF::CPlayer::pc_MakeTowerRequest, &CPlayer::pc_MakeTowerRequest);
            core.set_hook(&ATF::CPlayer::pc_GestureRequest, &CPlayer::pc_GestureRequest);
            core.set_hook(&ATF::CPlayer::pc_GuildManageRequest, &CPlayer::pc_GuildManageRequest);
            core.set_hook(&ATF::CPlayer::pc_CharacterRenameCheck, &CPlayer::pc_CharacterRenameCheck);
            core.set_hook(&ATF::CPlayer::pc_GotoBasePortalRequest, &CPlayer::pc_GotoBasePortalRequest);
        }

        void CPlayer::unload()
        {
            auto& core = ATF::CATFCore::get_instance();
            core.unset_hook(&ATF::CPlayer::Load);
            core.unset_hook(&ATF::CPlayer::CalcPvP);
            core.unset_hook(&ATF::CPlayer::CalPvpTempCash);
            core.unset_hook(&ATF::CPlayer::pc_MakeTrapRequest);
            core.unset_hook(&ATF::CPlayer::pc_MakeTowerRequest);
            core.unset_hook(&ATF::CPlayer::pc_GestureRequest);
            core.unset_hook(&ATF::CPlayer::pc_GuildManageRequest);
            core.unset_hook(&ATF::CPlayer::pc_CharacterRenameCheck);
            core.unset_hook(&ATF::CPlayer::pc_GotoBasePortalRequest);
        }

        void CPlayer::loop()
        {
        }

        ModuleVersion_t CPlayer::get_version()
        {
            return ATF::usVersion;
        }

        ModuleName_t CPlayer::get_name()
        {
            static const ModuleName_t name = "fix_Player";
            return name;
        }

        void CPlayer::configure(
            const rapidjson::Value & nodeConfig)
        {
            UNREFERENCED_PARAMETER(nodeConfig);
        }

        bool WINAPIV CPlayer::Load(
            ATF::CPlayer * pObj,
            ATF::CUserDB * pUser,
            bool bFirstStart,
            ATF::info::CPlayerLoad366_ptr next)
        {
            bool bResult = next(pObj, pUser, bFirstStart);
            if (bResult && !pObj->m_Param.m_pGuild)
            {
                auto dwDestroyerSerial = ATF::global::g_HolySys->GetDestroyerSerial();
                if (pObj->m_Param.GetCharSerial() != dwDestroyerSerial)
                {
                    pObj->SetLastAttBuff(false);
                }
            }
            return bResult;
        }

        void WINAPIV CPlayer::CalcPvP(
            ATF::CPlayer * pObj,
            ATF::CPlayer * pDier,
            char byKillerObjID,
            ATF::info::CPlayerCalcPvP74_ptr next)
        {
            if (pObj->m_Param.GetRaceCode() == pDier->m_Param.GetRaceCode())
            {
                return;
            }

            next(pObj, pDier, byKillerObjID);
        }

        void WINAPIV CPlayer::CalPvpTempCash(
            ATF::CPlayer * pObj,
            ATF::CPlayer * pDier,
            char byKillerObjID,
            ATF::info::CPlayerCalPvpTempCash52_ptr next)
        {
            if (pObj->m_Param.GetRaceCode() == pDier->m_Param.GetRaceCode())
                return;

            next(pObj, pDier, byKillerObjID);
        }

        void WINAPIV CPlayer::pc_MakeTrapRequest(
            ATF::CPlayer * pObj,
            uint16_t wSkillIndex,
            uint16_t wTrapItemSerial,
            float * pfPos,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MakeTrapRequest1783_ptr next)
        {
            do
            {
                if (pObj->IsMapLoading())
                    break;

                auto pTrapItem = pObj->m_Param.m_dbInven.GetPtrFromSerial(wTrapItemSerial);
                if (!pTrapItem)
                    break;

                if (pTrapItem->m_byTableCode != (BYTE)e_code_item_table::tbl_code_trap)
                    break;

                if (!pObj->IsEffectableEquip(pTrapItem))
                    break;

                next(pObj, wSkillIndex, wTrapItemSerial, pfPos, pConsumeSerial);
            } while (false);
        }

        void WINAPIV CPlayer::pc_MakeTowerRequest(
            ATF::CPlayer * pObj,
            uint16_t wSkillIndex,
            uint16_t wTowerItemSerial,
            char byMaterialNum,
            ATF::_make_tower_request_clzo::__material * pMaterial,
            float * pfPos,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MakeTowerRequest1781_ptr next)
        {
            do
            {
                if (pObj->IsMapLoading())
                    break;

                auto pTrapItem = pObj->m_Param.m_dbInven.GetPtrFromSerial(wTowerItemSerial);
                if (!pTrapItem)
                    break;

                if (pTrapItem->m_byTableCode != (BYTE)e_code_item_table::tbl_code_tower)
                    break;

                if (!pObj->IsEffectableEquip(pTrapItem))
                    break;

                next(pObj, wSkillIndex, wTowerItemSerial, byMaterialNum, pMaterial, pfPos, pConsumeSerial);
            } while (false);
        }

        void WINAPIV CPlayer::pc_GestureRequest(
            ATF::CPlayer * pObj,
            char byGestureType,
            ATF::info::CPlayerpc_GestureRequest1719_ptr next)
        {
            if (pObj->IsMineMode())
                return;

            if (pObj->IsActingSiegeMode() || pObj->IsSiegeMode())
                return;

            if (pObj->IsRidingUnit())
                return;

            next(pObj, byGestureType);
        }

        void WINAPIV CPlayer::pc_GuildManageRequest(
            ATF::CPlayer * pObj,
            char byType,
            unsigned int dwDst,
            unsigned int dwObj1,
            unsigned int dwObj2,
            unsigned int dwObj3,
            ATF::info::CPlayerpc_GuildManageRequest1745_ptr next)
        {
            if (!pObj->m_Param.m_pGuild)
            {
                pObj->SendMsg_GuildManageResult(202);
                return;
            }

            next(pObj, byType, dwDst, dwObj1, dwObj2, dwObj3);
        }

        void WINAPIV CPlayer::pc_MovePortal(
            ATF::CPlayer * pObj,
            int nPortalIndex,
            uint16_t * pConsumeSerial,
            ATF::info::CPlayerpc_MovePortal1795_ptr next)
        {
            if (pObj->Is_Battle_Mode())
            {
                float pNewPos[3]{ 0.0f };
                char byMapIndx = -1;
                char byPortalIndx = -1;

                pObj->SendMsg_MovePortal(4, byMapIndx, byPortalIndx, pNewPos, true);
                return;
            }

            next(pObj, nPortalIndex, pConsumeSerial);;
        }

        char WINAPIV CPlayer::pc_CharacterRenameCheck(
            ATF::CPlayer * pObj,
            char * strCharacterName,
            ATF::info::CPlayerpc_CharacterRenameCheck1629_ptr next)
        {
            if (ATF::global::IsSQLValidString(strCharacterName))
                return next(pObj, strCharacterName);

            return 6;
        }

        void WINAPIV CPlayer::pc_GotoBasePortalRequest(
            ATF::CPlayer * pObj,
            unsigned __int16 wItemSerial,
            ATF::info::CPlayerpc_GotoBasePortalRequest1725_ptr next)
        {
            if (pObj->Is_Battle_Mode() && pObj->m_byUserDgr == 0)
            {
                pObj->SendMsg_GotoBasePortalResult(8);
                return;
            }

            next(pObj, wItemSerial);
        }
    }
}<|endoftext|>


<commit_before>#include <handy/handy.h>

using namespace std;
using namespace handy;

struct Report {
    long connected;
    long retry;
    long sended;
    long recved;
    Report() { memset(this, 0, sizeof(*this)); }
};

int main(int argc, const char* argv[]) {
    if (argc < 9) {
        printf("usage %s <host> <begin port> <end port> <conn count> <create seconds> <subprocesses> <hearbeat interval> <send size> <management port>\n", argv[0]);
        return 1;
    }
    int c = 1;
    string host = argv[c++];
    int begin_port = atoi(argv[c++]);
    int end_port = atoi(argv[c++]);
    int conn_count = atoi(argv[c++]);
    int create_seconds = atoi(argv[c++]);
    int processes = atoi(argv[c++]);
    conn_count = conn_count / processes;
    int heartbeat_interval = atoi(argv[c++]);
    int bsz = atoi(argv[c++]);
    int man_port = atoi(argv[c++]);

    int pid = 1;
    for (int i = 0; i < processes; i ++) {
        pid = fork();
        if (pid == 0) { // a child process, break
            break;
        }
    }

    EventBase base;
    if (pid == 0) { //child process
        char *buf = new char[bsz];
        ExitCaller ec1([=] {delete[] buf; });
        Slice msg(buf, bsz);
        char heartbeat[] = "heartbeat";
        int send = 0;
        int connected = 0;
        int retry = 0;
        int recved = 0;

        vector<TcpConnPtr> allConns;
        info("creating %d connections", conn_count);
        for (int k = 0; k < create_seconds; k ++) {
            base.runAfter(1000*k, [&]{
                int c = conn_count / create_seconds;
                for (int i = 0; i < c; i++) {
                    short port = begin_port + (i % (end_port - begin_port));
                    auto con = TcpConn::createConnection(&base, host, port, 20*1000);
                    allConns.push_back(con);
                    con->setReconnectInterval(20*1000);
                    con->onMsg(new LengthCodec, [&](const TcpConnPtr& con, const Slice& msg) {
                        if (heartbeat_interval == 0) { // echo the msg if no interval
                            con->sendMsg(msg);
                            send++;
                        }
                        recved ++;
                    });
                    con->onState([&, i](const TcpConnPtr &con) {
                        TcpConn::State st = con->getState();
                        if (st == TcpConn::Connected) {
                            connected++;
//                            send ++;
//                            con->sendMsg(msg);
                        } else if (st == TcpConn::Failed || st == TcpConn::Closed) { //连接出错
                            if (st == TcpConn::Closed) { connected--; }
                            retry++;
                        }
                    });
                }

            });
        }
        if (heartbeat_interval) {
            base.runAfter(heartbeat_interval * 1000, [&] {
                for (int i = 0; i < heartbeat_interval; i ++) {
                    base.runAfter(i*1000, [&,i]{
                        size_t block = allConns.size() / heartbeat_interval;
                        for (size_t j=i*block; j<(i+1)*block && j<allConns.size(); j++) {
                            if (allConns[i]->getState() == TcpConn::Connected) {
                                allConns[i]->sendMsg(msg);
                                send++;
                            }
                        }
                    });
                }
            }, heartbeat_interval * 1000);
        }
        TcpConnPtr report = TcpConn::createConnection(&base, "127.0.0.1", man_port, 3000);
        report->onMsg(new LineCodec, [&](const TcpConnPtr& con, Slice msg) {
            if (msg == "exit") {
                info("recv exit msg from master, so exit");
                base.exit();
            }
        });
        report->onState([&](const TcpConnPtr& con) {
            if (con->getState() == TcpConn::Closed) {
                base.exit();
            }
        });
        base.runAfter(2000, [&]() {
            report->sendMsg(util::format("%d connected: %ld retry: %ld send: %ld recved: %ld",
                                         getpid(), connected, retry, send, recved));
        }, 100);
        base.loop();
    } else { // master process
        map<int, Report> subs;
        TcpServerPtr master = TcpServer::startServer(&base, "127.0.0.1", man_port);
        master->onConnMsg(new LineCodec, [&](const TcpConnPtr& con, Slice msg) {
            auto fs = msg.split(' ');
            if (fs.size() != 9) {
                error("number of fields is %lu expected 7", fs.size());
                return;
            }
            Report& c = subs[atoi(fs[0].data())];
            c.connected = atoi(fs[2].data());
            c.retry= atoi(fs[4].data());
            c.sended = atoi(fs[6].data());
            c.recved = atoi(fs[8].data());
        });
        base.runAfter(3000, [&](){
            for(auto& s: subs) {
                Report& r = s.second;
                printf("pid: %6ld connected %6ld retry %6ld sended %6ld recved %6ld\n", (long)s.first, r.connected, r.retry, r.sended, r.recved);
            }
            printf("\n");
        }, 3000);
        base.loop();
    }
    info("program exited");
}
<commit_msg>sleep 1s for master to listen<commit_after>#include <handy/handy.h>

using namespace std;
using namespace handy;

struct Report {
    long connected;
    long retry;
    long sended;
    long recved;
    Report() { memset(this, 0, sizeof(*this)); }
};

int main(int argc, const char* argv[]) {
    if (argc < 9) {
        printf("usage %s <host> <begin port> <end port> <conn count> <create seconds> <subprocesses> <hearbeat interval> <send size> <management port>\n", argv[0]);
        return 1;
    }
    int c = 1;
    string host = argv[c++];
    int begin_port = atoi(argv[c++]);
    int end_port = atoi(argv[c++]);
    int conn_count = atoi(argv[c++]);
    int create_seconds = atoi(argv[c++]);
    int processes = atoi(argv[c++]);
    conn_count = conn_count / processes;
    int heartbeat_interval = atoi(argv[c++]);
    int bsz = atoi(argv[c++]);
    int man_port = atoi(argv[c++]);

    int pid = 1;
    for (int i = 0; i < processes; i ++) {
        pid = fork();
        if (pid == 0) { // a child process, break
            sleep(1);
            break;
        }
    }

    EventBase base;
    if (pid == 0) { //child process
        char *buf = new char[bsz];
        ExitCaller ec1([=] {delete[] buf; });
        Slice msg(buf, bsz);
        char heartbeat[] = "heartbeat";
        int send = 0;
        int connected = 0;
        int retry = 0;
        int recved = 0;

        vector<TcpConnPtr> allConns;
        info("creating %d connections", conn_count);
        for (int k = 0; k < create_seconds; k ++) {
            base.runAfter(1000*k, [&]{
                int c = conn_count / create_seconds;
                for (int i = 0; i < c; i++) {
                    short port = begin_port + (i % (end_port - begin_port));
                    auto con = TcpConn::createConnection(&base, host, port, 20*1000);
                    allConns.push_back(con);
                    con->setReconnectInterval(20*1000);
                    con->onMsg(new LengthCodec, [&](const TcpConnPtr& con, const Slice& msg) {
                        if (heartbeat_interval == 0) { // echo the msg if no interval
                            con->sendMsg(msg);
                            send++;
                        }
                        recved ++;
                    });
                    con->onState([&, i](const TcpConnPtr &con) {
                        TcpConn::State st = con->getState();
                        if (st == TcpConn::Connected) {
                            connected++;
//                            send ++;
//                            con->sendMsg(msg);
                        } else if (st == TcpConn::Failed || st == TcpConn::Closed) { //连接出错
                            if (st == TcpConn::Closed) { connected--; }
                            retry++;
                        }
                    });
                }

            });
        }
        if (heartbeat_interval) {
            base.runAfter(heartbeat_interval * 1000, [&] {
                for (int i = 0; i < heartbeat_interval; i ++) {
                    base.runAfter(i*1000, [&,i]{
                        size_t block = allConns.size() / heartbeat_interval;
                        for (size_t j=i*block; j<(i+1)*block && j<allConns.size(); j++) {
                            if (allConns[i]->getState() == TcpConn::Connected) {
                                allConns[i]->sendMsg(msg);
                                send++;
                            }
                        }
                    });
                }
            }, heartbeat_interval * 1000);
        }
        TcpConnPtr report = TcpConn::createConnection(&base, "127.0.0.1", man_port, 3000);
        report->onMsg(new LineCodec, [&](const TcpConnPtr& con, Slice msg) {
            if (msg == "exit") {
                info("recv exit msg from master, so exit");
                base.exit();
            }
        });
        report->onState([&](const TcpConnPtr& con) {
            if (con->getState() == TcpConn::Closed) {
                base.exit();
            }
        });
        base.runAfter(2000, [&]() {
            report->sendMsg(util::format("%d connected: %ld retry: %ld send: %ld recved: %ld",
                                         getpid(), connected, retry, send, recved));
        }, 100);
        base.loop();
    } else { // master process
        map<int, Report> subs;
        TcpServerPtr master = TcpServer::startServer(&base, "127.0.0.1", man_port);
        master->onConnMsg(new LineCodec, [&](const TcpConnPtr& con, Slice msg) {
            auto fs = msg.split(' ');
            if (fs.size() != 9) {
                error("number of fields is %lu expected 7", fs.size());
                return;
            }
            Report& c = subs[atoi(fs[0].data())];
            c.connected = atoi(fs[2].data());
            c.retry= atoi(fs[4].data());
            c.sended = atoi(fs[6].data());
            c.recved = atoi(fs[8].data());
        });
        base.runAfter(3000, [&](){
            for(auto& s: subs) {
                Report& r = s.second;
                printf("pid: %6ld connected %6ld retry %6ld sended %6ld recved %6ld\n", (long)s.first, r.connected, r.retry, r.sended, r.recved);
            }
            printf("\n");
        }, 3000);
        base.loop();
    }
    info("program exited");
}
<|endoftext|>


<commit_before>/** My first Galois program -*- C++ -*-
 */
#include "Galois/Galois.h"
#include "Galois/Graph/Graph.h"
#include <boost/iterator/counting_iterator.hpp>
#include <iostream>
#include <string>

using namespace std;

//typedef Galois::Graph::FirstGraph<std::string,std::string,true> Graph;

typedef enum{pi,po,nd} node_type;

struct Node {
 string label_type;//a0,b0...
 node_type type;// pi,po,nd
 int fanins;
 bool fanout;
 int level;
 };

typedef Galois::Graph::FirstGraph<Node,int,true> Graph;

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

  /*
   * Subgraph 1 from Fig.2 in "DAG-Aware AIG Rewriting"
   *
   */
  Graph g;
  Node vn1, vn2, vn3, vn4, vn5, vn6, vn7, vn8;
  Graph::GraphNode n1, n2, n3, n4, n5, n6, n7, n8;

  /*
   * Label vertices
   */

  // primary inputs
  vn1.type = pi; vn2.type = pi; vn3.type = pi;
  vn1.level = 0; vn2.level = 0; vn3.level = 0;
  vn1.label_type = "a0"; vn2.label_type = "a1"; vn3.label_type = "a2";

  // intermediate nodes
  vn4.type = nd; vn5.type = nd; vn6.type = nd;
  vn4.level = 1; vn5.level = 1; vn6.level = 2;
  vn4.label_type = "n0"; vn5.label_type = "n1"; vn6.label_type = "n2";

  // primary output
  vn7.type = po; vn7.level = 3; vn7.label_type = "s0";

  /*
   * Create vertices
   */
  n1 = g.createNode(vn1);
  n2 = g.createNode(vn2);
  n3 = g.createNode(vn3);
  n4 = g.createNode(vn4);
  n5 = g.createNode(vn5);
  n6 = g.createNode(vn6);
  n7 = g.createNode(vn7);

  g.addNode(n1);
  g.addNode(n2);
  g.addNode(n3);
  g.addNode(n4);
  g.addNode(n5);
  g.addNode(n6);
  g.addNode(n7);

  /*
   * Create edges
   */
  g.getEdgeData(g.addEdge(n1, n4)) = 1; // a0->n0
  g.getEdgeData(g.addEdge(n4, n1)) = 3; // n0->a0 (back edge)
  g.getEdgeData(g.addEdge(n1, n5)) = 1; // a0->n1
  g.getEdgeData(g.addEdge(n5, n1)) = 3; // n1->a0 (back edge)

  g.getEdgeData(g.addEdge(n2, n4)) = 1; // a1->n0
  g.getEdgeData(g.addEdge(n4, n2)) = 3; // n0->a1 (back edge)

  g.getEdgeData(g.addEdge(n3, n5)) = 1; // a2->n1
  g.getEdgeData(g.addEdge(n5, n3)) = 3; // n1->a2 (back edge)

  g.getEdgeData(g.addEdge(n4, n6)) = 1; // n0->n2
  g.getEdgeData(g.addEdge(n6, n4)) = 3; // n2->n0 (back edge)

  g.getEdgeData(g.addEdge(n5, n6)) = 1; // n1->n2
  g.getEdgeData(g.addEdge(n6, n5)) = 3; // n2->n1 (back edge)

  g.getEdgeData(g.addEdge(n6, n7)) = 1; // n2->s0
  g.getEdgeData(g.addEdge(n7, n6)) = 3; // s0->n2 (back edge)

  // print graph
  for (Graph::iterator ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    Graph::GraphNode src = *ii;
    cout << "node: " << g.getData(src).label_type;
    cout << endl;
    for (Graph::edge_iterator jj = g.edge_begin(src), ej = g.edge_end(src); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int e = g.getEdgeData(jj);
      if ( e == 1 )
        cout << "Forward noninverted edge to ";
      else if ( e == 2 )
        cout << "Forward inverted edge to ";
      else
        cout << "Back edge to ";

      cout << g.getData(dst).label_type << endl;
      //cout << "edge weight: " << g.getEdgeData(jj);
      //cout << endl;
    }
    cout << endl;
  }
  cout << endl;

  /*
   *  Algorithm
   */

  cout << "Begin Algorithm:\n";

  for (Graph::iterator ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    Graph::GraphNode top = *ii; // node we are attempting to build the cut from (top of the pyramid)
    Graph::GraphNode left = NULL, right = NULL, input1 = NULL, input2 = NULL, input3 = NULL, input4 = NULL;
    Graph::GraphNode output = NULL;
    cout << "Checking node " << g.getData(top).label_type << endl;
    if ( g.getData(top).type != nd ) {
      cout << "Node is not nd type\n";
      nextcut:
      cout << "Moving to next node\n\n";
      continue; } // move to the next node if a node is a primary input or output

    cout << "Checking edges for top node\n";
    for (Graph::edge_iterator jj = g.edge_begin(top), ej = g.edge_end(top); jj != ej; ++jj) {
      // look for the back edges and follow them back to the input nodes to generate our cut
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( ew == 3 ) {
        if ( g.getData(dst).type != nd ) {
          cout << "Left or right node is not nd type\n";
          goto nextcut; }// bail out if we find a primary input or output
        else if ( left == NULL ) {
          left = dst; cout << "left = " << g.getData(left).label_type << endl; }
        else if ( right == NULL ) {
          right = dst; cout << "right = " << g.getData(right).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(top).label_type << " has more than two inputs\n"; exit; }
      }
      else { // build list of outputs
        }
    }
    if ( left == NULL || right == NULL ) {
      cerr << "Error: node " << g.getData(top).label_type << " does not have two inputs\n"; exit; }

    //cout << "Found nodes " << g.getData(left).label_type << " and " << g.getData(right).label_type << endl;

    int left_edge, right_edge;
    // find the edges leading back to the top node and the input edges
    cout << "Checking edges for left node\n";
    for (Graph::edge_iterator jj = g.edge_begin(left), ej = g.edge_end(left); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( dst == top ) {
        left_edge = ew; cout << "left_edge = " << left_edge << endl; }
      else if ( ew == 3 ) {
        if ( input1 == NULL ) {
          input1 = dst; cout << "input1 = " << g.getData(input1).label_type << endl; }
        else if ( input2 == NULL ) {
          input2 = dst; cout << "input2 = " << g.getData(input2).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(left).label_type << " has more than two inputs\n"; exit; }
      }
      else {
        goto nextcut; }

    }
    cout << "Checking edges for right node\n";
    for (Graph::edge_iterator jj = g.edge_begin(right), ej = g.edge_end(right); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( dst == top ) {
        right_edge = ew; cout << "right_edge = " << left_edge << endl; }
      else if ( ew == 3 ) {
        if ( input3 == NULL ) {
          input3 = dst; cout << "input3 = " << g.getData(input3).label_type << endl; }
        else if ( input4 == NULL ) {
          input4 = dst; cout << "input4 = " << g.getData(input4).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(right).label_type << " has more than two inputs\n"; exit; }
      }
      else {
        goto nextcut; }
    }

    cout << "\nCut:\n";
    cout << "Output(s) = ";
    cout << "Top = " << g.getData(top).label_type << endl;
    cout << "Left = " << g.getData(left).label_type << endl;
    cout << "Right = " << g.getData(left).label_type << endl;
    cout << "Left_edge = " << left_edge << endl;
    cout << "Right_edge = " << right_edge << endl;
    cout << "Input1 = " << g.getData(input1).label_type << endl;
    cout << "Input2 = " << g.getData(input2).label_type << endl;
    cout << "Input3 = " << g.getData(input3).label_type << endl;
    cout << "Input4 = " << g.getData(input4).label_type << endl;
    cout << endl;
  }
  cout << "Done\n";
  return 0;
}

<commit_msg>Now generates cuts<commit_after>/** My first Galois program -*- C++ -*-
 */
#include "Galois/Galois.h"
#include "Galois/Graph/Graph.h"
#include <boost/iterator/counting_iterator.hpp>
#include <iostream>
#include <string>

using namespace std;

//typedef Galois::Graph::FirstGraph<std::string,std::string,true> Graph;

typedef enum{pi,po,nd} node_type;

struct Node {
 string label_type;//a0,b0...
 node_type type;// pi,po,nd
 int fanins;
 bool fanout;
 int level;
 };

typedef Galois::Graph::FirstGraph<Node,int,true> Graph;

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

  /*
   * Subgraph 1 from Fig.2 in "DAG-Aware AIG Rewriting"
   *
   */
  Graph g;
  Node vn1, vn2, vn3, vn4, vn5, vn6, vn7, vn8;
  Graph::GraphNode n1, n2, n3, n4, n5, n6, n7, n8;

  /*
   * Label vertices
   */

  // primary inputs
  vn1.type = pi; vn2.type = pi; vn3.type = pi;
  vn1.level = 0; vn2.level = 0; vn3.level = 0;
  vn1.label_type = "a0"; vn2.label_type = "a1"; vn3.label_type = "a2";

  // intermediate nodes
  vn4.type = nd; vn5.type = nd; vn6.type = nd;
  vn4.level = 1; vn5.level = 1; vn6.level = 2;
  vn4.label_type = "n0"; vn5.label_type = "n1"; vn6.label_type = "n2";

  // primary output
  vn7.type = po; vn7.level = 3; vn7.label_type = "s0";

  /*
   * Create vertices
   */
  n1 = g.createNode(vn1);
  n2 = g.createNode(vn2);
  n3 = g.createNode(vn3);
  n4 = g.createNode(vn4);
  n5 = g.createNode(vn5);
  n6 = g.createNode(vn6);
  n7 = g.createNode(vn7);

  g.addNode(n1);
  g.addNode(n2);
  g.addNode(n3);
  g.addNode(n4);
  g.addNode(n5);
  g.addNode(n6);
  g.addNode(n7);

  /*
   * Create edges
   */
  g.getEdgeData(g.addEdge(n1, n4)) = 1; // a0->n0
  g.getEdgeData(g.addEdge(n4, n1)) = 3; // n0->a0 (back edge)
  g.getEdgeData(g.addEdge(n1, n5)) = 1; // a0->n1
  g.getEdgeData(g.addEdge(n5, n1)) = 3; // n1->a0 (back edge)

  g.getEdgeData(g.addEdge(n2, n4)) = 1; // a1->n0
  g.getEdgeData(g.addEdge(n4, n2)) = 3; // n0->a1 (back edge)

  g.getEdgeData(g.addEdge(n3, n5)) = 1; // a2->n1
  g.getEdgeData(g.addEdge(n5, n3)) = 3; // n1->a2 (back edge)

  g.getEdgeData(g.addEdge(n4, n6)) = 1; // n0->n2
  g.getEdgeData(g.addEdge(n6, n4)) = 3; // n2->n0 (back edge)

  g.getEdgeData(g.addEdge(n5, n6)) = 1; // n1->n2
  g.getEdgeData(g.addEdge(n6, n5)) = 3; // n2->n1 (back edge)

  g.getEdgeData(g.addEdge(n6, n7)) = 1; // n2->s0
  g.getEdgeData(g.addEdge(n7, n6)) = 3; // s0->n2 (back edge)

  // print graph
  for (Graph::iterator ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    Graph::GraphNode src = *ii;
    cout << "node: " << g.getData(src).label_type;
    cout << endl;
    for (Graph::edge_iterator jj = g.edge_begin(src), ej = g.edge_end(src); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int e = g.getEdgeData(jj);
      if ( e == 1 )
        cout << "Forward noninverted edge to ";
      else if ( e == 2 )
        cout << "Forward inverted edge to ";
      else
        cout << "Back edge to ";

      cout << g.getData(dst).label_type << endl;
      //cout << "edge weight: " << g.getEdgeData(jj);
      //cout << endl;
    }
    cout << endl;
  }
  cout << endl;

  /*
   *  Algorithm
   */

  cout << "Begin Algorithm:\n";

  for (Graph::iterator ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    Graph::GraphNode top = *ii; // node we are attempting to build the cut from (top of the pyramid)
    Graph::GraphNode left = NULL, right = NULL, input1 = NULL, input2 = NULL, input3 = NULL, input4 = NULL;
    Graph::GraphNode output = NULL;
    cout << "Checking node " << g.getData(top).label_type << endl;
    if ( g.getData(top).type != nd ) {
      cout << "Node is not nd type\n";
      nextcut:
      cout << "Moving to next node\n\n";
      continue; } // move to the next node if a node is a primary input or output

    cout << "Checking edges for top node\n";
    for (Graph::edge_iterator jj = g.edge_begin(top), ej = g.edge_end(top); jj != ej; ++jj) {
      // look for the back edges and follow them back to the input nodes to generate our cut
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( ew == 3 ) {
        if ( g.getData(dst).type != nd ) {
          cout << "Left or right node is not nd type\n";
          goto nextcut; }// bail out if we find a primary input or output
        else if ( left == NULL ) {
          left = dst; cout << "left = " << g.getData(left).label_type << endl; }
        else if ( right == NULL ) {
          right = dst; cout << "right = " << g.getData(right).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(top).label_type << " has more than two inputs\n"; exit; }
      }
      else { // build list of outputs
        }
    }
    if ( left == NULL || right == NULL ) {
      cerr << "Error: node " << g.getData(top).label_type << " does not have two inputs\n"; exit; }

    //cout << "Found nodes " << g.getData(left).label_type << " and " << g.getData(right).label_type << endl;

    int left_edge, right_edge;
    // find the edges leading back to the top node and the input edges
    cout << "Checking edges for left node\n";
    for (Graph::edge_iterator jj = g.edge_begin(left), ej = g.edge_end(left); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( dst == top ) {
        left_edge = ew; cout << "left_edge = " << left_edge << endl; }
      else if ( ew == 3 ) {
        if ( input1 == NULL ) {
          input1 = dst; cout << "input1 = " << g.getData(input1).label_type << endl; }
        else if ( input2 == NULL ) {
          input2 = dst; cout << "input2 = " << g.getData(input2).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(left).label_type << " has more than two inputs\n"; exit; }
      }
      else {
        goto nextcut; }

    }
    cout << "Checking edges for right node\n";
    for (Graph::edge_iterator jj = g.edge_begin(right), ej = g.edge_end(right); jj != ej; ++jj) {
      Graph::GraphNode dst = g.getEdgeDst(jj);
      int ew = g.getEdgeData(jj);
      cout << "Found edge of weight " << ew << " to node " << g.getData(dst).label_type << endl;
      if ( dst == top ) {
        right_edge = ew; cout << "right_edge = " << left_edge << endl; }
      else if ( ew == 3 ) {
        if ( input3 == NULL ) {
          input3 = dst; cout << "input3 = " << g.getData(input3).label_type << endl; }
        else if ( input4 == NULL ) {
          input4 = dst; cout << "input4 = " << g.getData(input4).label_type << endl; }
        else {
          cerr << "Error: node " << g.getData(right).label_type << " has more than two inputs\n"; exit; }
      }
      else {
        goto nextcut; }
    }

    cout << "\nCut:\n";
    cout << "Output(s) = "; cout << endl;
    cout << "Top = " << g.getData(top).label_type << endl;
    cout << "Left = " << g.getData(left).label_type << endl;
    cout << "Right = " << g.getData(left).label_type << endl;
    cout << "Left_edge = " << left_edge << endl;
    cout << "Right_edge = " << right_edge << endl;
    cout << "Input1 = " << g.getData(input1).label_type << endl;
    cout << "Input2 = " << g.getData(input2).label_type << endl;
    cout << "Input3 = " << g.getData(input3).label_type << endl;
    cout << "Input4 = " << g.getData(input4).label_type << endl;
    cout << endl;
  }
  cout << "Done\n";
  return 0;
}

<|endoftext|>


<commit_before>#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cstdarg>
#include <cerrno>
#include <vector>
#include <map>
#include <string>

#include <unistd.h>

#include "riscv-types.h"
#include "riscv-endian.h"
#include "riscv-regs.h"
#include "riscv-processor.h"
#include "riscv-format.h"
#include "riscv-opcodes.h"
#include "riscv-util.h"
#include "riscv-imm.h"
#include "riscv-decode.h"
#include "riscv-decode-switch.h"
#include "riscv-csr.h"
#include "riscv-compression.h"
#include "riscv-elf.h"
#include "riscv-elf-file.h"
#include "riscv-elf-format.h"

void riscv_execute_instruction(riscv_decode &dec, riscv_proc_state *proc)
{
	memset(&dec, 0, sizeof(dec));
	riscv_wu inst = htole16(*(riscv_hu*)proc->pc);
	riscv_wu op1 = inst & 0b11;
	if (op1 == 3) {
		inst |= htole16(*(riscv_hu*)(proc->pc + 2)) << 16;
		dec.sz = 4;
	} else {
		dec.sz = 2;
	}
	riscv_decode_opcode(dec, inst);
	riscv_decode_type(dec, inst);
	switch (dec.op) {
		case riscv_op_addi:
			proc->i_reg[dec.rd].lu = proc->i_reg[dec.rs1].lu + dec.imm;
			proc->pc += dec.sz;
			break;
		case riscv_op_lui:
			proc->i_reg[dec.rd].lu = dec.imm;
			proc->pc += dec.sz;
			break;
		case riscv_op_scall:
			switch (proc->i_reg[riscv_ireg_a7].lu) {
				case 64: /* sys_write */
					proc->i_reg[riscv_ireg_a0].lu = write(proc->i_reg[riscv_ireg_a0].lu,
						proc->mem + proc->i_reg[riscv_ireg_a1].lu - proc->vaddr, proc->i_reg[riscv_ireg_a2].lu);
					break;
				case 93: /* sys_exit */
					exit(proc->i_reg[riscv_ireg_a0].lu);
					break;
				default:
					panic("illegal syscall: %d", proc->i_reg[riscv_ireg_a7].lu);
			}
			proc->pc += dec.sz;
			break;
		default:
			panic("illegal instruciton: %s", riscv_instruction_name[dec.op]);
	}
}

void run_rv64(riscv_ptr mem, riscv_lu vaddr, riscv_lu entry)
{
	riscv_decode dec;
	riscv_proc_state proc = { 0 };
	proc.p_type = riscv_proc_type_rv64i;
	proc.mem = mem;
	proc.vaddr = vaddr;
	proc.pc = mem + entry - vaddr;
	while (true) {
		riscv_execute_instruction(dec, &proc);
	}
}

int main(int argc, char *argv[])
{
	if (argc != 2) panic("usage: %s <elf_file>", argv[0]);
	elf_file elf(argv[1]);
	for (size_t i = 0; i < elf.phdrs.size(); i++) {
		Elf64_Phdr &phdr = elf.phdrs[i];
		if (phdr.p_flags & PT_LOAD) {
			run_rv64(elf.buf.data(), phdr.p_vaddr, elf.ehdr.e_entry);
			break;
		}
	}
	panic("could not execute");
	return 0;
}
<commit_msg>Change instruction word size to riscv_lu (uint64_t)<commit_after>#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cstdarg>
#include <cerrno>
#include <vector>
#include <map>
#include <string>

#include <unistd.h>

#include "riscv-types.h"
#include "riscv-endian.h"
#include "riscv-regs.h"
#include "riscv-processor.h"
#include "riscv-format.h"
#include "riscv-opcodes.h"
#include "riscv-util.h"
#include "riscv-imm.h"
#include "riscv-decode.h"
#include "riscv-decode-switch.h"
#include "riscv-csr.h"
#include "riscv-compression.h"
#include "riscv-elf.h"
#include "riscv-elf-file.h"
#include "riscv-elf-format.h"

void riscv_execute_instruction(riscv_decode &dec, riscv_proc_state *proc)
{
	memset(&dec, 0, sizeof(dec));
	riscv_lu inst = htole16(*(riscv_hu*)proc->pc);
	riscv_wu op1 = inst & 0b11;
	if (op1 == 3) {
		inst |= htole16(*(riscv_hu*)(proc->pc + 2)) << 16;
		dec.sz = 4;
	} else {
		dec.sz = 2;
	}
	riscv_decode_opcode(dec, inst);
	riscv_decode_type(dec, inst);
	switch (dec.op) {
		case riscv_op_addi:
			proc->i_reg[dec.rd].lu = proc->i_reg[dec.rs1].lu + dec.imm;
			proc->pc += dec.sz;
			break;
		case riscv_op_lui:
			proc->i_reg[dec.rd].lu = dec.imm;
			proc->pc += dec.sz;
			break;
		case riscv_op_scall:
			switch (proc->i_reg[riscv_ireg_a7].lu) {
				case 64: /* sys_write */
					proc->i_reg[riscv_ireg_a0].lu = write(proc->i_reg[riscv_ireg_a0].lu,
						proc->mem + proc->i_reg[riscv_ireg_a1].lu - proc->vaddr, proc->i_reg[riscv_ireg_a2].lu);
					break;
				case 93: /* sys_exit */
					exit(proc->i_reg[riscv_ireg_a0].lu);
					break;
				default:
					panic("illegal syscall: %d", proc->i_reg[riscv_ireg_a7].lu);
			}
			proc->pc += dec.sz;
			break;
		default:
			panic("illegal instruciton: %s", riscv_instruction_name[dec.op]);
	}
}

void run_rv64(riscv_ptr mem, riscv_lu vaddr, riscv_lu entry)
{
	riscv_decode dec;
	riscv_proc_state proc = { 0 };
	proc.p_type = riscv_proc_type_rv64i;
	proc.mem = mem;
	proc.vaddr = vaddr;
	proc.pc = mem + entry - vaddr;
	while (true) {
		riscv_execute_instruction(dec, &proc);
	}
}

int main(int argc, char *argv[])
{
	if (argc != 2) panic("usage: %s <elf_file>", argv[0]);
	elf_file elf(argv[1]);
	for (size_t i = 0; i < elf.phdrs.size(); i++) {
		Elf64_Phdr &phdr = elf.phdrs[i];
		if (phdr.p_flags & PT_LOAD) {
			run_rv64(elf.buf.data(), phdr.p_vaddr, elf.ehdr.e_entry);
			break;
		}
	}
	panic("could not execute");
	return 0;
}
<|endoftext|>


<commit_before>//
//  riscv-test-emulate.cc
//

#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cinttypes>
#include <cstdarg>
#include <cerrno>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <deque>
#include <map>
#include <set>

#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/stat.h>

#include "riscv-types.h"
#include "riscv-endian.h"
#include "riscv-format.h"
#include "riscv-meta.h"
#include "riscv-util.h"
#include "riscv-cmdline.h"
#include "riscv-codec.h"
#include "riscv-elf.h"
#include "riscv-elf-file.h"
#include "riscv-elf-format.h"
#include "riscv-strings.h"
#include "riscv-disasm.h"
#include "riscv-processor.h"
#include "riscv-abi.h"
#include "riscv-proxy.h"

inline float f32_sqrt(float a) { return std::sqrt(a); }
inline double f64_sqrt(double a) { return std::sqrt(a); }
inline float f32_classify(float a) { return 0; } /* unimplemented */
inline double f64_classify(double a) { return 0; } /* unimplemented */

namespace riscv {
	#include "riscv-interp.h"
}

using namespace riscv;

struct riscv_proc_proxy_rv64 : riscv_processor_rv64
{
	uintptr_t heap_begin;
	uintptr_t heap_end;
	std::vector<std::pair<void*,size_t>> mapped_segments;

	riscv_proc_proxy_rv64() : riscv_processor_rv64(), heap_begin(0), heap_end(0) {}
};

struct riscv_emulator
{
	const size_t stack_top = 0x40000000;
	const size_t stack_size = 0x10000;

	elf_file elf;
	std::string filename;

	bool memory_debug = false;
	bool emulator_debug = false;
	bool log_registers = false;
	bool log_instructions = false;
	bool help_or_error = false;

	/*
		This simple proof of concept machine emulator uses a
		machine generated interpreter in src/asm/riscv-interp.h
		created by parse-meta using C-psuedo code in meta/instructions
	*/

	inline static const int elf_p_flags_mmap(int v)
	{
		int prot = 0;
		if (v & PF_X) prot |= PROT_EXEC;
		if (v & PF_W) prot |= PROT_WRITE;
		if (v & PF_R) prot |= PROT_READ;
		return prot;
	}

	template <typename P>
	void print_registers(P &proc)
	{
		for (size_t i = 0; i < 32; i++) {
			char fmt[32];
			snprintf(fmt, sizeof(fmt), "%%-4s: 0x%%0%ux%%s", (P::xlen >> 2));
			printf(fmt, riscv_ireg_name_sym[i], proc.ireg[i], (i + 1) % 4 == 0 ? "\n" : " ");
		}
	}

	std::string format_inst(uintptr_t pc)
	{
		char buf[20];
		uintptr_t inst_length;
		uint64_t inst = riscv_inst_fetch(pc, &inst_length);
		switch (inst_length) {
			case 2:  snprintf(buf, sizeof(buf), "0x%04tx", inst); break;
			case 4:  snprintf(buf, sizeof(buf), "0x%08tx", inst); break;
			case 6:  snprintf(buf, sizeof(buf), "0x%012tx", inst); break;
			case 8:  snprintf(buf, sizeof(buf), "0x%016tx", inst); break;
			default: snprintf(buf, sizeof(buf), "(invalid)"); break;
		}
		return buf;
	}

	template <typename T, typename P>
	void print_disassembly(T &dec, P &proc)
	{
		std::string args = riscv_disasm_inst_simple(dec);
		printf("core %3zu: 0x%016tx (%s) %-30s\n", proc.hart_id, proc.pc, format_inst(proc.pc).c_str(), args.c_str());
	}

	// Simple RV64 Linux syscall emulation (write, exit)
	template <typename P>
	void emulate_ecall(riscv_decode &dec, P &proc, uintptr_t inst_length)
	{
		switch (proc.ireg[riscv_ireg_a7]) {
			case riscv_syscall_close:  riscv_sys_close(proc); break;
			case riscv_syscall_write:  riscv_sys_write(proc); break;
			case riscv_syscall_fstat:  riscv_sys_fstat(proc); break;
			case riscv_syscall_exit:   riscv_sys_exit(proc);  break;
			case riscv_syscall_brk:    riscv_sys_brk(proc);   break;
			default: panic("unknown syscall: %d", proc.ireg[riscv_ireg_a7]);
		}
		proc.pc += inst_length;
	}

	// Simple RV64 emulator main loop
	template <typename P>
	void rv64_run(P &proc)
	{
		riscv_decode dec;
		size_t inst_length;
		uint64_t inst;
		while (true) {
			inst = riscv_inst_fetch(proc.pc, &inst_length);
			riscv_decode_inst_rv64(dec, inst);
			if (log_registers) print_registers(proc);
			if (log_instructions) print_disassembly(dec, proc);
			if (riscv::rv64_exec(dec, proc, inst_length)) continue;
			if (dec.op == riscv_op_ecall) emulate_ecall(dec, proc, inst_length);
			else {
				panic("illegal instruciton: pc=0x%" PRIx64 " inst=0x%" PRIx64,
					proc.pc, inst);
			}
		}
	}

	// Simple code to map a single stack segment
	template <typename P>
	void map_stack(P &proc, uintptr_t stack_top, uintptr_t stack_size)
	{
		void *addr = mmap((void*)(stack_top - stack_size), stack_size,
			PROT_READ | PROT_WRITE, MAP_FIXED | MAP_ANONYMOUS | MAP_PRIVATE, -1, 0);
		if (addr == MAP_FAILED) {
			panic("map_stack: error: mmap: %s", strerror(errno));
		}

		// keep track of the mapped segment and set the stack_top
		proc.mapped_segments.push_back(std::pair<void*,size_t>((void*)(stack_top - stack_size), stack_size));
		proc.ireg[riscv_ireg_sp] = stack_top - 0x8;

		if (emulator_debug) {
			debug("sp : mmap: 0x%016" PRIxPTR " - 0x%016" PRIxPTR " +R+W",
				(stack_top - stack_size), stack_top);
		}
	}

	// Simple code currently maps all segments copy-on-write RWX
	template <typename P>
	void map_load_segment(P &proc, const char* filename, Elf64_Phdr &phdr)
	{
		int fd = open(filename, O_RDONLY);
		if (fd < 0) {
			panic("map_executable: error: open: %s: %s", filename, strerror(errno));
		}
		void *addr = mmap((void*)phdr.p_vaddr, phdr.p_memsz,
			elf_p_flags_mmap(phdr.p_flags), MAP_FIXED | MAP_PRIVATE, fd, phdr.p_offset);
		close(fd);
		if (addr == MAP_FAILED) {
			panic("map_executable: error: mmap: %s: %s", filename, strerror(errno));
		}

		// keep track of the mapped segment and set the heap_end
		proc.mapped_segments.push_back(std::pair<void*,size_t>((void*)phdr.p_vaddr, phdr.p_memsz));
		uintptr_t seg_end = uintptr_t(phdr.p_vaddr + phdr.p_memsz);
		if (proc.heap_begin < seg_end) proc.heap_begin = proc.heap_end = seg_end;

		if (emulator_debug) {
			debug("elf: mmap: 0x%016" PRIxPTR " - 0x%016" PRIxPTR " %s",
				uintptr_t(phdr.p_vaddr), seg_end, elf_p_flags_name(phdr.p_flags).c_str());
		}
	}

	void parse_commandline(int argc, const char *argv[])
	{
		cmdline_option options[] =
		{
			{ "-m", "--memory-debug", cmdline_arg_type_none,
				"Memory debug",
				[&](std::string s) { return (memory_debug = true); } },
			{ "-d", "--emulator-debug", cmdline_arg_type_none,
				"Emulator debug",
				[&](std::string s) { return (emulator_debug = true); } },
			{ "-r", "--log-registers", cmdline_arg_type_none,
				"Log Registers",
				[&](std::string s) { return (log_registers = true); } },
			{ "-l", "--log-instructions", cmdline_arg_type_none,
				"Log Instructions",
				[&](std::string s) { return (log_instructions = true); } },
			{ "-h", "--help", cmdline_arg_type_none,
				"Show help",
				[&](std::string s) { return (help_or_error = true); } },
			{ nullptr, nullptr, cmdline_arg_type_none,   nullptr, nullptr }
		};

		auto result = cmdline_option::process_options(options, argc, argv);
		if (!result.second) {
			help_or_error = true;
		} else if (result.first.size() != 1) {
			printf("%s: wrong number of arguments\n", argv[0]);
			help_or_error = true;
		}

		if (help_or_error)
		{
			printf("usage: %s [<options>] <elf_file>\n", argv[0]);
			cmdline_option::print_options(options);
			exit(9);
		}

		filename = result.first[0];
	}

	// print approximate location of host text, heap and stack
	void memory_info(int argc, const char *argv[])
	{
		static const char *textptr = nullptr;
		void *heapptr = malloc(8);
		debug("text : ~0x%016" PRIxPTR, (uintptr_t)&textptr);
		debug("heap : ~0x%016" PRIxPTR, (uintptr_t)heapptr);
		debug("stack: ~0x%016" PRIxPTR, (uintptr_t)argv);
		free(heapptr);
	}

	void exec(int argc, const char *argv[])
	{
		// print process information
		if (memory_debug) {
			memory_info(argc, argv);
		}

		// load ELF (headers only)
		elf.load(filename, true);

		// Processor
		riscv_proc_proxy_rv64 proc;
		proc.flags = emulator_debug ? riscv_processor_flag_emulator_debug : 0;

		// Find the PT_LOAD segments and mmap then into memory
		for (size_t i = 0; i < elf.phdrs.size(); i++) {
			Elf64_Phdr &phdr = elf.phdrs[i];
			if (phdr.p_flags & PT_LOAD) {
				map_load_segment(proc, filename.c_str(), phdr);
			}
		}

		// Map a stack and set the stack pointer
		map_stack(proc, stack_top, stack_size);

		// Set the program counter to the entry address
		proc.pc = elf.ehdr.e_entry;

		// Start the emulator
		rv64_run(proc);

		// Unmap segments
		for (auto &seg: proc.mapped_segments) {
			munmap(seg.first, seg.second);
		}
	}
};

int main(int argc, const char *argv[])
{
	riscv_emulator emulator;
	emulator.parse_commandline(argc, argv);
	emulator.exec(argc, argv);
	return 0;
}
<commit_msg>Change test-emulate heap size and stack top<commit_after>//
//  riscv-test-emulate.cc
//

#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cinttypes>
#include <cstdarg>
#include <cerrno>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <deque>
#include <map>
#include <set>

#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/stat.h>

#include "riscv-types.h"
#include "riscv-endian.h"
#include "riscv-format.h"
#include "riscv-meta.h"
#include "riscv-util.h"
#include "riscv-cmdline.h"
#include "riscv-codec.h"
#include "riscv-elf.h"
#include "riscv-elf-file.h"
#include "riscv-elf-format.h"
#include "riscv-strings.h"
#include "riscv-disasm.h"
#include "riscv-processor.h"
#include "riscv-abi.h"
#include "riscv-proxy.h"

inline float f32_sqrt(float a) { return std::sqrt(a); }
inline double f64_sqrt(double a) { return std::sqrt(a); }
inline float f32_classify(float a) { return 0; } /* unimplemented */
inline double f64_classify(double a) { return 0; } /* unimplemented */

namespace riscv {
	#include "riscv-interp.h"
}

using namespace riscv;

struct riscv_proc_proxy_rv64 : riscv_processor_rv64
{
	uintptr_t heap_begin;
	uintptr_t heap_end;
	std::vector<std::pair<void*,size_t>> mapped_segments;

	riscv_proc_proxy_rv64() : riscv_processor_rv64(), heap_begin(0), heap_end(0) {}
};

struct riscv_emulator
{
	const size_t stack_top =  0x78000000; // 1920 MiB
	const size_t stack_size = 0x01000000; //   16 MiB

	elf_file elf;
	std::string filename;

	bool memory_debug = false;
	bool emulator_debug = false;
	bool log_registers = false;
	bool log_instructions = false;
	bool help_or_error = false;

	/*
		This simple proof of concept machine emulator uses a
		machine generated interpreter in src/asm/riscv-interp.h
		created by parse-meta using C-psuedo code in meta/instructions
	*/

	inline static const int elf_p_flags_mmap(int v)
	{
		int prot = 0;
		if (v & PF_X) prot |= PROT_EXEC;
		if (v & PF_W) prot |= PROT_WRITE;
		if (v & PF_R) prot |= PROT_READ;
		return prot;
	}

	template <typename P>
	void print_registers(P &proc)
	{
		for (size_t i = 0; i < 32; i++) {
			char fmt[32];
			snprintf(fmt, sizeof(fmt), "%%-4s: 0x%%0%ux%%s", (P::xlen >> 2));
			printf(fmt, riscv_ireg_name_sym[i], proc.ireg[i], (i + 1) % 4 == 0 ? "\n" : " ");
		}
	}

	std::string format_inst(uintptr_t pc)
	{
		char buf[20];
		uintptr_t inst_length;
		uint64_t inst = riscv_inst_fetch(pc, &inst_length);
		switch (inst_length) {
			case 2:  snprintf(buf, sizeof(buf), "0x%04tx", inst); break;
			case 4:  snprintf(buf, sizeof(buf), "0x%08tx", inst); break;
			case 6:  snprintf(buf, sizeof(buf), "0x%012tx", inst); break;
			case 8:  snprintf(buf, sizeof(buf), "0x%016tx", inst); break;
			default: snprintf(buf, sizeof(buf), "(invalid)"); break;
		}
		return buf;
	}

	template <typename T, typename P>
	void print_disassembly(T &dec, P &proc)
	{
		std::string args = riscv_disasm_inst_simple(dec);
		printf("core %3zu: 0x%016tx (%s) %-30s\n", proc.hart_id, proc.pc, format_inst(proc.pc).c_str(), args.c_str());
	}

	// Simple RV64 Linux syscall emulation (write, exit)
	template <typename P>
	void emulate_ecall(riscv_decode &dec, P &proc, uintptr_t inst_length)
	{
		switch (proc.ireg[riscv_ireg_a7]) {
			case riscv_syscall_close:  riscv_sys_close(proc); break;
			case riscv_syscall_write:  riscv_sys_write(proc); break;
			case riscv_syscall_fstat:  riscv_sys_fstat(proc); break;
			case riscv_syscall_exit:   riscv_sys_exit(proc);  break;
			case riscv_syscall_brk:    riscv_sys_brk(proc);   break;
			default: panic("unknown syscall: %d", proc.ireg[riscv_ireg_a7]);
		}
		proc.pc += inst_length;
	}

	// Simple RV64 emulator main loop
	template <typename P>
	void rv64_run(P &proc)
	{
		riscv_decode dec;
		size_t inst_length;
		uint64_t inst;
		while (true) {
			inst = riscv_inst_fetch(proc.pc, &inst_length);
			riscv_decode_inst_rv64(dec, inst);
			if (log_registers) print_registers(proc);
			if (log_instructions) print_disassembly(dec, proc);
			if (riscv::rv64_exec(dec, proc, inst_length)) continue;
			if (dec.op == riscv_op_ecall) emulate_ecall(dec, proc, inst_length);
			else {
				panic("illegal instruciton: pc=0x%" PRIx64 " inst=0x%" PRIx64,
					proc.pc, inst);
			}
		}
	}

	// Simple code to map a single stack segment
	template <typename P>
	void map_stack(P &proc, uintptr_t stack_top, uintptr_t stack_size)
	{
		void *addr = mmap((void*)(stack_top - stack_size), stack_size,
			PROT_READ | PROT_WRITE, MAP_FIXED | MAP_ANONYMOUS | MAP_PRIVATE, -1, 0);
		if (addr == MAP_FAILED) {
			panic("map_stack: error: mmap: %s", strerror(errno));
		}

		// keep track of the mapped segment and set the stack_top
		proc.mapped_segments.push_back(std::pair<void*,size_t>((void*)(stack_top - stack_size), stack_size));
		proc.ireg[riscv_ireg_sp] = stack_top - 0x8;

		if (emulator_debug) {
			debug("sp : mmap: 0x%016" PRIxPTR " - 0x%016" PRIxPTR " +R+W",
				(stack_top - stack_size), stack_top);
		}
	}

	// Simple code currently maps all segments copy-on-write RWX
	template <typename P>
	void map_load_segment(P &proc, const char* filename, Elf64_Phdr &phdr)
	{
		int fd = open(filename, O_RDONLY);
		if (fd < 0) {
			panic("map_executable: error: open: %s: %s", filename, strerror(errno));
		}
		void *addr = mmap((void*)phdr.p_vaddr, phdr.p_memsz,
			elf_p_flags_mmap(phdr.p_flags), MAP_FIXED | MAP_PRIVATE, fd, phdr.p_offset);
		close(fd);
		if (addr == MAP_FAILED) {
			panic("map_executable: error: mmap: %s: %s", filename, strerror(errno));
		}

		// keep track of the mapped segment and set the heap_end
		proc.mapped_segments.push_back(std::pair<void*,size_t>((void*)phdr.p_vaddr, phdr.p_memsz));
		uintptr_t seg_end = uintptr_t(phdr.p_vaddr + phdr.p_memsz);
		if (proc.heap_begin < seg_end) proc.heap_begin = proc.heap_end = seg_end;

		if (emulator_debug) {
			debug("elf: mmap: 0x%016" PRIxPTR " - 0x%016" PRIxPTR " %s",
				uintptr_t(phdr.p_vaddr), seg_end, elf_p_flags_name(phdr.p_flags).c_str());
		}
	}

	void parse_commandline(int argc, const char *argv[])
	{
		cmdline_option options[] =
		{
			{ "-m", "--memory-debug", cmdline_arg_type_none,
				"Memory debug",
				[&](std::string s) { return (memory_debug = true); } },
			{ "-d", "--emulator-debug", cmdline_arg_type_none,
				"Emulator debug",
				[&](std::string s) { return (emulator_debug = true); } },
			{ "-r", "--log-registers", cmdline_arg_type_none,
				"Log Registers",
				[&](std::string s) { return (log_registers = true); } },
			{ "-l", "--log-instructions", cmdline_arg_type_none,
				"Log Instructions",
				[&](std::string s) { return (log_instructions = true); } },
			{ "-h", "--help", cmdline_arg_type_none,
				"Show help",
				[&](std::string s) { return (help_or_error = true); } },
			{ nullptr, nullptr, cmdline_arg_type_none,   nullptr, nullptr }
		};

		auto result = cmdline_option::process_options(options, argc, argv);
		if (!result.second) {
			help_or_error = true;
		} else if (result.first.size() != 1) {
			printf("%s: wrong number of arguments\n", argv[0]);
			help_or_error = true;
		}

		if (help_or_error)
		{
			printf("usage: %s [<options>] <elf_file>\n", argv[0]);
			cmdline_option::print_options(options);
			exit(9);
		}

		filename = result.first[0];
	}

	// print approximate location of host text, heap and stack
	void memory_info(int argc, const char *argv[])
	{
		static const char *textptr = nullptr;
		void *heapptr = malloc(8);
		debug("text : ~0x%016" PRIxPTR, (uintptr_t)&textptr);
		debug("heap : ~0x%016" PRIxPTR, (uintptr_t)heapptr);
		debug("stack: ~0x%016" PRIxPTR, (uintptr_t)argv);
		free(heapptr);
	}

	void exec(int argc, const char *argv[])
	{
		// print process information
		if (memory_debug) {
			memory_info(argc, argv);
		}

		// load ELF (headers only)
		elf.load(filename, true);

		// Processor
		riscv_proc_proxy_rv64 proc;
		proc.flags = emulator_debug ? riscv_processor_flag_emulator_debug : 0;

		// Find the PT_LOAD segments and mmap then into memory
		for (size_t i = 0; i < elf.phdrs.size(); i++) {
			Elf64_Phdr &phdr = elf.phdrs[i];
			if (phdr.p_flags & PT_LOAD) {
				map_load_segment(proc, filename.c_str(), phdr);
			}
		}

		// Map a stack and set the stack pointer
		map_stack(proc, stack_top, stack_size);

		// Set the program counter to the entry address
		proc.pc = elf.ehdr.e_entry;

		// Start the emulator
		rv64_run(proc);

		// Unmap segments
		for (auto &seg: proc.mapped_segments) {
			munmap(seg.first, seg.second);
		}
	}
};

int main(int argc, const char *argv[])
{
	riscv_emulator emulator;
	emulator.parse_commandline(argc, argv);
	emulator.exec(argc, argv);
	return 0;
}
<|endoftext|>


<commit_before>#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <SPI.h>
#include <SD.h>

#include "session.h"
#include "bits.h"

void init_session_header(uint8_t* buf, uint64_t timestamp, uint16_t channels)
{
	buf[0] = 0xec; // Magic #1.
	buf[1] = 0x09; // Magic #2.
	buf[2] = 0x00; // Format epoch.
	buf[3] = 0x00; // Unused 1.
	buf[4] = 0x00; // Unused 2.
	buf[5] = 0x00; // Unused 3.
	uint16_t be_channels = swap_endian_16(channels);
	memcpy(&(buf[6]), &be_channels, 2);
	// Last eight bytes are for the timestamp.
	// XXX: this assumes we're compiled little-endian. Better to make it
	//      endianness-agnostic.
	uint64_t be_ts = swap_endian_64(timestamp);
	memcpy(&(buf[8]), &be_ts, 8);
}

int session_write(session *s, uint8_t *buf, size_t len)
{
	int written = s->fh.write(buf, len);
	s->fh.flush();
	return written;
}

// Given a *char of length at least 11, get_fname stores a string of the
// form "ecgses.XXX", where XXX is a hexadecimal integer between 0x0 and
// 0xfff. If no such unused filename exists -1 will be returned,
// otherwise the value of the integer used for the suffix plus one
// (assuming contiguity of session files, this is likely to be valid for
// the next one).
int get_fname(char *fname, int base_suffix)
{
	int suf = base_suffix;
	strncpy(fname, "ecgses.XXX", 10);
	fname[10] = '\0';
	char valid = 0;
	while (!valid && suf < 0x1000) {
		snprintf(fname+7, 4, "%3x", suf);
		if (!SD.exists(fname)) {
			valid = 1;
		}
		suf++;
	}
	return (valid ? suf : -1);
}

session* session_init(uint64_t timestamp, uint16_t channels)
{
	session *s = (session*) malloc(sizeof(session));
	if (s == NULL) {
		return NULL;
	}
	uint8_t buf[SESSION_HEADER_SIZE];
	init_session_header(buf, timestamp, channels);
	char fname[11];
	int next_suffix = get_fname(fname, 0);
	if (next_suffix < 0) {
		#ifdef DEBUG
		Serial.println("Could not find an unused filename.")
		Serial.println("Is the SD card full?")
		#endif
		free(s);
		return NULL;
	}
	s->next_suffix = next_suffix;
	s->init_time = timestamp;
	s->fh = SD.open(fname, FILE_WRITE);
	if (!s->fh) {
		free(s);
		return NULL;
	}
	session_write(s, buf, SESSION_HEADER_SIZE);
	return s;
}

void session_close(session *s)
{
	s->fh.close();
}
<commit_msg>Debug output for session init<commit_after>#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <SPI.h>
#include <SD.h>

#include "session.h"
#include "bits.h"

#include "config.h"

void init_session_header(uint8_t* buf, uint64_t timestamp, uint16_t channels)
{
	buf[0] = 0xec; // Magic #1.
	buf[1] = 0x09; // Magic #2.
	buf[2] = 0x00; // Format epoch.
	buf[3] = 0x00; // Unused 1.
	buf[4] = 0x00; // Unused 2.
	buf[5] = 0x00; // Unused 3.
	uint16_t be_channels = swap_endian_16(channels);
	memcpy(&(buf[6]), &be_channels, 2);
	// Last eight bytes are for the timestamp.
	// XXX: this assumes we're compiled little-endian. Better to make it
	//      endianness-agnostic.
	uint64_t be_ts = swap_endian_64(timestamp);
	memcpy(&(buf[8]), &be_ts, 8);
}

int session_write(session *s, uint8_t *buf, size_t len)
{
	int written = s->fh.write(buf, len);
	s->fh.flush();
	return written;
}

// Given a *char of length at least 11, get_fname stores a string of the
// form "ecgses.XXX", where XXX is a hexadecimal integer between 0x0 and
// 0xfff. If no such unused filename exists -1 will be returned,
// otherwise the value of the integer used for the suffix plus one
// (assuming contiguity of session files, this is likely to be valid for
// the next one).
int get_fname(char *fname, int base_suffix)
{
	int suf = base_suffix;
	strncpy(fname, "ecgses.XXX", 10);
	fname[10] = '\0';
	char valid = 0;
	while (!valid && suf < 0x1000) {
		snprintf(fname+7, 4, "%3x", suf);
		if (!SD.exists(fname)) {
			valid = 1;
		}
		suf++;
	}
	return (valid ? suf : -1);
}

session* session_init(uint64_t timestamp, uint16_t channels)
{
	session *s = (session*) malloc(sizeof(session));
	if (s == NULL) {
		return NULL;
	}
	uint8_t buf[SESSION_HEADER_SIZE];
	init_session_header(buf, timestamp, channels);
	char fname[11];
	int next_suffix = get_fname(fname, 0);
	if (next_suffix < 0) {
		#ifdef DEBUG
		Serial.println("Could not find an unused filename.")
		Serial.println("Is the SD card full?")
		#endif
		free(s);
		return NULL;
	}
	s->next_suffix = next_suffix;
	s->init_time = timestamp;
	s->fh = SD.open(fname, FILE_WRITE);
	if (!s->fh) {
		#ifdef DEBUG
		Serial.print("Could not open file ");
		Serial.print(fname);
		Serial.print(" for writing.");
		#endif
		free(s);
		return NULL;
	}
	session_write(s, buf, SESSION_HEADER_SIZE);
	return s;
}

void session_close(session *s)
{
	s->fh.close();
}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 the Willow Garage 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.
*********************************************************************/

/* Author: Wim Meeussen */

#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>
#include "robot_state_publisher/joint_state_listener.h"

using namespace std;
using namespace ros;
using namespace KDL;
using namespace robot_state_publisher;

// ----------------------------------
// ----- MAIN -----------------------
// ----------------------------------
int main(int argc, char** argv)
{
  // Initialize ros
  ros::init(argc, argv, "robot_state_publisher");
  NodeHandle node;

  // gets the location of the robot description on the parameter server
  string full_param_name;
  node.searchParam("robot_description",full_param_name);
  string robot_desc;

  // constructs a kdl tree from the robot model
  node.param(full_param_name, robot_desc, string());
  Tree tree;
  if (!kdl_parser::treeFromString(robot_desc, tree)){
    ROS_ERROR("Failed to extract kdl tree from xml robot description");
    return -1;
  }

  if (tree.getNrOfSegments() == 0){
    ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
    ros::spin();
  }
  else if (tree.getNrOfSegments() == 1){
    ROS_WARN("Robot state publisher got a tree with only one segment and cannot publish any state to tf");
    ros::spin();
  }
  else{
    JointStateListener state_publisher(tree);
    ros::spin();
  }
  return 0;
}
<commit_msg>fix check for number of links in urdf<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 the Willow Garage 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.
*********************************************************************/

/* Author: Wim Meeussen */

#include <kdl/tree.hpp>
#include <ros/ros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>
#include "robot_state_publisher/joint_state_listener.h"

using namespace std;
using namespace ros;
using namespace KDL;
using namespace robot_state_publisher;

// ----------------------------------
// ----- MAIN -----------------------
// ----------------------------------
int main(int argc, char** argv)
{
  // Initialize ros
  ros::init(argc, argv, "robot_state_publisher");
  NodeHandle node;

  // gets the location of the robot description on the parameter server
  string full_param_name;
  node.searchParam("robot_description",full_param_name);
  string robot_desc;

  // constructs a kdl tree from the robot model
  node.param(full_param_name, robot_desc, string());
  Tree tree;
  if (!kdl_parser::treeFromString(robot_desc, tree)){
    ROS_ERROR("Failed to extract kdl tree from xml robot description");
    return -1;
  }

  if (tree.getNrOfSegments() == 0){
    ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
  }
  else{
    JointStateListener state_publisher(tree);
  }
  ros::spin();
  return 0;
}
<|endoftext|>