<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you 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 .
 */

#include "reffind.hxx"
#include "global.hxx"
#include "compiler.hxx"
#include "document.hxx"

// STATIC DATA -----------------------------------------------------------

namespace {

// Include colon; addresses in range reference are handled individually.
const sal_Unicode pDelimiters[] = {
    '=','(',')','+','-','*','/','^','&',' ','{','}','<','>',':', 0
};

// =======================================================================

inline bool IsText( sal_Unicode c )
{
    bool bFound = ScGlobal::UnicodeStrChr( pDelimiters, c );
    if (bFound)
        // This is one of delimiters, therefore not text.
        return false;

    // argument separator is configurable.
    const sal_Unicode sep = ScCompiler::GetNativeSymbol(ocSep).GetChar(0);
    return c != sep;
}

inline bool IsText( bool& bQuote, sal_Unicode c )
{
    if (c == '\'')
    {
        bQuote = !bQuote;
        return true;
    }
    if (bQuote)
        return true;

    return IsText(c);
}

/**
 * Find first character position that is considered text.  A character is
 * considered a text when it's within the ascii range and when it's not a
 * delimiter.
 */
sal_Int32 FindStartPos(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    while (nStartPos <= nEndPos && !IsText(p[nStartPos]))
        ++nStartPos;

    return nStartPos;
}

sal_Int32 FindEndPosA1(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    bool bQuote = false;
    sal_Int32 nNewEnd = nStartPos;
    while (nNewEnd <= nEndPos && IsText(bQuote, p[nNewEnd]))
        ++nNewEnd;

    return nNewEnd;
}

sal_Int32 FindEndPosR1C1(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    sal_Int32 nNewEnd = nStartPos;
    p = &p[nStartPos];
    for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
    {
        if (*p == '\'')
        {
            // Skip until the closing quote.
            for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
                if (*p == '\'')
                    break;
            if (nNewEnd > nEndPos)
                break;
        }
        else if (*p == '[')
        {
            // Skip until the closing braket.
            for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
                if (*p == ']')
                    break;
            if (nNewEnd > nEndPos)
                break;
        }
        else if (!IsText(*p))
            break;
    }

    return nNewEnd;
}

/**
 * Find last character position that is considred text, from the specified
 * start position.
 */
sal_Int32 FindEndPos(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos,
                     formula::FormulaGrammar::AddressConvention eConv)
{
    switch (eConv)
    {
        case formula::FormulaGrammar::CONV_XL_R1C1:
            return FindEndPosR1C1(p, nStartPos, nEndPos);
        case formula::FormulaGrammar::CONV_OOO:
        case formula::FormulaGrammar::CONV_XL_A1:
        default:
            return FindEndPosA1(p, nStartPos, nEndPos);
    }
}

void ExpandToTextA1(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos)
{
    while (rStartPos > 0 && IsText(p[rStartPos - 1]) )
        --rStartPos;
    if (rEndPos)
        --rEndPos;
    while (rEndPos+1 < nLen && IsText(p[rEndPos + 1]) )
        ++rEndPos;
}

void ExpandToTextR1C1(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos)
{
    // move back the start position to the first text character.
    if (rStartPos > 0)
    {
        for (--rStartPos; rStartPos > 0; --rStartPos)
        {
            sal_Unicode c = p[rStartPos];
            if (c == '\'')
            {
                // Skip until the opening quote.
                for (--rStartPos; rStartPos > 0; --rStartPos)
                {
                    c = p[rStartPos];
                    if (c == '\'')
                        break;
                }
                if (rStartPos == 0)
                    break;
            }
            else if (c == ']')
            {
                // Skip until the opening braket.
                for (--rStartPos; rStartPos > 0; --rStartPos)
                {
                    c = p[rStartPos];
                    if (c == '[')
                        break;
                }
                if (rStartPos == 0)
                    break;
            }
            else if (!IsText(c))
            {
                ++rStartPos;
                break;
            }
        }
    }

    // move forward the end position to the last text character.
    rEndPos = FindEndPosR1C1(p, rEndPos, nLen-1);
}

void ExpandToText(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos,
                  formula::FormulaGrammar::AddressConvention eConv)
{
    switch (eConv)
    {
        case formula::FormulaGrammar::CONV_XL_R1C1:
            ExpandToTextR1C1(p, nLen, rStartPos, rEndPos);
        break;
        case formula::FormulaGrammar::CONV_OOO:
        case formula::FormulaGrammar::CONV_XL_A1:
        default:
            ExpandToTextA1(p, nLen, rStartPos, rEndPos);
    }
}

}

ScRefFinder::ScRefFinder(
    const OUString& rFormula, const ScAddress& rPos,
    ScDocument* pDoc, formula::FormulaGrammar::AddressConvention eConvP) :
    maFormula(rFormula),
    meConv(eConvP),
    mpDoc(pDoc),
    maPos(rPos),
    mnFound(0),
    mnSelStart(0),
    mnSelEnd(0)
{
}

ScRefFinder::~ScRefFinder()
{
}

static sal_uInt16 lcl_NextFlags( sal_uInt16 nOld )
{
    sal_uInt16 nNew = nOld & 7;                 // die drei Abs-Flags
    nNew = ( nNew - 1 ) & 7;                // weiterzaehlen

    if (!(nOld & SCA_TAB_3D))
        nNew &= ~SCA_TAB_ABSOLUTE;          // not 3D -> never absolute!

    return ( nOld & 0xfff8 ) | nNew;
}

