<commit_before>#include "CAppNodeImpl.h"
#include "Utility/Utils.h"

CAppNodeImpl::CAppNodeImpl( int argc, ACE_TCHAR *argv[], std::string appNameIn, int domainIDIn ):
	CAppNode( argc, argv, appNameIn )
	, m_applicationTerminate( false)
	, _domainID( domainIDIn )
	, _argCount( argc )
{
	for( size_t i = 0; i < _argCount; ++i)
	{
		_pargVect[i] = argv[i];
	}
}

CAppNodeImpl::~CAppNodeImpl()
{

}

void CAppNodeImpl::Initialize()
{
	try
	{
		// Setup signal handler for ctrl-c
		_psignalHandler->SetupSignalHandlers();

		// Init all DDS related components
		InitParticipant();

		InitPublisherAndSubscriber();

		InitTopicinfo();

		InitDataWriter();

		InitDataReader();

	}catch( CORBA::SystemException &excpt )
	{
		LOG( ERROR ) << "Failed: CORBA Exception.";

		CleanUp();
	}

}

std::string CAppNodeImpl::GetName()
{
	return _appName;
}

void CAppNodeImpl::Run()
{
	// Example Messaage to write
	ExampleApp::Event message;

	// For example writes
	int count = 0;

	while( !_psignalHandler->GotExitSignal() )
	{
		// Write out example message to ourselves
		message.kicker = "test";
		message.timestamp = nodeutils::GetUnixTimestampMs();

		//if( count % 5 != 0 )
		//{
			DDS::ReturnCode_t ret = _eventWriter->write( message, DDS::HANDLE_NIL );

			if( ret != DDS::RETCODE_OK )
			{
				LOG( ERROR ) << "Error write returned: " << ret;
			}
		//s}

		count++;

		//Wait for acknowledgement
//		DDS::Duration_t timeout = { 30, 0 };
//		_eventWriter->wait_for_acknowledgments( timeout );

		// Handle received messages
		HandleWaitCondition();
	}

	CleanUp();
}

void CAppNodeImpl::CleanUp()
{
	LOG( INFO ) << "Cleaning up application...";
	try
	{
		// Clean-up!
		_participant->delete_contained_entities();

		_domainParticipantFactory->delete_participant( _participant );

		TheServiceParticipant->shutdown();

	}catch( CORBA::SystemException &excpt )
	{
		LOG( ERROR ) << "Failed: CORBA Exception participants and factory not properly deleted.";
	}
}

void CAppNodeImpl::HandleWaitCondition()
{
	// Block until Subscriber is available
	DDS::StatusCondition_var condition = _writer->get_statuscondition();
	condition->set_enabled_statuses( DDS::PUBLICATION_MATCHED_STATUS );

	DDS::WaitSet_var waitSet = new DDS::WaitSet;
	waitSet->attach_condition( condition );

	DDS::ConditionSeq conditions;
	DDS::SubscriptionMatchedStatus matches = { 0, 0, 0, 0, 0 };
	DDS::Duration_t timeout = { 30, 0 };

	//do {
	  if ( waitSet->wait(conditions, timeout ) != DDS::RETCODE_OK )
	  {
		LOG( ERROR ) << "wait condition failed.";
	  }

	  if ( _reader->get_subscription_matched_status( matches ) != DDS::RETCODE_OK )
	  {
		LOG( ERROR ) << "Publication matched status failed.";
	  }

	//} while ( matches.current_count > 0 );

	waitSet->detach_condition( condition );
}

