<commit_before>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * 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 copyright holders 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.
 *
 * Authors: Gabe Black
 *          Ali Saidi
 */

#include "arch/sparc/intregfile.hh"
#include "base/trace.hh"
#include "sim/serialize.hh"

#include <string.h>

using namespace SparcISA;
using namespace std;

class Checkpoint;

string SparcISA::getIntRegName(RegIndex index)
{
    static std::string intRegName[NumIntArchRegs] =
        {"g0", "g1", "g2", "g3", "g4", "g5", "g6", "g7",
         "o0", "o1", "o2", "o3", "o4", "o5", "o6", "o7",
         "l0", "l1", "l2", "l3", "l4", "l5", "l6", "l7",
         "i0", "i1", "i2", "i3", "i4", "i5", "i6", "i7"};
    return intRegName[index];
}

int IntRegFile::flattenIndex(int reg)
{
    int flatIndex = offset[reg >> FrameOffsetBits]
        | (reg & FrameOffsetMask);
    DPRINTF(Sparc, "Flattened index %d into %d.\n", reg, flatIndex);
    return flatIndex;
}

void IntRegFile::clear()
{
    int x;
    for (x = 0; x < MaxGL; x++)
        memset(regGlobals[x], 0, sizeof(IntReg) * RegsPerFrame);
    for(int x = 0; x < 2 * NWindows; x++)
        memset(regSegments[x], 0, sizeof(IntReg) * RegsPerFrame);
}

IntRegFile::IntRegFile()
{
    offset[Globals] = 0;
    regView[Globals] = regGlobals[0];
    setCWP(0);
    clear();
}

IntReg IntRegFile::readReg(int intReg)
{
    IntReg val;
    if(intReg < NumIntArchRegs)
        val = regView[intReg >> FrameOffsetBits][intReg & FrameOffsetMask];
    else if((intReg -= NumIntArchRegs) < NumMicroIntRegs)
        val = microRegs[intReg];
    else
        panic("Tried to read non-existant integer register %d, %d\n", NumIntArchRegs + NumMicroIntRegs + intReg, intReg);

    DPRINTF(Sparc, "Read register %d = 0x%x\n", intReg, val);
    return val;
}

void IntRegFile::setReg(int intReg, const IntReg &val)
{
    if(intReg)
    {
        DPRINTF(Sparc, "Wrote register %d = 0x%x\n", intReg, val);
        if(intReg < NumIntArchRegs)
            regView[intReg >> FrameOffsetBits][intReg & FrameOffsetMask] = val;
        else if((intReg -= NumIntArchRegs) < NumMicroIntRegs)
            microRegs[intReg] = val;
        else
            panic("Tried to set non-existant integer register\n");
    }
}

//This doesn't effect the actual CWP register.
//It's purpose is to adjust the view of the register file
//to what it would be if CWP = cwp.
void IntRegFile::setCWP(int cwp)
{
    int index = ((NWindows - cwp) % NWindows) * 2;
    offset[Outputs] = FrameOffset + (index * RegsPerFrame);
    offset[Locals] = FrameOffset + ((index+1) * RegsPerFrame);
    offset[Inputs] = FrameOffset +
        (((index+2) % (NWindows * 2)) * RegsPerFrame);
    regView[Outputs] = regSegments[index];
    regView[Locals] = regSegments[index+1];
    regView[Inputs] = regSegments[(index+2) % (NWindows * 2)];

    DPRINTF(Sparc, "Changed the CWP value to %d\n", cwp);
}

void IntRegFile::setGlobals(int gl)
{
    DPRINTF(Sparc, "Now using %d globals\n", gl);

    regView[Globals] = regGlobals[gl];
    offset[Globals] = RegGlobalOffset + gl * RegsPerFrame;
}

void IntRegFile::serialize(std::ostream &os)
{
    unsigned int x;
    for(x = 0; x < MaxGL; x++)
        SERIALIZE_ARRAY(regGlobals[x], RegsPerFrame);
    for(x = 0; x < 2 * NWindows; x++)
        SERIALIZE_ARRAY(regSegments[x], RegsPerFrame);
    SERIALIZE_ARRAY(microRegs, NumMicroIntRegs);
}

void IntRegFile::unserialize(Checkpoint *cp, const std::string &section)
{
    unsigned int x;
    for(x = 0; x < MaxGL; x++)
        UNSERIALIZE_ARRAY(regGlobals[x], RegsPerFrame);
    for(unsigned int x = 0; x < 2 * NWindows; x++)
        UNSERIALIZE_ARRAY(regSegments[x], RegsPerFrame);
    UNSERIALIZE_ARRAY(microRegs, NumMicroIntRegs);
}
<commit_msg>Fix an include problem.<commit_after>/*
 * Copyright (c) 2003-2005 The Regents of The University of Michigan
 * 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 copyright holders 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.
 *
 * Authors: Gabe Black
 *          Ali Saidi
 */

#include "arch/sparc/intregfile.hh"
#include "base/trace.hh"
#include "base/misc.hh"
#include "sim/serialize.hh"

#include <string.h>

using namespace SparcISA;
using namespace std;

class Checkpoint;

string SparcISA::getIntRegName(RegIndex index)
{
    static std::string intRegName[NumIntArchRegs] =
        {"g0", "g1", "g2", "g3", "g4", "g5", "g6", "g7",
         "o0", "o1", "o2", "o3", "o4", "o5", "o6", "o7",
         "l0", "l1", "l2", "l3", "l4", "l5", "l6", "l7",
         "i0", "i1", "i2", "i3", "i4", "i5", "i6", "i7"};
    return intRegName[index];
}

int IntRegFile::flattenIndex(int reg)
{
    int flatIndex = offset[reg >> FrameOffsetBits]
        | (reg & FrameOffsetMask);
    DPRINTF(Sparc, "Flattened index %d into %d.\n", reg, flatIndex);
    return flatIndex;
}

void IntRegFile::clear()
{
    int x;
    for (x = 0; x < MaxGL; x++)
        memset(regGlobals[x], 0, sizeof(IntReg) * RegsPerFrame);
    for(int x = 0; x < 2 * NWindows; x++)
        memset(regSegments[x], 0, sizeof(IntReg) * RegsPerFrame);
}

IntRegFile::IntRegFile()
{
    offset[Globals] = 0;
    regView[Globals] = regGlobals[0];
    setCWP(0);
    clear();
}

IntReg IntRegFile::readReg(int intReg)
{
    IntReg val;
    if(intReg < NumIntArchRegs)
        val = regView[intReg >> FrameOffsetBits][intReg & FrameOffsetMask];
    else if((intReg -= NumIntArchRegs) < NumMicroIntRegs)
        val = microRegs[intReg];
    else
        panic("Tried to read non-existant integer register %d, %d\n",
                NumIntArchRegs + NumMicroIntRegs + intReg, intReg);

    DPRINTF(Sparc, "Read register %d = 0x%x\n", intReg, val);
    return val;
}

void IntRegFile::setReg(int intReg, const IntReg &val)
{
    if(intReg)
    {
        DPRINTF(Sparc, "Wrote register %d = 0x%x\n", intReg, val);
        if(intReg < NumIntArchRegs)
            regView[intReg >> FrameOffsetBits][intReg & FrameOffsetMask] = val;
        else if((intReg -= NumIntArchRegs) < NumMicroIntRegs)
            microRegs[intReg] = val;
        else
            panic("Tried to set non-existant integer register\n");
    }
}

//This doesn't effect the actual CWP register.
//It's purpose is to adjust the view of the register file
//to what it would be if CWP = cwp.
void IntRegFile::setCWP(int cwp)
{
    int index = ((NWindows - cwp) % NWindows) * 2;
    offset[Outputs] = FrameOffset + (index * RegsPerFrame);
    offset[Locals] = FrameOffset + ((index+1) * RegsPerFrame);
    offset[Inputs] = FrameOffset +
        (((index+2) % (NWindows * 2)) * RegsPerFrame);
    regView[Outputs] = regSegments[index];
    regView[Locals] = regSegments[index+1];
    regView[Inputs] = regSegments[(index+2) % (NWindows * 2)];

    DPRINTF(Sparc, "Changed the CWP value to %d\n", cwp);
}

void IntRegFile::setGlobals(int gl)
{
    DPRINTF(Sparc, "Now using %d globals\n", gl);

    regView[Globals] = regGlobals[gl];
    offset[Globals] = RegGlobalOffset + gl * RegsPerFrame;
}

void IntRegFile::serialize(std::ostream &os)
{
    unsigned int x;
    for(x = 0; x < MaxGL; x++)
        SERIALIZE_ARRAY(regGlobals[x], RegsPerFrame);
    for(x = 0; x < 2 * NWindows; x++)
        SERIALIZE_ARRAY(regSegments[x], RegsPerFrame);
    SERIALIZE_ARRAY(microRegs, NumMicroIntRegs);
}

void IntRegFile::unserialize(Checkpoint *cp, const std::string &section)
{
    unsigned int x;
    for(x = 0; x < MaxGL; x++)
        UNSERIALIZE_ARRAY(regGlobals[x], RegsPerFrame);
    for(unsigned int x = 0; x < 2 * NWindows; x++)
        UNSERIALIZE_ARRAY(regSegments[x], RegsPerFrame);
    UNSERIALIZE_ARRAY(microRegs, NumMicroIntRegs);
}
<|endoftext|>


<commit_before>/*
	This file is part of cpp-ethereum.

	cpp-ethereum 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.

	cpp-ethereum 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 cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file ContractCallDataEncoder.cpp
 * @author Yann yann@ethdev.com
 * @date 2014
 * Ethereum IDE client.
 */

#include <QDebug>
#include <QMap>
#include <QStringList>
#include <libethcore/CommonJS.h>
#include <libsolidity/AST.h>
#include "QVariableDeclaration.h"
#include "QVariableDefinition.h"
#include "QFunctionDefinition.h"
#include "ContractCallDataEncoder.h"
using namespace dev;
using namespace dev::solidity;
using namespace dev::mix;

bytes ContractCallDataEncoder::encodedData()
{
	bytes r(m_encodedData);
	size_t headerSize = m_encodedData.size() & ~0x1fUL; //ignore any prefix that is not 32-byte aligned
	//apply offsets
	for (auto const& p: m_offsetMap)
	{
		vector_ref<byte> offsetRef(r.data() + p.first, 32);
		toBigEndian<size_t, vector_ref<byte>>(p.second + headerSize, offsetRef); //add header size minus signature hash
	}

	r.insert(r.end(), m_dynamicData.begin(), m_dynamicData.end());
	return r;
}

void ContractCallDataEncoder::encode(QFunctionDefinition const* _function)
{
	bytes hash = _function->hash().asBytes();
	m_encodedData.insert(m_encodedData.end(), hash.begin(), hash.end());
}

void ContractCallDataEncoder::encode(QVariant const& _data, SolidityType const& _type)
{
	u256 count = 1;
	QStringList strList;
	if (_type.array)
	{
		if (_data.type() == QVariant::String)
			strList = _data.toString().split(",", QString::SkipEmptyParts);  //TODO: proper parsing
		else
			strList = _data.toStringList();
		count = strList.count();

	}
	else
		strList.append(_data.toString());

	if (_type.dynamicSize)
	{
		bytes empty(32);
		size_t sizePos = m_dynamicData.size();
		m_dynamicData.insert(m_dynamicData.end(), empty.begin(), empty.end()); //reserve space for count
		if (_type.type == SolidityType::Type::Bytes)
			count = encodeSingleItem(_data.toString(), _type, m_dynamicData);
		else
		{
			count = strList.count();
			for (auto const& item: strList)
				encodeSingleItem(item, _type, m_dynamicData);
		}
		vector_ref<byte> sizeRef(m_dynamicData.data() + sizePos, 32);
		toBigEndian(count, sizeRef);
		m_offsetMap.push_back(std::make_pair(m_encodedData.size(), sizePos));
		m_encodedData.insert(m_encodedData.end(), empty.begin(), empty.end()); //reserve space for offset
	}
	else
	{
		if (_type.array)
			count = _type.count;
		int c = static_cast<int>(count);
		if (strList.size() > c)
			strList.erase(strList.begin() + c, strList.end());
		else
			while (strList.size() < c)
				strList.append(QString());

		for (auto const& item: strList)
			encodeSingleItem(item, _type, m_encodedData);
	}
}