void ScRefFinder::ToggleRel( sal_Int32 nStartPos, sal_Int32 nEndPos )
{
    sal_Int32 nLen = maFormula.getLength();
    if (nLen <= 0)
        return;
    const sal_Unicode* pSource = maFormula.getStr();      // for quick access

    // expand selection, and instead of selection start- and end-index

    if ( nEndPos < nStartPos )
        ::std::swap(nEndPos, nStartPos);

    ExpandToText(pSource, nLen, nStartPos, nEndPos, meConv);

    OUString aResult;
    OUString aExpr;
    OUString aSep;
    ScAddress aAddr;
    mnFound = 0;

    sal_Int32 nLoopStart = nStartPos;
    while ( nLoopStart <= nEndPos )
    {
        // Determine the stard and end positions of a text segment.  Note that
        // the end position returned from FindEndPos may be one position after
        // the last character position in case of the last segment.
        sal_Int32 nEStart = FindStartPos(pSource, nLoopStart, nEndPos);
        sal_Int32 nEEnd  = FindEndPos(pSource, nEStart, nEndPos, meConv);

        aSep  = maFormula.copy(nLoopStart, nEStart-nLoopStart);
        if (nEEnd < maFormula.getLength())
            aExpr = maFormula.copy(nEStart, nEEnd-nEStart);
        else
            aExpr = maFormula.copy(nEStart);

        // Check the validity of the expression, and toggle the relative flag.
        ScAddress::Details aDetails(meConv, maPos.Row(), maPos.Col());
        sal_uInt16 nResult = aAddr.Parse(aExpr, mpDoc, aDetails);
        if ( nResult & SCA_VALID )
        {
            sal_uInt16 nFlags = lcl_NextFlags( nResult );
            aAddr.Format(aExpr, nFlags, mpDoc, aDetails);

            sal_Int32 nAbsStart = nStartPos+aResult.getLength()+aSep.getLength();

            if (!mnFound)                            // first reference ?
                mnSelStart = nAbsStart;
            mnSelEnd = nAbsStart + aExpr.getLength();        // selection, no indizes
            ++mnFound;
        }

        // assemble

        aResult += aSep;
        aResult += aExpr;

        nLoopStart = nEEnd;
    }

    OUString aTotal = maFormula.copy(0, nStartPos);
    aTotal += aResult;
    if (nEndPos < maFormula.getLength()-1)
        aTotal += maFormula.copy(nEndPos);

    maFormula = aTotal;
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>Fix erroneous reference conversion.<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you 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 .
 */

#include "reffind.hxx"
#include "global.hxx"
#include "compiler.hxx"
#include "document.hxx"

// STATIC DATA -----------------------------------------------------------

namespace {

// Include colon; addresses in range reference are handled individually.
const sal_Unicode pDelimiters[] = {
    '=','(',')','+','-','*','/','^','&',' ','{','}','<','>',':', 0
};

// =======================================================================

inline bool IsText( sal_Unicode c )
{
    bool bFound = ScGlobal::UnicodeStrChr( pDelimiters, c );
    if (bFound)
        // This is one of delimiters, therefore not text.
        return false;

    // argument separator is configurable.
    const sal_Unicode sep = ScCompiler::GetNativeSymbol(ocSep).GetChar(0);
    return c != sep;
}

inline bool IsText( bool& bQuote, sal_Unicode c )
{
    if (c == '\'')
    {
        bQuote = !bQuote;
        return true;
    }
    if (bQuote)
        return true;

    return IsText(c);
}

/**
 * Find first character position that is considered text.  A character is
 * considered a text when it's within the ascii range and when it's not a
 * delimiter.
 */
sal_Int32 FindStartPos(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    while (nStartPos <= nEndPos && !IsText(p[nStartPos]))
        ++nStartPos;

    return nStartPos;
}

sal_Int32 FindEndPosA1(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    bool bQuote = false;
    sal_Int32 nNewEnd = nStartPos;
    while (nNewEnd <= nEndPos && IsText(bQuote, p[nNewEnd]))
        ++nNewEnd;

    return nNewEnd;
}

sal_Int32 FindEndPosR1C1(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos)
{
    sal_Int32 nNewEnd = nStartPos;
    p = &p[nStartPos];
    for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
    {
        if (*p == '\'')
        {
            // Skip until the closing quote.
            for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
                if (*p == '\'')
                    break;
            if (nNewEnd > nEndPos)
                break;
        }
        else if (*p == '[')
        {
            // Skip until the closing braket.
            for (; nNewEnd <= nEndPos; ++p, ++nNewEnd)
                if (*p == ']')
                    break;
            if (nNewEnd > nEndPos)
                break;
        }
        else if (!IsText(*p))
            break;
    }

    return nNewEnd;
}

/**
 * Find last character position that is considred text, from the specified
 * start position.
 */
sal_Int32 FindEndPos(const sal_Unicode* p, sal_Int32 nStartPos, sal_Int32 nEndPos,
                     formula::FormulaGrammar::AddressConvention eConv)
{
    switch (eConv)
    {
        case formula::FormulaGrammar::CONV_XL_R1C1:
            return FindEndPosR1C1(p, nStartPos, nEndPos);
        case formula::FormulaGrammar::CONV_OOO:
        case formula::FormulaGrammar::CONV_XL_A1:
        default:
            return FindEndPosA1(p, nStartPos, nEndPos);
    }
}

void ExpandToTextA1(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos)
{
    while (rStartPos > 0 && IsText(p[rStartPos - 1]) )
        --rStartPos;
    if (rEndPos)
        --rEndPos;
    while (rEndPos+1 < nLen && IsText(p[rEndPos + 1]) )
        ++rEndPos;
}

void ExpandToTextR1C1(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos)
{
    // move back the start position to the first text character.
    if (rStartPos > 0)
    {
        for (--rStartPos; rStartPos > 0; --rStartPos)
        {
            sal_Unicode c = p[rStartPos];
            if (c == '\'')
            {
                // Skip until the opening quote.
                for (--rStartPos; rStartPos > 0; --rStartPos)
                {
                    c = p[rStartPos];
                    if (c == '\'')
                        break;
                }
                if (rStartPos == 0)
                    break;
            }
            else if (c == ']')
            {
                // Skip until the opening braket.
                for (--rStartPos; rStartPos > 0; --rStartPos)
                {
                    c = p[rStartPos];
                    if (c == '[')
                        break;
                }
                if (rStartPos == 0)
                    break;
            }
            else if (!IsText(c))
            {
                ++rStartPos;
                break;
            }
        }
    }

    // move forward the end position to the last text character.
    rEndPos = FindEndPosR1C1(p, rEndPos, nLen-1);
}

void ExpandToText(const sal_Unicode* p, sal_Int32 nLen, sal_Int32& rStartPos, sal_Int32& rEndPos,
                  formula::FormulaGrammar::AddressConvention eConv)
{
    switch (eConv)
    {
        case formula::FormulaGrammar::CONV_XL_R1C1:
            ExpandToTextR1C1(p, nLen, rStartPos, rEndPos);
        break;
        case formula::FormulaGrammar::CONV_OOO:
        case formula::FormulaGrammar::CONV_XL_A1:
        default:
            ExpandToTextA1(p, nLen, rStartPos, rEndPos);
    }
}

}

ScRefFinder::ScRefFinder(
    const OUString& rFormula, const ScAddress& rPos,
    ScDocument* pDoc, formula::FormulaGrammar::AddressConvention eConvP) :
    maFormula(rFormula),
    meConv(eConvP),
    mpDoc(pDoc),
    maPos(rPos),
    mnFound(0),
    mnSelStart(0),
    mnSelEnd(0)
{
}

ScRefFinder::~ScRefFinder()
{
}

static sal_uInt16 lcl_NextFlags( sal_uInt16 nOld )
{
    sal_uInt16 nNew = nOld & 7;                 // die drei Abs-Flags
    nNew = ( nNew - 1 ) & 7;                // weiterzaehlen

    if (!(nOld & SCA_TAB_3D))
        nNew &= ~SCA_TAB_ABSOLUTE;          // not 3D -> never absolute!

    return ( nOld & 0xfff8 ) | nNew;
}

void ScRefFinder::ToggleRel( sal_Int32 nStartPos, sal_Int32 nEndPos )
{
    sal_Int32 nLen = maFormula.getLength();
    if (nLen <= 0)
        return;
    const sal_Unicode* pSource = maFormula.getStr();      // for quick access

    // expand selection, and instead of selection start- and end-index

    if ( nEndPos < nStartPos )
        ::std::swap(nEndPos, nStartPos);

    ExpandToText(pSource, nLen, nStartPos, nEndPos, meConv);

    OUString aResult;
    OUString aExpr;
    OUString aSep;
    ScAddress aAddr;
    mnFound = 0;

    sal_Int32 nLoopStart = nStartPos;
    while ( nLoopStart <= nEndPos )
    {
        // Determine the stard and end positions of a text segment.  Note that
        // the end position returned from FindEndPos may be one position after
        // the last character position in case of the last segment.
        sal_Int32 nEStart = FindStartPos(pSource, nLoopStart, nEndPos);
        sal_Int32 nEEnd  = FindEndPos(pSource, nEStart, nEndPos, meConv);

        aSep  = maFormula.copy(nLoopStart, nEStart-nLoopStart);
        if (nEEnd < maFormula.getLength())
            aExpr = maFormula.copy(nEStart, nEEnd-nEStart);
        else
            aExpr = maFormula.copy(nEStart);

        // Check the validity of the expression, and toggle the relative flag.
        ScAddress::Details aDetails(meConv, maPos.Row(), maPos.Col());
        sal_uInt16 nResult = aAddr.Parse(aExpr, mpDoc, aDetails);
        if ( nResult & SCA_VALID )
        {
            sal_uInt16 nFlags = lcl_NextFlags( nResult );
            aAddr.Format(aExpr, nFlags, mpDoc, aDetails);

            sal_Int32 nAbsStart = nStartPos+aResult.getLength()+aSep.getLength();

            if (!mnFound)                            // first reference ?
                mnSelStart = nAbsStart;
            mnSelEnd = nAbsStart + aExpr.getLength();        // selection, no indizes
            ++mnFound;
        }

        // assemble

        aResult += aSep;
        aResult += aExpr;

        nLoopStart = nEEnd;
    }

    OUString aTotal = maFormula.copy(0, nStartPos);
    aTotal += aResult;
    if (nEndPos < maFormula.getLength()-1)
        aTotal += maFormula.copy(nEndPos+1);

    maFormula = aTotal;
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<|endoftext|>


<commit_before>#include "Energia.h"

#include "Ethernet.h"
#include "EthernetClient.h"
#include "EthernetServer.h"

#include "driverlib/interrupt.h"

/* directives for disabling and enabling interrupts */
#define INT_PROTECT_INIT(x)    int x = 0
#define INT_PROTECT(x)         x=IntMasterDisable()
#define INT_UNPROTECT(x)       do{if(!x)IntMasterEnable();}while(0)

/* SYNC_FETCH_AND_NULL: atomic{ tmp=*x; *x=NULL; return tmp; } */
#define SYNC_FETCH_AND_NULL(x)   (__sync_fetch_and_and(x, NULL))

EthernetClient::EthernetClient() {
	_connected = false;
	cs = &client_state;
	cs->mode = true;
	cs->cpcb = NULL;
	cs->p = NULL;
}

EthernetClient::EthernetClient(struct client *c) {
	if (c == NULL) {
		_connected = false;
		cs = &client_state;
		cs->cpcb = NULL;
		cs->p = NULL;
		return;
	}
	_connected = true;
	cs = c;
	cs->mode = false;
}

err_t EthernetClient::do_poll(void *arg, struct tcp_pcb *cpcb) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	if (client->_connected) {
		if (cpcb->keep_cnt_sent++ > 4) {
			cpcb->keep_cnt_sent = 0;
			/* Stop polling */
			tcp_poll(cpcb, NULL, 0);
			tcp_abort(cpcb);
			if (client->cs->cpcb == cpcb) /* cs may be already re-used by another connection */
			{
				client->cs->port = 0;
				client->cs->cpcb = 0;
			}
			return ERR_ABRT; /* calling tcp_abort() must return ERR_ABRT */
		}
		/* Send tcp keep alive probe */
		tcp_keepalive(cpcb);
		return ERR_OK;
	}

	/* We only end up here if the connection failed to close
	 * in an earlier call to tcp_close */
	err_t err = tcp_close(cpcb);

	if (err != ERR_OK) {
		/* error closing, try again later in poll (every 2 sec) */
		tcp_poll(cpcb, do_poll, 4);
	}

	if (client->cs->cpcb == cpcb) /* cs may be already re-used by another connection */
	{
		client->cs->cpcb = 0;
		client->cs->port = 0;
	}

	return err;
}

void EthernetClient::do_err(void * arg, err_t err) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	if (client->_connected) {
		client->_connected = false;
		return;
	}

	/*
	 * Set connected to true to finish connecting.
	 * ::connect wil take care of figuring out if we are truly connected
	 * by looking at the socket state
	 */
	client->_connected = true;
}

err_t EthernetClient::do_connected(void * arg, struct tcp_pcb * tpcb,
		err_t err) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	client->_connected = true;

	return err;
}

err_t EthernetClient::do_recv(void *arg, struct tcp_pcb *cpcb, struct pbuf *p,
		err_t err) {
	/*
	 * Get the client object from the argument
	 * to get access to variables and functions
	 */
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	/* p==0 for end-of-connection (TCP_FIN packet) */
	if (p == 0) {
		client->_connected = false;
		return ERR_OK;
	}

	if (client->cs->p != 0)
		pbuf_cat((pbuf*)client->cs->p, p);
	else
		client->cs->p = p;

	return ERR_OK;
}

void EthernetClient::do_dns(const char *name, struct ip_addr *ipaddr,
		void *arg) {
	ip_addr_t *result = (ip_addr_t *) arg;

	/* BEWARE: lwip stack has been modified to set ipaddr
	 * to IPADDR_NONE if the lookup failed */
	result->addr = ipaddr->addr;
}

int EthernetClient::connect(const char* host, uint16_t port) {
	ip_addr_t ip;
	ip.addr = 0;

	dns_gethostbyname(host, &ip, do_dns, &ip);

	while (!ip.addr) {
		delay(10);
	}

	if (ip.addr == IPADDR_NONE)
		return false;

	return (connect(IPAddress(ip.addr), port));
}

int EthernetClient::connect(IPAddress ip, uint16_t port) {
	ip_addr_t dest;
	dest.addr = ip;

	cs->cpcb = tcp_new();
	cs->read = 0;
	cs->p = NULL;

	if (cs->cpcb == NULL) {
		return false;
	}

	tcp_arg((tcp_pcb*)cs->cpcb, this);
	tcp_recv((tcp_pcb*)cs->cpcb, do_recv);
	tcp_err((tcp_pcb*)cs->cpcb, do_err);

	uint8_t val = tcp_connect((tcp_pcb *)cs->cpcb, &dest, port, do_connected);

	if (val != ERR_OK) {
		return false;
	}

	/* Wait for the connection.
	 * Abort if the connection does not succeed within 10 sec */
	unsigned long then = millis();

	while (!_connected) {
		unsigned long now = millis();
		delay(10);
		if (now - then > CONNECTION_TIMEOUT) {
			tcp_close((tcp_pcb*)cs->cpcb);
			cs->cpcb = NULL;
			return false;
		}
	}

	if (cs->cpcb->state != ESTABLISHED) {
		_connected = false;
	}

	/* Poll to determine if the peer is still alive */
	tcp_poll((tcp_pcb*)cs->cpcb, do_poll, 10);
	return _connected;
}

size_t EthernetClient::write(uint8_t b) {
	return write(&b, 1);
}

size_t EthernetClient::write(const uint8_t *buf, size_t size) {
	uint32_t i = 0, inc = 0;
	boolean stuffed_buffer = false;

	struct tcp_pcb * cpcb = (tcp_pcb*)cs->cpcb; /* cs->cpcb may change to NULL during interrupt servicing */

	if (!cpcb)
		return 0;

	// Attempt to write in 1024-byte increments.
	while (i < size) {
		inc = (size - i) < 1024 ? size - i : 1024;
		err_t err = tcp_write(cpcb, buf + i, inc, TCP_WRITE_FLAG_COPY);
		if (err != ERR_MEM) {
			// Keep enqueueing the lwIP buffer until it's full...
			i += inc;
			stuffed_buffer = false;
		} else {
			if (!stuffed_buffer) {
				// Buffer full; force output
				if (cs->mode)
					tcp_output(cpcb);
				stuffed_buffer = true;
			} else {
				delay(1); // else wait a little bit for lwIP to flush its buffers
			}
		}
	}
	// flush any remaining queue contents
	if (!stuffed_buffer) {
		if (cs->mode)
			tcp_output(cpcb);
	}

	return size;
}