void CAppNodeImpl::InitParticipant()
{
	// Init the participant factory and participant
	_domainParticipantFactory = TheParticipantFactoryWithArgs( _argCount, _pargVect );

	_participant = _domainParticipantFactory->create_participant( _domainID, PARTICIPANT_QOS_DEFAULT,
																							DDS::DomainParticipantListener::_nil(),
																							OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	// Exit is nil
	if( !_participant )
	{
		LOG( ERROR ) << "Create Participant Failed.";
	}
}

void CAppNodeImpl::InitPublisherAndSubscriber()
{
	// Publisher
	_publisher = _participant->create_publisher( PUBLISHER_QOS_DEFAULT,
																	DDS::PublisherListener::_nil(),
																	OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	// Exit if nil
	if( !_publisher )
	{
		LOG( ERROR ) << "Create publisher failed.";
	}

	// Subscriber
	_subscriber = _participant->create_subscriber( SUBSCRIBER_QOS_DEFAULT,
													DDS::SubscriberListener::_nil(),
													OpenDDS::DCPS::DEFAULT_STATUS_MASK );
	if( !_subscriber )
	{
		LOG( ERROR ) << "Create subscriber failed.";
	}
}

void CAppNodeImpl::InitTopicinfo()
{
	// Type registration
	_exampleTypeSupport = new ExampleApp::EventTypeSupportImpl();

	// Exit if retcode ! ok
	if( DDS::RETCODE_OK != _exampleTypeSupport->register_type( _participant, "" ) )
	{
		LOG( ERROR ) << "register type failed.";
	}

	// Create a topic
	_topicTypeName = _exampleTypeSupport->get_type_name();

	_topic = _participant->create_topic( "Test Topic", _topicTypeName.in(),
														TOPIC_QOS_DEFAULT,
														DDS::TopicListener::_nil(),
														OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_topic )
	{
		LOG( ERROR ) << "Create topic failed";
	}
}

void CAppNodeImpl::InitDataWriter()
{
	// Create the data writer
	_writer = _publisher->create_datawriter( _topic, DATAWRITER_QOS_DEFAULT,
																DDS::DataWriterListener::_nil(),
																OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_writer )
	{
		LOG( ERROR ) << "Create datawriter failed";
	}

	_eventWriter = ExampleApp::EventDataWriter::_narrow( _writer );

	if( !_eventWriter )
	{
		LOG( ERROR ) << "_narrow failed";
	}

}

void CAppNodeImpl::InitDataReader()
{
	// Data Reader
	_listener = new CDataReaderListenerImpl;

	_reader = _subscriber->create_datareader( _topic, DATAREADER_QOS_DEFAULT,
												_listener,
												OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_reader )
	{
		LOG( ERROR ) << " Create data reader failed";
	}

	_readerI = ExampleApp::EventDataReader::_narrow( _reader );

	if( !_readerI )
	{

		LOG( ERROR ) << " _narrow failed";
	}
}
<commit_msg>example write throttling<commit_after>#include "CAppNodeImpl.h"
#include "Utility/Utils.h"

CAppNodeImpl::CAppNodeImpl( int argc, ACE_TCHAR *argv[], std::string appNameIn, int domainIDIn ):
	CAppNode( argc, argv, appNameIn )
	, m_applicationTerminate( false)
	, _domainID( domainIDIn )
	, _argCount( argc )
{
	for( size_t i = 0; i < _argCount; ++i)
	{
		_pargVect[i] = argv[i];
	}
}

CAppNodeImpl::~CAppNodeImpl()
{

}

void CAppNodeImpl::Initialize()
{
	try
	{
		// Setup signal handler for ctrl-c
		_psignalHandler->SetupSignalHandlers();

		// Init all DDS related components
		InitParticipant();

		InitPublisherAndSubscriber();

		InitTopicinfo();

		InitDataWriter();

		InitDataReader();

	}catch( CORBA::SystemException &excpt )
	{
		LOG( ERROR ) << "Failed: CORBA Exception.";

		CleanUp();
	}

}

std::string CAppNodeImpl::GetName()
{
	return _appName;
}

void CAppNodeImpl::Run()
{
	// Example Messaage to write
	ExampleApp::Event message;

	while( !_psignalHandler->GotExitSignal() )
	{
		// Write out example message to ourselves
		message.kicker = "test";
		message.timestamp = nodeutils::GetUnixTimestampMs();

			DDS::ReturnCode_t ret = _eventWriter->write( message, DDS::HANDLE_NIL );

			if( ret != DDS::RETCODE_OK )
			{
				LOG( ERROR ) << "Error write returned: " << ret;
			}

		// Handle received messages
		HandleWaitCondition();

		// Pause - example write throttling
		sleep( 3 );
	}

	CleanUp();
}

void CAppNodeImpl::CleanUp()
{
	LOG( INFO ) << "Cleaning up application...";
	try
	{
		// Clean-up!
		_participant->delete_contained_entities();

		_domainParticipantFactory->delete_participant( _participant );

		TheServiceParticipant->shutdown();

	}catch( CORBA::SystemException &excpt )
	{
		LOG( ERROR ) << "Failed: CORBA Exception participants and factory not properly deleted.";
	}
}

void CAppNodeImpl::HandleWaitCondition()
{
	// Block until Subscriber is available
	DDS::StatusCondition_var condition = _writer->get_statuscondition();
	condition->set_enabled_statuses( DDS::PUBLICATION_MATCHED_STATUS );

	DDS::WaitSet_var waitSet = new DDS::WaitSet;
	waitSet->attach_condition( condition );

	DDS::ConditionSeq conditions;
	DDS::SubscriptionMatchedStatus matches = { 0, 0, 0, 0, 0 };
	DDS::Duration_t timeout = { 30, 0 };

	//do {
	  if ( waitSet->wait(conditions, timeout ) != DDS::RETCODE_OK )
	  {
		LOG( ERROR ) << "wait condition failed.";
	  }

	  if ( _reader->get_subscription_matched_status( matches ) != DDS::RETCODE_OK )
	  {
		LOG( ERROR ) << "Publication matched status failed.";
	  }

	//} while ( matches.current_count > 0 );

	waitSet->detach_condition( condition );
}

void CAppNodeImpl::InitParticipant()
{
	// Init the participant factory and participant
	_domainParticipantFactory = TheParticipantFactoryWithArgs( _argCount, _pargVect );

	_participant = _domainParticipantFactory->create_participant( _domainID, PARTICIPANT_QOS_DEFAULT,
																							DDS::DomainParticipantListener::_nil(),
																							OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	// Exit is nil
	if( !_participant )
	{
		LOG( ERROR ) << "Create Participant Failed.";
	}
}

void CAppNodeImpl::InitPublisherAndSubscriber()
{
	// Publisher
	_publisher = _participant->create_publisher( PUBLISHER_QOS_DEFAULT,
																	DDS::PublisherListener::_nil(),
																	OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	// Exit if nil
	if( !_publisher )
	{
		LOG( ERROR ) << "Create publisher failed.";
	}

	// Subscriber
	_subscriber = _participant->create_subscriber( SUBSCRIBER_QOS_DEFAULT,
													DDS::SubscriberListener::_nil(),
													OpenDDS::DCPS::DEFAULT_STATUS_MASK );
	if( !_subscriber )
	{
		LOG( ERROR ) << "Create subscriber failed.";
	}
}

void CAppNodeImpl::InitTopicinfo()
{
	// Type registration
	_exampleTypeSupport = new ExampleApp::EventTypeSupportImpl();

	// Exit if retcode ! ok
	if( DDS::RETCODE_OK != _exampleTypeSupport->register_type( _participant, "" ) )
	{
		LOG( ERROR ) << "register type failed.";
	}

	// Create a topic
	_topicTypeName = _exampleTypeSupport->get_type_name();

	_topic = _participant->create_topic( "Test Topic", _topicTypeName.in(),
														TOPIC_QOS_DEFAULT,
														DDS::TopicListener::_nil(),
														OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_topic )
	{
		LOG( ERROR ) << "Create topic failed";
	}
}

void CAppNodeImpl::InitDataWriter()
{
	// Create the data writer
	_writer = _publisher->create_datawriter( _topic, DATAWRITER_QOS_DEFAULT,
																DDS::DataWriterListener::_nil(),
																OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_writer )
	{
		LOG( ERROR ) << "Create datawriter failed";
	}

	_eventWriter = ExampleApp::EventDataWriter::_narrow( _writer );

	if( !_eventWriter )
	{
		LOG( ERROR ) << "_narrow failed";
	}

}

void CAppNodeImpl::InitDataReader()
{
	// Data Reader
	_listener = new CDataReaderListenerImpl;

	_reader = _subscriber->create_datareader( _topic, DATAREADER_QOS_DEFAULT,
												_listener,
												OpenDDS::DCPS::DEFAULT_STATUS_MASK );

	if( !_reader )
	{
		LOG( ERROR ) << " Create data reader failed";
	}

	_readerI = ExampleApp::EventDataReader::_narrow( _reader );

	if( !_readerI )
	{

		LOG( ERROR ) << " _narrow failed";
	}
}
<|endoftext|>


<commit_before>#include "SysConfig.h"
#if(HAS_PCA9539)

#include <CI2C.h>
#include "CPCA9539.h"
#include "CTimer.h"
#include "NDataManager.h"

namespace
{
    CTimer pca9539_sample_timer;
    bool toggle = true;

}

CPCA9539::CPCA9539( CI2C *i2cInterfaceIn )
    : m_pca( i2cInterfaceIn )
{

}

void CPCA9539::Initialize()
{
    Serial.println( "CPCA9539.Status:INIT;" );

    pca9539_sample_timer.Reset();
    m_pca.Initialize();
    m_pca.PinMode( OUTPUT );

    Serial.println( "CPCA9539.Status:POST_INIT;");
}

void CPCA9539::Update( CCommand &commandIn )
{
    if( pca9539_sample_timer.HasElapsed( 1000 ) )
    {
        Serial.println( "PCA9539.Status:LOOP;" );
        if( toggle )
        {
            auto ret = m_pca.DigitalWrite( 0 , HIGH )
            Serial.println( ret );
        }
        else
        {
            auto ret = m_pca.DigitalWrite( 0 , LOW )
            Serial.println( ret );
        }

    }
}

#endif<commit_msg>semicolons<commit_after>#include "SysConfig.h"
#if(HAS_PCA9539)

#include <CI2C.h>
#include "CPCA9539.h"
#include "CTimer.h"
#include "NDataManager.h"

namespace
{
    CTimer pca9539_sample_timer;
    bool toggle = true;

}

CPCA9539::CPCA9539( CI2C *i2cInterfaceIn )
    : m_pca( i2cInterfaceIn )
{

}

void CPCA9539::Initialize()
{
    Serial.println( "CPCA9539.Status:INIT;" );

    pca9539_sample_timer.Reset();
    m_pca.Initialize();
    m_pca.PinMode( OUTPUT );

    Serial.println( "CPCA9539.Status:POST_INIT;");
}

void CPCA9539::Update( CCommand &commandIn )
{
    if( pca9539_sample_timer.HasElapsed( 1000 ) )
    {
        Serial.println( "PCA9539.Status:LOOP;" );
        if( toggle )
        {
            auto ret = m_pca.DigitalWrite( 0 , HIGH );
            Serial.println( ret );
        }
        else
        {
            auto ret = m_pca.DigitalWrite( 0 , LOW );
            Serial.println( ret );
        }

    }
}

#endif<|endoftext|>


<commit_before>/* dataproxy.cc			KPilot
**
** Copyright (C) 2007 by Bertjan Broeksema <b.broeksema@kdemail.net>
** Copyright (C) 2007 by Jason "vanRijn" Kasper <vr@movingparts.net>
*/

/*
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU Lesser General Public License as published by
** the Free Software Foundation; either version 2.1 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU Lesser General Public License for more details.
**
** You should have received a copy of the GNU Lesser General Public License
** along with this program in a file called COPYING; if not, write to
** the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
** MA 02110-1301, USA.
*/

/*
** Bug reports and questions can be sent to kde-pim@kde.org
*/

#include "dataproxy.h"
#include "record.h"

#include "options.h"

DataProxy::DataProxy() : fIterator( fRecords )
{
	FUNCTIONSETUP;
}

DataProxy::~DataProxy()
{
	FUNCTIONSETUP;

	qDeleteAll( fRecords );
}

QString DataProxy::create( Record *record )
{
	FUNCTIONSETUP;

	// Temporary id.
	QString uniqueId = generateUniqueId();

	// Make sure that the new record has the right id and add the record.
	record->setId( uniqueId );

	DEBUGKPILOT << "Record created with id: [" << uniqueId << "]";

	fRecords.insert( uniqueId, record );

	// Update rollback/volatility information.
	fCreated.insert( uniqueId, false );
	fCounter.created();

	return uniqueId;
}

void DataProxy::remove( const QString &id )
{
	FUNCTIONSETUP;

	Record *rec = fRecords.value( id );
	if( rec == 0L )
	{
		// No record
		return;
	}

	DEBUGKPILOT << "Removing record id: [" << id << "]";
	// Remove record.
	fRecords.remove( id );

	// Update rollback/volatility information.
	fDeletedRecords.insert( rec->id(), rec );
	fDeleted.insert( rec->id(), false );
	fCounter.deleted();
}

void DataProxy::update( const QString &id, Record *newRecord )
{
	FUNCTIONSETUP;

	Record *oldRecord = fRecords.value( id );
	if( oldRecord == 0L )
	{
		// No record, should not happen.
		DEBUGKPILOT << "There is no record with id: [" << id
			<< "]. Record not updated and not added.";
		return;
	}
	DEBUGKPILOT << "Updating record id: [" << id << "]";
	// Make sure that the new record has the right id and update the old record.
	newRecord->setId( id );
	fRecords.insert( id, newRecord );

	// Update rollback/volatility information.
	fOldRecords.insert( id, oldRecord );
	fCounter.updated();
}

QList<QString> DataProxy::ids() const
{
	return fRecords.keys();
}

const CUDCounter* DataProxy::counter() const
{
	FUNCTIONSETUP;

	return &fCounter;
}

void DataProxy::setEndcount()
{
	FUNCTIONSETUP;

	fCounter.setEndCount( fRecords.size() );
}

void DataProxy::setIterateMode( const Mode m )
{
	FUNCTIONSETUP;

	fMode = m;
}

unsigned int DataProxy::recordCount() const
{
	return fRecords.size();
}

Record* DataProxy::find( const QString &id ) const
{
	FUNCTIONSETUP;
	return fRecords.value( id );
}

void DataProxy::resetIterator()
{
	fIterator = QMapIterator<QString, Record*>( fRecords );
}

bool DataProxy::hasNext() const
{
	FUNCTIONSETUP;

	if( fMode == All )
	{
		return fIterator.hasNext();
	}
	else
	{
		QMapIterator<QString, Record*> tmpIt = fIterator;
		while( tmpIt.hasNext() )
		{
			Record *rec = tmpIt.next().value();
			if( rec->isModified() )
			{
				return true;
			}
		}
	}

	return false;
}

Record* DataProxy::next()
{
	FUNCTIONSETUP;

	if( fMode == All )
	{
			return fIterator.next().value();
	}
	else
	{
		while( fIterator.hasNext() )
		{
			Record *rec = fIterator.next().value();
			if( rec->isModified() )
			{
				return rec;
			}
		}
	}

	return 0L;
}

bool DataProxy::commit()
{
	FUNCTIONSETUP;

	// Commit created records.
	QStringListIterator it( fCreated.keys() );

	DEBUGKPILOT << "Committing: [" << fCreated.size() << "] records.";

	// Reset the map
	fCreated.clear();

	while( it.hasNext() )
	{
		QString id = it.next();

		DEBUGKPILOT << "Committing id: [" << id << "]";

		Record *rec = find( id );
		if( rec )
		{
			commitCreate( rec );

			// Put the record with the new id in.
			if( rec->id() != id )
			{
				fCreated.remove( id );
				fCreated.insert( rec->id(), true );

				fRecords.remove( id );
				fRecords.insert( rec->id(), rec );

				fChangedIds.insert( id, rec->id() );
			}
			else
			{
				fCreated.insert( rec->id(), true );
			}
		}
		else
		{
			DEBUGKPILOT << "Record with id: [" << id << "] not found!";
		}
	}

	// Commit updated records.
	DEBUGKPILOT << "Updating: [" << fOldRecords.size() << "] records.";

	QListIterator<Record*> i( fOldRecords.values() );
	while( i.hasNext() )
	{
		// i.next() contains the old values.
		Record *oldRec = i.next();
		QString id = oldRec->id();

		// Look up the new values
		Record *rec = find( id );

		if( rec && !fCreated.value( id ) )
		{
			commitUpdate( rec );
			QString newId = rec->id();

			if( newId != id )
			{
				oldRec->setId( newId );

				fChangedIds.insert( id, newId );
			}
			fUpdated.insert( rec->id(), true );
		}
	}

	// Commit deleted records
	DEBUGKPILOT << "Updating: [" << fDeletedRecords.size() << "] records.";

	i = QListIterator<Record*>( fDeletedRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();

		if( !fDeleted.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Deleting record: [" << oldRec->id() << "].";
			commitDelete( oldRec );
			fDeleted.insert( oldRec->id(), true );
		}
	}

	// Give implementing classes the change to do things if necessary.
	return _commit();
}

bool DataProxy::rollback()
{
	FUNCTIONSETUP;

	// Delete committed new records.
	QStringListIterator it( fCreated.keys() );

	while( it.hasNext() )
	{
		QString id = it.next();

		// Only undo creates that are committed.
		Record *rec = find( id );
		if( rec && fCreated.value( id ) )
		{
			DEBUGKPILOT << "Deleting created record: [" << rec->id() << "].";

			commitDelete( rec );
			fCreated.insert( rec->id(), false );
		}
	}

	// Reset the map
	fCreated.clear();

	// Undo changes to updated records.
	QListIterator<Record*> i( fOldRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();
		if( fUpdated.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Restoring changed record: [" << oldRec->id() << "].";

			QString oldId = oldRec->id();
			commitUpdate( oldRec );

			// Id might have changed
			if( oldRec->id() != oldId )
			{
				fUpdated.remove( oldId );

				fChangedIds.insert( oldId, oldRec->id() );
			}

			fUpdated.insert( oldRec->id(), false );
		}
	}

	// Restore deleted records.
	i = QListIterator<Record*>( fDeletedRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();

		if( fDeleted.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Restoring deleted record: [" << oldRec->id() << "].";

			QString oldId = oldRec->id();
			commitCreate( oldRec );

			// Id might have changed
			if( oldRec->id() != oldId )
			{
				fDeleted.remove( oldId );

				fChangedIds.insert( oldId, oldRec->id() );
			}

			fDeleted.insert( oldRec->id(), false );
		}
	}

	// Give implementing classes the change to rollback things if necessary.
	return _rollback();
}

QMap<QString,QString> DataProxy::changedIds()
{
	return fChangedIds;
}
<commit_msg>SVN_SILENT: Debug info update.<commit_after>/* dataproxy.cc			KPilot
**
** Copyright (C) 2007 by Bertjan Broeksema <b.broeksema@kdemail.net>
** Copyright (C) 2007 by Jason "vanRijn" Kasper <vr@movingparts.net>
*/

/*
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU Lesser General Public License as published by
** the Free Software Foundation; either version 2.1 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU Lesser General Public License for more details.
**
** You should have received a copy of the GNU Lesser General Public License
** along with this program in a file called COPYING; if not, write to
** the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
** MA 02110-1301, USA.
*/

/*
** Bug reports and questions can be sent to kde-pim@kde.org
*/

#include "dataproxy.h"
#include "record.h"

#include "options.h"

DataProxy::DataProxy() : fIterator( fRecords )
{
	FUNCTIONSETUP;
}

DataProxy::~DataProxy()
{
	FUNCTIONSETUP;

	qDeleteAll( fRecords );
}

QString DataProxy::create( Record *record )
{
	FUNCTIONSETUP;

	// Temporary id.
	QString uniqueId = generateUniqueId();

	// Make sure that the new record has the right id and add the record.
	record->setId( uniqueId );

	DEBUGKPILOT << "Record created with id: [" << uniqueId << "]";

	fRecords.insert( uniqueId, record );

	// Update rollback/volatility information.
	fCreated.insert( uniqueId, false );
	fCounter.created();

	return uniqueId;
}

void DataProxy::remove( const QString &id )
{
	FUNCTIONSETUP;

	Record *rec = fRecords.value( id );
	if( rec == 0L )
	{
		// No record
		return;
	}

	DEBUGKPILOT << "Removing record id: [" << id << "]";
	// Remove record.
	fRecords.remove( id );

	// Update rollback/volatility information.
	fDeletedRecords.insert( rec->id(), rec );
	fDeleted.insert( rec->id(), false );
	fCounter.deleted();
}

void DataProxy::update( const QString &id, Record *newRecord )
{
	FUNCTIONSETUP;

	Record *oldRecord = fRecords.value( id );
	if( oldRecord == 0L )
	{
		// No record, should not happen.
		DEBUGKPILOT << "There is no record with id: [" << id
			<< "]. Record not updated and not added.";
		return;
	}
	DEBUGKPILOT << "Updating record id: [" << id << "]";
	// Make sure that the new record has the right id and update the old record.
	newRecord->setId( id );
	fRecords.insert( id, newRecord );

	// Update rollback/volatility information.
	fOldRecords.insert( id, oldRecord );
	fCounter.updated();
}

QList<QString> DataProxy::ids() const
{
	return fRecords.keys();
}

const CUDCounter* DataProxy::counter() const
{
	FUNCTIONSETUP;

	return &fCounter;
}

void DataProxy::setEndcount()
{
	FUNCTIONSETUP;

	fCounter.setEndCount( fRecords.size() );
}

void DataProxy::setIterateMode( const Mode m )
{
	FUNCTIONSETUP;

	fMode = m;
}

unsigned int DataProxy::recordCount() const
{
	return fRecords.size();
}

Record* DataProxy::find( const QString &id ) const
{
	FUNCTIONSETUP;
	return fRecords.value( id );
}

void DataProxy::resetIterator()
{
	fIterator = QMapIterator<QString, Record*>( fRecords );
}

bool DataProxy::hasNext() const
{
	FUNCTIONSETUP;

	if( fMode == All )
	{
		return fIterator.hasNext();
	}
	else
	{
		QMapIterator<QString, Record*> tmpIt = fIterator;
		while( tmpIt.hasNext() )
		{
			Record *rec = tmpIt.next().value();
			if( rec->isModified() )
			{
				return true;
			}
		}
	}

	return false;
}

Record* DataProxy::next()
{
	FUNCTIONSETUP;

	if( fMode == All )
	{
			return fIterator.next().value();
	}
	else
	{
		while( fIterator.hasNext() )
		{
			Record *rec = fIterator.next().value();
			if( rec->isModified() )
			{
				return rec;
			}
		}
	}

	return 0L;
}

bool DataProxy::commit()
{
	FUNCTIONSETUP;

	// Commit created records.
	QStringListIterator it( fCreated.keys() );

	DEBUGKPILOT << "Committing: [" << fCreated.size() << "] records.";

	// Reset the map
	fCreated.clear();

	while( it.hasNext() )
	{
		QString id = it.next();

		DEBUGKPILOT << "Committing id: [" << id << "]";

		Record *rec = find( id );
		if( rec )
		{
			commitCreate( rec );

			// Put the record with the new id in.
			if( rec->id() != id )
			{
				fCreated.remove( id );
				fCreated.insert( rec->id(), true );

				fRecords.remove( id );
				fRecords.insert( rec->id(), rec );

				fChangedIds.insert( id, rec->id() );
			}
			else
			{
				fCreated.insert( rec->id(), true );
			}
		}
		else
		{
			DEBUGKPILOT << "Record with id: [" << id << "] not found!";
		}
	}

	// Commit updated records.
	DEBUGKPILOT << "Updating: [" << fOldRecords.size() << "] records.";

	QListIterator<Record*> i( fOldRecords.values() );
	while( i.hasNext() )
	{
		// i.next() contains the old values.
		Record *oldRec = i.next();
		QString id = oldRec->id();

		// Look up the new values
		Record *rec = find( id );

		if( rec && !fCreated.value( id ) )
		{
			commitUpdate( rec );
			QString newId = rec->id();

			if( newId != id )
			{
				oldRec->setId( newId );

				fChangedIds.insert( id, newId );
			}
			fUpdated.insert( rec->id(), true );
		}
	}

	// Commit deleted records
	DEBUGKPILOT << "Deleting: [" << fDeletedRecords.size() << "] records.";

	i = QListIterator<Record*>( fDeletedRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();

		if( !fDeleted.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Deleting record: [" << oldRec->id() << "].";
			commitDelete( oldRec );
			fDeleted.insert( oldRec->id(), true );
		}
	}

	// Give implementing classes the change to do things if necessary.
	return _commit();
}

bool DataProxy::rollback()
{
	FUNCTIONSETUP;

	// Delete committed new records.
	QStringListIterator it( fCreated.keys() );

	while( it.hasNext() )
	{
		QString id = it.next();

		// Only undo creates that are committed.
		Record *rec = find( id );
		if( rec && fCreated.value( id ) )
		{
			DEBUGKPILOT << "Deleting created record: [" << rec->id() << "].";

			commitDelete( rec );
			fCreated.insert( rec->id(), false );
		}
	}

	// Reset the map
	fCreated.clear();

	// Undo changes to updated records.
	QListIterator<Record*> i( fOldRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();
		if( fUpdated.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Restoring changed record: [" << oldRec->id() << "].";

			QString oldId = oldRec->id();
			commitUpdate( oldRec );

			// Id might have changed
			if( oldRec->id() != oldId )
			{
				fUpdated.remove( oldId );

				fChangedIds.insert( oldId, oldRec->id() );
			}

			fUpdated.insert( oldRec->id(), false );
		}
	}

	// Restore deleted records.
	i = QListIterator<Record*>( fDeletedRecords.values() );
	while( i.hasNext() )
	{
		Record *oldRec = i.next();

		if( fDeleted.value( oldRec->id() ) )
		{
			DEBUGKPILOT << "Restoring deleted record: [" << oldRec->id() << "].";

			QString oldId = oldRec->id();
			commitCreate( oldRec );

			// Id might have changed
			if( oldRec->id() != oldId )
			{
				fDeleted.remove( oldId );

				fChangedIds.insert( oldId, oldRec->id() );
			}

			fDeleted.insert( oldRec->id(), false );
		}
	}

	// Give implementing classes the change to rollback things if necessary.
	return _rollback();
}

QMap<QString,QString> DataProxy::changedIds()
{
	return fChangedIds;
}
<|endoftext|>


<commit_before>// Copyright by Contributors
#include "../../src/tree/param.h"

#include "../helpers.h"

TEST(Param, VectorIOStream) {
  std::vector<int> vals = {3, 2, 1};
  std::stringstream ss;
  std::vector<int> vals_in;

  ss << vals;
  EXPECT_EQ(ss.str(), "(3,2,1)");

  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals.clear(); ss.flush(); ss.clear(); ss.str("");
  vals = {1};
  ss << vals;
  EXPECT_EQ(ss.str(), "(1,)");
}

TEST(Param, VectorStreamRead) {
  std::vector<int> vals = {3, 2, 1};
  std::stringstream ss;
  std::vector<int> vals_in;

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3, 2, 1)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3L,2L,1L)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " (3,2,1,)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " ( 3, 2,1 )";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " ( 3, 2,1 ) ";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " 321 ";
  ss >> vals_in;
  EXPECT_EQ(vals_in[0], 321);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3.0,2,1)";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "1a";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "abcde";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3,2,1";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);
}
<commit_msg>tests/cpp: Add tests for SplitEntry<commit_after>// Copyright by Contributors
#include "../../src/tree/param.h"