unsigned ContractCallDataEncoder::encodeSingleItem(QString const& _data, SolidityType const& _type, bytes& _dest)
{
	if (_type.type == SolidityType::Type::Struct)
		BOOST_THROW_EXCEPTION(dev::Exception() << dev::errinfo_comment("Struct parameters are not supported yet"));

	unsigned const alignSize = 32;
	QString src = _data;
	bytes result;

	if ((src.startsWith("\"") && src.endsWith("\"")) || (src.startsWith("\'") && src.endsWith("\'")))
		src = src.remove(src.length() - 1, 1).remove(0, 1);

	if (src.startsWith("0x"))
	{
		result = fromHex(src.toStdString().substr(2));
		if (_type.type != SolidityType::Type::Bytes)
			result = padded(result, alignSize);
	}
	else
	{
		try
		{
			bigint i(src.toStdString());
			result = bytes(alignSize);
			toBigEndian((u256)i, result);
		}
		catch (std::exception const&)
		{
			// manage input as a string.
			result = encodeStringParam(src, alignSize);
		}
	}

	size_t dataSize = _type.dynamicSize ? result.size() : alignSize;
	if (result.size() % alignSize != 0)
		result.resize((result.size() & ~(alignSize - 1)) + alignSize);
	_dest.insert(_dest.end(), result.begin(), result.end());
	return dataSize;
}

bigint ContractCallDataEncoder::decodeInt(dev::bytes const& _rawValue)
{
	dev::u256 un = dev::fromBigEndian<dev::u256>(_rawValue);
	if (un >> 255)
		return (-s256(~un + 1));
	return un;
}

QString ContractCallDataEncoder::toString(dev::bigint const& _int)
{
	std::stringstream str;
	str << std::dec << _int;
	return QString::fromStdString(str.str());
}

dev::bytes ContractCallDataEncoder::encodeBool(QString const& _str)
{
	bytes b(1);
	b[0] = _str == "1" || _str.toLower() == "true " ? 1 : 0;
	return padded(b, 32);
}

bool ContractCallDataEncoder::decodeBool(dev::bytes const& _rawValue)
{
	byte ret = _rawValue.at(_rawValue.size() - 1);
	return (ret != 0);
}

QString ContractCallDataEncoder::toString(bool _b)
{
	return _b ? "true" : "false";
}

dev::bytes ContractCallDataEncoder::encodeStringParam(QString const& _str, unsigned alignSize)
{
	bytes result;
	QByteArray bytesAr = _str.toLocal8Bit();
	result = bytes(bytesAr.begin(), bytesAr.end());
	return paddedRight(result, alignSize);
}

dev::bytes ContractCallDataEncoder::encodeBytes(QString const& _str)
{
	QByteArray bytesAr = _str.toLocal8Bit();
	bytes r = bytes(bytesAr.begin(), bytesAr.end());
	return padded(r, 32);
}

dev::bytes ContractCallDataEncoder::decodeBytes(dev::bytes const& _rawValue)
{
	return _rawValue;
}

QString ContractCallDataEncoder::toString(dev::bytes const& _b)
{
	QString str;
	if (asString(_b, str))
		return  "\"" + str +  "\" " + QString::fromStdString(dev::toJS(_b));
	else
		return QString::fromStdString(dev::toJS(_b));
}

QVariant ContractCallDataEncoder::decode(SolidityType const& _type, bytes const& _value)
{
	bytesConstRef value(&_value);
	bytes rawParam(32);
	value.populate(&rawParam);
	QSolidityType::Type type = _type.type;
	if (type == QSolidityType::Type::SignedInteger || type == QSolidityType::Type::UnsignedInteger)
		return QVariant::fromValue(toString(decodeInt(rawParam)));
	else if (type == QSolidityType::Type::Bool)
		return QVariant::fromValue(toString(decodeBool(rawParam)));
	else if (type == QSolidityType::Type::Bytes || type == QSolidityType::Type::Hash)
		return QVariant::fromValue(toString(decodeBytes(rawParam)));
	else if (type == QSolidityType::Type::Struct)
		return QVariant::fromValue(QString("struct")); //TODO
	else if (type == QSolidityType::Type::Address)
		return QVariant::fromValue(toString(decodeBytes(unpadLeft(rawParam))));
	else
		BOOST_THROW_EXCEPTION(Exception() << errinfo_comment("Parameter declaration not found"));
}

QStringList ContractCallDataEncoder::decode(QList<QVariableDeclaration*> const& _returnParameters, bytes _value)
{
	bytesConstRef value(&_value);
	bytes rawParam(32);
	QStringList r;

	for (int k = 0; k <_returnParameters.length(); k++)
	{
		value.populate(&rawParam);
		value = value.cropped(32);
		QVariableDeclaration* dec = static_cast<QVariableDeclaration*>(_returnParameters.at(k));
		SolidityType const& type = dec->type()->type();
		r.append(decode(type, rawParam).toString());
	}
	return r;
}

bool ContractCallDataEncoder::asString(dev::bytes const& _b, QString& _str)
{
	dev::bytes bunPad = unpadded(_b);
	for (unsigned i = 0; i < bunPad.size(); i++)
	{
		if (bunPad.at(i) < 9 || bunPad.at(i) > 127)
			return false;
		else
			_str += QString::fromStdString(dev::toJS(bunPad.at(i))).replace("0x", "");
	}
	return true;
}
<commit_msg>used operator+= for bytes<commit_after>/*
	This file is part of cpp-ethereum.

	cpp-ethereum 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.

	cpp-ethereum 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 cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file ContractCallDataEncoder.cpp
 * @author Yann yann@ethdev.com
 * @date 2014
 * Ethereum IDE client.
 */

#include <QDebug>
#include <QMap>
#include <QStringList>
#include <libethcore/CommonJS.h>
#include <libsolidity/AST.h>
#include "QVariableDeclaration.h"
#include "QVariableDefinition.h"
#include "QFunctionDefinition.h"
#include "ContractCallDataEncoder.h"
using namespace dev;
using namespace dev::solidity;
using namespace dev::mix;

bytes ContractCallDataEncoder::encodedData()
{
	bytes r(m_encodedData);
	size_t headerSize = m_encodedData.size() & ~0x1fUL; //ignore any prefix that is not 32-byte aligned
	//apply offsets
	for (auto const& p: m_offsetMap)
	{
		vector_ref<byte> offsetRef(r.data() + p.first, 32);
		toBigEndian<size_t, vector_ref<byte>>(p.second + headerSize, offsetRef); //add header size minus signature hash
	}

	r.insert(r.end(), m_dynamicData.begin(), m_dynamicData.end());
	return r;
}

void ContractCallDataEncoder::encode(QFunctionDefinition const* _function)
{
	bytes hash = _function->hash().asBytes();
	m_encodedData.insert(m_encodedData.end(), hash.begin(), hash.end());
}

void ContractCallDataEncoder::encode(QVariant const& _data, SolidityType const& _type)
{
	u256 count = 1;
	QStringList strList;
	if (_type.array)
	{
		if (_data.type() == QVariant::String)
			strList = _data.toString().split(",", QString::SkipEmptyParts);  //TODO: proper parsing
		else
			strList = _data.toStringList();
		count = strList.count();

	}
	else
		strList.append(_data.toString());

	if (_type.dynamicSize)
	{
		bytes empty(32);
		size_t sizePos = m_dynamicData.size();
		m_dynamicData += empty; //reserve space for count
		if (_type.type == SolidityType::Type::Bytes)
			count = encodeSingleItem(_data.toString(), _type, m_dynamicData);
		else
		{
			count = strList.count();
			for (auto const& item: strList)
				encodeSingleItem(item, _type, m_dynamicData);
		}
		vector_ref<byte> sizeRef(m_dynamicData.data() + sizePos, 32);
		toBigEndian(count, sizeRef);
		m_offsetMap.push_back(std::make_pair(m_encodedData.size(), sizePos));
		m_encodedData += empty; //reserve space for offset
	}
	else
	{
		if (_type.array)
			count = _type.count;
		int c = static_cast<int>(count);
		if (strList.size() > c)
			strList.erase(strList.begin() + c, strList.end());
		else
			while (strList.size() < c)
				strList.append(QString());

		for (auto const& item: strList)
			encodeSingleItem(item, _type, m_encodedData);
	}
}

unsigned ContractCallDataEncoder::encodeSingleItem(QString const& _data, SolidityType const& _type, bytes& _dest)
{
	if (_type.type == SolidityType::Type::Struct)
		BOOST_THROW_EXCEPTION(dev::Exception() << dev::errinfo_comment("Struct parameters are not supported yet"));

	unsigned const alignSize = 32;
	QString src = _data;
	bytes result;

	if ((src.startsWith("\"") && src.endsWith("\"")) || (src.startsWith("\'") && src.endsWith("\'")))
		src = src.remove(src.length() - 1, 1).remove(0, 1);

	if (src.startsWith("0x"))
	{
		result = fromHex(src.toStdString().substr(2));
		if (_type.type != SolidityType::Type::Bytes)
			result = padded(result, alignSize);
	}
	else
	{
		try
		{
			bigint i(src.toStdString());
			result = bytes(alignSize);
			toBigEndian((u256)i, result);
		}
		catch (std::exception const&)
		{
			// manage input as a string.
			result = encodeStringParam(src, alignSize);
		}
	}

	size_t dataSize = _type.dynamicSize ? result.size() : alignSize;
	if (result.size() % alignSize != 0)
		result.resize((result.size() & ~(alignSize - 1)) + alignSize);
	_dest.insert(_dest.end(), result.begin(), result.end());
	return dataSize;
}

bigint ContractCallDataEncoder::decodeInt(dev::bytes const& _rawValue)
{
	dev::u256 un = dev::fromBigEndian<dev::u256>(_rawValue);
	if (un >> 255)
		return (-s256(~un + 1));
	return un;
}

QString ContractCallDataEncoder::toString(dev::bigint const& _int)
{
	std::stringstream str;
	str << std::dec << _int;
	return QString::fromStdString(str.str());
}

dev::bytes ContractCallDataEncoder::encodeBool(QString const& _str)
{
	bytes b(1);
	b[0] = _str == "1" || _str.toLower() == "true " ? 1 : 0;
	return padded(b, 32);
}

bool ContractCallDataEncoder::decodeBool(dev::bytes const& _rawValue)
{
	byte ret = _rawValue.at(_rawValue.size() - 1);
	return (ret != 0);
}

QString ContractCallDataEncoder::toString(bool _b)
{
	return _b ? "true" : "false";
}

dev::bytes ContractCallDataEncoder::encodeStringParam(QString const& _str, unsigned alignSize)
{
	bytes result;
	QByteArray bytesAr = _str.toLocal8Bit();
	result = bytes(bytesAr.begin(), bytesAr.end());
	return paddedRight(result, alignSize);
}

dev::bytes ContractCallDataEncoder::encodeBytes(QString const& _str)
{
	QByteArray bytesAr = _str.toLocal8Bit();
	bytes r = bytes(bytesAr.begin(), bytesAr.end());
	return padded(r, 32);
}