int EthernetClient::available() {
	struct pbuf * p = (pbuf*)cs->p; /* cs->p may change to NULL during interrupt servicing */
	if (!p)
		return 0;
	return p->tot_len - cs->read;
}

int EthernetClient::port() {
	return cs->port;
}

int EthernetClient::readLocked() {
	INT_PROTECT_INIT(oldLevel);

	/* protect the code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	if (!available()) {
		INT_UNPROTECT(oldLevel);
		return -1;
	}

	if (!cs->cpcb) {
		INT_UNPROTECT(oldLevel);
		return -1;
	}

	uint8_t *buf = (uint8_t *) cs->p->payload;
	uint8_t b = buf[cs->read];
	cs->read++;

	tcp_recved((tcp_pcb*)cs->cpcb, cs->read);

	if ((cs->read == cs->p->len) && cs->p->next) {
		cs->read = 0;
		struct pbuf * q = (pbuf*)cs->p;
		cs->p = cs->p->next;
		/* Increase ref count on p->next
		 * 1->3->1->etc */
		pbuf_ref((pbuf*)cs->p);
		/* Free p which decreases ref count of the chain
		 * and frees up to p->next in this case
		 * ...->1->1->etc */
		pbuf_free(q);
	} else if (cs->read == cs->p->len) {
		cs->read = 0;
		pbuf_free((pbuf*)cs->p);
		cs->p = NULL;
	}

	INT_UNPROTECT(oldLevel);

	return b;
}

int EthernetClient::read() {
	return readLocked();
}

int EthernetClient::read(uint8_t *buf, size_t size) {
	uint16_t i;
	int b;

	if (available() <= 0)
		return -1;

	/* TODO: Replace lazy method with optimized on */
	for (i = 0; i < size; i++) {
		b = read();
		if (b == -1)
			break;
		buf[i] = b;
	}

	return i;
}

int EthernetClient::peek() {
	INT_PROTECT_INIT(oldLevel);
	uint8_t b;

	/* protect code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	if (!available()) {
		INT_UNPROTECT(oldLevel);
		return -1;
	}

	uint8_t *buf = (uint8_t *) cs->p->payload;
	b = buf[cs->read];

	INT_UNPROTECT(oldLevel);

	return b;
}

void EthernetClient::flush() {
	INT_PROTECT_INIT(oldLevel);
	/* protect code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);
	if (available()) {
		cs->read = cs->p->tot_len;
		tcp_recved((tcp_pcb*)cs->cpcb, 0);
	}
	INT_UNPROTECT(oldLevel);
}

void EthernetClient::stop() {
	/* Stop frees any resources including any unread buffers */
	err_t err;

	INT_PROTECT_INIT(oldLevel);

	/* protect the code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	struct tcp_pcb * cpcb_copy = (tcp_pcb *) SYNC_FETCH_AND_NULL(&cs->cpcb);
	struct pbuf * p_copy = (pbuf *) SYNC_FETCH_AND_NULL(&cs->p);
	_connected = false;
	cs->port = 0;

	if (cpcb_copy) {
		tcp_err(cpcb_copy, NULL);

		if (p_copy) {
			tcp_recved(cpcb_copy, p_copy->tot_len);
			pbuf_free(p_copy);
		}

		err = tcp_close(cpcb_copy);

		if (err != ERR_OK) {
			/* Error closing, try again later in poll (every 2 sec) */
			tcp_poll(cpcb_copy, do_poll, 4);
		}
	}

	INT_UNPROTECT(oldLevel);
}

uint8_t EthernetClient::connected() {
	/* TODO: test the local client mode and _connected flag */
	return (available() || (status() == ESTABLISHED) || _connected);
}

uint8_t EthernetClient::status() {
	struct tcp_pcb * cpcb = (tcp_pcb*)cs->cpcb;
	if (cpcb == NULL)
		return CLOSED;
	return cpcb->state;
}

EthernetClient::operator bool() {
	return (status() == ESTABLISHED);
}
<commit_msg>Fix read() issue when other end closes the connection<commit_after>#include "Energia.h"

#include "Ethernet.h"
#include "EthernetClient.h"
#include "EthernetServer.h"

#include "driverlib/interrupt.h"

/* directives for disabling and enabling interrupts */
#define INT_PROTECT_INIT(x)    int x = 0
#define INT_PROTECT(x)         x=IntMasterDisable()
#define INT_UNPROTECT(x)       do{if(!x)IntMasterEnable();}while(0)

/* SYNC_FETCH_AND_NULL: atomic{ tmp=*x; *x=NULL; return tmp; } */
#define SYNC_FETCH_AND_NULL(x)   (__sync_fetch_and_and(x, NULL))

EthernetClient::EthernetClient() {
	_connected = false;
	cs = &client_state;
	cs->mode = true;
	cs->cpcb = NULL;
	cs->p = NULL;
}

EthernetClient::EthernetClient(struct client *c) {
	if (c == NULL) {
		_connected = false;
		cs = &client_state;
		cs->cpcb = NULL;
		cs->p = NULL;
		return;
	}
	_connected = true;
	cs = c;
	cs->mode = false;
}

err_t EthernetClient::do_poll(void *arg, struct tcp_pcb *cpcb) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	if (client->_connected) {
		if (cpcb->keep_cnt_sent++ > 4) {
			cpcb->keep_cnt_sent = 0;
			/* Stop polling */
			tcp_poll(cpcb, NULL, 0);
			tcp_abort(cpcb);
			if (client->cs->cpcb == cpcb) /* cs may be already re-used by another connection */
			{
				client->cs->port = 0;
				client->cs->cpcb = 0;
			}
			return ERR_ABRT; /* calling tcp_abort() must return ERR_ABRT */
		}
		/* Send tcp keep alive probe */
		tcp_keepalive(cpcb);
		return ERR_OK;
	}

	/* We only end up here if the connection failed to close
	 * in an earlier call to tcp_close */
	err_t err = tcp_close(cpcb);

	if (err != ERR_OK) {
		/* error closing, try again later in poll (every 2 sec) */
		tcp_poll(cpcb, do_poll, 4);
	}

	if (client->cs->cpcb == cpcb) /* cs may be already re-used by another connection */
	{
		client->cs->cpcb = 0;
		client->cs->port = 0;
	}

	return err;
}

void EthernetClient::do_err(void * arg, err_t err) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	if (client->_connected) {
		client->_connected = false;
		return;
	}

	/*
	 * Set connected to true to finish connecting.
	 * ::connect wil take care of figuring out if we are truly connected
	 * by looking at the socket state
	 */
	client->_connected = true;
}

err_t EthernetClient::do_connected(void * arg, struct tcp_pcb * tpcb,
		err_t err) {
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	client->_connected = true;

	return err;
}

err_t EthernetClient::do_recv(void *arg, struct tcp_pcb *cpcb, struct pbuf *p,
		err_t err) {
	/*
	 * Get the client object from the argument
	 * to get access to variables and functions
	 */
	EthernetClient *client = static_cast<EthernetClient*>(arg);

	/* p==0 for end-of-connection (TCP_FIN packet) */
	if (p == 0) {
		client->_connected = false;
		return ERR_OK;
	}

	if (client->cs->p != 0)
		pbuf_cat((pbuf*)client->cs->p, p);
	else
		client->cs->p = p;

	return ERR_OK;
}

void EthernetClient::do_dns(const char *name, struct ip_addr *ipaddr,
		void *arg) {
	ip_addr_t *result = (ip_addr_t *) arg;

	/* BEWARE: lwip stack has been modified to set ipaddr
	 * to IPADDR_NONE if the lookup failed */
	result->addr = ipaddr->addr;
}

int EthernetClient::connect(const char* host, uint16_t port) {
	ip_addr_t ip;
	ip.addr = 0;

	dns_gethostbyname(host, &ip, do_dns, &ip);

	while (!ip.addr) {
		delay(10);
	}

	if (ip.addr == IPADDR_NONE)
		return false;

	return (connect(IPAddress(ip.addr), port));
}

int EthernetClient::connect(IPAddress ip, uint16_t port) {
	ip_addr_t dest;
	dest.addr = ip;

	cs->cpcb = tcp_new();
	cs->read = 0;
	cs->p = NULL;

	if (cs->cpcb == NULL) {
		return false;
	}

	tcp_arg((tcp_pcb*)cs->cpcb, this);
	tcp_recv((tcp_pcb*)cs->cpcb, do_recv);
	tcp_err((tcp_pcb*)cs->cpcb, do_err);

	uint8_t val = tcp_connect((tcp_pcb *)cs->cpcb, &dest, port, do_connected);

	if (val != ERR_OK) {
		return false;
	}

	/* Wait for the connection.
	 * Abort if the connection does not succeed within 10 sec */
	unsigned long then = millis();

	while (!_connected) {
		unsigned long now = millis();
		delay(10);
		if (now - then > CONNECTION_TIMEOUT) {
			tcp_close((tcp_pcb*)cs->cpcb);
			cs->cpcb = NULL;
			return false;
		}
	}

	if (cs->cpcb->state != ESTABLISHED) {
		_connected = false;
	}

	/* Poll to determine if the peer is still alive */
	tcp_poll((tcp_pcb*)cs->cpcb, do_poll, 10);
	return _connected;
}

size_t EthernetClient::write(uint8_t b) {
	return write(&b, 1);
}

size_t EthernetClient::write(const uint8_t *buf, size_t size) {
	uint32_t i = 0, inc = 0;
	boolean stuffed_buffer = false;

	struct tcp_pcb * cpcb = (tcp_pcb*)cs->cpcb; /* cs->cpcb may change to NULL during interrupt servicing */

	if (!cpcb)
		return 0;

	// Attempt to write in 1024-byte increments.
	while (i < size) {
		inc = (size - i) < 1024 ? size - i : 1024;
		err_t err = tcp_write(cpcb, buf + i, inc, TCP_WRITE_FLAG_COPY);
		if (err != ERR_MEM) {
			// Keep enqueueing the lwIP buffer until it's full...
			i += inc;
			stuffed_buffer = false;
		} else {
			if (!stuffed_buffer) {
				// Buffer full; force output
				if (cs->mode)
					tcp_output(cpcb);
				stuffed_buffer = true;
			} else {
				delay(1); // else wait a little bit for lwIP to flush its buffers
			}
		}
	}
	// flush any remaining queue contents
	if (!stuffed_buffer) {
		if (cs->mode)
			tcp_output(cpcb);
	}

	return size;
}

int EthernetClient::available() {
	struct pbuf * p = (pbuf*)cs->p; /* cs->p may change to NULL during interrupt servicing */
	if (!p)
		return 0;
	return p->tot_len - cs->read;
}

int EthernetClient::port() {
	return cs->port;
}