#include "../helpers.h"

TEST(Param, VectorIOStream) {
  std::vector<int> vals = {3, 2, 1};
  std::stringstream ss;
  std::vector<int> vals_in;

  ss << vals;
  EXPECT_EQ(ss.str(), "(3,2,1)");

  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals.clear(); ss.flush(); ss.clear(); ss.str("");
  vals = {1};
  ss << vals;
  EXPECT_EQ(ss.str(), "(1,)");
}

TEST(Param, VectorStreamRead) {
  std::vector<int> vals = {3, 2, 1};
  std::stringstream ss;
  std::vector<int> vals_in;

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3, 2, 1)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3L,2L,1L)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " (3,2,1,)";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " ( 3, 2,1 )";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " ( 3, 2,1 ) ";
  ss >> vals_in;
  EXPECT_EQ(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << " 321 ";
  ss >> vals_in;
  EXPECT_EQ(vals_in[0], 321);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3.0,2,1)";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "1a";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "abcde";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);

  vals_in.clear(); ss.flush(); ss.clear(); ss.str("");
  ss << "(3,2,1";
  ss >> vals_in;
  EXPECT_NE(vals_in, vals);
}

TEST(Param, SplitEntry) {
  xgboost::tree::SplitEntry se1;
  EXPECT_FALSE(se1.NeedReplace(-1, 100));

  xgboost::tree::SplitEntry se2;
  EXPECT_FALSE(se1.Update(se2));
  EXPECT_FALSE(se2.Update(-1, 100, 0, true));
  ASSERT_TRUE(se2.Update(1, 100, 0, true));
  ASSERT_TRUE(se1.Update(se2));

  xgboost::tree::SplitEntry se3;
  se3.Update(2, 101, 0, false);
  xgboost::tree::SplitEntry::Reduce(se2, se3);
  EXPECT_EQ(se2.split_index(), 101);
  EXPECT_FALSE(se2.default_left());

  EXPECT_TRUE(se1.NeedReplace(3, 1));
}
<|endoftext|>