dev::bytes ContractCallDataEncoder::decodeBytes(dev::bytes const& _rawValue)
{
	return _rawValue;
}

QString ContractCallDataEncoder::toString(dev::bytes const& _b)
{
	QString str;
	if (asString(_b, str))
		return  "\"" + str +  "\" " + QString::fromStdString(dev::toJS(_b));
	else
		return QString::fromStdString(dev::toJS(_b));
}

QVariant ContractCallDataEncoder::decode(SolidityType const& _type, bytes const& _value)
{
	bytesConstRef value(&_value);
	bytes rawParam(32);
	value.populate(&rawParam);
	QSolidityType::Type type = _type.type;
	if (type == QSolidityType::Type::SignedInteger || type == QSolidityType::Type::UnsignedInteger)
		return QVariant::fromValue(toString(decodeInt(rawParam)));
	else if (type == QSolidityType::Type::Bool)
		return QVariant::fromValue(toString(decodeBool(rawParam)));
	else if (type == QSolidityType::Type::Bytes || type == QSolidityType::Type::Hash)
		return QVariant::fromValue(toString(decodeBytes(rawParam)));
	else if (type == QSolidityType::Type::Struct)
		return QVariant::fromValue(QString("struct")); //TODO
	else if (type == QSolidityType::Type::Address)
		return QVariant::fromValue(toString(decodeBytes(unpadLeft(rawParam))));
	else
		BOOST_THROW_EXCEPTION(Exception() << errinfo_comment("Parameter declaration not found"));
}

QStringList ContractCallDataEncoder::decode(QList<QVariableDeclaration*> const& _returnParameters, bytes _value)
{
	bytesConstRef value(&_value);
	bytes rawParam(32);
	QStringList r;

	for (int k = 0; k <_returnParameters.length(); k++)
	{
		value.populate(&rawParam);
		value = value.cropped(32);
		QVariableDeclaration* dec = static_cast<QVariableDeclaration*>(_returnParameters.at(k));
		SolidityType const& type = dec->type()->type();
		r.append(decode(type, rawParam).toString());
	}
	return r;
}

bool ContractCallDataEncoder::asString(dev::bytes const& _b, QString& _str)
{
	dev::bytes bunPad = unpadded(_b);
	for (unsigned i = 0; i < bunPad.size(); i++)
	{
		if (bunPad.at(i) < 9 || bunPad.at(i) > 127)
			return false;
		else
			_str += QString::fromStdString(dev::toJS(bunPad.at(i))).replace("0x", "");
	}
	return true;
}
<|endoftext|>


<commit_before>//
// GEOnet.cpp
//
// Copyright (c) 2002 Virtual Terrain Project
// Free for all uses, see license.txt for details.
//

#include "GEOnet.h"
#include <stdio.h>

bool Country::FindPlace(const char *place_val, DPoint2 &point, bool bFullLength)
{
	int j, num_places = m_places.GetSize();

	bool success = false;
	for (j = 0; j < num_places; j++)
	{
		Place *place = m_places[j];
		if (bFullLength)
			success = (place->m_fullname_nd.CompareNoCase(place_val) == 0);
		else
		{
			int len = strlen(place_val);
			if (len > 2)
				success = (place->m_fullname_nd.Left(len).CompareNoCase(place_val) == 0);
		}
		if (success)
		{
			point = place->m_pos;
			return true;
		}
	}
	return false;
}

void Countries::ReadCountryList(const char *fname)
{
	FILE *fp = fopen(fname, "rb");

	int off;
	vtString str, name, abb;
	char buf[400];
	while (fgets(buf, 400, fp) != NULL)
	{
		str = buf;
		off = str.Find(':');
		Country *country = new Country;
		country->m_full = str.Left(off);
		country->m_abb = str.Mid(off+1, 2);
		m_countries.Append(country);
	}
	fclose(fp);
}

void Countries::ParseRawCountryFiles(const char *path_prefix)
{
	m_path = path_prefix;

	int num = m_countries.GetSize();
	for (int i = 0; i < num; i++)
	{
		ParseRawCountry(i);
	}
}

void Countries::ParseRawCountry(int which)
{
	Country *country = m_countries[which];

	printf("Parsing %s...", country->m_full);

	char fc;	// feature classification, P = populated palce
	char pc;	// Populated Place Classification
	DPoint2 point;
	char *p, *w;
	int i, j;
	char buf[4000];
	char word[100];
	Place *place;

	Array<char> importance;
	int num_important[7];
	int line_number = 0;
	int duplicates = 0;

	for (i = 0; i < 7; i++)
		num_important[i] = 0;

	FILE *fp = fopen(m_path + country->m_abb + ".txt", "rb");
	if (!fp)
	{
		printf("couldn't open.\n");
		return;
	}
	while (fgets(buf, 4000, fp) != NULL)
	{
		line_number++;
		i = 0;
		w = word;
		for (p = buf; *p != '\n'; p++)
		{
			if (*p == '\t')
			{
				*w = '\0';
				// handle word
				if (i == 3)
					point.y = atof(word); // lat
				if (i == 4)
					point.x = atof(word); // lon
				if (i == 9)
					fc = word[0];
				if (i == 11)
				{
					pc = word[0] ? word[0] - '0' : 6;	// "importance" 1-5, 1 most
				}
				if (i == 23)
				{
					if (fc == 'P')	// populated place
					{
						// count hhow many of each importance
						if (pc >= 1 && pc <= 6) num_important[pc] ++;

						bool bReplace = false;
						if (pc < 5)	// might be more important than existing
						{
							int size = country->m_places.GetSize();
							for (j = 0; j < size; j++)
							{
								place = country->m_places.GetAt(j);
								if (!place->m_fullname_nd.Compare(word))
								{
									duplicates++;
									// if name already exists, replace the existing
									// place if this one is more important
									if (pc < importance[j])
									{
										bReplace = true;
										break;
									}
								}
							}
						}
						place = new Place();
						place->m_pos = point;
						place->m_fullname_nd = word;
						if (bReplace)
						{
							country->m_places[j] = place;
							importance[j] = pc;
						}
						else
						{
							country->m_places.Append(place);
							importance.Append(pc);
						}
					}
				}
				//printf("i=%d word=%s\n", i, word);
				i++;
				w = word;
			}
			else
			{
				*w = *p;
				w++;
			}
		}
	}
	fclose(fp);
	int size = country->m_places.GetSize();
	printf("%d places. Imps: %d %d %d %d %d %d. Dupes: %d\n",
		size, num_important[1], num_important[2], num_important[3],
		num_important[4], num_important[5], num_important[6], duplicates);
}

void WriteString(FILE *fp, const vtString &str)
{
	short len = str.GetLength();
	fwrite(&len, 2, 1, fp);
	const char *buf = (const char *)str;
	fwrite(buf, len, 1, fp);
}

void ReadString(FILE *fp, vtString &str)
{
	char buf[100];
	short len;
	fread(&len, 2, 1, fp);
	fread(buf, len, 1, fp);
	buf[len] = '\0';
	str = buf;
}

void Countries::WriteGCF(const char *fname)
{
	FILE *fp = fopen(fname, "wb");

	int num = m_countries.GetSize();
	fwrite(&num, sizeof(int), 1, fp);

	int i, j, num_places;
	for (i = 0; i < num; i++)
	{
		Country *country = m_countries[i];
		num_places = country->m_places.GetSize();

		WriteString(fp, country->m_full);
		fwrite(&num_places, sizeof(int), 1, fp);

		for (j = 0; j < num_places; j++)
		{
			Place *place = country->m_places[j];
			fwrite(&place->m_pos.x, sizeof(double), 2, fp);
			WriteString(fp, place->m_fullname_nd);
		}
	}
	fclose(fp);
}

void Countries::ReadGCF(const char *fname, void progress_callback(int))
{
	FILE *fp = fopen(fname, "rb");
	if (!fp)
		return;

	int num;
	fread(&num, sizeof(int), 1, fp);

	m_countries.SetMaxSize(num);

	int i, j, num_places;
	for (i = 0; i < num; i++)
	{
		if (progress_callback != NULL)
			progress_callback(i * 100 / num);

		Country *country = new Country();
		m_countries.Append(country);

		ReadString(fp, country->m_full);
		printf("Reading %s...\n", country->m_full);

		fread(&num_places, sizeof(int), 1, fp);
		country->m_places.SetMaxSize(num_places);

		for (j = 0; j < num_places; j++)
		{
			Place *place = new Place();
			fread(&place->m_pos.x, sizeof(double), 2, fp);
			ReadString(fp, place->m_fullname_nd);
			country->m_places.Append(place);
		}
	}
	fclose(fp);
}

bool Countries::FindPlace(const char *country_val, const char *place_val,
						  DPoint2 &point)
{
	Country *country;

	int num = m_countries.GetSize();

	int i;
	for (i = 0; i < num; i++)
	{
		country = m_countries[i];

		if (country->m_full.CompareNoCase(country_val) != 0)
			continue;

		bool success = country->FindPlace(place_val, point, true);

		// try again with just the length of the initial sea
		if (!success)
			success = country->FindPlace(place_val, point, false);

		if (success)
			return true;
	}
	return false;
}

<commit_msg>fixed string case for gcc<commit_after>//
// GEOnet.cpp
//
// Copyright (c) 2002 Virtual Terrain Project
// Free for all uses, see license.txt for details.
//

#include "GEOnet.h"
#include <stdio.h>

bool Country::FindPlace(const char *place_val, DPoint2 &point, bool bFullLength)
{
	int j, num_places = m_places.GetSize();

	bool success = false;
	for (j = 0; j < num_places; j++)
	{
		Place *place = m_places[j];
		if (bFullLength)
			success = (place->m_fullname_nd.CompareNoCase(place_val) == 0);
		else
		{
			int len = strlen(place_val);
			if (len > 2)
				success = (place->m_fullname_nd.Left(len).CompareNoCase(place_val) == 0);
		}
		if (success)
		{
			point = place->m_pos;
			return true;
		}
	}
	return false;
}

void Countries::ReadCountryList(const char *fname)
{
	FILE *fp = fopen(fname, "rb");

	int off;
	vtString str, name, abb;
	char buf[400];
	while (fgets(buf, 400, fp) != NULL)
	{
		str = buf;
		off = str.Find(':');
		Country *country = new Country;
		country->m_full = str.Left(off);
		country->m_abb = str.Mid(off+1, 2);
		m_countries.Append(country);
	}
	fclose(fp);
}

void Countries::ParseRawCountryFiles(const char *path_prefix)
{
	m_path = path_prefix;

	int num = m_countries.GetSize();
	for (int i = 0; i < num; i++)
	{
		ParseRawCountry(i);
	}
}