int EthernetClient::readLocked() {
	INT_PROTECT_INIT(oldLevel);

	/* protect the code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	if (!available()) {
		INT_UNPROTECT(oldLevel);
		return -1;
	}

	uint8_t *buf = (uint8_t *) cs->p->payload;
	uint8_t b = buf[cs->read];
	cs->read++;

	/* Indicate data was received only if still connected */
	if (cs->cpcb) {
		tcp_recved((tcp_pcb*)cs->cpcb, cs->read);
	}

	/* Read any data still in the buffer regardless of connection state */
	if ((cs->read == cs->p->len) && cs->p->next) {
		cs->read = 0;
		struct pbuf * q = (pbuf*)cs->p;
		cs->p = cs->p->next;
		/* Increase ref count on p->next
		 * 1->3->1->etc */
		pbuf_ref((pbuf*)cs->p);
		/* Free p which decreases ref count of the chain
		 * and frees up to p->next in this case
		 * ...->1->1->etc */
		pbuf_free(q);
	} else if (cs->read == cs->p->len) {
		cs->read = 0;
		pbuf_free((pbuf*)cs->p);
		cs->p = NULL;
	}

	INT_UNPROTECT(oldLevel);

	return b;
}

int EthernetClient::read() {
	return readLocked();
}

int EthernetClient::read(uint8_t *buf, size_t size) {
	uint16_t i;
	int b;

	if (available() <= 0)
		return -1;

	/* TODO: Replace lazy method with optimized on */
	for (i = 0; i < size; i++) {
		b = read();
		if (b == -1)
			break;
		buf[i] = b;
	}

	return i;
}

int EthernetClient::peek() {
	INT_PROTECT_INIT(oldLevel);
	uint8_t b;

	/* protect code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	if (!available()) {
		INT_UNPROTECT(oldLevel);
		return -1;
	}

	uint8_t *buf = (uint8_t *) cs->p->payload;
	b = buf[cs->read];

	INT_UNPROTECT(oldLevel);

	return b;
}

void EthernetClient::flush() {
	INT_PROTECT_INIT(oldLevel);
	/* protect code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);
	if (available()) {
		cs->read = cs->p->tot_len;
		tcp_recved((tcp_pcb*)cs->cpcb, 0);
	}
	INT_UNPROTECT(oldLevel);
}

void EthernetClient::stop() {
	/* Stop frees any resources including any unread buffers */
	err_t err;

	INT_PROTECT_INIT(oldLevel);

	/* protect the code from preemption of the ethernet interrupt servicing */
	INT_PROTECT(oldLevel);

	struct tcp_pcb * cpcb_copy = (tcp_pcb *) SYNC_FETCH_AND_NULL(&cs->cpcb);
	struct pbuf * p_copy = (pbuf *) SYNC_FETCH_AND_NULL(&cs->p);
	_connected = false;
	cs->port = 0;

	if (cpcb_copy) {
		tcp_err(cpcb_copy, NULL);

		if (p_copy) {
			tcp_recved(cpcb_copy, p_copy->tot_len);
			pbuf_free(p_copy);
		}

		err = tcp_close(cpcb_copy);

		if (err != ERR_OK) {
			/* Error closing, try again later in poll (every 2 sec) */
			tcp_poll(cpcb_copy, do_poll, 4);
		}
	}

	INT_UNPROTECT(oldLevel);
}

uint8_t EthernetClient::connected() {
	/* TODO: test the local client mode and _connected flag */
	return (available() || (status() == ESTABLISHED) || _connected);
}

uint8_t EthernetClient::status() {
	struct tcp_pcb * cpcb = (tcp_pcb*)cs->cpcb;
	if (cpcb == NULL)
		return CLOSED;
	return cpcb->state;
}

EthernetClient::operator bool() {
	return (status() == ESTABLISHED);
}
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the QtCore module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "mock_cntdb.h"

CContactDatabase* CContactDatabase::OpenL()
{
    return new CContactDatabase;
}

CContactDatabase* CContactDatabase::CreateL()
{
    return new CContactDatabase;
}

void CContactDatabase::AddObserverL(MContactDbObserver& aObserver)
{
    iObserver = &aObserver;
}

void CContactDatabase::sendEventsL()
{
    if (!iObserver)
        User::Leave(KErrBadHandle);
    TContactDbObserverEvent event;
    event.iType = EContactDbObserverEventContactAdded;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventOwnCardDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventContactDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventContactChanged;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupAdded;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupChanged;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventOwnCardChanged;
    iObserver->HandleDatabaseEventL(event);
}

CContactChangeNotifier::CContactChangeNotifier(CContactDatabase& aDbase, MContactDbObserver* aObserver)
{
    iDbase = &aDbase;
    iDbase->AddObserverL(*aObserver);
}

CContactChangeNotifier* CContactChangeNotifier::NewL(CContactDatabase& aDatabase, MContactDbObserver *aObserver)
{
    return new CContactChangeNotifier(aDatabase, aObserver);
}

<commit_msg>Fix build break in unit test<commit_after>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the QtCore module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "mock_cntdb.h"

#include <e32err.h>

CContactDatabase* CContactDatabase::OpenL()
{
    return new CContactDatabase;
}

CContactDatabase* CContactDatabase::CreateL()
{
    return new CContactDatabase;
}

void CContactDatabase::AddObserverL(MContactDbObserver& aObserver)
{
    iObserver = &aObserver;
}

void CContactDatabase::sendEventsL()
{
    if (!iObserver)
        User::Leave(KErrBadHandle);
    TContactDbObserverEvent event;
    event.iType = EContactDbObserverEventContactAdded;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventOwnCardDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventContactDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventContactChanged;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupAdded;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupDeleted;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventGroupChanged;
    iObserver->HandleDatabaseEventL(event);
    event.iType = EContactDbObserverEventOwnCardChanged;
    iObserver->HandleDatabaseEventL(event);
}

CContactChangeNotifier::CContactChangeNotifier(CContactDatabase& aDbase, MContactDbObserver* aObserver)
{
    iDbase = &aDbase;
    iDbase->AddObserverL(*aObserver);
}

CContactChangeNotifier* CContactChangeNotifier::NewL(CContactDatabase& aDatabase, MContactDbObserver *aObserver)
{
    return new CContactChangeNotifier(aDatabase, aObserver);
}

<|endoftext|>


<commit_before>#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>

#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/transformation_estimation_svd.h>

using namespace std;
using namespace sensor_msgs;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
using namespace pcl::registration;
PointCloud<PointXYZ>::Ptr src, tgt;