<commit_before>/*
 * tidyup_planning_scene_updater.cpp
 *
 *  Created on: 10 Aug 2012
 *      Author: andreas
 */

#include "planner_modules_pr2/tidyup_planning_scene_updater.h"
#include <tidyup_utils/planning_scene_interface.h>
#include <tidyup_utils/arm_state.h>
#include <tidyup_utils/stringutil.h>
#include <hardcoded_facts/geometryPoses.h>
#include <ros/ros.h>

using std::vector;
using std::map;
using std::set;
using std::string;

TidyupPlanningSceneUpdater* TidyupPlanningSceneUpdater::instance = NULL;

TidyupPlanningSceneUpdater::TidyupPlanningSceneUpdater() : logName("[psu]")
{
    string doorLocationFileName;
    ros::param::get("/continual_planning_executive/door_location_file", doorLocationFileName);
    loadDoorPoses(doorLocationFileName);

    defaultAttachPose.position.x = 0.032;
    defaultAttachPose.position.y = 0.015;
    defaultAttachPose.position.z = 0.0;
    defaultAttachPose.orientation.x = 0.707;
    defaultAttachPose.orientation.y = -0.106;
    defaultAttachPose.orientation.z = -0.690;
    defaultAttachPose.orientation.w = 0.105;

    ROS_INFO("%s initialized.\n", logName.c_str());
}

TidyupPlanningSceneUpdater::~TidyupPlanningSceneUpdater()
{
}

bool TidyupPlanningSceneUpdater::readState(
        const string& robotLocation,
        predicateCallbackType predicateCallback,
        numericalFluentCallbackType numericalFluentCallback,
        geometry_msgs::Pose& robotPose,
        map<string, geometry_msgs::Pose>& movableObjects,
        map<string, string>& graspedObjects,
        map<string, string>& objectsOnStatic,
        set<string>& openDoors)
{
    if (instance == NULL) instance = new TidyupPlanningSceneUpdater();
    return instance->readState_(robotLocation, predicateCallback, numericalFluentCallback, robotPose, movableObjects, graspedObjects, objectsOnStatic, openDoors);
}

bool TidyupPlanningSceneUpdater::readState_(
        const string& robotLocation,
        predicateCallbackType predicateCallback,
        numericalFluentCallbackType numericalFluentCallback,
        geometry_msgs::Pose& robotPose,
        map<string, geometry_msgs::Pose>& movableObjects,
        map<string, string>& graspedObjects,
        map<string, string>& objectsOnStatic,
        set<string>& openDoors)
{
    // get poses of all movable objects
    PlanningSceneInterface* psi = PlanningSceneInterface::instance();
   // psi->resetPlanningScene();
    geometry_msgs::Pose pose;
    const vector <arm_navigation_msgs::CollisionObject>& collisionObjects = psi->getCollisionObjects();
    for (vector <arm_navigation_msgs::CollisionObject>::const_iterator it = collisionObjects.begin();
            it != collisionObjects.end(); it++)
    {
        const string& objectName = it->id;
        if (StringUtil::startsWith(objectName, "table"))
            continue;
        if (StringUtil::startsWith(objectName, "door"))
            continue;
        if (! fillPoseFromState(pose, objectName, numericalFluentCallback))
        {
            psi->removeObject(objectName);
        }
        else
        {
            movableObjects.insert(make_pair(objectName, pose));
        }
    }
    const vector <arm_navigation_msgs::AttachedCollisionObject>& attachedObjects = psi->getAttachedCollisionObjects();
    for (vector <arm_navigation_msgs::AttachedCollisionObject>::const_iterator it = attachedObjects.begin(); it != attachedObjects.end(); it++)
    {
        const string& objectName = it->object.id;
        if (! fillPoseFromState(pose, objectName, numericalFluentCallback))
        {
            psi->removeObject(objectName);
        }
        else
        {
            movableObjects.insert(make_pair(objectName, pose));
        }
    }

    // find grasped objects and objects on tables
    PredicateList* predicates = NULL;
    if (!predicateCallback(predicates))
    {
        ROS_ERROR("%s predicateCallback failed.", logName.c_str());
        return false;
    }
    ROS_ASSERT(predicates != NULL);
    for (PredicateList::iterator it = predicates->begin(); it != predicates->end(); it++)
    {
        Predicate p = *it;
        if (!p.value)
            continue;
        if (p.name == "on")
        {
            ROS_ASSERT(p.parameters.size() == 2);
            // (on movable static)
            objectsOnStatic.insert(make_pair(p.parameters.front().value, p.parameters.back().value));
        }
        else if (p.name == "grasped")
        {
            ROS_ASSERT(p.parameters.size() == 2);
            // (grasped object arm)
            graspedObjects.insert(make_pair(p.parameters.front().value, p.parameters.back().value));
        }
        else if (p.name == "door-open")
        {
            ROS_ASSERT(p.parameters.size() == 1);
            // (door-open door)
            openDoors.insert(p.parameters.front().value);
        }
    }

    // Robot pose
    if (! fillPoseFromState(robotPose, robotLocation, numericalFluentCallback))
    {
        ROS_ERROR("%s get robot location failed.", logName.c_str());
    }
    return true;
}

bool TidyupPlanningSceneUpdater::update(const geometry_msgs::Pose& robotPose,
        const map<string, geometry_msgs::Pose>& movableObjects,
        const map<string, string>& graspedObjects,
        const set<string>& openDoors)
{
    if (instance == NULL) instance = new TidyupPlanningSceneUpdater();
    return instance->update_(robotPose, movableObjects, graspedObjects, openDoors);
}

bool TidyupPlanningSceneUpdater::update_(const geometry_msgs::Pose& robotPose,
        const map<string, geometry_msgs::Pose>& movableObjects,
        const map<string, string>& graspedObjects,
        const set<string>& openDoors)
{
    PlanningSceneInterface* psi = PlanningSceneInterface::instance();
//    psi->resetPlanningScene();

    // set robot state in planning scene
    ROS_INFO("%s update robot state in planning scene", logName.c_str());
    arm_navigation_msgs::RobotState state = psi->getRobotState();
    state.multi_dof_joint_state.poses[0] = robotPose;
    ArmState::get("/arm_configurations/side_tuck/position/", "right_arm").replaceJointPositions(state.joint_state);
    ArmState::get("/arm_configurations/side_tuck/position/", "left_arm").replaceJointPositions(state.joint_state);

    // update pose of graspable object in the planning scene
    for(map<string, geometry_msgs::Pose>::const_iterator movabelObjectIt = movableObjects.begin(); movabelObjectIt != movableObjects.end(); movabelObjectIt++)
    {
        string object_name = movabelObjectIt->first;
        ROS_INFO("%s updating object %s", logName.c_str(), object_name.c_str());
        // if this object is attached somewhere we need to detach it
        if (psi->getAttachedCollisionObject(object_name) != NULL)
        {
            psi->detachObjectAndAdd(object_name);
        }
        // object is not attached, update pose
        if (psi->getCollisionObject(object_name) != NULL)
        {
            psi->updateObject(object_name, movabelObjectIt->second);
        }
        else
        {
            ROS_ERROR("%s object %s does not exist in planning scene.", logName.c_str(), object_name.c_str());
            return false;
        }
    }

    // attach putdown object to the correct arm
//    const arm_navigation_msgs::CollisionObject* object = psi->getCollisionObject(request.putdown_object);
    for (map<string, string>::const_iterator graspedIt = graspedObjects.begin(); graspedIt != graspedObjects.end(); graspedIt++)
    {
        const string& objectName = graspedIt->first;
        const string& arm = graspedIt->second;
        ROS_INFO("%s attaching object %s to arm %s", logName.c_str(), objectName.c_str(), arm.c_str());
        ArmState::get("/arm_configurations/side_carry/position/", arm).replaceJointPositions(state.joint_state);
        psi->attachObjectToGripper(objectName, arm);
        psi->updateObject(objectName, defaultAttachPose);
    }
    psi->setRobotState(state);

    // update doors
    for (map<string, Door>::const_iterator doorIt = doors.begin(); doorIt != doors.end(); doorIt++)
    {
        if (openDoors.find(doorIt->first) != openDoors.end())
        {
            // door is open
            psi->updateObject(doorIt->first, doorIt->second.openPose);
        }
        else
        {
            // door is closed
            psi->updateObject(doorIt->first, doorIt->second.closedPose);
        }
    }

    // send changes
    return psi->sendDiff();
}

bool TidyupPlanningSceneUpdater::fillPoseFromState(geometry_msgs::Pose& pose,
        const string& poseName,
        numericalFluentCallbackType numericalFluentCallback)
{
    // create the numerical fluent request
    ParameterList startParams;
    startParams.push_back(Parameter("", "", poseName));
    NumericalFluentList nfRequest;
    nfRequest.reserve(7);
    nfRequest.push_back(NumericalFluent("x", startParams));
    nfRequest.push_back(NumericalFluent("y", startParams));
    nfRequest.push_back(NumericalFluent("z", startParams));
    nfRequest.push_back(NumericalFluent("qx", startParams));
    nfRequest.push_back(NumericalFluent("qy", startParams));
    nfRequest.push_back(NumericalFluent("qz", startParams));
    nfRequest.push_back(NumericalFluent("qw", startParams));

    // get the fluents
    NumericalFluentList* nfRequestP = &nfRequest;
    if (!numericalFluentCallback(nfRequestP))
    {
        ROS_ERROR("fillPoseFromState failed for object: %s", poseName.c_str());
        return false;
    }

    // fill pose stamped
    pose.position.x = nfRequest[0].value;
    pose.position.y = nfRequest[1].value;
    pose.position.z = nfRequest[2].value;
    pose.orientation.x = nfRequest[3].value;
    pose.orientation.y = nfRequest[4].value;
    pose.orientation.z = nfRequest[5].value;
    pose.orientation.w = nfRequest[6].value;
    return true;
}