void Countries::ParseRawCountry(int which)
{
	Country *country = m_countries[which];

	printf("Parsing %s...", (const char *) country->m_full);

	char fc;	// feature classification, P = populated palce
	char pc;	// Populated Place Classification
	DPoint2 point;
	char *p, *w;
	int i, j;
	char buf[4000];
	char word[100];
	Place *place;

	Array<char> importance;
	int num_important[7];
	int line_number = 0;
	int duplicates = 0;

	for (i = 0; i < 7; i++)
		num_important[i] = 0;

	FILE *fp = fopen(m_path + country->m_abb + ".txt", "rb");
	if (!fp)
	{
		printf("couldn't open.\n");
		return;
	}
	while (fgets(buf, 4000, fp) != NULL)
	{
		line_number++;
		i = 0;
		w = word;
		for (p = buf; *p != '\n'; p++)
		{
			if (*p == '\t')
			{
				*w = '\0';
				// handle word
				if (i == 3)
					point.y = atof(word); // lat
				if (i == 4)
					point.x = atof(word); // lon
				if (i == 9)
					fc = word[0];
				if (i == 11)
				{
					pc = word[0] ? word[0] - '0' : 6;	// "importance" 1-5, 1 most
				}
				if (i == 23)
				{
					if (fc == 'P')	// populated place
					{
						// count hhow many of each importance
						if (pc >= 1 && pc <= 6) num_important[pc] ++;

						bool bReplace = false;
						if (pc < 5)	// might be more important than existing
						{
							int size = country->m_places.GetSize();
							for (j = 0; j < size; j++)
							{
								place = country->m_places.GetAt(j);
								if (!place->m_fullname_nd.Compare(word))
								{
									duplicates++;
									// if name already exists, replace the existing
									// place if this one is more important
									if (pc < importance[j])
									{
										bReplace = true;
										break;
									}
								}
							}
						}
						place = new Place();
						place->m_pos = point;
						place->m_fullname_nd = word;
						if (bReplace)
						{
							country->m_places[j] = place;
							importance[j] = pc;
						}
						else
						{
							country->m_places.Append(place);
							importance.Append(pc);
						}
					}
				}
				//printf("i=%d word=%s\n", i, word);
				i++;
				w = word;
			}
			else
			{
				*w = *p;
				w++;
			}
		}
	}
	fclose(fp);
	int size = country->m_places.GetSize();
	printf("%d places. Imps: %d %d %d %d %d %d. Dupes: %d\n",
		size, num_important[1], num_important[2], num_important[3],
		num_important[4], num_important[5], num_important[6], duplicates);
}

void WriteString(FILE *fp, const vtString &str)
{
	short len = str.GetLength();
	fwrite(&len, 2, 1, fp);
	const char *buf = (const char *)str;
	fwrite(buf, len, 1, fp);
}

void ReadString(FILE *fp, vtString &str)
{
	char buf[100];
	short len;
	fread(&len, 2, 1, fp);
	fread(buf, len, 1, fp);
	buf[len] = '\0';
	str = buf;
}

void Countries::WriteGCF(const char *fname)
{
	FILE *fp = fopen(fname, "wb");

	int num = m_countries.GetSize();
	fwrite(&num, sizeof(int), 1, fp);

	int i, j, num_places;
	for (i = 0; i < num; i++)
	{
		Country *country = m_countries[i];
		num_places = country->m_places.GetSize();

		WriteString(fp, country->m_full);
		fwrite(&num_places, sizeof(int), 1, fp);

		for (j = 0; j < num_places; j++)
		{
			Place *place = country->m_places[j];
			fwrite(&place->m_pos.x, sizeof(double), 2, fp);
			WriteString(fp, place->m_fullname_nd);
		}
	}
	fclose(fp);
}

void Countries::ReadGCF(const char *fname, void progress_callback(int))
{
	FILE *fp = fopen(fname, "rb");
	if (!fp)
		return;

	int num;
	fread(&num, sizeof(int), 1, fp);

	m_countries.SetMaxSize(num);

	int i, j, num_places;
	for (i = 0; i < num; i++)
	{
		if (progress_callback != NULL)
			progress_callback(i * 100 / num);

		Country *country = new Country();
		m_countries.Append(country);

		ReadString(fp, country->m_full);
		printf("Reading %s...\n", (const char *) country->m_full);

		fread(&num_places, sizeof(int), 1, fp);
		country->m_places.SetMaxSize(num_places);

		for (j = 0; j < num_places; j++)
		{
			Place *place = new Place();
			fread(&place->m_pos.x, sizeof(double), 2, fp);
			ReadString(fp, place->m_fullname_nd);
			country->m_places.Append(place);
		}
	}
	fclose(fp);
}

bool Countries::FindPlace(const char *country_val, const char *place_val,
						  DPoint2 &point)
{
	Country *country;

	int num = m_countries.GetSize();

	int i;
	for (i = 0; i < num; i++)
	{
		country = m_countries[i];

		if (country->m_full.CompareNoCase(country_val) != 0)
			continue;

		bool success = country->FindPlace(place_val, point, true);

		// try again with just the length of the initial sea
		if (!success)
			success = country->FindPlace(place_val, point, false);

		if (success)
			return true;
	}
	return false;
}

<|endoftext|>


<commit_before>#include "BallDetectorEvaluator.h"

#include <glib.h>
#include <glib/gstdio.h>

#include <strstream>
#include <fstream>
#include <sstream>

#include <Representations/Perception/BallCandidates.h>

#include <Extern/libb64/encode.h>

#include <Tools/naoth_opencv.h>

#if defined(__GNUC__) && defined(_NAOTH_CHECK_CONVERSION_)
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 5) || (__GNUC__ > 4) // version >= 4.6
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 8) || (__GNUC__ > 4) // version >= 4.9
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#endif
#endif

#include <opencv2/imgcodecs/imgcodecs.hpp>

#if defined(__GNUC__) && defined(_NAOTH_CHECK_CONVERSION_)
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 5) || (__GNUC__ > 4)  // version >= 4.6
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic error "-Wconversion"
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 8) || (__GNUC__ > 4) // version >= 4.9
#pragma GCC diagnostic error "-Wfloat-conversion"
#endif
#endif

BallDetectorEvaluator::BallDetectorEvaluator(const std::string &fileArg)
  : fileArg(fileArg), minNeighbours(0)
{

}

BallDetectorEvaluator::~BallDetectorEvaluator()
{

}

void BallDetectorEvaluator::execute()
{
  results.clear();
  std::string outFileName = "HaarBallDetector_Evaluation.html";

  // do experiment for different parameters
  for(minNeighbours=0; minNeighbours < 6; minNeighbours++)
  {

    ExperimentResult r;

    r.truePositives = 0;
    r.falseNegatives = 0;
    r.falsePositives = 0;
    r.falsePositivePatches.clear();
    r.falseNegativePatches.clear();
    r.totalSize = 0;

    if(g_file_test(fileArg.c_str(), G_FILE_TEST_IS_DIR))
    {
      std::string dirlocation = fileArg;
      if (!g_str_has_suffix(dirlocation.c_str(), "/"))
      {
        dirlocation = dirlocation + "/";
      }

      GDir* dir = g_dir_open(dirlocation.c_str(), 0, NULL);
      if (dir != NULL)
      {
        const gchar* name;
        while ((name = g_dir_read_name(dir)) != NULL)
        {
          if (g_str_has_suffix(name, ".log"))
          {
            std::string completeFileName = dirlocation + name;
            if (g_file_test(completeFileName.c_str(), G_FILE_TEST_EXISTS)
                && g_file_test(completeFileName.c_str(), G_FILE_TEST_IS_REGULAR))
            {
              r.totalSize += executeSingleFile(completeFileName, r);
            }
          }

        }
        g_dir_close(dir);
      }
    }
    else
    {
      // only one file
      r.totalSize = executeSingleFile(fileArg, r);
    }

    r.precision = 1.0;
    if(r.truePositives + r.falsePositives > 0)
    {
      r.precision = (double) r.truePositives / ((double) (r.truePositives + r.falsePositives));
    }
    r.recall = 1.0;
    if(r.truePositives + r.falsePositives > 0)
    {
      r.recall = (double) r.truePositives / ((double) (r.truePositives + r.falseNegatives));
    }

    results[minNeighbours] = r;

    std::cout << "=============" << std::endl;
    std::cout << "minNeighbours=" << minNeighbours << std::endl;

    std::cout << "precision: " << r.precision << std::endl;
    std::cout << "recall: " << r.recall << std::endl;

    std::cout << "=============" << std::endl;
    std::cout << "Written detailed report to " << outFileName << std::endl;

  }

  outputResults(outFileName);

}

void BallDetectorEvaluator::outputResults(std::string outFileName)
{
  base64::Encoder base64Encoder(64);

  std::ofstream html;
  html.open(outFileName);

  html << "" << std::endl;
  html << "" << std::endl;
  html << "" << std::endl;
  html << "" << std::endl;

  html << "" << std::endl;

  html << "
Overview
" << std::endl;

  for(std::map<unsigned int, ExperimentResult>::const_iterator it=results.begin();
      it != results.end(); it++)
  {
    const ExperimentResult& r = it->second;

    html << "
minNeighbours=" << it->first << "
" << std::endl;

    html << "
Summary
" << std::endl;
    html << "
precision: " << r.precision << "
recall: " << r.recall << "
" << std::endl;

    unsigned int numOfBalls = r.truePositives + r.falseNegatives;
    html << "
total number of samples: " << r.totalSize << " (" << numOfBalls << " balls, " << (r.totalSize - numOfBalls) << " non-balls)
" << std::endl;

    html << "
back to top
" << std::endl;

    html << "
False Positives
" << std::endl;

    html << "
number: " << r.falsePositives << "
" << std::endl;

    html << "
" << std::endl;
    for(std::list<ErrorEntry>::const_iterator it=r.falsePositivePatches.begin(); it != r.falsePositivePatches.end(); it++)
    {
      // use a data URI to embed the image in PNG format
      std::string imgPNG = createPNG(it->patch);
      html << "<img class=\"patch\" title=\"" << it->idx << "@" << it->fileName
           << "\" src=\"data:image/png;base64," << base64Encoder.encode(imgPNG.c_str(), (int) imgPNG.size())  << "\" />" << std::endl;
    }
    html << "
" << std::endl;

    html << "
False Negatives
" << std::endl;

    html << "
number: " << r.falseNegatives << "
" << std::endl;

    html << "
" << std::endl;
    for(std::list<ErrorEntry>::const_iterator it=r.falseNegativePatches.begin(); it != r.falseNegativePatches.end(); it++)
    {
      // use a data URI to embed the image in PNG format
      std::string imgPNG = createPNG(it->patch);
      html << "<img class=\"patch\" title=\"" << it->idx << "@" << it->fileName
           << "\" src=\"data:image/png;base64," << base64Encoder.encode(imgPNG.c_str(), (int) imgPNG.size())  << "\" />" << std::endl;
    }
    html << "
" << std::endl;
  }

  html << "" << std::endl;
  html.close();
}

unsigned int BallDetectorEvaluator::executeSingleFile(std::string file, ExperimentResult& r)
{
  LogFileScanner logFileScanner(file);

  std::set<unsigned int> expectedBallIdx;
  unsigned int maxValidIdx = loadGroundTruth(file, expectedBallIdx);

  unsigned int patchIdx = 0;

  // read in each frame
  LogFileScanner::FrameIterator secondLastFrame = logFileScanner.end();
  secondLastFrame--;
  for(LogFileScanner::FrameIterator it = logFileScanner.begin(); it != secondLastFrame; it++)
  {
    // reset all existing candidates
    getBallCandidates().reset();
    getBallCandidatesTop().reset();

    LogFileScanner::Frame frame;
    logFileScanner.readFrame(*it, frame);

    // deserialize all ball candidates (bottom and top camera)
    LogFileScanner::Frame::const_iterator frameBallCandidate = frame.find("BallCandidates");
    LogFileScanner::Frame::const_iterator frameBallCandidateTop = frame.find("BallCandidatesTop");
    if(frameBallCandidate!= frame.end())
    {
      std::istrstream stream(frameBallCandidate->second.data.data(), frameBallCandidate->second.data.size());
      naoth::Serializer<BallCandidates>::deserialize(stream, getBallCandidates());
    }
    if(frameBallCandidateTop != frame.end())
    {
      std::istrstream stream(frameBallCandidateTop->second.data.data(), frameBallCandidateTop->second.data.size());
      naoth::Serializer<BallCandidatesTop>::deserialize(stream, getBallCandidatesTop());
    }

    // The python script will always read the bottom patches first, thus in order to have the correct index
    // the loops have to bee in the same order.

    for(const BallCandidates::Patch& p : getBallCandidates().patches)
    {
      evaluatePatch(p, patchIdx++, CameraInfo::Bottom, expectedBallIdx, file, r);
    }

    for(const BallCandidates::Patch& p : getBallCandidatesTop().patches)
    {
      evaluatePatch(p, patchIdx++, CameraInfo::Bottom, expectedBallIdx, file, r);
    }

    if(patchIdx >= maxValidIdx)
    {
      break;
    }
  }

  return patchIdx;
}