////////////////////////////////////////////////////////////////////////////////
void
estimateKeypoints (const PointCloud<PointXYZ>::Ptr &src,
                   const PointCloud<PointXYZ>::Ptr &tgt,
                   PointCloud<PointXYZ> &keypoints_src,
                   PointCloud<PointXYZ> &keypoints_tgt)
{
  PointCloud<int> keypoints_src_idx, keypoints_tgt_idx;
  // Get an uniform grid of keypoints
  UniformSampling<PointXYZ> uniform;
  uniform.setRadiusSearch (1);  // 1m

  uniform.setInputCloud (src);
  uniform.compute (keypoints_src_idx);
  copyPointCloud<PointXYZ, PointXYZ> (*src, keypoints_src_idx.points, keypoints_src);

  uniform.setInputCloud (tgt);
  uniform.compute (keypoints_tgt_idx);
  copyPointCloud<PointXYZ, PointXYZ> (*tgt, keypoints_tgt_idx.points, keypoints_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer source_pcd keypoints_src.pcd -ps 1 -ps 10
  savePCDFileBinary ("keypoints_src.pcd", keypoints_src);
  savePCDFileBinary ("keypoints_tgt.pcd", keypoints_tgt);
}

////////////////////////////////////////////////////////////////////////////////
void
estimateNormals (const PointCloud<PointXYZ>::Ptr &src,
                 const PointCloud<PointXYZ>::Ptr &tgt,
                 PointCloud<Normal> &normals_src,
                 PointCloud<Normal> &normals_tgt)
{
  NormalEstimation<PointXYZ, Normal> normal_est;
  normal_est.setInputCloud (src);
  normal_est.setRadiusSearch (0.5);  // 50cm
  normal_est.compute (normals_src);

  normal_est.setInputCloud (tgt);
  normal_est.compute (normals_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer normals_src.pcd
  PointCloud<PointNormal> s, t;
  copyPointCloud<PointXYZ, PointNormal> (*src, s);
  copyPointCloud<Normal, PointNormal> (normals_src, s);
  copyPointCloud<PointXYZ, PointNormal> (*tgt, t);
  copyPointCloud<Normal, PointNormal> (normals_tgt, t);
  savePCDFileBinary ("normals_src.pcd", s);
  savePCDFileBinary ("normals_tgt.pcd", t);
}

////////////////////////////////////////////////////////////////////////////////
void
estimateFPFH (const PointCloud<PointXYZ>::Ptr &src,
              const PointCloud<PointXYZ>::Ptr &tgt,
              const PointCloud<Normal>::Ptr &normals_src,
              const PointCloud<Normal>::Ptr &normals_tgt,
              const PointCloud<PointXYZ>::Ptr &keypoints_src,
              const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
              PointCloud<FPFHSignature33> &fpfhs_src,
              PointCloud<FPFHSignature33> &fpfhs_tgt)
{
  FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
  fpfh_est.setInputCloud (keypoints_src);
  fpfh_est.setInputNormals (normals_src);
  fpfh_est.setRadiusSearch (1); // 1m
  fpfh_est.setSearchSurface (src);
  fpfh_est.compute (fpfhs_src);

  fpfh_est.setInputCloud (keypoints_tgt);
  fpfh_est.setInputNormals (normals_tgt);
  fpfh_est.setSearchSurface (tgt);
  fpfh_est.compute (fpfhs_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer fpfhs_src.pcd
  PointCloud2 s, t, out;
  toROSMsg (*src, s); toROSMsg (fpfhs_src, t); concatenateFields (s, t, out);
  savePCDFile ("fpfhs_src.pcd", out);
  toROSMsg (*tgt, s); toROSMsg (fpfhs_tgt, t); concatenateFields (s, t, out);
  savePCDFile ("fpfhs_tgt.pcd", out);
}

////////////////////////////////////////////////////////////////////////////////
void
findCorrespondences (const PointCloud<FPFHSignature33>::Ptr &fpfhs_src,
                     const PointCloud<FPFHSignature33>::Ptr &fpfhs_tgt,
                     Correspondences &all_correspondences)
{
  CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;
  est.setInputCloud (fpfhs_src);
  est.setInputTarget (fpfhs_tgt);
  est.determineReciprocalCorrespondences (all_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
                          const PointCloud<PointXYZ>::Ptr &keypoints_src,
                          const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
                          Correspondences &remaining_correspondences)
{
  CorrespondenceRejectorDistance rej;
  rej.setInputCloud<PointXYZ> (keypoints_src);
  rej.setInputTarget<PointXYZ> (keypoints_tgt);
  rej.setMaximumDistance (1);    // 1m
  rej.setInputCorrespondences (all_correspondences);
  rej.getCorrespondences (remaining_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
computeTransformation (const PointCloud<PointXYZ>::Ptr &src,
                       const PointCloud<PointXYZ>::Ptr &tgt,
                       Eigen::Matrix4f &transform)
{
  // Get an uniform grid of keypoints
  PointCloud<PointXYZ>::Ptr keypoints_src (new PointCloud<PointXYZ>),
                            keypoints_tgt (new PointCloud<PointXYZ>);

  estimateKeypoints (src, tgt, *keypoints_src, *keypoints_tgt);
  print_info ("Found %zu and %zu keypoints for the source and target datasets.\n", keypoints_src->points.size (), keypoints_tgt->points.size ());

  // Compute normals for all points keypoint
  PointCloud<Normal>::Ptr normals_src (new PointCloud<Normal>),
                          normals_tgt (new PointCloud<Normal>);
  estimateNormals (src, tgt, *normals_src, *normals_tgt);
  print_info ("Estimated %zu and %zu normals for the source and target datasets.\n", normals_src->points.size (), normals_tgt->points.size ());

  // Compute FPFH features at each keypoint
  PointCloud<FPFHSignature33>::Ptr fpfhs_src (new PointCloud<FPFHSignature33>),
                                   fpfhs_tgt (new PointCloud<FPFHSignature33>);
  estimateFPFH (src, tgt, normals_src, normals_tgt, keypoints_src, keypoints_tgt, *fpfhs_src, *fpfhs_tgt);

  // Copy the data and save it to disk
/*  PointCloud<PointNormal> s, t;
  copyPointCloud<PointXYZ, PointNormal> (*keypoints_src, s);
  copyPointCloud<Normal, PointNormal> (normals_src, s);
  copyPointCloud<PointXYZ, PointNormal> (*keypoints_tgt, t);
  copyPointCloud<Normal, PointNormal> (normals_tgt, t);*/

  // Find correspondences between keypoints in FPFH space
  CorrespondencesPtr all_correspondences (new Correspondences),
                     good_correspondences (new Correspondences);
  findCorrespondences (fpfhs_src, fpfhs_tgt, *all_correspondences);

  // Reject correspondences based on their XYZ distance
  rejectBadCorrespondences (all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences);

  for (int i = 0; i < good_correspondences->size (); ++i)
    std::cerr << good_correspondences->at (i) << std::endl;
  // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
  TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
  trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd\n", argv[0]);
    return (-1);
  }

  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]);
  src.reset (new PointCloud<PointXYZ>);
  tgt.reset (new PointCloud<PointXYZ>);
  if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4f transform;
  computeTransformation (src, tgt, transform);

  std::cerr << transform << std::endl;
  // Transform the data and write it to disk
  PointCloud<PointXYZ> output;
  transformPointCloud (*src, output, transform);
  savePCDFileBinary ("source_transformed.pcd", output);
}
/* ]--- */
<commit_msg>Minor bug fixed in registration_api tutorial code<commit_after>#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>

#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/transformation_estimation_svd.h>

using namespace std;
using namespace sensor_msgs;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
using namespace pcl::registration;
PointCloud<PointXYZ>::Ptr src, tgt;

////////////////////////////////////////////////////////////////////////////////
void
estimateKeypoints (const PointCloud<PointXYZ>::Ptr &src,
                   const PointCloud<PointXYZ>::Ptr &tgt,
                   PointCloud<PointXYZ> &keypoints_src,
                   PointCloud<PointXYZ> &keypoints_tgt)
{
  PointCloud<int> keypoints_src_idx, keypoints_tgt_idx;
  // Get an uniform grid of keypoints
  UniformSampling<PointXYZ> uniform;
  uniform.setRadiusSearch (1);  // 1m

  uniform.setInputCloud (src);
  uniform.compute (keypoints_src_idx);
  copyPointCloud<PointXYZ, PointXYZ> (*src, keypoints_src_idx.points, keypoints_src);

  uniform.setInputCloud (tgt);
  uniform.compute (keypoints_tgt_idx);
  copyPointCloud<PointXYZ, PointXYZ> (*tgt, keypoints_tgt_idx.points, keypoints_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer source_pcd keypoints_src.pcd -ps 1 -ps 10
  savePCDFileBinary ("keypoints_src.pcd", keypoints_src);
  savePCDFileBinary ("keypoints_tgt.pcd", keypoints_tgt);
}

////////////////////////////////////////////////////////////////////////////////
void
estimateNormals (const PointCloud<PointXYZ>::Ptr &src,
                 const PointCloud<PointXYZ>::Ptr &tgt,
                 PointCloud<Normal> &normals_src,
                 PointCloud<Normal> &normals_tgt)
{
  NormalEstimation<PointXYZ, Normal> normal_est;
  normal_est.setInputCloud (src);
  normal_est.setRadiusSearch (0.5);  // 50cm
  normal_est.compute (normals_src);

  normal_est.setInputCloud (tgt);
  normal_est.compute (normals_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer normals_src.pcd
  PointCloud<PointNormal> s, t;
  copyPointCloud<PointXYZ, PointNormal> (*src, s);
  copyPointCloud<Normal, PointNormal> (normals_src, s);
  copyPointCloud<PointXYZ, PointNormal> (*tgt, t);
  copyPointCloud<Normal, PointNormal> (normals_tgt, t);
  savePCDFileBinary ("normals_src.pcd", s);
  savePCDFileBinary ("normals_tgt.pcd", t);
}

////////////////////////////////////////////////////////////////////////////////
void
estimateFPFH (const PointCloud<PointXYZ>::Ptr &src,
              const PointCloud<PointXYZ>::Ptr &tgt,
              const PointCloud<Normal>::Ptr &normals_src,
              const PointCloud<Normal>::Ptr &normals_tgt,
              const PointCloud<PointXYZ>::Ptr &keypoints_src,
              const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
              PointCloud<FPFHSignature33> &fpfhs_src,
              PointCloud<FPFHSignature33> &fpfhs_tgt)
{
  FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
  fpfh_est.setInputCloud (keypoints_src);
  fpfh_est.setInputNormals (normals_src);
  fpfh_est.setRadiusSearch (1); // 1m
  fpfh_est.setSearchSurface (src);
  fpfh_est.compute (fpfhs_src);

  fpfh_est.setInputCloud (keypoints_tgt);
  fpfh_est.setInputNormals (normals_tgt);
  fpfh_est.setSearchSurface (tgt);
  fpfh_est.compute (fpfhs_tgt);

  // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
  // pcd_viewer fpfhs_src.pcd
  PointCloud2 s, t, out;
  toROSMsg (*keypoints_src, s); toROSMsg (fpfhs_src, t); concatenateFields (s, t, out);
  savePCDFile ("fpfhs_src.pcd", out);
  toROSMsg (*keypoints_tgt, s); toROSMsg (fpfhs_tgt, t); concatenateFields (s, t, out);
  savePCDFile ("fpfhs_tgt.pcd", out);
}

////////////////////////////////////////////////////////////////////////////////
void
findCorrespondences (const PointCloud<FPFHSignature33>::Ptr &fpfhs_src,
                     const PointCloud<FPFHSignature33>::Ptr &fpfhs_tgt,
                     Correspondences &all_correspondences)
{
  CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;
  est.setInputCloud (fpfhs_src);
  est.setInputTarget (fpfhs_tgt);
  est.determineReciprocalCorrespondences (all_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
                          const PointCloud<PointXYZ>::Ptr &keypoints_src,
                          const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
                          Correspondences &remaining_correspondences)
{
  CorrespondenceRejectorDistance rej;
  rej.setInputCloud<PointXYZ> (keypoints_src);
  rej.setInputTarget<PointXYZ> (keypoints_tgt);
  rej.setMaximumDistance (1);    // 1m
  rej.setInputCorrespondences (all_correspondences);
  rej.getCorrespondences (remaining_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
computeTransformation (const PointCloud<PointXYZ>::Ptr &src,
                       const PointCloud<PointXYZ>::Ptr &tgt,
                       Eigen::Matrix4f &transform)
{
  // Get an uniform grid of keypoints
  PointCloud<PointXYZ>::Ptr keypoints_src (new PointCloud<PointXYZ>),
                            keypoints_tgt (new PointCloud<PointXYZ>);

  estimateKeypoints (src, tgt, *keypoints_src, *keypoints_tgt);
  print_info ("Found %zu and %zu keypoints for the source and target datasets.\n", keypoints_src->points.size (), keypoints_tgt->points.size ());

  // Compute normals for all points keypoint
  PointCloud<Normal>::Ptr normals_src (new PointCloud<Normal>),
                          normals_tgt (new PointCloud<Normal>);
  estimateNormals (src, tgt, *normals_src, *normals_tgt);
  print_info ("Estimated %zu and %zu normals for the source and target datasets.\n", normals_src->points.size (), normals_tgt->points.size ());

  // Compute FPFH features at each keypoint
  PointCloud<FPFHSignature33>::Ptr fpfhs_src (new PointCloud<FPFHSignature33>),
                                   fpfhs_tgt (new PointCloud<FPFHSignature33>);
  estimateFPFH (src, tgt, normals_src, normals_tgt, keypoints_src, keypoints_tgt, *fpfhs_src, *fpfhs_tgt);

  // Copy the data and save it to disk
/*  PointCloud<PointNormal> s, t;
  copyPointCloud<PointXYZ, PointNormal> (*keypoints_src, s);
  copyPointCloud<Normal, PointNormal> (normals_src, s);
  copyPointCloud<PointXYZ, PointNormal> (*keypoints_tgt, t);
  copyPointCloud<Normal, PointNormal> (normals_tgt, t);*/

  // Find correspondences between keypoints in FPFH space
  CorrespondencesPtr all_correspondences (new Correspondences),
                     good_correspondences (new Correspondences);
  findCorrespondences (fpfhs_src, fpfhs_tgt, *all_correspondences);

  // Reject correspondences based on their XYZ distance
  rejectBadCorrespondences (all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences);

  for (int i = 0; i < good_correspondences->size (); ++i)
    std::cerr << good_correspondences->at (i) << std::endl;
  // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
  TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
  trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd\n", argv[0]);
    return (-1);
  }

  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]);
  src.reset (new PointCloud<PointXYZ>);
  tgt.reset (new PointCloud<PointXYZ>);
  if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4f transform;
  computeTransformation (src, tgt, transform);

  std::cerr << transform << std::endl;
  // Transform the data and write it to disk
  PointCloud<PointXYZ> output;
  transformPointCloud (*src, output, transform);
  savePCDFileBinary ("source_transformed.pcd", output);
}
/* ]--- */
<|endoftext|>


<commit_before>#pragma once

#include <vector>
#include <string>
#include <string_view>
#include <functional>
#include <mutex>
#include <condition_variable>

#include "config/config.hpp"
#include "base/type.h"

namespace autocxxpy
{
    template <class tag, size_t size>
    struct get_string
    {
        auto &operator()(string_literal<size> &val) const noexcept
        {
            return val;
        }
    };

    template <class tag, size_t size>
    struct set_string
    {
        void operator()(string_literal<size> &val, const char *str)
        {
#ifdef _MSC_VER
            strcpy_s(val, str);
#else
            strcpy(val, str);
#endif
        }
    };

    template <class tag, class class_type, class value_type>
    inline constexpr auto default_getter_wrap(value_type class_type::*member)
    { // match normal case
        return [member](class_type &instance)->const value_type & {
            return instance.*member;
        };
    }

    template <class tag, class class_type, class value_type>
    inline constexpr auto default_setter_wrap(value_type class_type::*member)
    { // match normal case
        return [member](class_type &instance, const value_type &value) {
            instance.*member = value;
        };
    }

    // specialization for const setter
    template <class tag, class class_type, class value_type>
    inline constexpr auto default_setter_wrap(const value_type class_type::*member)
    { // match const
        return nullptr;
    }

    // specialization for any []
    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_getter_wrap(literal_array<element_t, size> class_type::*member)
    { // match get any []
        return [member](class_type &instance) {
            auto es = std::vector<const element_t *>(size);
            for (size_t i = 0; i < size ; i++)
            {
                es[i] = instance.*member + i;
            }
            return std::move(es);
        };
    }

    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_setter_wrap(literal_array<element_t, size> class_type::*member)
    { // match set any []
        return [member](class_type &instance, const std::vector<element_t> &value) {
            if (value.size() >= size)
            {
                auto s = std::string("Array too large, maximum size : ") + std::to_string(size) + " your size: " + std::to_string(value.size());
                throw std::runtime_error(s);
            }
            for (int i = 0; i < value.size(); i++)
            {
                (instance.*member)[i] = value.at(i);
            }
        };
    }

    // specialization for any *[]
    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_getter_wrap(literal_array<element_t *, size> class_type::*member)
    { // match get (any *)[]
        return [member](class_type &instance) {
            std::vector<element_t *> arr;
            for (auto &v : instance.*member)
            {
                arr.push_back(v);
            }
            return arr;
        };
    }

    // specialization for char[]
    template <class tag, class class_type, size_t size>
    inline constexpr auto default_getter_wrap(string_literal<size> class_type::*member)
    { // match get char []
        return [member](class_type &instance) {
            return get_string<tag, size>{}(instance.*member);
        };
    }

    template <class tag, class class_type, size_t size>
    inline constexpr auto default_setter_wrap(string_literal<size> class_type::*member)
    { // match set char []
        return [member](class_type &instance, const std::string_view &value) {
            return set_string<tag, size>{}(instance.*member, value.data());
        };
    }

    template <class tag, class MemberConstant>
    struct getter_wrap
    {
        using value_type = decltype(default_getter_wrap<tag>(MemberConstant::value));
        static constexpr value_type value = default_getter_wrap<tag>(MemberConstant::value);
    };

    template <class tag, class MemberConstant>
    struct setter_wrap
    {
        using value_type = decltype(default_setter_wrap<tag>(MemberConstant::value));
        static constexpr value_type value = default_setter_wrap<tag>(MemberConstant::value);
    };
}
#define AUTOCXXPY_DEF_PROPERTY(cls, name, member) \
    def_property(name, autocxxpy::getter_wrap<module_tag, std::integral_constant<decltype(&cls::member), &cls::member>>::value,\
        autocxxpy::setter_wrap<module_tag, std::integral_constant<decltype(&cls::member), &cls::member>>::value)
<commit_msg>[Fix] 更新autocxxpy脚本<commit_after>#pragma once

#include <vector>
#include <string>
#include <string_view>
#include <functional>
#include <mutex>
#include <condition_variable>

#include "config/config.hpp"
#include "base/type.h"

namespace autocxxpy
{
    template <class tag, size_t size>
    struct get_string
    {
        auto &operator()(string_literal<size> &val) const noexcept
        {
            return val;
        }
        auto &operator()(const string_literal<size> &val) const noexcept
        {
            return val;
        }
    };

    template <class tag, size_t size>
    struct set_string
    {
        void operator()(string_literal<size> &val, const char *str)
        {
#ifdef _MSC_VER
            strcpy_s(val, str);
#else
            strcpy(val, str);
#endif
        }
    };

    template <class tag, class class_type, class value_type>
    inline constexpr auto default_getter_wrap(value_type class_type::*member)
    { // match normal case
        return [member](class_type &instance)->const value_type & {
            return instance.*member;
        };
    }

    template <class tag, class class_type, class value_type>
    inline constexpr auto default_setter_wrap(value_type class_type::*member)
    { // match normal case
        return [member](class_type &instance, const value_type &value) {
            instance.*member = value;
        };
    }

    // specialization for const setter
    template <class tag, class class_type, class value_type>
    inline constexpr auto default_setter_wrap(const value_type class_type::*member)
    { // match const
        return nullptr;
    }

    // specialization for any []
    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_getter_wrap(literal_array<element_t, size> class_type::*member)
    { // match get any []
        return [member](class_type &instance) {
            auto es = std::vector<const element_t *>(size);
            for (size_t i = 0; i < size ; i++)
            {
                es[i] = instance.*member + i;
            }
            return std::move(es);
        };
    }

    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_setter_wrap(literal_array<element_t, size> class_type::*member)
    { // match set any []
        return [member](class_type &instance, const std::vector<element_t> &value) {
            if (value.size() >= size)
            {
                auto s = std::string("Array too large, maximum size : ") + std::to_string(size) + " your size: " + std::to_string(value.size());
                throw std::runtime_error(s);
            }
            for (int i = 0; i < value.size(); i++)
            {
                (instance.*member)[i] = value.at(i);
            }
        };
    }

    // specialization for any *[]
    template <class tag, class class_type, class element_t, size_t size>
    inline constexpr auto default_getter_wrap(literal_array<element_t *, size> class_type::*member)
    { // match get (any *)[]
        return [member](class_type &instance) {
            std::vector<element_t *> arr;
            for (auto &v : instance.*member)
            {
                arr.push_back(v);
            }
            return arr;
        };
    }

    // specialization for char[]
    template <class tag, class class_type, size_t size>
    inline constexpr auto default_getter_wrap(string_literal<size> class_type::*member)
    { // match get char []
        return [member](class_type &instance) {
            return get_string<tag, size>{}(instance.*member);
        };
    }

    template <class tag, class class_type, size_t size>
    inline constexpr auto default_setter_wrap(string_literal<size> class_type::*member)
    { // match set char []
        return [member](class_type &instance, const std::string_view &value) {
            return set_string<tag, size>{}(instance.*member, value.data());
        };
    }

    template <class tag, class MemberConstant>
    struct getter_wrap
    {
        using value_type = decltype(default_getter_wrap<tag>(MemberConstant::value));
        static constexpr value_type value = default_getter_wrap<tag>(MemberConstant::value);
    };

    template <class tag, class MemberConstant>
    struct setter_wrap
    {
        using value_type = decltype(default_setter_wrap<tag>(MemberConstant::value));
        static constexpr value_type value = default_setter_wrap<tag>(MemberConstant::value);
    };
}
#define AUTOCXXPY_DEF_PROPERTY(cls, name, member) \
    def_property(name, autocxxpy::getter_wrap<module_tag, std::integral_constant<decltype(&cls::member), &cls::member>>::value,\
        autocxxpy::setter_wrap<module_tag, std::integral_constant<decltype(&cls::member), &cls::member>>::value)
<|endoftext|>


<commit_before>// Copyright (c) 2019 The Syscoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#include <uint256.h>
#include <arith_uint256.h>
#include <ethereum/ethereum.h>
#include <ethereum/sha3.h>
#include <logging.h>
#include <util/strencodings.h>
#include <key_io.h>

int nibblesToTraverse(const std::string &encodedPartialPath, const std::string &path, int pathPtr) {
  std::string partialPath;
  char pathPtrInt[2] = {encodedPartialPath[0], '\0'};
  int partialPathInt = strtol(pathPtrInt, NULL, 10);
  if(partialPathInt == 0 || partialPathInt == 2){
    partialPath = encodedPartialPath.substr(2);
  }else{
    partialPath = encodedPartialPath.substr(1);
  }
  if(partialPath == path.substr(pathPtr, partialPath.size())){
    return partialPath.size();
  }else{
    return -1;
  }
}
bool VerifyProof(dev::bytesConstRef path, const dev::RLP& value, const dev::RLP& parentNodes, const dev::RLP& root) {
    try{
        dev::RLP currentNode;
        const int len = parentNodes.itemCount();
        dev::RLP nodeKey = root;
        int pathPtr = 0;

    	const std::string pathString = dev::toHex(path);

        int nibbles;
        char pathPtrInt[2];
        for (int i = 0 ; i < len ; i++) {
          currentNode = parentNodes[i];
          if(!nodeKey.payload().contentsEqual(sha3(currentNode.data()).ref().toVector())){
            return false;
          }

          if(pathPtr > (int)pathString.size()){
            return false;
          }

          switch(currentNode.itemCount()){
            case 17://branch node
              if(pathPtr == (int)pathString.size()){
                if(currentNode[16].payload().contentsEqual(value.data().toVector())){

                  return true;
                }else{
                  return false;
                }
              }

              pathPtrInt[0] = pathString[pathPtr];
              pathPtrInt[1] = '\0';

              nodeKey = currentNode[strtol(pathPtrInt, NULL, 16)]; //must == sha3(rlp.encode(currentNode[path[pathptr]]))
              pathPtr += 1;
              break;
            case 2:
              nibbles = nibblesToTraverse(toHex(currentNode[0].payload()), pathString, pathPtr);

              if(nibbles <= -1)
                return false;
              pathPtr += nibbles;

              if(pathPtr == (int)pathString.size()) { //leaf node
                if(currentNode[1].payload().contentsEqual(value.data().toVector())){

                  return true;
                } else {
                  return false;
                }
              } else {//extension node
                nodeKey = currentNode[1];
              }
              break;
            default:
              return false;
          }
        }
    }
    catch(...){
        return false;
    }
  return false;
}

/**
 * Parse eth input string expected to contain smart contract method call data. If the method call is not what we
 * expected, or the length of the expected string is not what we expect then return false.
 *
 * @param vchInputExpectedMethodHash The expected method hash
 * @param nERC20Precision The erc20 precision to know how to convert ethereum's uint256 to a uint64 with truncation of insignifficant bits
 * @param nLocalPrecision The local precision to know how to convert ethereum's uint256 to a uint64 with truncation of insignifficant bits
 * @param vchInputData The input to parse
 * @param outputAmount The amount burned
 * @param nAsset The asset burned
 * @param witnessAddress The destination witness address for the minting
 * @return true if everything is valid
 */
bool parseEthMethodInputData(const std::vector<unsigned char>& vchInputExpectedMethodHash, const uint8_t &nERC20Precision, const uint8_t& nLocalPrecision, const std::vector<unsigned char>& vchInputData, uint64_t& outputAmount, uint32_t& nAsset, CTxDestination& witnessAddress) {
    // total 5 or 6 fields are expected @ 32 bytes each field, 6 fields if witness > 32 bytes + 4 byte method hash
    if(vchInputData.size() < 164 || vchInputData.size() > 196) {
      return false;
    }
    // method hash is 4 bytes
    std::vector<unsigned char>::const_iterator firstMethod = vchInputData.begin();
    std::vector<unsigned char>::const_iterator lastMethod = firstMethod + 4;
    const std::vector<unsigned char> vchMethodHash(firstMethod,lastMethod);
    // if the method hash doesn't match the expected method hash then return false
    if(vchMethodHash != vchInputExpectedMethodHash) {
      return false;
    }

    std::vector<unsigned char> vchAmount(lastMethod,lastMethod + 32);
    // reverse endian
    std::reverse(vchAmount.begin(), vchAmount.end());
    arith_uint256 outputAmountArith = UintToArith256(uint256(vchAmount));
    // local precision can range between 0 and 8 decimal places, so it should fit within a CAmount
    // we pad zero's if erc20's precision is less than ours so we can accurately get the whole value of the amount transferred
    if(nLocalPrecision > nERC20Precision){
      outputAmountArith *= pow(10, nLocalPrecision-nERC20Precision);
    // ensure we truncate decimals to fit within int64 if erc20's precision is more than our asset precision
    } else if(nLocalPrecision < nERC20Precision){
      outputAmountArith /= pow(10, nERC20Precision-nLocalPrecision);
    }
    outputAmount = outputAmountArith.GetLow64();

    // convert the vch into a uint32_t (nAsset)
    // should be in position 68 walking backwards
    nAsset = static_cast<uint32_t>(vchInputData[67]);
    nAsset |= static_cast<uint32_t>(vchInputData[66]) << 8;
    nAsset |= static_cast<uint32_t>(vchInputData[65]) << 16;
    nAsset |= static_cast<uint32_t>(vchInputData[64]) << 24;

    // skip data field marker (32 bytes) + 31 bytes offset to the varint _byte
    const unsigned char &dataLength = vchInputData[131];
    // bech32 addresses to 46 bytes (sys1 vs bc1), min length is 9 for min witness address https://en.bitcoin.it/wiki/BIP_0173
    if(dataLength > 46 || dataLength < 9) {
      return false;
    }

    // witness address information starting at position 132 till the end
    witnessAddress = DecodeDestination(std::to_string(vchInputData.begin()+132, vchInputData.begin()+132 + dataLength));
    return true;
}<commit_msg>fix witness address<commit_after>// Copyright (c) 2019 The Syscoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#include <uint256.h>
#include <arith_uint256.h>
#include <ethereum/ethereum.h>
#include <ethereum/sha3.h>
#include <logging.h>
#include <util/strencodings.h>
#include <key_io.h>

int nibblesToTraverse(const std::string &encodedPartialPath, const std::string &path, int pathPtr) {
  std::string partialPath;
  char pathPtrInt[2] = {encodedPartialPath[0], '\0'};
  int partialPathInt = strtol(pathPtrInt, NULL, 10);
  if(partialPathInt == 0 || partialPathInt == 2){
    partialPath = encodedPartialPath.substr(2);
  }else{
    partialPath = encodedPartialPath.substr(1);
  }
  if(partialPath == path.substr(pathPtr, partialPath.size())){
    return partialPath.size();
  }else{
    return -1;
  }
}
bool VerifyProof(dev::bytesConstRef path, const dev::RLP& value, const dev::RLP& parentNodes, const dev::RLP& root) {
    try{
        dev::RLP currentNode;
        const int len = parentNodes.itemCount();
        dev::RLP nodeKey = root;
        int pathPtr = 0;

    	const std::string pathString = dev::toHex(path);

        int nibbles;
        char pathPtrInt[2];
        for (int i = 0 ; i < len ; i++) {
          currentNode = parentNodes[i];
          if(!nodeKey.payload().contentsEqual(sha3(currentNode.data()).ref().toVector())){
            return false;
          }

          if(pathPtr > (int)pathString.size()){
            return false;
          }

          switch(currentNode.itemCount()){
            case 17://branch node
              if(pathPtr == (int)pathString.size()){
                if(currentNode[16].payload().contentsEqual(value.data().toVector())){

                  return true;
                }else{
                  return false;
                }
              }

              pathPtrInt[0] = pathString[pathPtr];
              pathPtrInt[1] = '\0';

              nodeKey = currentNode[strtol(pathPtrInt, NULL, 16)]; //must == sha3(rlp.encode(currentNode[path[pathptr]]))
              pathPtr += 1;
              break;
            case 2:
              nibbles = nibblesToTraverse(toHex(currentNode[0].payload()), pathString, pathPtr);

              if(nibbles <= -1)
                return false;
              pathPtr += nibbles;

              if(pathPtr == (int)pathString.size()) { //leaf node
                if(currentNode[1].payload().contentsEqual(value.data().toVector())){

                  return true;
                } else {
                  return false;
                }
              } else {//extension node
                nodeKey = currentNode[1];
              }
              break;
            default:
              return false;
          }
        }
    }
    catch(...){
        return false;
    }
  return false;
}

/**
 * Parse eth input string expected to contain smart contract method call data. If the method call is not what we
 * expected, or the length of the expected string is not what we expect then return false.
 *
 * @param vchInputExpectedMethodHash The expected method hash
 * @param nERC20Precision The erc20 precision to know how to convert ethereum's uint256 to a uint64 with truncation of insignifficant bits
 * @param nLocalPrecision The local precision to know how to convert ethereum's uint256 to a uint64 with truncation of insignifficant bits
 * @param vchInputData The input to parse
 * @param outputAmount The amount burned
 * @param nAsset The asset burned
 * @param witnessAddress The destination witness address for the minting
 * @return true if everything is valid
 */
bool parseEthMethodInputData(const std::vector<unsigned char>& vchInputExpectedMethodHash, const uint8_t &nERC20Precision, const uint8_t& nLocalPrecision, const std::vector<unsigned char>& vchInputData, uint64_t& outputAmount, uint32_t& nAsset, CTxDestination& witnessAddress) {
    // total 5 or 6 fields are expected @ 32 bytes each field, 6 fields if witness > 32 bytes + 4 byte method hash
    if(vchInputData.size() < 164 || vchInputData.size() > 196) {
      return false;
    }
    // method hash is 4 bytes
    std::vector<unsigned char>::const_iterator firstMethod = vchInputData.begin();
    std::vector<unsigned char>::const_iterator lastMethod = firstMethod + 4;
    const std::vector<unsigned char> vchMethodHash(firstMethod,lastMethod);
    // if the method hash doesn't match the expected method hash then return false
    if(vchMethodHash != vchInputExpectedMethodHash) {
      return false;
    }

    std::vector<unsigned char> vchAmount(lastMethod,lastMethod + 32);
    // reverse endian
    std::reverse(vchAmount.begin(), vchAmount.end());
    arith_uint256 outputAmountArith = UintToArith256(uint256(vchAmount));
    // local precision can range between 0 and 8 decimal places, so it should fit within a CAmount
    // we pad zero's if erc20's precision is less than ours so we can accurately get the whole value of the amount transferred
    if(nLocalPrecision > nERC20Precision){
      outputAmountArith *= pow(10, nLocalPrecision-nERC20Precision);
    // ensure we truncate decimals to fit within int64 if erc20's precision is more than our asset precision
    } else if(nLocalPrecision < nERC20Precision){
      outputAmountArith /= pow(10, nERC20Precision-nLocalPrecision);
    }
    outputAmount = outputAmountArith.GetLow64();

    // convert the vch into a uint32_t (nAsset)
    // should be in position 68 walking backwards
    nAsset = static_cast<uint32_t>(vchInputData[67]);
    nAsset |= static_cast<uint32_t>(vchInputData[66]) << 8;
    nAsset |= static_cast<uint32_t>(vchInputData[65]) << 16;
    nAsset |= static_cast<uint32_t>(vchInputData[64]) << 24;

    // skip data field marker (32 bytes) + 31 bytes offset to the varint _byte
    const unsigned char &dataLength = vchInputData[131];
    // bech32 addresses to 46 bytes (sys1 vs bc1), min length is 9 for min witness address https://en.bitcoin.it/wiki/BIP_0173
    if(dataLength > 46 || dataLength < 9) {
      return false;
    }

    // witness address information starting at position 132 till the end
    witnessAddress = DecodeDestination(std::to_string(HexStr(vchInputData.begin()+132, vchInputData.begin()+132 + dataLength)));
    return true;
}<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <walrus_stair_detector/walrus_stair_detector.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <gtest/gtest.h>
#include "yaml-cpp/yaml.h"
#include <boost/filesystem.hpp>

std::string pcd_file;
std::string config_file;

namespace YAML {
template<>
struct convert<Eigen::Vector3f> {
  static bool decode(const YAML::Node& node, Eigen::Vector3f& rhs) {
    if(!node.IsSequence() || node.size() != 3) {
      return false;
    }

    rhs[0] = node[0].as<double>();
    rhs[1] = node[1].as<double>();
    rhs[2] = node[2].as<double>();
    return true;
  }
};
}

#define EXPECT_VECTOR_ANGLE_LE(val1, val2, max_angle)			\
  do {									\
    Eigen::Vector3f val1_norm = val1.normalized();			\
    Eigen::Vector3f val2_norm = val2.normalized();			\
    if(val1_norm != val2_norm) {					\
      double angle = acos(val1_norm.dot(val2_norm));			\
      EXPECT_LE(fabs(angle), max_angle)					\
	<< "Expected angle between "					\
	<< "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
	<< " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
	<< " <= " << max_angle << ", but was " << angle;		\
    }									\
  } while(0)

#define EXPECT_VECTOR_NEAR(val1, val2, abs_error)			\
  do {									\
    double error = (val1 - val2).norm();				\
    EXPECT_LE(error, abs_error)						\
      << "Expected error between "					\
      << "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
      << " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
      << " <= " << abs_error << ", but was " << error;			\
  } while(0)

struct StairDetectorTestData {
  StairDetectorTestData(std::string pcd_file, std::string config_file) : pcd_file(pcd_file), config_file(config_file) {}
  std::string pcd_file;
  std::string config_file;
};
void PrintTo(const StairDetectorTestData& data, ::std::ostream* os) {
  *os << "PCD File: " << data.pcd_file << ", Config File: " << data.config_file;
}

class StairDetectorTest : public ::testing::TestWithParam<StairDetectorTestData> {};

TEST_P(StairDetectorTest, detectStairs) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile(GetParam().pcd_file, *cloud);

  YAML::Node config = YAML::LoadFile(GetParam().config_file);
  YAML::Node sensor_config = config["sensor"];
  YAML::Node stairs_config = config["stairs"];

  Eigen::Vector3f vertical_estimate = sensor_config["vertical"].as<Eigen::Vector3f>();

  walrus_stair_detector::WalrusStairDetector detector;
  std::vector<walrus_stair_detector::StairModel> stairs;
  detector.detect(cloud, vertical_estimate, &stairs);

  ASSERT_EQ(stairs_config.size(), stairs.size());

  for(int i = 0; i < stairs.size(); ++i) {
    YAML::Node stair_config = stairs_config[i];
    walrus_stair_detector::StairModel& stair = stairs[i];
    EXPECT_NEAR(stair_config["rise"].as<double>(), stair.rise, 0.1);
    EXPECT_NEAR(stair_config["run"].as<double>(), stair.run, 0.1);

    EXPECT_VECTOR_ANGLE_LE(stair_config["direction"].as<Eigen::Vector3f>(), stair.direction, 0.06);

    Eigen::Vector3f expected_origin = stair_config["origin"].as<Eigen::Vector3f>();

    double expected_x = expected_origin.dot(stair.horizontal);
    double expected_y = expected_origin.dot(stair.vertical);
    double expected_z = expected_origin.dot(stair.direction);

    double actual_x = stair.origin.dot(stair.horizontal);
    double actual_y = stair.origin.dot(stair.vertical);
    double actual_z = stair.origin.dot(stair.direction);

    // Expect the horizontal component to be less accurate
    EXPECT_NEAR(expected_x, actual_x, 0.14);

    double max_origin_center_error = 0.05;
    Eigen::Vector3f expected_origin_center(0, expected_y, expected_z);
    Eigen::Vector3f origin_center(0, actual_y, actual_z);
    if(stair_config["can_miss_first_stair"].IsDefined() && stair_config["can_miss_first_stair"].as<bool>()){
      Eigen::Vector3f upper_origin_center(0, actual_y - stair.rise, actual_z - stair.run);
      if((expected_origin_center - upper_origin_center).norm() < max_origin_center_error)
	EXPECT_VECTOR_NEAR(expected_origin_center, upper_origin_center, max_origin_center_error);
      else
	EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }
    else {
      EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }

    EXPECT_GE(stair_config["width"].as<double>(), stair.width);
    EXPECT_LE(stair_config["width"].as<double>()-0.18, stair.width);

    if(stair_config["num_stairs"].IsScalar()) {
      EXPECT_EQ(stair_config["num_stairs"].as<int>(), stair.num_stairs);
    }
    else {
      EXPECT_LE(stair_config["num_stairs"][0].as<int>(), stair.num_stairs);
      EXPECT_GE(stair_config["num_stairs"][1].as<int>(), stair.num_stairs);
    }
  }
}

std::string find_package(const std::string& package, const std::string& path) {
  std::stringstream command;
  command << "catkin_find " << package << " " << path;

  char buf[1000];
  FILE *fp = popen(command.str().c_str(), "r");
  if (fp == NULL) {
    return "";
  }

  std::string result;
  while (fgets(buf, sizeof(buf), fp) != NULL) {
    result = buf;
    break;
  }

  if(result.size() > 0 && result[result.size()-1] == '\n')
    result = result.substr(0, result.size()-1);

  pclose(fp);
  return result;
}

std::vector<StairDetectorTestData> GenerateStairDetectorParameterList() {
  std::vector<StairDetectorTestData> data;
  std::string testdata_root_string = find_package("walrus_testdata", "kinectv2/stairs");

  if(testdata_root_string.size() > 0) {
    boost::filesystem::path testdata_root (testdata_root_string);
    std::cout << "Searching for test data in: " << testdata_root << std::endl;
    if(!boost::filesystem::is_directory(testdata_root))
      throw new std::runtime_error("Test data path is not a directory");

    boost::filesystem::directory_iterator end;
    for(boost::filesystem::directory_iterator itr(testdata_root); itr != end; ++itr) {
      boost::filesystem::path test_collection = itr->path();
      std::cout << "\tFound test collection: " << test_collection.filename() << std::endl;

      boost::filesystem::path test_collection_description = test_collection;
      test_collection_description/="description.yaml";
      if(boost::filesystem::exists(test_collection_description)){
	for(boost::filesystem::directory_iterator itr2(test_collection); itr2 != end; ++itr2) {
	  if(itr2->path().extension() == ".pcd") {
	    std::cout << "\t\tFound test: " << itr2->path().filename() << std::endl;
	    data.push_back(StairDetectorTestData(itr2->path().native(), test_collection_description.native()));
	  }
	}
      }
      else
	std::cout << "Did not find test description: " << test_collection_description << std::endl;
    }

  }
  else
    throw new std::runtime_error("Could not find test data folder in walrus_testdata");
  return data;
}
INSTANTIATE_TEST_CASE_P(
			StairTestData,
			StairDetectorTest,
			testing::ValuesIn(GenerateStairDetectorParameterList()));

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, "stair_detector_test");

  return RUN_ALL_TESTS();
}
<commit_msg>Made stair detector test nolonger a ROS node<commit_after>#include <walrus_stair_detector/walrus_stair_detector.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <gtest/gtest.h>
#include "yaml-cpp/yaml.h"
#include <boost/filesystem.hpp>

std::string pcd_file;
std::string config_file;

namespace YAML {
template<>
struct convert<Eigen::Vector3f> {
  static bool decode(const YAML::Node& node, Eigen::Vector3f& rhs) {
    if(!node.IsSequence() || node.size() != 3) {
      return false;
    }

    rhs[0] = node[0].as<double>();
    rhs[1] = node[1].as<double>();
    rhs[2] = node[2].as<double>();
    return true;
  }
};
}

#define EXPECT_VECTOR_ANGLE_LE(val1, val2, max_angle)			\
  do {									\
    Eigen::Vector3f val1_norm = val1.normalized();			\
    Eigen::Vector3f val2_norm = val2.normalized();			\
    if(val1_norm != val2_norm) {					\
      double angle = acos(val1_norm.dot(val2_norm));			\
      EXPECT_LE(fabs(angle), max_angle)					\
	<< "Expected angle between "					\
	<< "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
	<< " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
	<< " <= " << max_angle << ", but was " << angle;		\
    }									\
  } while(0)

#define EXPECT_VECTOR_NEAR(val1, val2, abs_error)			\
  do {									\
    double error = (val1 - val2).norm();				\
    EXPECT_LE(error, abs_error)						\
      << "Expected error between "					\
      << "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
      << " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
      << " <= " << abs_error << ", but was " << error;			\
  } while(0)

struct StairDetectorTestData {
  StairDetectorTestData(std::string pcd_file, std::string config_file) : pcd_file(pcd_file), config_file(config_file) {}
  std::string pcd_file;
  std::string config_file;
};
void PrintTo(const StairDetectorTestData& data, ::std::ostream* os) {
  *os << "PCD File: " << data.pcd_file << ", Config File: " << data.config_file;
}

class StairDetectorTest : public ::testing::TestWithParam<StairDetectorTestData> {};

TEST_P(StairDetectorTest, detectStairs) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile(GetParam().pcd_file, *cloud);

  YAML::Node config = YAML::LoadFile(GetParam().config_file);
  YAML::Node sensor_config = config["sensor"];
  YAML::Node stairs_config = config["stairs"];

  Eigen::Vector3f vertical_estimate = sensor_config["vertical"].as<Eigen::Vector3f>();

  walrus_stair_detector::WalrusStairDetector detector;
  std::vector<walrus_stair_detector::StairModel> stairs;
  detector.detect(cloud, vertical_estimate, &stairs);

  ASSERT_EQ(stairs_config.size(), stairs.size());

  for(int i = 0; i < stairs.size(); ++i) {
    YAML::Node stair_config = stairs_config[i];
    walrus_stair_detector::StairModel& stair = stairs[i];
    EXPECT_NEAR(stair_config["rise"].as<double>(), stair.rise, 0.1);
    EXPECT_NEAR(stair_config["run"].as<double>(), stair.run, 0.1);

    EXPECT_VECTOR_ANGLE_LE(stair_config["direction"].as<Eigen::Vector3f>(), stair.direction, 0.06);

    Eigen::Vector3f expected_origin = stair_config["origin"].as<Eigen::Vector3f>();

    double expected_x = expected_origin.dot(stair.horizontal);
    double expected_y = expected_origin.dot(stair.vertical);
    double expected_z = expected_origin.dot(stair.direction);

    double actual_x = stair.origin.dot(stair.horizontal);
    double actual_y = stair.origin.dot(stair.vertical);
    double actual_z = stair.origin.dot(stair.direction);

    // Expect the horizontal component to be less accurate
    EXPECT_NEAR(expected_x, actual_x, 0.14);

    double max_origin_center_error = 0.05;
    Eigen::Vector3f expected_origin_center(0, expected_y, expected_z);
    Eigen::Vector3f origin_center(0, actual_y, actual_z);
    if(stair_config["can_miss_first_stair"].IsDefined() && stair_config["can_miss_first_stair"].as<bool>()){
      Eigen::Vector3f upper_origin_center(0, actual_y - stair.rise, actual_z - stair.run);
      if((expected_origin_center - upper_origin_center).norm() < max_origin_center_error)
	EXPECT_VECTOR_NEAR(expected_origin_center, upper_origin_center, max_origin_center_error);
      else
	EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }
    else {
      EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }

    EXPECT_GE(stair_config["width"].as<double>(), stair.width);
    EXPECT_LE(stair_config["width"].as<double>()-0.18, stair.width);

    if(stair_config["num_stairs"].IsScalar()) {
      EXPECT_EQ(stair_config["num_stairs"].as<int>(), stair.num_stairs);
    }
    else {
      EXPECT_LE(stair_config["num_stairs"][0].as<int>(), stair.num_stairs);
      EXPECT_GE(stair_config["num_stairs"][1].as<int>(), stair.num_stairs);
    }
  }
}

std::string find_package(const std::string& package, const std::string& path) {
  std::stringstream command;
  command << "catkin_find " << package << " " << path;

  char buf[1000];
  FILE *fp = popen(command.str().c_str(), "r");
  if (fp == NULL) {
    return "";
  }

  std::string result;
  while (fgets(buf, sizeof(buf), fp) != NULL) {
    result = buf;
    break;
  }

  if(result.size() > 0 && result[result.size()-1] == '\n')
    result = result.substr(0, result.size()-1);

  pclose(fp);
  return result;
}

std::vector<StairDetectorTestData> GenerateStairDetectorParameterList() {
  std::vector<StairDetectorTestData> data;
  std::string testdata_root_string = find_package("walrus_testdata", "kinectv2/stairs");

  if(testdata_root_string.size() > 0) {
    boost::filesystem::path testdata_root (testdata_root_string);
    std::cout << "Searching for test data in: " << testdata_root << std::endl;
    if(!boost::filesystem::is_directory(testdata_root))
      throw new std::runtime_error("Test data path is not a directory");

    boost::filesystem::directory_iterator end;
    for(boost::filesystem::directory_iterator itr(testdata_root); itr != end; ++itr) {
      boost::filesystem::path test_collection = itr->path();
      std::cout << "\tFound test collection: " << test_collection.filename() << std::endl;

      boost::filesystem::path test_collection_description = test_collection;
      test_collection_description/="description.yaml";
      if(boost::filesystem::exists(test_collection_description)){
	for(boost::filesystem::directory_iterator itr2(test_collection); itr2 != end; ++itr2) {
	  if(itr2->path().extension() == ".pcd") {
	    std::cout << "\t\tFound test: " << itr2->path().filename() << std::endl;
	    data.push_back(StairDetectorTestData(itr2->path().native(), test_collection_description.native()));
	  }
	}
      }
      else
	std::cout << "Did not find test description: " << test_collection_description << std::endl;
    }

  }
  else
    throw new std::runtime_error("Could not find test data folder in walrus_testdata");
  return data;
}
INSTANTIATE_TEST_CASE_P(
			StairTestData,
			StairDetectorTest,
			testing::ValuesIn(GenerateStairDetectorParameterList()));

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}
<|endoftext|>