void TidyupPlanningSceneUpdater::loadDoorPoses(const string& doorLocationFileName)
{
    GeometryPoses locations = GeometryPoses();
    ROS_ASSERT_MSG(locations.load(doorLocationFileName), "Could not load locations from \"%s\".", doorLocationFileName.c_str());
    const std::map<std::string, geometry_msgs::PoseStamped>& poses = locations.getPoses();
    for(std::map<std::string, geometry_msgs::PoseStamped>::const_iterator posesIterator = poses.begin(); posesIterator != poses.end(); posesIterator++)
    {
        string name = posesIterator->first;
        if (StringUtil::startsWith(name, "door"))
        {
            string doorName;
            bool open = false;
            if(StringUtil::endsWith(name, "_closed"))
            {
                doorName = name.substr(0, name.length()-7);
            }
            else if(StringUtil::endsWith(name, "_open"))
            {
                doorName = name.substr(0, name.length()-5);
                open = true;
            }
            else
            {
                ROS_ERROR("navstack planning scene: misformated door location entry %s in file %s", name.c_str(), doorLocationFileName.c_str());
                continue;
            }
            map<string, Door>::iterator doorIterator = doors.find(doorName);
            if (doorIterator == doors.end())
            {
                doors.insert(make_pair(doorName, Door(doorName)));
                doorIterator = doors.find(doorName);
            }
            Door& door = doorIterator->second;
            if (open)
            {
                door.openPose = posesIterator->second.pose;
            }
            else
            {
                door.closedPose = posesIterator->second.pose;
            }
        }
    }
}

<commit_msg>reset planning scene before update<commit_after>/*
 * tidyup_planning_scene_updater.cpp
 *
 *  Created on: 10 Aug 2012
 *      Author: andreas
 */

#include "planner_modules_pr2/tidyup_planning_scene_updater.h"
#include <tidyup_utils/planning_scene_interface.h>
#include <tidyup_utils/arm_state.h>
#include <tidyup_utils/stringutil.h>
#include <hardcoded_facts/geometryPoses.h>
#include <ros/ros.h>

using std::vector;
using std::map;
using std::set;
using std::string;

TidyupPlanningSceneUpdater* TidyupPlanningSceneUpdater::instance = NULL;

TidyupPlanningSceneUpdater::TidyupPlanningSceneUpdater() : logName("[psu]")
{
    string doorLocationFileName;
    ros::param::get("/continual_planning_executive/door_location_file", doorLocationFileName);
    loadDoorPoses(doorLocationFileName);

    defaultAttachPose.position.x = 0.032;
    defaultAttachPose.position.y = 0.015;
    defaultAttachPose.position.z = 0.0;
    defaultAttachPose.orientation.x = 0.707;
    defaultAttachPose.orientation.y = -0.106;
    defaultAttachPose.orientation.z = -0.690;
    defaultAttachPose.orientation.w = 0.105;

    ROS_INFO("%s initialized.\n", logName.c_str());
}

TidyupPlanningSceneUpdater::~TidyupPlanningSceneUpdater()
{
}

bool TidyupPlanningSceneUpdater::readState(
        const string& robotLocation,
        predicateCallbackType predicateCallback,
        numericalFluentCallbackType numericalFluentCallback,
        geometry_msgs::Pose& robotPose,
        map<string, geometry_msgs::Pose>& movableObjects,
        map<string, string>& graspedObjects,
        map<string, string>& objectsOnStatic,
        set<string>& openDoors)
{
    if (instance == NULL) instance = new TidyupPlanningSceneUpdater();
    return instance->readState_(robotLocation, predicateCallback, numericalFluentCallback, robotPose, movableObjects, graspedObjects, objectsOnStatic, openDoors);
}

bool TidyupPlanningSceneUpdater::readState_(
        const string& robotLocation,
        predicateCallbackType predicateCallback,
        numericalFluentCallbackType numericalFluentCallback,
        geometry_msgs::Pose& robotPose,
        map<string, geometry_msgs::Pose>& movableObjects,
        map<string, string>& graspedObjects,
        map<string, string>& objectsOnStatic,
        set<string>& openDoors)
{
    // get poses of all movable objects
    PlanningSceneInterface* psi = PlanningSceneInterface::instance();
    psi->resetPlanningScene();
    geometry_msgs::Pose pose;
    const vector <arm_navigation_msgs::CollisionObject>& collisionObjects = psi->getCollisionObjects();
    for (vector <arm_navigation_msgs::CollisionObject>::const_iterator it = collisionObjects.begin();
            it != collisionObjects.end(); it++)
    {
        const string& objectName = it->id;
        if (StringUtil::startsWith(objectName, "table"))
            continue;
        if (StringUtil::startsWith(objectName, "door"))
            continue;
        if (! fillPoseFromState(pose, objectName, numericalFluentCallback))
        {
            psi->removeObject(objectName);
        }
        else
        {
            movableObjects.insert(make_pair(objectName, pose));
        }
    }
    const vector <arm_navigation_msgs::AttachedCollisionObject>& attachedObjects = psi->getAttachedCollisionObjects();
    for (vector <arm_navigation_msgs::AttachedCollisionObject>::const_iterator it = attachedObjects.begin(); it != attachedObjects.end(); it++)
    {
        const string& objectName = it->object.id;
        if (! fillPoseFromState(pose, objectName, numericalFluentCallback))
        {
            psi->removeObject(objectName);
        }
        else
        {
            movableObjects.insert(make_pair(objectName, pose));
        }
    }

    // find grasped objects and objects on tables
    PredicateList* predicates = NULL;
    if (!predicateCallback(predicates))
    {
        ROS_ERROR("%s predicateCallback failed.", logName.c_str());
        return false;
    }
    ROS_ASSERT(predicates != NULL);
    for (PredicateList::iterator it = predicates->begin(); it != predicates->end(); it++)
    {
        Predicate p = *it;
        if (!p.value)
            continue;
        if (p.name == "on")
        {
            ROS_ASSERT(p.parameters.size() == 2);
            // (on movable static)
            objectsOnStatic.insert(make_pair(p.parameters.front().value, p.parameters.back().value));
        }
        else if (p.name == "grasped")
        {
            ROS_ASSERT(p.parameters.size() == 2);
            // (grasped object arm)
            graspedObjects.insert(make_pair(p.parameters.front().value, p.parameters.back().value));
        }
        else if (p.name == "door-open")
        {
            ROS_ASSERT(p.parameters.size() == 1);
            // (door-open door)
            openDoors.insert(p.parameters.front().value);
        }
    }

    // Robot pose
    if (! fillPoseFromState(robotPose, robotLocation, numericalFluentCallback))
    {
        ROS_ERROR("%s get robot location failed.", logName.c_str());
    }
    return true;
}

bool TidyupPlanningSceneUpdater::update(const geometry_msgs::Pose& robotPose,
        const map<string, geometry_msgs::Pose>& movableObjects,
        const map<string, string>& graspedObjects,
        const set<string>& openDoors)
{
    if (instance == NULL) instance = new TidyupPlanningSceneUpdater();
    return instance->update_(robotPose, movableObjects, graspedObjects, openDoors);
}

bool TidyupPlanningSceneUpdater::update_(const geometry_msgs::Pose& robotPose,
        const map<string, geometry_msgs::Pose>& movableObjects,
        const map<string, string>& graspedObjects,
        const set<string>& openDoors)
{
    PlanningSceneInterface* psi = PlanningSceneInterface::instance();
//    psi->resetPlanningScene();

    // set robot state in planning scene
    ROS_INFO("%s update robot state in planning scene", logName.c_str());
    arm_navigation_msgs::RobotState state = psi->getRobotState();
    state.multi_dof_joint_state.poses[0] = robotPose;
    ArmState::get("/arm_configurations/side_tuck/position/", "right_arm").replaceJointPositions(state.joint_state);
    ArmState::get("/arm_configurations/side_tuck/position/", "left_arm").replaceJointPositions(state.joint_state);

    // update pose of graspable object in the planning scene
    for(map<string, geometry_msgs::Pose>::const_iterator movabelObjectIt = movableObjects.begin(); movabelObjectIt != movableObjects.end(); movabelObjectIt++)
    {
        string object_name = movabelObjectIt->first;
        ROS_INFO("%s updating object %s", logName.c_str(), object_name.c_str());
        // if this object is attached somewhere we need to detach it
        if (psi->getAttachedCollisionObject(object_name) != NULL)
        {
            psi->detachObjectAndAdd(object_name);
        }
        // object is not attached, update pose
        if (psi->getCollisionObject(object_name) != NULL)
        {
            psi->updateObject(object_name, movabelObjectIt->second);
        }
        else
        {
            ROS_ERROR("%s object %s does not exist in planning scene.", logName.c_str(), object_name.c_str());
            return false;
        }
    }

    // attach putdown object to the correct arm
//    const arm_navigation_msgs::CollisionObject* object = psi->getCollisionObject(request.putdown_object);
    for (map<string, string>::const_iterator graspedIt = graspedObjects.begin(); graspedIt != graspedObjects.end(); graspedIt++)
    {
        const string& objectName = graspedIt->first;
        const string& arm = graspedIt->second;
        ROS_INFO("%s attaching object %s to arm %s", logName.c_str(), objectName.c_str(), arm.c_str());
        ArmState::get("/arm_configurations/side_carry/position/", arm).replaceJointPositions(state.joint_state);
        psi->attachObjectToGripper(objectName, arm);
        psi->updateObject(objectName, defaultAttachPose);
    }
    psi->setRobotState(state);

    // update doors
    for (map<string, Door>::const_iterator doorIt = doors.begin(); doorIt != doors.end(); doorIt++)
    {
        if (openDoors.find(doorIt->first) != openDoors.end())
        {
            // door is open
            psi->updateObject(doorIt->first, doorIt->second.openPose);
        }
        else
        {
            // door is closed
            psi->updateObject(doorIt->first, doorIt->second.closedPose);
        }
    }

    // send changes
    return psi->sendDiff();
}

bool TidyupPlanningSceneUpdater::fillPoseFromState(geometry_msgs::Pose& pose,
        const string& poseName,
        numericalFluentCallbackType numericalFluentCallback)
{
    // create the numerical fluent request
    ParameterList startParams;
    startParams.push_back(Parameter("", "", poseName));
    NumericalFluentList nfRequest;
    nfRequest.reserve(7);
    nfRequest.push_back(NumericalFluent("x", startParams));
    nfRequest.push_back(NumericalFluent("y", startParams));
    nfRequest.push_back(NumericalFluent("z", startParams));
    nfRequest.push_back(NumericalFluent("qx", startParams));
    nfRequest.push_back(NumericalFluent("qy", startParams));
    nfRequest.push_back(NumericalFluent("qz", startParams));
    nfRequest.push_back(NumericalFluent("qw", startParams));

    // get the fluents
    NumericalFluentList* nfRequestP = &nfRequest;
    if (!numericalFluentCallback(nfRequestP))
    {
        ROS_ERROR("fillPoseFromState failed for object: %s", poseName.c_str());
        return false;
    }

    // fill pose stamped
    pose.position.x = nfRequest[0].value;
    pose.position.y = nfRequest[1].value;
    pose.position.z = nfRequest[2].value;
    pose.orientation.x = nfRequest[3].value;
    pose.orientation.y = nfRequest[4].value;
    pose.orientation.z = nfRequest[5].value;
    pose.orientation.w = nfRequest[6].value;
    return true;
}