void BallDetectorEvaluator::evaluatePatch(const BallCandidates::Patch &p, unsigned int patchIdx,
                                          CameraInfo::CameraID camID,
                                          const std::set<unsigned int>& expectedBallIdx,
                                          std::string fileName,
                                          ExperimentResult& r)
{
  bool expected = expectedBallIdx.find(patchIdx) != expectedBallIdx.end();
  bool actual = classifier.classify(p, minNeighbours) > 0;

  if(expected == actual)
  {
    if(expected)
    {
      r.truePositives++;
    }
  }
  else
  {
    ErrorEntry error;
    error.patch= p;
    error.idx = patchIdx;
    error.fileName = fileName;
    if(actual)
    {
      r.falsePositivePatches.push_back(error);
      r.falsePositives++;
    }
    else
    {
      r.falseNegativePatches.push_back(error);
      r.falseNegatives++;
    }
  }
}

int BallDetectorEvaluator::loadGroundTruth(std::string file, std::set<unsigned int>& expectedBallIdx)
{
  typedef std::vector<picojson::value> array;
  typedef std::map<std::string, picojson::value> object;

  int maxValidIdx = 0;

  size_t dotPos = file.find_last_of('.');

  std::string jsonFile = dotPos == std::string::npos ? file : file.substr(0, dotPos) + ".json";
  std::cout << "loading ground truth from '" << jsonFile << "'" << std::endl;
  std::ifstream groundTruthStream(jsonFile);

  picojson::value parsedJson;
  picojson::parse(parsedJson, groundTruthStream);

  groundTruthStream.close();

  array ballIdx;
  array noBallIdx;
  if(parsedJson.is<object>())
  {
    if(parsedJson.get("ball").is<array>())
    {
      ballIdx = parsedJson.get("ball").get<array>();
    }

    if(parsedJson.get("noball").is<array>())
    {
      noBallIdx = parsedJson.get("noball").get<array>();
    }

    for(picojson::value idx : ballIdx)
    {
      if(idx.is<double>())
      {
        int idxVal = static_cast<int>(idx.get<double>());
        expectedBallIdx.insert(idxVal);
        maxValidIdx = std::max(maxValidIdx, idxVal);
      }
    }

    for(picojson::value idx : noBallIdx)
    {
      if(idx.is<double>())
      {
        int idxVal = static_cast<int>(idx.get<double>());
        maxValidIdx = std::max(maxValidIdx, idxVal);
      }
    }
  }

  return maxValidIdx;
}

std::string BallDetectorEvaluator::createPGM(const BallCandidates::Patch &p)
{
  std::stringstream str;

  // header (we are gray scale, thus P2)
  str << "P2\n";
  // the width and the height of the image
  str << BallCandidates::Patch::SIZE << "\n" << BallCandidates::Patch::SIZE << "\n";
  // the maximum value we use
  str << "255\n";

  // output each pixel
  for(unsigned int y=0; y < BallCandidates::Patch::SIZE; y++)
  {
    for(unsigned int x=0; x < BallCandidates::Patch::SIZE; x++)
    {
      str << (int) p.data[x*BallCandidates::Patch::SIZE + y];
      if(x < BallCandidates::Patch::SIZE - 1)
      {
        str << " ";
      }
    }
    str << "\n";
  }
  return str.str();
}

std::string BallDetectorEvaluator::createPNG(const BallCandidates::Patch &p)
{
  cv::Mat wrappedImg(12, 12, CV_8UC1, (void*) p.data.data());

  std::vector<uchar> buffer;

  cv::imencode(".png", wrappedImg, buffer);

  return std::string(buffer.begin(), buffer.end());
}
<commit_msg>Output overview table.<commit_after>#include "BallDetectorEvaluator.h"

#include <glib.h>
#include <glib/gstdio.h>

#include <strstream>
#include <fstream>
#include <sstream>

#include <Representations/Perception/BallCandidates.h>

#include <Extern/libb64/encode.h>

#include <Tools/naoth_opencv.h>

#if defined(__GNUC__) && defined(_NAOTH_CHECK_CONVERSION_)
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 5) || (__GNUC__ > 4) // version >= 4.6
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 8) || (__GNUC__ > 4) // version >= 4.9
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#endif
#endif

#include <opencv2/imgcodecs/imgcodecs.hpp>

#if defined(__GNUC__) && defined(_NAOTH_CHECK_CONVERSION_)
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 5) || (__GNUC__ > 4)  // version >= 4.6
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic error "-Wconversion"
#if (__GNUC__ > 3 && __GNUC_MINOR__ > 8) || (__GNUC__ > 4) // version >= 4.9
#pragma GCC diagnostic error "-Wfloat-conversion"
#endif
#endif

BallDetectorEvaluator::BallDetectorEvaluator(const std::string &fileArg)
  : fileArg(fileArg), minNeighbours(0)
{

}

BallDetectorEvaluator::~BallDetectorEvaluator()
{

}

void BallDetectorEvaluator::execute()
{
  results.clear();
  std::string outFileName = "HaarBallDetector_Evaluation.html";

  // do experiment for different parameters
  for(minNeighbours=0; minNeighbours < 6; minNeighbours++)
  {

    ExperimentResult r;

    r.truePositives = 0;
    r.falseNegatives = 0;
    r.falsePositives = 0;
    r.falsePositivePatches.clear();
    r.falseNegativePatches.clear();
    r.totalSize = 0;

    if(g_file_test(fileArg.c_str(), G_FILE_TEST_IS_DIR))
    {
      std::string dirlocation = fileArg;
      if (!g_str_has_suffix(dirlocation.c_str(), "/"))
      {
        dirlocation = dirlocation + "/";
      }

      GDir* dir = g_dir_open(dirlocation.c_str(), 0, NULL);
      if (dir != NULL)
      {
        const gchar* name;
        while ((name = g_dir_read_name(dir)) != NULL)
        {
          if (g_str_has_suffix(name, ".log"))
          {
            std::string completeFileName = dirlocation + name;
            if (g_file_test(completeFileName.c_str(), G_FILE_TEST_EXISTS)
                && g_file_test(completeFileName.c_str(), G_FILE_TEST_IS_REGULAR))
            {
              r.totalSize += executeSingleFile(completeFileName, r);
            }
          }

        }
        g_dir_close(dir);
      }
    }
    else
    {
      // only one file
      r.totalSize = executeSingleFile(fileArg, r);
    }

    r.precision = 1.0;
    if(r.truePositives + r.falsePositives > 0)
    {
      r.precision = (double) r.truePositives / ((double) (r.truePositives + r.falsePositives));
    }
    r.recall = 1.0;
    if(r.truePositives + r.falsePositives > 0)
    {
      r.recall = (double) r.truePositives / ((double) (r.truePositives + r.falseNegatives));
    }

    results[minNeighbours] = r;

    std::cout << "=============" << std::endl;
    std::cout << "minNeighbours=" << minNeighbours << std::endl;

    std::cout << "precision: " << r.precision << std::endl;
    std::cout << "recall: " << r.recall << std::endl;

    std::cout << "=============" << std::endl;
    std::cout << "Written detailed report to " << outFileName << std::endl;

  }

  outputResults(outFileName);

}

void BallDetectorEvaluator::outputResults(std::string outFileName)
{
  base64::Encoder base64Encoder(64);

  std::ofstream html;
  html.open(outFileName);

  html << "" << std::endl;
  html << "" << std::endl;
  html << "" << std::endl;
  html << "" << std::endl;

  html << "" << std::endl;

  html << "
Overview
" << std::endl;

  html << "
" << std::endl;
  html << "
" << std::endl;
  html << "" << "minNeighbours" << "	" << std::endl;
  html << "" << "precision" << "	" << std::endl;
  html << "" << "recall" << "	" << std::endl;
  html << "
" << std::endl;
  for(std::map<unsigned int, ExperimentResult>::const_iterator it=results.begin();
      it != results.end(); it++)
  {
    const ExperimentResult& r = it->second;
    html << "
" << std::endl;
    html << "" << it->first << "	" << std::endl;
    html << "" << r.precision << "	" << std::endl;
    html << "" << r.recall << "	" << std::endl;
    html << "
" << std::endl;
  }
  html << "
" << std::endl;

  for(std::map<unsigned int, ExperimentResult>::const_iterator it=results.begin();
      it != results.end(); it++)
  {
    const ExperimentResult& r = it->second;

    html << "
minNeighbours=" << it->first << "
" << std::endl;

    html << "
Summary
" << std::endl;
    html << "
precision: " << r.precision << "
recall: " << r.recall << "
" << std::endl;

    unsigned int numOfBalls = r.truePositives + r.falseNegatives;
    html << "
total number of samples: " << r.totalSize << " (" << numOfBalls << " balls, " << (r.totalSize - numOfBalls) << " non-balls)
" << std::endl;

    html << "
back to top
" << std::endl;

    html << "
False Positives
" << std::endl;

    html << "
number: " << r.falsePositives << "
" << std::endl;

    html << "
" << std::endl;
    for(std::list<ErrorEntry>::const_iterator it=r.falsePositivePatches.begin(); it != r.falsePositivePatches.end(); it++)
    {
      // use a data URI to embed the image in PNG format
      std::string imgPNG = createPNG(it->patch);
      html << "<img class=\"patch\" title=\"" << it->idx << "@" << it->fileName
           << "\" src=\"data:image/png;base64," << base64Encoder.encode(imgPNG.c_str(), (int) imgPNG.size())  << "\" />" << std::endl;
    }
    html << "
" << std::endl;

    html << "
False Negatives
" << std::endl;

    html << "
number: " << r.falseNegatives << "
" << std::endl;

    html << "
" << std::endl;
    for(std::list<ErrorEntry>::const_iterator it=r.falseNegativePatches.begin(); it != r.falseNegativePatches.end(); it++)
    {
      // use a data URI to embed the image in PNG format
      std::string imgPNG = createPNG(it->patch);
      html << "<img class=\"patch\" title=\"" << it->idx << "@" << it->fileName
           << "\" src=\"data:image/png;base64," << base64Encoder.encode(imgPNG.c_str(), (int) imgPNG.size())  << "\" />" << std::endl;
    }
    html << "
" << std::endl;
  }

  html << "" << std::endl;
  html.close();
}