void TidyupPlanningSceneUpdater::loadDoorPoses(const string& doorLocationFileName)
{
    GeometryPoses locations = GeometryPoses();
    ROS_ASSERT_MSG(locations.load(doorLocationFileName), "Could not load locations from \"%s\".", doorLocationFileName.c_str());
    const std::map<std::string, geometry_msgs::PoseStamped>& poses = locations.getPoses();
    for(std::map<std::string, geometry_msgs::PoseStamped>::const_iterator posesIterator = poses.begin(); posesIterator != poses.end(); posesIterator++)
    {
        string name = posesIterator->first;
        if (StringUtil::startsWith(name, "door"))
        {
            string doorName;
            bool open = false;
            if(StringUtil::endsWith(name, "_closed"))
            {
                doorName = name.substr(0, name.length()-7);
            }
            else if(StringUtil::endsWith(name, "_open"))
            {
                doorName = name.substr(0, name.length()-5);
                open = true;
            }
            else
            {
                ROS_ERROR("navstack planning scene: misformated door location entry %s in file %s", name.c_str(), doorLocationFileName.c_str());
                continue;
            }
            map<string, Door>::iterator doorIterator = doors.find(doorName);
            if (doorIterator == doors.end())
            {
                doors.insert(make_pair(doorName, Door(doorName)));
                doorIterator = doors.find(doorName);
            }
            Door& door = doorIterator->second;
            if (open)
            {
                door.openPose = posesIterator->second.pose;
            }
            else
            {
                door.closedPose = posesIterator->second.pose;
            }
        }
    }
}

<|endoftext|>


<commit_before>/* Lantern - A path tracer
*
* Lantern is the legal property of Adrian Astley
* Copyright Adrian Astley 2015 - 2016
*/

#include "renderer/renderer.h"

#include "renderer/surface_interaction.h"

#include "scene/scene.h"
#include "scene/ray.h"

#include "bsdfs/bsdf.h"

#include "math/uniform_sampler.h"
#include "math/vector_math.h"
#include "math/sampling.h"

#include <tbb/parallel_for.h>

#include <algorithm>

namespace Lantern {

void Renderer::RenderFrame() {
	uint width = m_scene->Camera.FrameBuffer.Width;
	uint height = m_scene->Camera.FrameBuffer.Height;

	const int numTilesX = (width + kTileSize - 1) / kTileSize;
	const int numTilesY = (height + kTileSize - 1) / kTileSize;

	tbb::parallel_for(size_t(0), size_t(numTilesX * numTilesY), [=](size_t i) {
		RenderTile(i, width, height, numTilesX, numTilesY);
	});

	++m_frameNumber;
}

// MurmurHash3
inline uint HashMix(uint hash, uint k) {
	const uint c1 = 0xcc9e2d51;
	const uint c2 = 0x1b873593;
	const uint r1 = 15;
	const uint r2 = 13;
	const uint m = 5;
	const uint n = 0xe6546b64;

	k *= c1;
	k = (k << r1) | (k >> (32 - r1));
	k *= c2;

	hash ^= k;
	hash = ((hash << r2) | (hash >> (32 - r2))) * m + n;

	return hash;
}

// MurmurHash3
inline uint HashFinalize(uint hash) {
	hash ^= hash >> 16;
	hash *= 0x85ebca6b;
	hash ^= hash >> 13;
	hash *= 0xc2b2ae35;
	hash ^= hash >> 16;

	return hash;
}

void Renderer::RenderTile(uint index, uint width, uint height, uint numTilesX, uint numTilesY) {
	uint tileY = index / numTilesX;
	uint tileX = index - tileY * numTilesX;

	uint x0 = tileX * kTileSize;
	uint x1 = std::min(x0 + kTileSize, width);
	uint y0 = tileY * kTileSize;
	uint y1 = std::min(y0 + kTileSize, height);

	uint hash = 0u;
	hash = HashMix(hash, index);
	hash = HashMix(hash, m_frameNumber);
	hash = HashFinalize(hash);

	UniformSampler sampler(hash, m_frameNumber);

	for (uint y = y0; y < y1; ++y) {
		for (uint x = x0; x < x1; ++x) {
			RenderPixel(x, y, &sampler);
		}
	}
}

void Renderer::RenderPixel(uint x, uint y, UniformSampler *sampler) {
	Ray ray = m_scene->Camera.CalculateRayFromPixel(x, y, sampler);

	float3 color(0.0f);
	float3 throughput(1.0f);

	// Bounce the ray around the scene
	for (uint bounces = 0; bounces < 10; ++bounces) {
		m_scene->Intersect(ray);

		// The ray missed. Return the background color
		if (ray.GeomID == INVALID_GEOMETRY_ID) {
			color += throughput * m_scene->BackgroundColor;
			break;
		}

		// We hit an object

		// Fetch the bsdf
		BSDF *bsdf = m_scene->GetBSDF(ray.GeomID);
		// The object might be emissive. If so, it will have a corresponding light
		// Otherwise, GetLight will return nullptr
		Light *light = m_scene->GetLight(ray.GeomID);

		// If this is the first bounce or if we just had a specular bounce,
		// we need to add the emmisive light
		if (bounces == 0 && light != nullptr) {
			color += throughput * light->Le();
		}

		SurfaceInteraction interaction;
		interaction.Position = ray.Origin + ray.Direction * ray.TFar;
		interaction.Normal = normalize(ray.GeomNormal);
		interaction.OutputDirection = normalize(-ray.Direction);

		// Calculate the direct lighting
		color += throughput * SampleOneLight(sampler, interaction, bsdf, light);

		// Get the new ray direction
		// Choose the direction based on the bsdf
		bsdf->Sample(interaction, sampler);
		float pdf = bsdf->Pdf(interaction);

		// Accumulate the diffuse/specular weight
		throughput = throughput * bsdf->Eval(interaction) / pdf;

		// Russian Roulette
		if (bounces > 3) {
			float p = std::max(throughput.x, std::max(throughput.y, throughput.z));
			if (sampler->NextFloat() > p) {
				break;
			}

			throughput *= 1 / p;
		}

		// Shoot a new ray

		// Set the origin at the intersection point
		ray.Origin = interaction.Position;

		// Reset the other ray properties
		ray.Direction = interaction.InputDirection;
		ray.TNear = 0.001f;
		ray.TFar = infinity;
		ray.GeomID = INVALID_GEOMETRY_ID;
		ray.PrimID = INVALID_PRIMATIVE_ID;
		ray.InstID = INVALID_INSTANCE_ID;
		ray.Mask = 0xFFFFFFFF;
		ray.Time = 0.0f;
	}

	m_scene->Camera.FrameBuffer.SplatPixel(x, y, color);
}

float3 Renderer::SampleOneLight(UniformSampler *sampler, SurfaceInteraction interaction, BSDF *bsdf, Light *hitLight) const {
	std::size_t numLights = m_scene->NumLights();

	// Return black if there are no lights
	// And don't let a light contribute light to itself
	// Aka, if we hit a light
	// This is the special case where there is only 1 light
	if (numLights == 0 || numLights == 1 && hitLight != nullptr) {
		return float3(0.0f);
	}

	// Don't let a light contribute light to itself
	// Choose another one
	Light *light;
	do {
		light = m_scene->RandomOneLight(sampler);
	} while (light == hitLight);

	return numLights * EstimateDirect(light, sampler, interaction, bsdf);
}

float3 Renderer::EstimateDirect(Light *light, UniformSampler *sampler, SurfaceInteraction &interaction, BSDF *bsdf) const {
	float3 directLighting = float3(0.0f);
	float3 f;
	float lightPdf, scatteringPdf;

	// Sample lighting with multiple importance sampling

	float3 Li = light->SampleLi(sampler, m_scene, interaction, &lightPdf);

	// Make sure the pdf isn't zero and the radiance isn't black
	if (lightPdf != 0.0f && !all(Li)) {
		// Calculate the brdf value
		f = bsdf->Eval(interaction);
		scatteringPdf = bsdf->Pdf(interaction);

		if (scatteringPdf != 0.0f && !all(f)) {
			float weight = PowerHeuristic(1, lightPdf, 1, scatteringPdf);
			directLighting += f * Li * weight / lightPdf;
		}
	}

	// Sample brdf with multiple importance sampling

	bsdf->Sample(interaction, sampler);
	f = bsdf->Eval(interaction);
	scatteringPdf = bsdf->Pdf(interaction);
	if (scatteringPdf != 0.0f && !all(f)) {
		lightPdf = light->PdfLi(m_scene, interaction);
		if (lightPdf == 0.0f) {
			// We didn't hit anything, so ignore the brdf sample
			return directLighting;
		}

		float weight = PowerHeuristic(1, scatteringPdf, 1, lightPdf);
		Li = light->Le();
		directLighting += f * Li * weight / scatteringPdf;
	}

	return directLighting;
}

} // End of namespace Lantern
<commit_msg>RenderTile and RenderPixel are const<commit_after>/* Lantern - A path tracer
*
* Lantern is the legal property of Adrian Astley
* Copyright Adrian Astley 2015 - 2016
*/

#include "renderer/renderer.h"

#include "renderer/surface_interaction.h"

#include "scene/scene.h"
#include "scene/ray.h"

#include "bsdfs/bsdf.h"

#include "math/uniform_sampler.h"
#include "math/vector_math.h"
#include "math/sampling.h"

#include <tbb/parallel_for.h>

#include <algorithm>