unsigned int BallDetectorEvaluator::executeSingleFile(std::string file, ExperimentResult& r)
{
  LogFileScanner logFileScanner(file);

  std::set<unsigned int> expectedBallIdx;
  unsigned int maxValidIdx = loadGroundTruth(file, expectedBallIdx);

  unsigned int patchIdx = 0;

  // read in each frame
  LogFileScanner::FrameIterator secondLastFrame = logFileScanner.end();
  secondLastFrame--;
  for(LogFileScanner::FrameIterator it = logFileScanner.begin(); it != secondLastFrame; it++)
  {
    // reset all existing candidates
    getBallCandidates().reset();
    getBallCandidatesTop().reset();

    LogFileScanner::Frame frame;
    logFileScanner.readFrame(*it, frame);

    // deserialize all ball candidates (bottom and top camera)
    LogFileScanner::Frame::const_iterator frameBallCandidate = frame.find("BallCandidates");
    LogFileScanner::Frame::const_iterator frameBallCandidateTop = frame.find("BallCandidatesTop");
    if(frameBallCandidate!= frame.end())
    {
      std::istrstream stream(frameBallCandidate->second.data.data(), frameBallCandidate->second.data.size());
      naoth::Serializer<BallCandidates>::deserialize(stream, getBallCandidates());
    }
    if(frameBallCandidateTop != frame.end())
    {
      std::istrstream stream(frameBallCandidateTop->second.data.data(), frameBallCandidateTop->second.data.size());
      naoth::Serializer<BallCandidatesTop>::deserialize(stream, getBallCandidatesTop());
    }

    // The python script will always read the bottom patches first, thus in order to have the correct index
    // the loops have to bee in the same order.

    for(const BallCandidates::Patch& p : getBallCandidates().patches)
    {
      evaluatePatch(p, patchIdx++, CameraInfo::Bottom, expectedBallIdx, file, r);
    }

    for(const BallCandidates::Patch& p : getBallCandidatesTop().patches)
    {
      evaluatePatch(p, patchIdx++, CameraInfo::Bottom, expectedBallIdx, file, r);
    }

    if(patchIdx >= maxValidIdx)
    {
      break;
    }
  }

  return patchIdx;
}

void BallDetectorEvaluator::evaluatePatch(const BallCandidates::Patch &p, unsigned int patchIdx,
                                          CameraInfo::CameraID camID,
                                          const std::set<unsigned int>& expectedBallIdx,
                                          std::string fileName,
                                          ExperimentResult& r)
{
  bool expected = expectedBallIdx.find(patchIdx) != expectedBallIdx.end();
  bool actual = classifier.classify(p, minNeighbours) > 0;

  if(expected == actual)
  {
    if(expected)
    {
      r.truePositives++;
    }
  }
  else
  {
    ErrorEntry error;
    error.patch= p;
    error.idx = patchIdx;
    error.fileName = fileName;
    if(actual)
    {
      r.falsePositivePatches.push_back(error);
      r.falsePositives++;
    }
    else
    {
      r.falseNegativePatches.push_back(error);
      r.falseNegatives++;
    }
  }
}

int BallDetectorEvaluator::loadGroundTruth(std::string file, std::set<unsigned int>& expectedBallIdx)
{
  typedef std::vector<picojson::value> array;
  typedef std::map<std::string, picojson::value> object;

  int maxValidIdx = 0;

  size_t dotPos = file.find_last_of('.');

  std::string jsonFile = dotPos == std::string::npos ? file : file.substr(0, dotPos) + ".json";
  std::cout << "loading ground truth from '" << jsonFile << "'" << std::endl;
  std::ifstream groundTruthStream(jsonFile);

  picojson::value parsedJson;
  picojson::parse(parsedJson, groundTruthStream);

  groundTruthStream.close();

  array ballIdx;
  array noBallIdx;
  if(parsedJson.is<object>())
  {
    if(parsedJson.get("ball").is<array>())
    {
      ballIdx = parsedJson.get("ball").get<array>();
    }

    if(parsedJson.get("noball").is<array>())
    {
      noBallIdx = parsedJson.get("noball").get<array>();
    }

    for(picojson::value idx : ballIdx)
    {
      if(idx.is<double>())
      {
        int idxVal = static_cast<int>(idx.get<double>());
        expectedBallIdx.insert(idxVal);
        maxValidIdx = std::max(maxValidIdx, idxVal);
      }
    }

    for(picojson::value idx : noBallIdx)
    {
      if(idx.is<double>())
      {
        int idxVal = static_cast<int>(idx.get<double>());
        maxValidIdx = std::max(maxValidIdx, idxVal);
      }
    }
  }

  return maxValidIdx;
}

std::string BallDetectorEvaluator::createPGM(const BallCandidates::Patch &p)
{
  std::stringstream str;

  // header (we are gray scale, thus P2)
  str << "P2\n";
  // the width and the height of the image
  str << BallCandidates::Patch::SIZE << "\n" << BallCandidates::Patch::SIZE << "\n";
  // the maximum value we use
  str << "255\n";

  // output each pixel
  for(unsigned int y=0; y < BallCandidates::Patch::SIZE; y++)
  {
    for(unsigned int x=0; x < BallCandidates::Patch::SIZE; x++)
    {
      str << (int) p.data[x*BallCandidates::Patch::SIZE + y];
      if(x < BallCandidates::Patch::SIZE - 1)
      {
        str << " ";
      }
    }
    str << "\n";
  }
  return str.str();
}

std::string BallDetectorEvaluator::createPNG(const BallCandidates::Patch &p)
{
  cv::Mat wrappedImg(12, 12, CV_8UC1, (void*) p.data.data());

  std::vector<uchar> buffer;

  cv::imencode(".png", wrappedImg, buffer);

  return std::string(buffer.begin(), buffer.end());
}
<|endoftext|>


<commit_before>#include "main.h"
#include <Eigen/MPRealSupport>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <sstream>

using namespace mpfr;
using namespace Eigen;

void test_mpreal_support()
{
  // set precision to 256 bits (double has only 53 bits)
  mpreal::set_default_prec(256);
  typedef Matrix<mpreal,Eigen::Dynamic,Eigen::Dynamic> MatrixXmp;

  std::cerr << "epsilon =         " << NumTraits<mpreal>::epsilon() << "\n";
  std::cerr << "dummy_precision = " << NumTraits<mpreal>::dummy_precision() << "\n";
  std::cerr << "highest =         " << NumTraits<mpreal>::highest() << "\n";
  std::cerr << "lowest =          " << NumTraits<mpreal>::lowest() << "\n";
  std::cerr << "digits10 =        " << NumTraits<mpreal>::digits10() << "\n";

  for(int i = 0; i < g_repeat; i++) {
    int s = Eigen::internal::random<int>(1,100);
    MatrixXmp A = MatrixXmp::Random(s,s);
    MatrixXmp B = MatrixXmp::Random(s,s);
    MatrixXmp S = A.adjoint() * A;
    MatrixXmp X;

    // Basic stuffs
    VERIFY_IS_APPROX(A.real(), A);
    VERIFY(Eigen::internal::isApprox(A.array().abs2().sum(), A.squaredNorm()));
    VERIFY_IS_APPROX(A.array().exp(),         exp(A.array()));
    VERIFY_IS_APPROX(A.array().abs2().sqrt(), A.array().abs());
    VERIFY_IS_APPROX(A.array().sin(),         sin(A.array()));
    VERIFY_IS_APPROX(A.array().cos(),         cos(A.array()));

    // Cholesky
    X = S.selfadjointView<Lower>().llt().solve(B);
    VERIFY_IS_APPROX((S.selfadjointView<Lower>()*X).eval(),B);

    // partial LU
    X = A.lu().solve(B);
    VERIFY_IS_APPROX((A*X).eval(),B);

    // symmetric eigenvalues
    SelfAdjointEigenSolver<MatrixXmp> eig(S);
    VERIFY_IS_EQUAL(eig.info(), Success);
    VERIFY( (S.selfadjointView<Lower>() * eig.eigenvectors()).isApprox(eig.eigenvectors() * eig.eigenvalues().asDiagonal(), NumTraits<mpreal>::dummy_precision()*1e3) );
  }

  {
    MatrixXmp A(8,3); A.setRandom();
    // test output (interesting things happen in this code)
    std::stringstream stream;
    stream << A;
  }
}
<commit_msg>Extend mpreal unit test to check LLT with complexes.<commit_after>#include "main.h"
#include <Eigen/MPRealSupport>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <sstream>

using namespace mpfr;
using namespace Eigen;