namespace Lantern {

void Renderer::RenderFrame() {
	uint width = m_scene->Camera.FrameBuffer.Width;
	uint height = m_scene->Camera.FrameBuffer.Height;

	const int numTilesX = (width + kTileSize - 1) / kTileSize;
	const int numTilesY = (height + kTileSize - 1) / kTileSize;

	tbb::parallel_for(size_t(0), size_t(numTilesX * numTilesY), [=](size_t i) {
		RenderTile(i, width, height, numTilesX, numTilesY);
	});

	++m_frameNumber;
}

// MurmurHash3
inline uint HashMix(uint hash, uint k) {
	const uint c1 = 0xcc9e2d51;
	const uint c2 = 0x1b873593;
	const uint r1 = 15;
	const uint r2 = 13;
	const uint m = 5;
	const uint n = 0xe6546b64;

	k *= c1;
	k = (k << r1) | (k >> (32 - r1));
	k *= c2;

	hash ^= k;
	hash = ((hash << r2) | (hash >> (32 - r2))) * m + n;

	return hash;
}

// MurmurHash3
inline uint HashFinalize(uint hash) {
	hash ^= hash >> 16;
	hash *= 0x85ebca6b;
	hash ^= hash >> 13;
	hash *= 0xc2b2ae35;
	hash ^= hash >> 16;

	return hash;
}

void Renderer::RenderTile(uint index, uint width, uint height, uint numTilesX, uint numTilesY) const {
	uint tileY = index / numTilesX;
	uint tileX = index - tileY * numTilesX;

	uint x0 = tileX * kTileSize;
	uint x1 = std::min(x0 + kTileSize, width);
	uint y0 = tileY * kTileSize;
	uint y1 = std::min(y0 + kTileSize, height);

	uint hash = 0u;
	hash = HashMix(hash, index);
	hash = HashMix(hash, m_frameNumber);
	hash = HashFinalize(hash);

	UniformSampler sampler(hash, m_frameNumber);

	for (uint y = y0; y < y1; ++y) {
		for (uint x = x0; x < x1; ++x) {
			RenderPixel(x, y, &sampler);
		}
	}
}

void Renderer::RenderPixel(uint x, uint y, UniformSampler *sampler) const {
	Ray ray = m_scene->Camera.CalculateRayFromPixel(x, y, sampler);

	float3 color(0.0f);
	float3 throughput(1.0f);

	// Bounce the ray around the scene
	for (uint bounces = 0; bounces < 10; ++bounces) {
		m_scene->Intersect(ray);

		// The ray missed. Return the background color
		if (ray.GeomID == INVALID_GEOMETRY_ID) {
			color += throughput * m_scene->BackgroundColor;
			break;
		}

		// We hit an object

		// Fetch the bsdf
		BSDF *bsdf = m_scene->GetBSDF(ray.GeomID);
		// The object might be emissive. If so, it will have a corresponding light
		// Otherwise, GetLight will return nullptr
		Light *light = m_scene->GetLight(ray.GeomID);

		// If this is the first bounce or if we just had a specular bounce,
		// we need to add the emmisive light
		if (bounces == 0 && light != nullptr) {
			color += throughput * light->Le();
		}

		SurfaceInteraction interaction;
		interaction.Position = ray.Origin + ray.Direction * ray.TFar;
		interaction.Normal = normalize(ray.GeomNormal);
		interaction.OutputDirection = normalize(-ray.Direction);

		// Calculate the direct lighting
		color += throughput * SampleOneLight(sampler, interaction, bsdf, light);

		// Get the new ray direction
		// Choose the direction based on the bsdf
		bsdf->Sample(interaction, sampler);
		float pdf = bsdf->Pdf(interaction);

		// Accumulate the diffuse/specular weight
		throughput = throughput * bsdf->Eval(interaction) / pdf;

		// Russian Roulette
		if (bounces > 3) {
			float p = std::max(throughput.x, std::max(throughput.y, throughput.z));
			if (sampler->NextFloat() > p) {
				break;
			}

			throughput *= 1 / p;
		}

		// Shoot a new ray

		// Set the origin at the intersection point
		ray.Origin = interaction.Position;

		// Reset the other ray properties
		ray.Direction = interaction.InputDirection;
		ray.TNear = 0.001f;
		ray.TFar = infinity;
		ray.GeomID = INVALID_GEOMETRY_ID;
		ray.PrimID = INVALID_PRIMATIVE_ID;
		ray.InstID = INVALID_INSTANCE_ID;
		ray.Mask = 0xFFFFFFFF;
		ray.Time = 0.0f;
	}

	m_scene->Camera.FrameBuffer.SplatPixel(x, y, color);
}

float3 Renderer::SampleOneLight(UniformSampler *sampler, SurfaceInteraction interaction, BSDF *bsdf, Light *hitLight) const {
	std::size_t numLights = m_scene->NumLights();

	// Return black if there are no lights
	// And don't let a light contribute light to itself
	// Aka, if we hit a light
	// This is the special case where there is only 1 light
	if (numLights == 0 || numLights == 1 && hitLight != nullptr) {
		return float3(0.0f);
	}

	// Don't let a light contribute light to itself
	// Choose another one
	Light *light;
	do {
		light = m_scene->RandomOneLight(sampler);
	} while (light == hitLight);

	return numLights * EstimateDirect(light, sampler, interaction, bsdf);
}

float3 Renderer::EstimateDirect(Light *light, UniformSampler *sampler, SurfaceInteraction &interaction, BSDF *bsdf) const {
	float3 directLighting = float3(0.0f);
	float3 f;
	float lightPdf, scatteringPdf;

	// Sample lighting with multiple importance sampling

	float3 Li = light->SampleLi(sampler, m_scene, interaction, &lightPdf);

	// Make sure the pdf isn't zero and the radiance isn't black
	if (lightPdf != 0.0f && !all(Li)) {
		// Calculate the brdf value
		f = bsdf->Eval(interaction);
		scatteringPdf = bsdf->Pdf(interaction);

		if (scatteringPdf != 0.0f && !all(f)) {
			float weight = PowerHeuristic(1, lightPdf, 1, scatteringPdf);
			directLighting += f * Li * weight / lightPdf;
		}
	}

	// Sample brdf with multiple importance sampling

	bsdf->Sample(interaction, sampler);
	f = bsdf->Eval(interaction);
	scatteringPdf = bsdf->Pdf(interaction);
	if (scatteringPdf != 0.0f && !all(f)) {
		lightPdf = light->PdfLi(m_scene, interaction);
		if (lightPdf == 0.0f) {
			// We didn't hit anything, so ignore the brdf sample
			return directLighting;
		}

		float weight = PowerHeuristic(1, scatteringPdf, 1, lightPdf);
		Li = light->Le();
		directLighting += f * Li * weight / scatteringPdf;
	}

	return directLighting;
}

} // End of namespace Lantern
<|endoftext|>


<commit_before>#include <rviz_helper/kr_marker.hpp>
#include <rviz_helper/viz_manager.hpp>

namespace kr {
namespace viz {

// // // // // // // KR Marker // // // // // // // // // // // // // // //

int Marker::getUniqueId() {
  static int id = 0;
  return id++;
}

Marker::Marker() {
  // set default values
  mark_.color.a = 1.0;
  mark_.color.r = kr::viz::colors::FUCHSIA.r_;
  mark_.color.g = kr::viz::colors::FUCHSIA.g_;
  mark_.color.b = kr::viz::colors::FUCHSIA.b_;

  mark_.pose.orientation.w = 1;
  mark_.ns = "viz";
  mark_.header.frame_id = "world";
  mark_.id = getUniqueId();
  mark_.action = visualization_msgs::Marker::ADD;

  mark_.scale.x = 1.0;
  mark_.scale.y = 1.0;
  mark_.scale.z = 1.0;

  mark_.type = visualization_msgs::Marker::SPHERE;
  mark_.header.stamp = ros::Time::now();
}

// Implementations
Marker &Marker::color(const std_msgs::ColorRGBA &col) {
  mark_.color = col;
  return *this;
}
Marker &Marker::color(const rviz::Color &col) {
  mark_.color.r = col.r_;
  mark_.color.b = col.b_;
  mark_.color.g = col.g_;
  return *this;
}
Marker &Marker::scale(const geometry_msgs::Vector3 &sca) {
  mark_.scale = sca;
  return *this;
}
Marker &Marker::scale(double x, double y, double z) {
  mark_.scale.x = x;
  mark_.scale.y = y;
  mark_.scale.z = z;
  return *this;
}
Marker &Marker::position(const geometry_msgs::Point pose) {
  mark_.pose.position = pose;
  return *this;
}
Marker &Marker::scale(double s) { return scale(s, s, s); }
Marker &Marker::alpha(double a) {
  mark_.color.a = a;
  return *this;
}

MarkerArray MarkerArray::operator+(const MarkerArray &array) {
  for (auto &mark : array.array_) array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+(const Marker &mark) {
  array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+=(const MarkerArray &array) {
  for (auto &mark : array.array_) array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+=(const Marker &marker) {
  array_.push_back(marker);
  return *this;
}
// // // // // // // Viz Manager // // // // // // // // // // // // // // //

VizManager::VizManager() : nh_(""), nh_priv_("~") {}
VizManager &VizManager::instance() {
  static VizManager man;
  return man;
}
MarkerPubHandle::MarkerPubHandle(const std::string &topic)
    : manager_(VizManager::instance()), pub_(manager_.getPublisher(topic)) {}
ros::Publisher &VizManager::getPublisher(const std::string &topic) {
  auto element = publishers_.find(topic);
  if (element == publishers_.end()) {
    publishers_[topic] = nh_priv_.advertise<visualization_msgs::MarkerArray>(
        topic.c_str(), 10, this);
    return publishers_[topic];
  } else {
    return element->second;
  }
}
Marker::operator visualization_msgs::MarkerArray() {
  MarkerArray array;
  array.push_back(*this);
  return array;
}
MarkerArray::operator visualization_msgs::MarkerArray() {
  visualization_msgs::MarkerArray array;
  for (auto &mark : array_) array.markers.push_back(mark);
  return array;
}
void MarkerArray::push_back(const Marker &mark) { array_.push_back(mark); }
MarkerArray::MarkerArray() {}
Marker &Marker::mesh(const std::string &resource) {
  mark_.mesh_resource = resource;
  mark_.type = visualization_msgs::Marker::MESH_RESOURCE;
  return *this;
}
Marker &Marker::type(int32_t tp) {
  mark_.type = tp;
  return *this;
}
Marker &Marker::point_push_back(const geometry_msgs::Point &pt) {
  mark_.points.push_back(pt);
  return *this;
}
Marker &Marker::action(u_int8_t a) {
  mark_.action = a;
  return *this;
}

MarkerArray::MarkerArray(double lx, double ly, double lz, double ux, double uy,
                         double uz, rviz::Color col) {
  kr::viz::Marker mark_lines;
  kr::viz::Marker mark_points;

  geometry_msgs::Point p000;
  geometry_msgs::Point p001;
  geometry_msgs::Point p010;
  geometry_msgs::Point p011;
  geometry_msgs::Point p100;
  geometry_msgs::Point p101;
  geometry_msgs::Point p110;
  geometry_msgs::Point p111;

  p000.x = lx;
  p000.y = ly;
  p000.z = lz;
  p001.x = lx;
  p001.y = ly;
  p001.z = uz;
  p010.x = lx;
  p010.y = uy;
  p010.z = lz;
  p011.x = lx;
  p011.y = uy;
  p011.z = uz;
  p100.x = ux;
  p100.y = ly;
  p100.z = lz;
  p101.x = ux;
  p101.y = ly;
  p101.z = uz;
  p110.x = ux;
  p110.y = uy;
  p110.z = lz;
  p111.x = ux;
  p111.y = uy;
  p111.z = uz;

  // edges
  mark_lines.type(visualization_msgs::Marker::LINE_LIST).color(col);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p100);

  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p101);
  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p011);

  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p011);

  mark_lines.point_push_back(p100);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p100);
  mark_lines.point_push_back(p101);

  mark_lines.point_push_back(p011);
  mark_lines.point_push_back(p111);
  mark_lines.point_push_back(p101);
  mark_lines.point_push_back(p111);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p111);

  // corners
  mark_points.type(visualization_msgs::Marker::SPHERE_LIST).color(col);
  mark_points.point_push_back(p000);
  mark_points.point_push_back(p001);
  mark_points.point_push_back(p010);
  mark_points.point_push_back(p011);
  mark_points.point_push_back(p100);
  mark_points.point_push_back(p101);
  mark_points.point_push_back(p110);
  mark_points.point_push_back(p111);

  mark_lines.scale(0.1);
  mark_points.scale(0.1);

  array_.push_back(mark_lines);
  array_.push_back(mark_points);
}

// batch opperations
void MarkerArray::push_back(const MarkerArray &ma) {
  for(uint i = 0 ; i < ma.size(); i++)
    array_.push_back(ma.at(i));
}

MarkerArray &MarkerArray::color(const rviz::Color &col) {
  for (auto &mark : array_) mark.color(col);
  return *this;
}
MarkerArray &MarkerArray::color(const std_msgs::ColorRGBA &col) {
  for (auto &mark : array_) mark.color(col);
  return *this;
}
MarkerArray &MarkerArray::scale(const geometry_msgs::Vector3 &sca) {
  for (auto &mark : array_) mark.scale(sca);
  return *this;
}
MarkerArray &MarkerArray::scale(double x, double y, double z) {
  for (auto &mark : array_) mark.scale(x, y, z);
  return *this;
}
MarkerArray &MarkerArray::scale(double s) {
  for (auto &mark : array_) mark.scale(s);
  return *this;
}
MarkerArray &MarkerArray::mesh(const std::string &resource) {
  for (auto &mark : array_) mark.mesh(resource);
  return *this;
}
MarkerArray &MarkerArray::type(int32_t tp) {
  for (auto &mark : array_) mark.type(tp);
  return *this;
}
MarkerArray &MarkerArray::point_push_back(const geometry_msgs::Point &pt) {
  for (auto &mark : array_) mark.point_push_back(pt);
  return *this;
}
MarkerArray &MarkerArray::position(const geometry_msgs::Point pose) {
  for (auto &mark : array_) mark.position(pose);
  return *this;
}
MarkerArray &MarkerArray::alpha(double a) {
  for (auto &mark : array_) mark.alpha(a);
  return *this;
}
MarkerArray &MarkerArray::action(u_int8_t a) {
  for (auto &mark : array_) mark.action(a);
  return *this;
}

const Marker & MarkerArray::at(int pos) const {
  return array_.at(pos);
}
Marker & MarkerArray::at(int pos){
  return array_.at(pos);
}

}  // namespace viz
}  // namespace kr
<commit_msg>added missing implementation<commit_after>#include <rviz_helper/kr_marker.hpp>
#include <rviz_helper/viz_manager.hpp>

namespace kr {
namespace viz {

// // // // // // // KR Marker // // // // // // // // // // // // // // //

int Marker::getUniqueId() {
  static int id = 0;
  return id++;
}

Marker::Marker() {
  // set default values
  mark_.color.a = 1.0;
  mark_.color.r = kr::viz::colors::FUCHSIA.r_;
  mark_.color.g = kr::viz::colors::FUCHSIA.g_;
  mark_.color.b = kr::viz::colors::FUCHSIA.b_;

  mark_.pose.orientation.w = 1;
  mark_.ns = "viz";
  mark_.header.frame_id = "world";
  mark_.id = getUniqueId();
  mark_.action = visualization_msgs::Marker::ADD;

  mark_.scale.x = 1.0;
  mark_.scale.y = 1.0;
  mark_.scale.z = 1.0;

  mark_.type = visualization_msgs::Marker::SPHERE;
  mark_.header.stamp = ros::Time::now();
}

// Implementations
Marker &Marker::color(const std_msgs::ColorRGBA &col) {
  mark_.color = col;
  return *this;
}
Marker &Marker::color(const rviz::Color &col) {
  mark_.color.r = col.r_;
  mark_.color.b = col.b_;
  mark_.color.g = col.g_;
  return *this;
}
Marker &Marker::scale(const geometry_msgs::Vector3 &sca) {
  mark_.scale = sca;
  return *this;
}
Marker &Marker::scale(double x, double y, double z) {
  mark_.scale.x = x;
  mark_.scale.y = y;
  mark_.scale.z = z;
  return *this;
}
Marker &Marker::position(const geometry_msgs::Point pose) {
  mark_.pose.position = pose;
  return *this;
}
Marker &Marker::scale(double s) { return scale(s, s, s); }
Marker &Marker::alpha(double a) {
  mark_.color.a = a;
  return *this;
}

MarkerArray MarkerArray::operator+(const MarkerArray &array) {
  for (auto &mark : array.array_) array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+(const Marker &mark) {
  array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+=(const MarkerArray &array) {
  for (auto &mark : array.array_) array_.push_back(mark);
  return *this;
}
MarkerArray MarkerArray::operator+=(const Marker &marker) {
  array_.push_back(marker);
  return *this;
}
// // // // // // // Viz Manager // // // // // // // // // // // // // // //

VizManager::VizManager() : nh_(""), nh_priv_("~") {}
VizManager &VizManager::instance() {
  static VizManager man;
  return man;
}
MarkerPubHandle::MarkerPubHandle(const std::string &topic)
    : manager_(VizManager::instance()), pub_(manager_.getPublisher(topic)) {}
ros::Publisher &VizManager::getPublisher(const std::string &topic) {
  auto element = publishers_.find(topic);
  if (element == publishers_.end()) {
    publishers_[topic] = nh_priv_.advertise<visualization_msgs::MarkerArray>(
        topic.c_str(), 10, this);
    return publishers_[topic];
  } else {
    return element->second;
  }
}
Marker::operator visualization_msgs::MarkerArray() {
  MarkerArray array;
  array.push_back(*this);
  return array;
}
MarkerArray::operator visualization_msgs::MarkerArray() {
  visualization_msgs::MarkerArray array;
  for (auto &mark : array_) array.markers.push_back(mark);
  return array;
}
void MarkerArray::push_back(const Marker &mark) { array_.push_back(mark); }
MarkerArray::MarkerArray() {}
Marker &Marker::mesh(const std::string &resource) {
  mark_.mesh_resource = resource;
  mark_.type = visualization_msgs::Marker::MESH_RESOURCE;
  return *this;
}
Marker &Marker::type(int32_t tp) {
  mark_.type = tp;
  return *this;
}
Marker &Marker::point_push_back(const geometry_msgs::Point &pt) {
  mark_.points.push_back(pt);
  return *this;
}
Marker &Marker::action(u_int8_t a) {
  mark_.action = a;
  return *this;
}

MarkerArray::MarkerArray(double lx, double ly, double lz, double ux, double uy,
                         double uz, rviz::Color col) {
  kr::viz::Marker mark_lines;
  kr::viz::Marker mark_points;

  geometry_msgs::Point p000;
  geometry_msgs::Point p001;
  geometry_msgs::Point p010;
  geometry_msgs::Point p011;
  geometry_msgs::Point p100;
  geometry_msgs::Point p101;
  geometry_msgs::Point p110;
  geometry_msgs::Point p111;

  p000.x = lx;
  p000.y = ly;
  p000.z = lz;
  p001.x = lx;
  p001.y = ly;
  p001.z = uz;
  p010.x = lx;
  p010.y = uy;
  p010.z = lz;
  p011.x = lx;
  p011.y = uy;
  p011.z = uz;
  p100.x = ux;
  p100.y = ly;
  p100.z = lz;
  p101.x = ux;
  p101.y = ly;
  p101.z = uz;
  p110.x = ux;
  p110.y = uy;
  p110.z = lz;
  p111.x = ux;
  p111.y = uy;
  p111.z = uz;

  // edges
  mark_lines.type(visualization_msgs::Marker::LINE_LIST).color(col);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p000);
  mark_lines.point_push_back(p100);

  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p101);
  mark_lines.point_push_back(p001);
  mark_lines.point_push_back(p011);

  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p010);
  mark_lines.point_push_back(p011);

  mark_lines.point_push_back(p100);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p100);
  mark_lines.point_push_back(p101);

  mark_lines.point_push_back(p011);
  mark_lines.point_push_back(p111);
  mark_lines.point_push_back(p101);
  mark_lines.point_push_back(p111);
  mark_lines.point_push_back(p110);
  mark_lines.point_push_back(p111);

  // corners
  mark_points.type(visualization_msgs::Marker::SPHERE_LIST).color(col);
  mark_points.point_push_back(p000);
  mark_points.point_push_back(p001);
  mark_points.point_push_back(p010);
  mark_points.point_push_back(p011);
  mark_points.point_push_back(p100);
  mark_points.point_push_back(p101);
  mark_points.point_push_back(p110);
  mark_points.point_push_back(p111);

  mark_lines.scale(0.1);
  mark_points.scale(0.1);

  array_.push_back(mark_lines);
  array_.push_back(mark_points);
}
MarkerArray::MarkerArray(const visualization_msgs::MarkerArray &array) {
  for(auto &mark:array.markers)
    array_.push_back(mark);
}

// batch opperations
void MarkerArray::push_back(const MarkerArray &ma) {
  for(uint i = 0 ; i < ma.size(); i++)
    array_.push_back(ma.at(i));
}

MarkerArray &MarkerArray::color(const rviz::Color &col) {
  for (auto &mark : array_) mark.color(col);
  return *this;
}
MarkerArray &MarkerArray::color(const std_msgs::ColorRGBA &col) {
  for (auto &mark : array_) mark.color(col);
  return *this;
}
MarkerArray &MarkerArray::scale(const geometry_msgs::Vector3 &sca) {
  for (auto &mark : array_) mark.scale(sca);
  return *this;
}
MarkerArray &MarkerArray::scale(double x, double y, double z) {
  for (auto &mark : array_) mark.scale(x, y, z);
  return *this;
}
MarkerArray &MarkerArray::scale(double s) {
  for (auto &mark : array_) mark.scale(s);
  return *this;
}
MarkerArray &MarkerArray::mesh(const std::string &resource) {
  for (auto &mark : array_) mark.mesh(resource);
  return *this;
}
MarkerArray &MarkerArray::type(int32_t tp) {
  for (auto &mark : array_) mark.type(tp);
  return *this;
}
MarkerArray &MarkerArray::point_push_back(const geometry_msgs::Point &pt) {
  for (auto &mark : array_) mark.point_push_back(pt);
  return *this;
}
MarkerArray &MarkerArray::position(const geometry_msgs::Point pose) {
  for (auto &mark : array_) mark.position(pose);
  return *this;
}
MarkerArray &MarkerArray::alpha(double a) {
  for (auto &mark : array_) mark.alpha(a);
  return *this;
}
MarkerArray &MarkerArray::action(u_int8_t a) {
  for (auto &mark : array_) mark.action(a);
  return *this;
}

const Marker & MarkerArray::at(int pos) const {
  return array_.at(pos);
}
Marker & MarkerArray::at(int pos){
  return array_.at(pos);
}

}  // namespace viz
}  // namespace kr
<|endoftext|>