void test_mpreal_support()
{
  // set precision to 256 bits (double has only 53 bits)
  mpreal::set_default_prec(256);
  typedef Matrix<mpreal,Eigen::Dynamic,Eigen::Dynamic> MatrixXmp;
  typedef Matrix<std::complex<mpreal>,Eigen::Dynamic,Eigen::Dynamic> MatrixXcmp;

  std::cerr << "epsilon =         " << NumTraits<mpreal>::epsilon() << "\n";
  std::cerr << "dummy_precision = " << NumTraits<mpreal>::dummy_precision() << "\n";
  std::cerr << "highest =         " << NumTraits<mpreal>::highest() << "\n";
  std::cerr << "lowest =          " << NumTraits<mpreal>::lowest() << "\n";
  std::cerr << "digits10 =        " << NumTraits<mpreal>::digits10() << "\n";

  for(int i = 0; i < g_repeat; i++) {
    int s = Eigen::internal::random<int>(1,100);
    MatrixXmp A = MatrixXmp::Random(s,s);
    MatrixXmp B = MatrixXmp::Random(s,s);
    MatrixXmp S = A.adjoint() * A;
    MatrixXmp X;
    MatrixXcmp Ac = MatrixXcmp::Random(s,s);
    MatrixXcmp Bc = MatrixXcmp::Random(s,s);
    MatrixXcmp Sc = Ac.adjoint() * Ac;
    MatrixXcmp Xc;

    // Basic stuffs
    VERIFY_IS_APPROX(A.real(), A);
    VERIFY(Eigen::internal::isApprox(A.array().abs2().sum(), A.squaredNorm()));
    VERIFY_IS_APPROX(A.array().exp(),         exp(A.array()));
    VERIFY_IS_APPROX(A.array().abs2().sqrt(), A.array().abs());
    VERIFY_IS_APPROX(A.array().sin(),         sin(A.array()));
    VERIFY_IS_APPROX(A.array().cos(),         cos(A.array()));

    // Cholesky
    X = S.selfadjointView<Lower>().llt().solve(B);
    VERIFY_IS_APPROX((S.selfadjointView<Lower>()*X).eval(),B);

    Xc = Sc.selfadjointView<Lower>().llt().solve(Bc);
    VERIFY_IS_APPROX((Sc.selfadjointView<Lower>()*Xc).eval(),Bc);

    // partial LU
    X = A.lu().solve(B);
    VERIFY_IS_APPROX((A*X).eval(),B);

    // symmetric eigenvalues
    SelfAdjointEigenSolver<MatrixXmp> eig(S);
    VERIFY_IS_EQUAL(eig.info(), Success);
    VERIFY( (S.selfadjointView<Lower>() * eig.eigenvectors()).isApprox(eig.eigenvectors() * eig.eigenvalues().asDiagonal(), NumTraits<mpreal>::dummy_precision()*1e3) );
  }

  {
    MatrixXmp A(8,3); A.setRandom();
    // test output (interesting things happen in this code)
    std::stringstream stream;
    stream << A;
  }
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    DicomSeriesReadImageWrite2.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

//  Software Guide : BeginLatex
//
//  Probably the most common representation of datasets in clinical
//  applications is the one that uses sets of DICOM slices in order to compose
//  tridimensional images. This is the case for CT, MRI and PET scanners. It is
//  very common therefore for image analysts to have to process volumetric
//  images that are stored in the form of a set of DICOM files belonging to a
//  common DICOM series.
//
//  The following example illustrates how to use ITK functionalities in order
//  to read a DICOM series into a volume and then save this volume in another
//  file format.
//
//  The example begins by including the appropriate headers. In particular we
//  will need the GDCMImageIO object in order to have access to the
//  capabilities of the GDCM library for reading DICOM files, and the
//  GDCMSeriesFileNames object for generating the lists of filenames
//  identifying the slices of a common volumetric dataset.
//
//  \index{itk::ImageSeriesReader!header}
//  \index{itk::GDCMImageIO!header}
//  \index{itk::GDCMSeriesFileNames!header}
//  \index{itk::ImageFileWriter!header}
//
//  Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
#include "itkGDCMImageIO.h"
#include "itkGDCMSeriesFileNames.h"
#include "itkImageSeriesReader.h"
#include "itkImageFileWriter.h"
// Software Guide : EndCodeSnippet

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

  if( argc < 3 )
    {
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << " DicomDirectory  outputFileName  [seriesName]" << std::endl;
    return EXIT_FAILURE;
    }

// Software Guide : BeginLatex
//
// We define the pixel type and dimension of the image to be read. In this
// particular case, the dimensionality of the image is 3, and we assume a
// \code{signed short} pixel type that is commonly used for X-Rays CT scanners.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef signed short    PixelType;
  const unsigned int      Dimension = 3;

  typedef itk::Image< PixelType, Dimension >         ImageType;
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// We use the image type for instantiating the type of the series reader and
// for constructing one object of its type.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::ImageSeriesReader< ImageType >        ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// A GDCMImageIO object is created and connected to the reader. This object is
// the one that is aware of the internal intricacies of the DICOM format.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::GDCMImageIO       ImageIOType;
  ImageIOType::Pointer dicomIO = ImageIOType::New();

  reader->SetImageIO( dicomIO );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Now we face one of the main challenges of the process of reading a DICOM
// series. That is, to identify from a given directory the set of filenames
// that belong together to the same volumetric image. Fortunately for us, GDCM
// offers functionalities for solving this problem and we just need to invoke
// those functionalities through an ITK class that encapsulates a communication
// with GDCM classes. This ITK object is the GDCMSeriesFileNames. Conveniently
// for us, we only need to pass to this class the name of the directory where
// the DICOM slices are stored. This is done with the
// \code{SetInputDirectory()} method. The GDCMSeriesFileNames object will
// explore the directory and will generate a sequence of filenames for DICOM
// files for one study/series.
//
// \index{itk::GDCMSeriesFileNames!SetInputDirectory()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::GDCMSeriesFileNames NamesGeneratorType;
  NamesGeneratorType::Pointer nameGenerator = NamesGeneratorType::New();

  nameGenerator->SetDirectory( argv[1] );
// Software Guide : EndCodeSnippet

  try
    {
    std::cout << std::endl << "The directory: " << std::endl;
    std::cout << std::endl << argv[1] << std::endl << std::endl;
    std::cout << "Contains the following DICOM Series: ";
    std::cout << std::endl << std::endl;

// Software Guide : BeginLatex
//
// The GDCMSeriesFileNames object first identifies the list of DICOM series
// that are present in the given directory. We receive that list in a reference
// to a container of strings and then we can do things like printing out all
// the series identifiers that the generator had found. Since the process of
// finding the series identifiers can potentially throw exceptions, it is
// wise to put this code inside a try/catch block.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef std::vector< std::string >    SeriesIdContainer;

    const SeriesIdContainer & seriesUID = nameGenerator->GetSeriesUIDs();

    SeriesIdContainer::const_iterator seriesItr = seriesUID.begin();
    SeriesIdContainer::const_iterator seriesEnd = seriesUID.end();
    while( seriesItr != seriesEnd )
      {
      std::cout << seriesItr->c_str() << std::endl;
      seriesItr++;
      }
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Given that it is common to find multiple DICOM series in the same directory,
// we must tell the GDCM classes what specific series do we want to read. In
// this example we do this by checking first if the user has provided a series
// identifier in the command line arguments. If no series identifier has been
// passed, then we simply use the first series found during the exploration of
// the directory.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    std::string seriesIdentifier;

    if( argc > 3 ) // If no optional series identifier
      {
      seriesIdentifier = argv[3];
      }
    else
      {
      seriesIdentifier = seriesUID.begin()->c_str();
      }
// Software Guide : EndCodeSnippet

    std::cout << std::endl << std::endl;
    std::cout << "Now reading series: " << std::endl << std::endl;
    std::cout << seriesIdentifier << std::endl;
    std::cout << std::endl << std::endl;

// Software Guide : BeginLatex
//
// We pass the series identifier to the name generator and ask for all the
// filenames associated to that series. This list is returned in a container of
// strings by the \code{GetFileNames()} method.
//
// \index{itk::GDCMSeriesFileNames!GetFileNames()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef std::vector< std::string >   FileNamesContainer;
    FileNamesContainer fileNames;

    fileNames = nameGenerator->GetFileNames( seriesIdentifier );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
//
// The list of filenames can now be passed to the \doxygen{ImageSeriesReader}
// using the \code{SetFileNames()} method.
//
//  \index{itk::ImageSeriesReader!SetFileNames()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    reader->SetFileNames( fileNames );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Finally we can trigger the reading process by invoking the \code{Update()}
// method in the reader. This call as usual is placed inside a \code{try/catch}
// block.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    try
      {
      reader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
      std::cout << ex << std::endl;
      return EXIT_FAILURE;
      }
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// At this point, we have a volumetric image in memory that we can access by
// invoking the \code{GetOutput()} method of the reader.
//
// Software Guide : EndLatex

// Software Guide : BeginLatex
//
// We proceed now to save the volumetric image in another file, as specified by
// the user in the command line arguments of this program. Thanks to the
// ImageIO factory mechanism, only the filename extension is needed to identify
// the fileformat in this case.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef itk::ImageFileWriter< ImageType > WriterType;
    WriterType::Pointer writer = WriterType::New();

    writer->SetFileName( argv[2] );

    writer->SetInput( reader->GetOutput() );
// Software Guide : EndCodeSnippet

    std::cout  << "Writing the image as " << std::endl << std::endl;
    std::cout  << argv[2] << std::endl << std::endl;

// Software Guide : BeginLatex
//
// The process of writing the image is initiated by invoking the
// \code{Update()} method of the writer.
//
// Software Guide : EndLatex

    try
      {
// Software Guide : BeginCodeSnippet
      writer->Update();
// Software Guide : EndCodeSnippet
      }
    catch (itk::ExceptionObject &ex)
      {
      std::cout << ex << std::endl;
      return EXIT_FAILURE;
      }
    }
  catch (itk::ExceptionObject &ex)
    {
      std::cout << ex << std::endl;
    return EXIT_FAILURE;
    }

// Software Guide : BeginLatex
//
// Note that in addition to writing the volumetric image to a file we could
// have used it as the input for any 3D processing pipeline. Keep in mind that
// DICOM is simply a file format and a network protocol. Once the image data
// has been loaded into memory, it behaves as any other volumetric dataset that
// you could have loaded from any other file format.
//
// Software Guide : EndLatex

  return EXIT_SUCCESS;

}
<commit_msg>STYLE: Added \doxygen for GDCMImageIO and GDCMFileNames.        Fixed comments regarding the use of SetInputDirectory and SetDirectory.        It is still ambiguous when one or the other should be used.<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    DicomSeriesReadImageWrite2.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

//  Software Guide : BeginLatex
//
//  Probably the most common representation of datasets in clinical
//  applications is the one that uses sets of DICOM slices in order to compose
//  tridimensional images. This is the case for CT, MRI and PET scanners. It is
//  very common therefore for image analysts to have to process volumetric
//  images that are stored in the form of a set of DICOM files belonging to a
//  common DICOM series.
//
//  The following example illustrates how to use ITK functionalities in order
//  to read a DICOM series into a volume and then save this volume in another
//  file format.
//
//  The example begins by including the appropriate headers. In particular we
//  will need the \doxygen{GDCMImageIO} object in order to have access to the
//  capabilities of the GDCM library for reading DICOM files, and the
//  \doxygen{GDCMSeriesFileNames} object for generating the lists of filenames
//  identifying the slices of a common volumetric dataset.
//
//  \index{itk::ImageSeriesReader!header}
//  \index{itk::GDCMImageIO!header}
//  \index{itk::GDCMSeriesFileNames!header}
//  \index{itk::ImageFileWriter!header}
//
//  Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
#include "itkGDCMImageIO.h"
#include "itkGDCMSeriesFileNames.h"
#include "itkImageSeriesReader.h"
#include "itkImageFileWriter.h"
// Software Guide : EndCodeSnippet

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

  if( argc < 3 )
    {
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << " DicomDirectory  outputFileName  [seriesName]" << std::endl;
    return EXIT_FAILURE;
    }

// Software Guide : BeginLatex
//
// We define the pixel type and dimension of the image to be read. In this
// particular case, the dimensionality of the image is 3, and we assume a
// \code{signed short} pixel type that is commonly used for X-Rays CT scanners.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef signed short    PixelType;
  const unsigned int      Dimension = 3;

  typedef itk::Image< PixelType, Dimension >         ImageType;
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// We use the image type for instantiating the type of the series reader and
// for constructing one object of its type.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::ImageSeriesReader< ImageType >        ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// A GDCMImageIO object is created and connected to the reader. This object is
// the one that is aware of the internal intricacies of the DICOM format.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::GDCMImageIO       ImageIOType;
  ImageIOType::Pointer dicomIO = ImageIOType::New();

  reader->SetImageIO( dicomIO );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Now we face one of the main challenges of the process of reading a DICOM
// series. That is, to identify from a given directory the set of filenames
// that belong together to the same volumetric image. Fortunately for us, GDCM
// offers functionalities for solving this problem and we just need to invoke
// those functionalities through an ITK class that encapsulates a communication
// with GDCM classes. This ITK object is the GDCMSeriesFileNames. Conveniently
// for us, we only need to pass to this class the name of the directory where
// the DICOM slices are stored. This is done with the \code{SetDirectory()}
// method. The GDCMSeriesFileNames object will explore the directory and will
// generate a sequence of filenames for DICOM files for one study/series.
//
// \index{itk::GDCMSeriesFileNames!SetDirectory()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
  typedef itk::GDCMSeriesFileNames NamesGeneratorType;
  NamesGeneratorType::Pointer nameGenerator = NamesGeneratorType::New();

  nameGenerator->SetDirectory( argv[1] );
// Software Guide : EndCodeSnippet

  try
    {
    std::cout << std::endl << "The directory: " << std::endl;
    std::cout << std::endl << argv[1] << std::endl << std::endl;
    std::cout << "Contains the following DICOM Series: ";
    std::cout << std::endl << std::endl;

// Software Guide : BeginLatex
//
// The GDCMSeriesFileNames object first identifies the list of DICOM series
// that are present in the given directory. We receive that list in a reference
// to a container of strings and then we can do things like printing out all
// the series identifiers that the generator had found. Since the process of
// finding the series identifiers can potentially throw exceptions, it is
// wise to put this code inside a try/catch block.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef std::vector< std::string >    SeriesIdContainer;

    const SeriesIdContainer & seriesUID = nameGenerator->GetSeriesUIDs();

    SeriesIdContainer::const_iterator seriesItr = seriesUID.begin();
    SeriesIdContainer::const_iterator seriesEnd = seriesUID.end();
    while( seriesItr != seriesEnd )
      {
      std::cout << seriesItr->c_str() << std::endl;
      seriesItr++;
      }
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Given that it is common to find multiple DICOM series in the same directory,
// we must tell the GDCM classes what specific series do we want to read. In
// this example we do this by checking first if the user has provided a series
// identifier in the command line arguments. If no series identifier has been
// passed, then we simply use the first series found during the exploration of
// the directory.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    std::string seriesIdentifier;

    if( argc > 3 ) // If no optional series identifier
      {
      seriesIdentifier = argv[3];
      }
    else
      {
      seriesIdentifier = seriesUID.begin()->c_str();
      }
// Software Guide : EndCodeSnippet

    std::cout << std::endl << std::endl;
    std::cout << "Now reading series: " << std::endl << std::endl;
    std::cout << seriesIdentifier << std::endl;
    std::cout << std::endl << std::endl;

// Software Guide : BeginLatex
//
// We pass the series identifier to the name generator and ask for all the
// filenames associated to that series. This list is returned in a container of
// strings by the \code{GetFileNames()} method.
//
// \index{itk::GDCMSeriesFileNames!GetFileNames()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef std::vector< std::string >   FileNamesContainer;
    FileNamesContainer fileNames;

    fileNames = nameGenerator->GetFileNames( seriesIdentifier );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
//
// The list of filenames can now be passed to the \doxygen{ImageSeriesReader}
// using the \code{SetFileNames()} method.
//
//  \index{itk::ImageSeriesReader!SetFileNames()}
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    reader->SetFileNames( fileNames );
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// Finally we can trigger the reading process by invoking the \code{Update()}
// method in the reader. This call as usual is placed inside a \code{try/catch}
// block.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    try
      {
      reader->Update();
      }
    catch (itk::ExceptionObject &ex)
      {
      std::cout << ex << std::endl;
      return EXIT_FAILURE;
      }
// Software Guide : EndCodeSnippet

// Software Guide : BeginLatex
//
// At this point, we have a volumetric image in memory that we can access by
// invoking the \code{GetOutput()} method of the reader.
//
// Software Guide : EndLatex

// Software Guide : BeginLatex
//
// We proceed now to save the volumetric image in another file, as specified by
// the user in the command line arguments of this program. Thanks to the
// ImageIO factory mechanism, only the filename extension is needed to identify
// the fileformat in this case.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet
    typedef itk::ImageFileWriter< ImageType > WriterType;
    WriterType::Pointer writer = WriterType::New();

    writer->SetFileName( argv[2] );

    writer->SetInput( reader->GetOutput() );
// Software Guide : EndCodeSnippet

    std::cout  << "Writing the image as " << std::endl << std::endl;
    std::cout  << argv[2] << std::endl << std::endl;

// Software Guide : BeginLatex
//
// The process of writing the image is initiated by invoking the
// \code{Update()} method of the writer.
//
// Software Guide : EndLatex

    try
      {
// Software Guide : BeginCodeSnippet
      writer->Update();
// Software Guide : EndCodeSnippet
      }
    catch (itk::ExceptionObject &ex)
      {
      std::cout << ex << std::endl;
      return EXIT_FAILURE;
      }
    }
  catch (itk::ExceptionObject &ex)
    {
      std::cout << ex << std::endl;
    return EXIT_FAILURE;
    }

// Software Guide : BeginLatex
//
// Note that in addition to writing the volumetric image to a file we could
// have used it as the input for any 3D processing pipeline. Keep in mind that
// DICOM is simply a file format and a network protocol. Once the image data
// has been loaded into memory, it behaves as any other volumetric dataset that
// you could have loaded from any other file format.
//
// Software Guide : EndLatex

  return EXIT_SUCCESS;

}
<|endoftext|>


<commit_before>// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Many parts from this (Most of the URScript program) comes from the ur_modern_driver
// Copyright 2017, 2018 Simon Rasmussen (refactor)
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
//
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Felix Mauch mauch@fzi.de
 * \date    2019-04-11
 *
 */
//----------------------------------------------------------------------

#include "ur_rtde_driver/ur/ur_driver.h"
#include <memory>

namespace ur_driver
{
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def myProg():
  global steptime = get_steptime()
  textmsg("steptime=", steptime)
  MULT_jointstate = {{JOINT_STATE_REPLACE}}

  SERVO_UNINITIALIZED = -1
  SERVO_IDLE = 0
  SERVO_RUNNING = 1
  cmd_servo_state = SERVO_UNINITIALIZED
  cmd_servo_q = get_actual_joint_positions()
  cmd_servo_q_last = get_actual_joint_positions()
  def set_servo_setpoint(q):
    enter_critical
    cmd_servo_state = SERVO_RUNNING
    cmd_servo_q_last = cmd_servo_q
    cmd_servo_q = q
    exit_critical
  end

  def extrapolate():
    enter_critical
    diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
    cmd_servo_q_last = cmd_servo_q
    cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
    exit_critical

    return cmd_servo_q
  end

  thread servoThread():
    state = SERVO_IDLE
    while True:
      enter_critical
      q = cmd_servo_q
      do_extrapolate = False
      if (cmd_servo_state == SERVO_IDLE):
        do_extrapolate = True
      end
      state = cmd_servo_state
      cmd_servo_state = SERVO_IDLE
      exit_critical
      if do_extrapolate:
        textmsg("No new setpoint received. Extrapolating.")
        q = extrapolate()
        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
      elif state == SERVO_RUNNING:
        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
      else:
        textmsg("Should not be here")
        sync()
      end
    end
    stopj(0.1)
  end
  socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")

  thread_servo = run servoThread()
  keepalive = -2
  params_mult = socket_read_binary_integer(6+1, "reverse_socket")
  keepalive = params_mult[7]
  while keepalive > 0:
    params_mult = socket_read_binary_integer(6+1, "reverse_socket", steptime)
    if params_mult[0] > 0:
      keepalive = params_mult[7]
      q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
      set_servo_setpoint(q)
    else:
      # TODO: Extrapolation goes here
      keepalive = keepalive - 1
    end
  end
  sleep(.1)
  socket_close()
  kill thread_servo
end
)";

ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
  : servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03)
{
  ROS_INFO_STREAM("Initializing RTDE client");
  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_));

  if (!rtde_client_->init())
  {
    throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
  }

  rtde_frequency_ = rtde_client_->getMaxFrequency();
  servoj_time_ = 1.0 / rtde_frequency_;

  // Open Stream to get own IP
  // TODO: Open Primary interface to query version and calibration
  comm::URStream<rtde_interface::PackageHeader> stream(robot_ip, 30001);
  stream.connect();
  std::string local_ip = stream.getIP();

  uint32_t reverse_port = 50001;  // TODO: Make this a parameter

  std::string prog = POSITION_PROGRAM;
  prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
  std::ostringstream out;
  out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
  prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
  prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
  size_t len = prog.size();
  const uint8_t* data = reinterpret_cast<const uint8_t*>(prog.c_str());
  size_t written;

  if (stream.write(data, len, written))
  {
    LOG_INFO("Sent program to robot");
  }
  else
  {
    LOG_ERROR("Could not send program to robot");
  }

  reverse_interface_.reset(new comm::ReverseInterface(reverse_port));

  ROS_INFO_STREAM("Created reverse interface");

  rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
  ROS_INFO_STREAM("Initialization done");
}

std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
{
  // TODO: This goes into the rtde_client
  std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
  uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
  // std::chrono::milliseconds timeout(period_ms);
  std::chrono::milliseconds timeout(100);
  if (rtde_client_->getDataPackage(urpackage, timeout))
  {
    rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
    if (tmp != nullptr)
    {
      urpackage.release();
      return std::unique_ptr<rtde_interface::DataPackage>(tmp);
    }
  }
  return nullptr;
}

bool UrDriver::writeJointCommand(const vector6d_t& values)
{
  if (reverse_interface_)
  {
    reverse_interface_->write(values);
  }
  else
  {
    return false;
  }

  return true;
}
}  // namespace ur_driver
<commit_msg>Use correct package header for temp primary client<commit_after>// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Many parts from this (Most of the URScript program) comes from the ur_modern_driver
// Copyright 2017, 2018 Simon Rasmussen (refactor)
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
//
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Felix Mauch mauch@fzi.de
 * \date    2019-04-11
 *
 */
//----------------------------------------------------------------------

#include "ur_rtde_driver/ur/ur_driver.h"
#include "ur_rtde_driver/primary/package_header.h"
#include <memory>

namespace ur_driver
{
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def myProg():
  global steptime = get_steptime()
  textmsg("steptime=", steptime)
  MULT_jointstate = {{JOINT_STATE_REPLACE}}

  SERVO_UNINITIALIZED = -1
  SERVO_IDLE = 0
  SERVO_RUNNING = 1
  cmd_servo_state = SERVO_UNINITIALIZED
  cmd_servo_q = get_actual_joint_positions()
  cmd_servo_q_last = get_actual_joint_positions()
  def set_servo_setpoint(q):
    enter_critical
    cmd_servo_state = SERVO_RUNNING
    cmd_servo_q_last = cmd_servo_q
    cmd_servo_q = q
    exit_critical
  end

  def extrapolate():
    enter_critical
    diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
    cmd_servo_q_last = cmd_servo_q
    cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
    exit_critical

    return cmd_servo_q
  end

  thread servoThread():
    state = SERVO_IDLE
    while True:
      enter_critical
      q = cmd_servo_q
      do_extrapolate = False
      if (cmd_servo_state == SERVO_IDLE):
        do_extrapolate = True
      end
      state = cmd_servo_state
      cmd_servo_state = SERVO_IDLE
      exit_critical
      if do_extrapolate:
        textmsg("No new setpoint received. Extrapolating.")
        q = extrapolate()
        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
      elif state == SERVO_RUNNING:
        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
      else:
        textmsg("Should not be here")
        sync()
      end
    end
    stopj(0.1)
  end
  socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")

  thread_servo = run servoThread()
  keepalive = -2
  params_mult = socket_read_binary_integer(6+1, "reverse_socket")
  keepalive = params_mult[7]
  while keepalive > 0:
    params_mult = socket_read_binary_integer(6+1, "reverse_socket", steptime)
    if params_mult[0] > 0:
      keepalive = params_mult[7]
      q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
      set_servo_setpoint(q)
    else:
      # TODO: Extrapolation goes here
      keepalive = keepalive - 1
    end
  end
  sleep(.1)
  socket_close()
  kill thread_servo
end
)";

ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
  : servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03)
{
  ROS_INFO_STREAM("Initializing RTDE client");
  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_));

  if (!rtde_client_->init())
  {
    throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
  }

  rtde_frequency_ = rtde_client_->getMaxFrequency();
  servoj_time_ = 1.0 / rtde_frequency_;

  // Open Stream to get own IP
  // TODO: Open Primary interface to query version and calibration
  comm::URStream<primary_interface::PackageHeader> stream(robot_ip, 30001);
  stream.connect();
  std::string local_ip = stream.getIP();

  uint32_t reverse_port = 50001;  // TODO: Make this a parameter

  std::string prog = POSITION_PROGRAM;
  prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
  std::ostringstream out;
  out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
  prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
  prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
  size_t len = prog.size();
  const uint8_t* data = reinterpret_cast<const uint8_t*>(prog.c_str());
  size_t written;

  if (stream.write(data, len, written))
  {
    LOG_INFO("Sent program to robot");
  }
  else
  {
    LOG_ERROR("Could not send program to robot");
  }

  reverse_interface_.reset(new comm::ReverseInterface(reverse_port));

  ROS_INFO_STREAM("Created reverse interface");

  rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
  ROS_INFO_STREAM("Initialization done");
}

std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
{
  // TODO: This goes into the rtde_client
  std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
  uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
  // std::chrono::milliseconds timeout(period_ms);
  std::chrono::milliseconds timeout(100);
  if (rtde_client_->getDataPackage(urpackage, timeout))
  {
    rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
    if (tmp != nullptr)
    {
      urpackage.release();
      return std::unique_ptr<rtde_interface::DataPackage>(tmp);
    }
  }
  return nullptr;
}

bool UrDriver::writeJointCommand(const vector6d_t& values)
{
  if (reverse_interface_)
  {
    reverse_interface_->write(values);
  }
  else
  {
    return false;
  }

  return true;
}
}  // namespace ur_driver
<|endoftext|>