<commit_before>/*****************************************************************************
 * PokerTH - The open source texas holdem engine                             *
 * Copyright (C) 2006-2011 Felix Hammer, Florian Thauer, Lothar May          *
 *                                                                           *
 * This program is free software: you can redistribute it and/or modify      *
 * it under the terms of the GNU Affero General Public License as            *
 * published by the Free Software Foundation, either version 3 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 Affero General Public License for more details.                       *
 *                                                                           *
 * You should have received a copy of the GNU Affero General Public License  *
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.     *
 *****************************************************************************/
#include "chattools.h"
#include "session.h"
#include "configfile.h"
#include "gametablestylereader.h"
#include "gamelobbydialogimpl.h"
#include <iostream>

using namespace std;

ChatTools::ChatTools(QLineEdit* l, ConfigFile *c, ChatType ct, QTextBrowser *b, QStandardItemModel *m, gameLobbyDialogImpl *lo) : nickAutoCompletitionCounter(0), myLineEdit(l), myNickListModel(m), myNickStringList(NULL), myTextBrowser(b), myChatType(ct), myConfig(c), myNick(""), myLobby(lo)
{
	myNick = QString::fromUtf8(myConfig->readConfigString("MyName").c_str());
    ignoreList = myConfig->readConfigStringList("PlayerIgnoreList");
}

ChatTools::~ChatTools()
{
}

void ChatTools::sendMessage()
{

	if(myLineEdit->text().size() && mySession) {
		fillChatLinesHistory(myLineEdit->text());
		QString chatText(myLineEdit->text());
		if(myChatType == INGAME_CHAT) {
			mySession->sendGameChatMessage(chatText.toUtf8().constData());
		} else {
			// Parse user name for private messages.
			if(chatText.indexOf(QString("/msg ")) == 0) {
				chatText.remove(0, 5);
				unsigned playerId = parsePrivateMessageTarget(chatText);
				if (playerId) {
					mySession->sendPrivateChatMessage(playerId, chatText.toUtf8().constData());
					QString tmp = tr("private message sent to player: %1");
					myTextBrowser->append(""+tmp.arg(QString::fromUtf8(mySession->getClientPlayerInfo(playerId).playerName.c_str()))+"");
				}
			} else {
				mySession->sendLobbyChatMessage(chatText.toUtf8().constData());
			}
		}
		myLineEdit->setText("");
	}
}

void ChatTools::receiveMessage(QString playerName, QString message, bool pm)
{

	if(myTextBrowser) {

		message = message.replace("<","<");
		message = message.replace(">",">");
		//doing the links
		message = message.replace(QRegExp("((?:https?)://\\S+)"), "\\1");

		//refresh myNick if it was changed during runtime
		myNick = QString::fromUtf8(myConfig->readConfigString("MyName").c_str());

		QString tempMsg;

		if(myChatType == INET_LOBBY_CHAT && playerName == "(chat bot)" && message.startsWith(myNick)) {

			tempMsg = QString(""+message+"");
			//play beep sound only in INET-lobby-chat
			if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification")) {
				myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
			}
		} else if(message.contains(myNick, Qt::CaseInsensitive)) {

			switch (myChatType) {
			case INET_LOBBY_CHAT: {
				tempMsg = QString("palette().link().color().name()+";\">"+message+"");
				//play beep sound only in INET-lobby-chat
				//						TODO dont play when message is from yourself
				if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification")) {
					myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
				}
			}
			break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatLogTextColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatTextNickNotifyColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}
		} else if(playerName == myNick) {
			switch (myChatType) {
			case INET_LOBBY_CHAT:
				tempMsg = QString("palette().link().color().name()+";\">"+message+"");
				break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatTextNickNotifyColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatLogTextColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}
		} else {
			switch (myChatType) {
			case INET_LOBBY_CHAT:
				tempMsg = QString("palette().text().color().name()+";\">"+message+"");
				break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatTextNickNotifyColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatLogTextColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}

		}

		bool nickFoundOnIgnoreList = false;
		list<std::string>::iterator it1;
		for(it1=ignoreList.begin(); it1 != ignoreList.end(); ++it1) {

			if(playerName == QString::fromUtf8(it1->c_str())) {
				nickFoundOnIgnoreList = true;
			}
		}

		if(!nickFoundOnIgnoreList) {

            tempMsg = checkForEmotes(tempMsg);

			if(message.indexOf(QString("/me "))==0) {
				myTextBrowser->append(tempMsg.replace("/me ","*"+playerName+" ")+"");
			} else if(pm == true) {
				myTextBrowser->append(""+playerName+"(pm): " + tempMsg+"");
			} else {
                myTextBrowser->append(playerName + ": " + tempMsg);
			}
		}
	}
}

void ChatTools::privateMessage(QString playerName, QString message)
{
	bool pm=true;
	receiveMessage(playerName, message, pm);
}

void ChatTools::clearChat()
{

	if(myTextBrowser)
		myTextBrowser->clear();
}

void ChatTools::checkInputLength(QString string)
{

	if(string.toUtf8().length() > 120) myLineEdit->setMaxLength(string.length());
}

void ChatTools::fillChatLinesHistory(QString fillString)
{

	chatLinesHistory << fillString;
	if(chatLinesHistory.size() > 50) chatLinesHistory.removeFirst();

}

void ChatTools::showChatHistoryIndex(int index)
{

	if(index <= chatLinesHistory.size()) {

		// 		cout << chatLinesHistory.size() << " : " <<  index << endl;
		if(index > 0)
			myLineEdit->setText(chatLinesHistory.at(chatLinesHistory.size()-(index)));
		else
			myLineEdit->setText("");
	}
}

void ChatTools::nickAutoCompletition()
{

	QString myChatString = myLineEdit->text();
	QStringList myChatStringList = myChatString.split(" ");

	QStringList matchStringList;

	if(nickAutoCompletitionCounter == 0) {

		if(myNickListModel) {
			int it = 0;
			while (myNickListModel->item(it)) {
				QString text = myNickListModel->item(it, 0)->data(Qt::DisplayRole).toString();
				if(text.startsWith(myChatStringList.last(), Qt::CaseInsensitive) && myChatStringList.last() != "") {
					matchStringList << text;
				}
				++it;
			}
		}

		if(!myNickStringList.isEmpty()) {

			QStringListIterator it(myNickStringList);
			while (it.hasNext()) {
				QString next = it.next();
				if (next.startsWith(myChatStringList.last(), Qt::CaseInsensitive) && myChatStringList.last() != "")
					matchStringList << next;
			}
		}
	}

	if(!matchStringList.isEmpty() || nickAutoCompletitionCounter > 0) {

		myChatStringList.removeLast();

		// 		cout << nickAutoCompletitionCounter << endl;

		if(nickAutoCompletitionCounter == 0) {
			//first one
			lastChatString = myChatStringList.join(" ");
			lastMatchStringList = matchStringList;
		}

		if(nickAutoCompletitionCounter == lastMatchStringList.size()) nickAutoCompletitionCounter = 0;

		// 		cout << nickAutoCompletitionCounter << "\n";

		if(lastChatString == "") {
			myLineEdit->setText(lastMatchStringList.at(nickAutoCompletitionCounter)+": ");
		} else {
			//check if lastChatString is pm-code
			if((lastChatString == "/msg" || lastChatString == "/msg ") && lastMatchStringList.at(nickAutoCompletitionCounter).contains(" ")) {
				myLineEdit->setText(lastChatString+" \""+lastMatchStringList.at(nickAutoCompletitionCounter)+"\" ");
			} else {
				myLineEdit->setText(lastChatString+" "+lastMatchStringList.at(nickAutoCompletitionCounter)+" ");
			}
		}

		nickAutoCompletitionCounter++;
	}
}

void ChatTools::setChatTextEdited()
{

	nickAutoCompletitionCounter = 0;
}

void ChatTools::refreshIgnoreList()
{
	ignoreList = myConfig->readConfigStringList("PlayerIgnoreList");
}

unsigned ChatTools::parsePrivateMessageTarget(QString &chatText)
{
	QString playerName;
	int endPosName = -1;
	// Target player is either in the format "this is a user" or singlename.
	if (chatText.startsWith('"')) {
		chatText.remove(0, 1);
		endPosName = chatText.indexOf('"');
	} else {
		endPosName = chatText.indexOf(' ');
	}
	if (endPosName > 0) {
		playerName = chatText.left(endPosName);
		chatText.remove(0, endPosName + 1);
	}
	chatText = chatText.trimmed();
	unsigned playerId = 0;
	if (!playerName.isEmpty() && !chatText.isEmpty()) {
		if(myNickListModel) {
			int it = 0;
			while (myNickListModel->item(it)) {
				QString text = myNickListModel->item(it, 0)->data(Qt::DisplayRole).toString();
				if(text == playerName) {
					playerId = myNickListModel->item(it, 0)->data(Qt::UserRole).toUInt();
					break;
				}
				++it;
			}
		}
	}
	return playerId;
}

QString ChatTools::checkForEmotes(QString msg) {

    qDebug() << msg;

    msg.replace("0:-)", "");
    msg.replace("X-(", "");
    msg.replace("B-)", "");
    msg.replace("8-)", "");
    msg.replace(":'(", "");
    msg.replace(">:-)", "");
    msg.replace(":-[", "");
    msg.replace(":-*", "");
    msg.replace(":-))", "");
    msg.replace(":))", "");
    msg.replace(":-|", "");
    msg.replace(":-P", "");
    msg.replace(":-p", "");
    msg.replace(":-(", "");
    msg.replace(":(", "");
    msg.replace(":-&", "");
    msg.replace(":-D", "");
    msg.replace(":D", "");
    msg.replace(":-!", "");
    msg.replace(":-0", "");
    msg.replace(":-O", "");
    msg.replace(":-o", "");
    msg.replace(":-/", "");
    msg.replace(":/", "");
    msg.replace(";-)", "");
    msg.replace(";)", "");
    msg.replace(":-S", "");
    msg.replace(":-s", "");
    msg.replace(":-)", "");
    msg.replace(":)", "");

    return msg;
}

<commit_msg>remove debug msg<commit_after>/*****************************************************************************
 * PokerTH - The open source texas holdem engine                             *
 * Copyright (C) 2006-2011 Felix Hammer, Florian Thauer, Lothar May          *
 *                                                                           *
 * This program is free software: you can redistribute it and/or modify      *
 * it under the terms of the GNU Affero General Public License as            *
 * published by the Free Software Foundation, either version 3 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 Affero General Public License for more details.                       *
 *                                                                           *
 * You should have received a copy of the GNU Affero General Public License  *
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.     *
 *****************************************************************************/
#include "chattools.h"
#include "session.h"
#include "configfile.h"
#include "gametablestylereader.h"
#include "gamelobbydialogimpl.h"
#include <iostream>

using namespace std;

ChatTools::ChatTools(QLineEdit* l, ConfigFile *c, ChatType ct, QTextBrowser *b, QStandardItemModel *m, gameLobbyDialogImpl *lo) : nickAutoCompletitionCounter(0), myLineEdit(l), myNickListModel(m), myNickStringList(NULL), myTextBrowser(b), myChatType(ct), myConfig(c), myNick(""), myLobby(lo)
{
	myNick = QString::fromUtf8(myConfig->readConfigString("MyName").c_str());
    ignoreList = myConfig->readConfigStringList("PlayerIgnoreList");
}

ChatTools::~ChatTools()
{
}

void ChatTools::sendMessage()
{

	if(myLineEdit->text().size() && mySession) {
		fillChatLinesHistory(myLineEdit->text());
		QString chatText(myLineEdit->text());
		if(myChatType == INGAME_CHAT) {
			mySession->sendGameChatMessage(chatText.toUtf8().constData());
		} else {
			// Parse user name for private messages.
			if(chatText.indexOf(QString("/msg ")) == 0) {
				chatText.remove(0, 5);
				unsigned playerId = parsePrivateMessageTarget(chatText);
				if (playerId) {
					mySession->sendPrivateChatMessage(playerId, chatText.toUtf8().constData());
					QString tmp = tr("private message sent to player: %1");
					myTextBrowser->append(""+tmp.arg(QString::fromUtf8(mySession->getClientPlayerInfo(playerId).playerName.c_str()))+"");
				}
			} else {
				mySession->sendLobbyChatMessage(chatText.toUtf8().constData());
			}
		}
		myLineEdit->setText("");
	}
}

void ChatTools::receiveMessage(QString playerName, QString message, bool pm)
{

	if(myTextBrowser) {

		message = message.replace("<","<");
		message = message.replace(">",">");
		//doing the links
		message = message.replace(QRegExp("((?:https?)://\\S+)"), "\\1");

		//refresh myNick if it was changed during runtime
		myNick = QString::fromUtf8(myConfig->readConfigString("MyName").c_str());

		QString tempMsg;

		if(myChatType == INET_LOBBY_CHAT && playerName == "(chat bot)" && message.startsWith(myNick)) {

			tempMsg = QString(""+message+"");
			//play beep sound only in INET-lobby-chat
			if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification")) {
				myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
			}
		} else if(message.contains(myNick, Qt::CaseInsensitive)) {

			switch (myChatType) {
			case INET_LOBBY_CHAT: {
				tempMsg = QString("palette().link().color().name()+";\">"+message+"");
				//play beep sound only in INET-lobby-chat
				//						TODO dont play when message is from yourself
				if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification")) {
					myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
				}
			}
			break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatLogTextColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatTextNickNotifyColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}
		} else if(playerName == myNick) {
			switch (myChatType) {
			case INET_LOBBY_CHAT:
				tempMsg = QString("palette().link().color().name()+";\">"+message+"");
				break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatTextNickNotifyColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatLogTextColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}
		} else {
			switch (myChatType) {
			case INET_LOBBY_CHAT:
				tempMsg = QString("palette().text().color().name()+";\">"+message+"");
				break;
			case LAN_LOBBY_CHAT:
				tempMsg = QString(""+message+"");
				break;
			case INGAME_CHAT: {
				message = message.replace("<a href","getChatTextNickNotifyColor()+"; text-decoration: underline;\" href");
				tempMsg = QString("getChatLogTextColor()+";\">"+message+"");
			}
			break;
			default:
				tempMsg = message;
			}

		}

		bool nickFoundOnIgnoreList = false;
		list<std::string>::iterator it1;
		for(it1=ignoreList.begin(); it1 != ignoreList.end(); ++it1) {

			if(playerName == QString::fromUtf8(it1->c_str())) {
				nickFoundOnIgnoreList = true;
			}
		}

		if(!nickFoundOnIgnoreList) {

            tempMsg = checkForEmotes(tempMsg);

			if(message.indexOf(QString("/me "))==0) {
				myTextBrowser->append(tempMsg.replace("/me ","*"+playerName+" ")+"");
			} else if(pm == true) {
				myTextBrowser->append(""+playerName+"(pm): " + tempMsg+"");
			} else {
                myTextBrowser->append(playerName + ": " + tempMsg);
			}
		}
	}
}

void ChatTools::privateMessage(QString playerName, QString message)
{
	bool pm=true;
	receiveMessage(playerName, message, pm);
}

void ChatTools::clearChat()
{

	if(myTextBrowser)
		myTextBrowser->clear();
}

void ChatTools::checkInputLength(QString string)
{

	if(string.toUtf8().length() > 120) myLineEdit->setMaxLength(string.length());
}

void ChatTools::fillChatLinesHistory(QString fillString)
{

	chatLinesHistory << fillString;
	if(chatLinesHistory.size() > 50) chatLinesHistory.removeFirst();

}

void ChatTools::showChatHistoryIndex(int index)
{

	if(index <= chatLinesHistory.size()) {

		// 		cout << chatLinesHistory.size() << " : " <<  index << endl;
		if(index > 0)
			myLineEdit->setText(chatLinesHistory.at(chatLinesHistory.size()-(index)));
		else
			myLineEdit->setText("");
	}
}

void ChatTools::nickAutoCompletition()
{

	QString myChatString = myLineEdit->text();
	QStringList myChatStringList = myChatString.split(" ");

	QStringList matchStringList;

	if(nickAutoCompletitionCounter == 0) {

		if(myNickListModel) {
			int it = 0;
			while (myNickListModel->item(it)) {
				QString text = myNickListModel->item(it, 0)->data(Qt::DisplayRole).toString();
				if(text.startsWith(myChatStringList.last(), Qt::CaseInsensitive) && myChatStringList.last() != "") {
					matchStringList << text;
				}
				++it;
			}
		}

		if(!myNickStringList.isEmpty()) {

			QStringListIterator it(myNickStringList);
			while (it.hasNext()) {
				QString next = it.next();
				if (next.startsWith(myChatStringList.last(), Qt::CaseInsensitive) && myChatStringList.last() != "")
					matchStringList << next;
			}
		}
	}

	if(!matchStringList.isEmpty() || nickAutoCompletitionCounter > 0) {

		myChatStringList.removeLast();

		// 		cout << nickAutoCompletitionCounter << endl;

		if(nickAutoCompletitionCounter == 0) {
			//first one
			lastChatString = myChatStringList.join(" ");
			lastMatchStringList = matchStringList;
		}

		if(nickAutoCompletitionCounter == lastMatchStringList.size()) nickAutoCompletitionCounter = 0;

		// 		cout << nickAutoCompletitionCounter << "\n";

		if(lastChatString == "") {
			myLineEdit->setText(lastMatchStringList.at(nickAutoCompletitionCounter)+": ");
		} else {
			//check if lastChatString is pm-code
			if((lastChatString == "/msg" || lastChatString == "/msg ") && lastMatchStringList.at(nickAutoCompletitionCounter).contains(" ")) {
				myLineEdit->setText(lastChatString+" \""+lastMatchStringList.at(nickAutoCompletitionCounter)+"\" ");
			} else {
				myLineEdit->setText(lastChatString+" "+lastMatchStringList.at(nickAutoCompletitionCounter)+" ");
			}
		}

		nickAutoCompletitionCounter++;
	}
}

void ChatTools::setChatTextEdited()
{

	nickAutoCompletitionCounter = 0;
}

void ChatTools::refreshIgnoreList()
{
	ignoreList = myConfig->readConfigStringList("PlayerIgnoreList");
}

unsigned ChatTools::parsePrivateMessageTarget(QString &chatText)
{
	QString playerName;
	int endPosName = -1;
	// Target player is either in the format "this is a user" or singlename.
	if (chatText.startsWith('"')) {
		chatText.remove(0, 1);
		endPosName = chatText.indexOf('"');
	} else {
		endPosName = chatText.indexOf(' ');
	}
	if (endPosName > 0) {
		playerName = chatText.left(endPosName);
		chatText.remove(0, endPosName + 1);
	}
	chatText = chatText.trimmed();
	unsigned playerId = 0;
	if (!playerName.isEmpty() && !chatText.isEmpty()) {
		if(myNickListModel) {
			int it = 0;
			while (myNickListModel->item(it)) {
				QString text = myNickListModel->item(it, 0)->data(Qt::DisplayRole).toString();
				if(text == playerName) {
					playerId = myNickListModel->item(it, 0)->data(Qt::UserRole).toUInt();
					break;
				}
				++it;
			}
		}
	}
	return playerId;
}

QString ChatTools::checkForEmotes(QString msg) {

    msg.replace("0:-)", "");
    msg.replace("X-(", "");
    msg.replace("B-)", "");
    msg.replace("8-)", "");
    msg.replace(":'(", "");
    msg.replace(">:-)", "");
    msg.replace(":-[", "");
    msg.replace(":-*", "");
    msg.replace(":-))", "");
    msg.replace(":))", "");
    msg.replace(":-|", "");
    msg.replace(":-P", "");
    msg.replace(":-p", "");
    msg.replace(":-(", "");
    msg.replace(":(", "");
    msg.replace(":-&", "");
    msg.replace(":-D", "");
    msg.replace(":D", "");
    msg.replace(":-!", "");
    msg.replace(":-0", "");
    msg.replace(":-O", "");
    msg.replace(":-o", "");
    msg.replace(":-/", "");
    msg.replace(":/", "");
    msg.replace(";-)", "");
    msg.replace(";)", "");
    msg.replace(":-S", "");
    msg.replace(":-s", "");
    msg.replace(":-)", "");
    msg.replace(":)", "");

    return msg;
}

<|endoftext|>


<commit_before>#include <iostream>
#include <stdio.h>   								/* Standard input/output definitions */
#include <stdlib.h>  								/* exit */
#include <string.h>  								/* String function definitions */
#include <unistd.h>  								/* UNIX standard function definitions */
#include <fcntl.h>  	 							/* File control definitions */
#include <errno.h>   								/* Error number definitions */
#include <termios.h> 								/* POSIX terminal control definitions */
#include <ctype.h>   								/* isxxx() */

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

struct termios orig;
int filedesc;
int fd;
unsigned char serialBuffer[16];						// Serial buffer to store data for I/O

int openSerialPort(const char * device, int bps)
{
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //usleep(400000);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);
}

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
    ROS_INFO("I heard: [%f]", vel_cmd.linear.y);
    std::cout << "Twist Received " << std::endl;

    //hier code um msg in seriellen Befehl umzuwandeln

        //code

    //
}

int main( int argc, char* argv[] )
{
ros::init(argc, argv, "base_controller" );

ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);

filedesc = openSerialPort("/dev/ttyAMA0", B38400);
if (filedesc == -1) exit(1);
usleep(40000);									// Sleep for UART to power up and set options

ROS_INFO_STREAM("serial Port opened \n");

while( n.ok() )
{
    //read_MD49_Data();
    //usleep(100000);
    ROS_INFO_STREAM("cycle? \n");
    ros::spin();
    //hier code/funktion um md49 daten zu lesen und und auszugeben in konsole und /odom später

}

return 1;
}
<commit_msg>Update code<commit_after>#include <iostream>
#include <stdio.h>   								/* Standard input/output definitions */
#include <stdlib.h>  								/* exit */
#include <string.h>  								/* String function definitions */
#include <unistd.h>  								/* UNIX standard function definitions */
#include <fcntl.h>  	 							/* File control definitions */
#include <errno.h>   								/* Error number definitions */
#include <termios.h> 								/* POSIX terminal control definitions */
#include <ctype.h>   								/* isxxx() */

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

struct termios orig;
int filedesc;
int fd;
unsigned char serialBuffer[16];						// Serial buffer to store data for I/O

int openSerialPort(const char * device, int bps)
{
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //usleep(400000);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    printf("Byte2: %i ",serialBuffer[1]);
    printf("Byte3: % i ",serialBuffer[2]);
    printf("Byte4: %i \n",serialBuffer[3]);
    printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    printf("Byte2: %i ",serialBuffer[5]);
    printf("Byte3: %i ",serialBuffer[6]);
    printf("Byte4: %i \n",serialBuffer[7]);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);
}

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
    ROS_INFO("I heard: [%f]", vel_cmd.linear.y);
    std::cout << "Twist Received " << std::endl;

    //hier code um msg in seriellen Befehl umzuwandeln

        //code

    //
}

int main( int argc, char* argv[] )
{
ros::init(argc, argv, "base_controller" );

ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);

filedesc = openSerialPort("/dev/ttyAMA0", B38400);
if (filedesc == -1) exit(1);
usleep(40000);									// Sleep for UART to power up and set options

ROS_INFO_STREAM("serial Port opened \n");

while( n.ok() )
{
    while(1){
        read_MD49_Data();
        usleep(100000);
        ros::spin();
    }

}

return 1;
}
<|endoftext|>


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

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/**
 * @author Christian <c@ethdev.com>
 * @date 2014
 * Solidity AST to EVM bytecode compiler for expressions.
 */

#include <cassert>
#include <utility>
#include <numeric>
#include <libsolidity/AST.h>
#include <libsolidity/ExpressionCompiler.h>
#include <libsolidity/CompilerContext.h>

using namespace std;

namespace dev {
namespace solidity {

void ExpressionCompiler::compileExpression(CompilerContext& _context, Expression& _expression)
{
	ExpressionCompiler compiler(_context);
	_expression.accept(compiler);
}

bool ExpressionCompiler::visit(Assignment& _assignment)
{
	m_currentLValue = nullptr;

	Expression& rightHandSide = _assignment.getRightHandSide();
	rightHandSide.accept(*this);
	Type const& resultType = *_assignment.getType();
	cleanHigherOrderBitsIfNeeded(*rightHandSide.getType(), resultType);
	_assignment.getLeftHandSide().accept(*this);

	Token::Value op = _assignment.getAssignmentOperator();
	if (op != Token::ASSIGN)
	{
		// compound assignment
		m_context << eth::Instruction::SWAP1;
		appendOrdinaryBinaryOperatorCode(Token::AssignmentToBinaryOp(op), resultType);
	}
	else
		m_context << eth::Instruction::POP; //@todo do not retrieve the value in the first place

	storeInLValue(_assignment);
	return false;
}

void ExpressionCompiler::endVisit(UnaryOperation& _unaryOperation)
{
	//@todo type checking and creating code for an operator should be in the same place:
	// the operator should know how to convert itself and to which types it applies, so
	// put this code together with "Type::acceptsBinary/UnaryOperator" into a class that
	// represents the operator
	switch (_unaryOperation.getOperator())
	{
	case Token::NOT: // !
		m_context << eth::Instruction::ISZERO;
		break;
	case Token::BIT_NOT: // ~
		m_context << eth::Instruction::NOT;
		break;
	case Token::DELETE: // delete
	{
		// a -> a xor a (= 0).
		// @todo semantics change for complex types
		m_context << eth::Instruction::DUP1 << eth::Instruction::XOR;
		storeInLValue(_unaryOperation);
		break;
	}
	case Token::INC: // ++ (pre- or postfix)
	case Token::DEC: // -- (pre- or postfix)
		if (!_unaryOperation.isPrefixOperation())
			m_context << eth::Instruction::DUP1;
		m_context << u256(1);
		if (_unaryOperation.getOperator() == Token::INC)
			m_context << eth::Instruction::ADD;
		else
			m_context << eth::Instruction::SWAP1 << eth::Instruction::SUB; // @todo avoid the swap
		if (_unaryOperation.isPrefixOperation())
			storeInLValue(_unaryOperation);
		else
			moveToLValue(_unaryOperation);
		break;
	case Token::ADD: // +
		// unary add, so basically no-op
		break;
	case Token::SUB: // -
		m_context << u256(0) << eth::Instruction::SUB;
		break;
	default:
		assert(false); // invalid operation
	}
}

bool ExpressionCompiler::visit(BinaryOperation& _binaryOperation)
{
	Expression& leftExpression = _binaryOperation.getLeftExpression();
	Expression& rightExpression = _binaryOperation.getRightExpression();
	Type const& resultType = *_binaryOperation.getType();
	Token::Value const op = _binaryOperation.getOperator();

	if (op == Token::AND || op == Token::OR)
	{
		// special case: short-circuiting
		appendAndOrOperatorCode(_binaryOperation);
	}
	else if (Token::isCompareOp(op))
	{
		leftExpression.accept(*this);
		rightExpression.accept(*this);

		// the types to compare have to be the same, but the resulting type is always bool
		assert(*leftExpression.getType() == *rightExpression.getType());
		appendCompareOperatorCode(op, *leftExpression.getType());
	}
	else
	{
		leftExpression.accept(*this);
		cleanHigherOrderBitsIfNeeded(*leftExpression.getType(), resultType);
		rightExpression.accept(*this);
		cleanHigherOrderBitsIfNeeded(*rightExpression.getType(), resultType);
		appendOrdinaryBinaryOperatorCode(op, resultType);
	}

	// do not visit the child nodes, we already did that explicitly
	return false;
}

bool ExpressionCompiler::visit(FunctionCall& _functionCall)
{
	if (_functionCall.isTypeConversion())
	{
		//@todo we only have integers and bools for now which cannot be explicitly converted
		assert(_functionCall.getArguments().size() == 1);
		Expression& firstArgument = *_functionCall.getArguments().front();
		firstArgument.accept(*this);
		cleanHigherOrderBitsIfNeeded(*firstArgument.getType(), *_functionCall.getType());
	}
	else
	{
		// Calling convention: Caller pushes return address and arguments
		// Callee removes them and pushes return values
		m_currentLValue = nullptr;
		_functionCall.getExpression().accept(*this);
		FunctionDefinition const* function = dynamic_cast<FunctionDefinition*>(m_currentLValue);
		assert(function);

		eth::AssemblyItem returnLabel = m_context.pushNewTag();
		std::vector<ASTPointer<Expression>> const& arguments = _functionCall.getArguments();
		assert(arguments.size() == function->getParameters().size());
		for (unsigned i = 0; i < arguments.size(); ++i)
		{
			arguments[i]->accept(*this);
			cleanHigherOrderBitsIfNeeded(*arguments[i]->getType(),
										 *function->getParameters()[i]->getType());
		}

		m_context.appendJumpTo(m_context.getFunctionEntryLabel(*function));
		m_context << returnLabel;

		// callee adds return parameters, but removes arguments and return label
		m_context.adjustStackOffset(function->getReturnParameters().size() - arguments.size() - 1);

		// @todo for now, the return value of a function is its first return value, so remove
		// all others
		for (unsigned i = 1; i < function->getReturnParameters().size(); ++i)
			m_context << eth::Instruction::POP;
	}
	return false;
}

void ExpressionCompiler::endVisit(MemberAccess&)
{

}

void ExpressionCompiler::endVisit(IndexAccess&)
{

}

void ExpressionCompiler::endVisit(Identifier& _identifier)
{
	m_currentLValue = _identifier.getReferencedDeclaration();
	switch (_identifier.getType()->getCategory())
	{
	case Type::Category::BOOL:
	case Type::Category::INTEGER:
	case Type::Category::REAL:
	{
		//@todo we also have to check where to retrieve them from once we add storage variables
		unsigned stackPos = stackPositionOfLValue();
		if (stackPos >= 15) //@todo correct this by fetching earlier or moving to memory
			BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_identifier.getLocation())
												  << errinfo_comment("Stack too deep."));
		m_context << eth::dupInstruction(stackPos + 1);
		break;
	}
	default:
		break;
	}
}

void ExpressionCompiler::endVisit(Literal& _literal)
{
	switch (_literal.getType()->getCategory())
	{
	case Type::Category::INTEGER:
	case Type::Category::BOOL:
		m_context << _literal.getType()->literalValue(_literal);
		break;
	default:
		assert(false); // @todo
	}
}

void ExpressionCompiler::cleanHigherOrderBitsIfNeeded(Type const& _typeOnStack, Type const& _targetType)
{
	// If the type of one of the operands is extended, we need to remove all
	// higher-order bits that we might have ignored in previous operations.
	// @todo: store in the AST whether the operand might have "dirty" higher
	// order bits

	if (_typeOnStack == _targetType)
		return;
	if (_typeOnStack.getCategory() == Type::Category::INTEGER &&
			_targetType.getCategory() == Type::Category::INTEGER)
	{
		//@todo
	}
	else
	{
		// If we get here, there is either an implementation missing to clean higher oder bits
		// for non-integer types that are explicitly convertible or we got here in error.
		assert(!_typeOnStack.isExplicitlyConvertibleTo(_targetType));
		assert(false); // these types should not be convertible.
	}
}

void ExpressionCompiler::appendAndOrOperatorCode(BinaryOperation& _binaryOperation)
{
	Token::Value const op = _binaryOperation.getOperator();
	assert(op == Token::OR || op == Token::AND);

	_binaryOperation.getLeftExpression().accept(*this);
	m_context << eth::Instruction::DUP1;
	if (op == Token::AND)
		m_context << eth::Instruction::ISZERO;
	eth::AssemblyItem endLabel = m_context.appendConditionalJump();
	_binaryOperation.getRightExpression().accept(*this);
	m_context << endLabel;
}

void ExpressionCompiler::appendCompareOperatorCode(Token::Value _operator, Type const& _type)
{
	if (_operator == Token::EQ || _operator == Token::NE)
	{
		m_context << eth::Instruction::EQ;
		if (_operator == Token::NE)
			m_context << eth::Instruction::ISZERO;
	}
	else
	{
		IntegerType const* type = dynamic_cast<IntegerType const*>(&_type);
		assert(type);
		bool const isSigned = type->isSigned();

		// note that EVM opcodes compare like "stack[0] < stack[1]",
		// but our left value is at stack[1], so everyhing is reversed.
		switch (_operator)
		{
		case Token::GTE:
			m_context << (isSigned ? eth::Instruction::SGT : eth::Instruction::GT)
					  << eth::Instruction::ISZERO;
			break;
		case Token::LTE:
			m_context << (isSigned ? eth::Instruction::SLT : eth::Instruction::LT)
					  << eth::Instruction::ISZERO;
			break;
		case Token::GT:
			m_context << (isSigned ? eth::Instruction::SLT : eth::Instruction::LT);
			break;
		case Token::LT:
			m_context << (isSigned ? eth::Instruction::SGT : eth::Instruction::GT);
			break;
		default:
			assert(false);
		}
	}
}

void ExpressionCompiler::appendOrdinaryBinaryOperatorCode(Token::Value _operator, Type const& _type)
{
	if (Token::isArithmeticOp(_operator))
		appendArithmeticOperatorCode(_operator, _type);
	else if (Token::isBitOp(_operator))
		appendBitOperatorCode(_operator);
	else if (Token::isShiftOp(_operator))
		appendShiftOperatorCode(_operator);
	else
		assert(false); // unknown binary operator
}

void ExpressionCompiler::appendArithmeticOperatorCode(Token::Value _operator, Type const& _type)
{
	IntegerType const* type = dynamic_cast<IntegerType const*>(&_type);
	assert(type);
	bool const isSigned = type->isSigned();

	switch (_operator)
	{
	case Token::ADD:
		m_context << eth::Instruction::ADD;
		break;
	case Token::SUB:
		m_context << eth::Instruction::SWAP1 << eth::Instruction::SUB;
		break;
	case Token::MUL:
		m_context << eth::Instruction::MUL;
		break;
	case Token::DIV:
		m_context << (isSigned ? eth::Instruction::SDIV : eth::Instruction::DIV);
		break;
	case Token::MOD:
		m_context << (isSigned ? eth::Instruction::SMOD : eth::Instruction::MOD);
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::appendBitOperatorCode(Token::Value _operator)
{
	switch (_operator)
	{
	case Token::BIT_OR:
		m_context << eth::Instruction::OR;
		break;
	case Token::BIT_AND:
		m_context << eth::Instruction::AND;
		break;
	case Token::BIT_XOR:
		m_context << eth::Instruction::XOR;
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::appendShiftOperatorCode(Token::Value _operator)
{
	switch (_operator)
	{
	case Token::SHL:
		assert(false); //@todo
		break;
	case Token::SAR:
		assert(false); //@todo
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::storeInLValue(Expression const& _expression)
{
	moveToLValue(_expression);
	unsigned stackPos = stackPositionOfLValue();
	if (stackPos > 16)
		BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_expression.getLocation())
											  << errinfo_comment("Stack too deep."));
	m_context << eth::dupInstruction(stackPos + 1);
}

void ExpressionCompiler::moveToLValue(Expression const& _expression)
{
	unsigned stackPos = stackPositionOfLValue();
	if (stackPos > 16)
		BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_expression.getLocation())
											  << errinfo_comment("Stack too deep."));
	else if (stackPos > 0)
		m_context << eth::swapInstruction(stackPos) << eth::Instruction::POP;
}

unsigned ExpressionCompiler::stackPositionOfLValue() const
{
	assert(m_currentLValue);
	return m_context.getStackPositionOfVariable(*m_currentLValue);
}

}
}
<commit_msg>Bugfix: Swap before mod and div.<commit_after>/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/**
 * @author Christian <c@ethdev.com>
 * @date 2014
 * Solidity AST to EVM bytecode compiler for expressions.
 */

#include <cassert>
#include <utility>
#include <numeric>
#include <libsolidity/AST.h>
#include <libsolidity/ExpressionCompiler.h>
#include <libsolidity/CompilerContext.h>

using namespace std;

namespace dev {
namespace solidity {

void ExpressionCompiler::compileExpression(CompilerContext& _context, Expression& _expression)
{
	ExpressionCompiler compiler(_context);
	_expression.accept(compiler);
}

bool ExpressionCompiler::visit(Assignment& _assignment)
{
	m_currentLValue = nullptr;

	Expression& rightHandSide = _assignment.getRightHandSide();
	rightHandSide.accept(*this);
	Type const& resultType = *_assignment.getType();
	cleanHigherOrderBitsIfNeeded(*rightHandSide.getType(), resultType);
	_assignment.getLeftHandSide().accept(*this);

	Token::Value op = _assignment.getAssignmentOperator();
	if (op != Token::ASSIGN)
	{
		// compound assignment
		m_context << eth::Instruction::SWAP1;
		appendOrdinaryBinaryOperatorCode(Token::AssignmentToBinaryOp(op), resultType);
	}
	else
		m_context << eth::Instruction::POP; //@todo do not retrieve the value in the first place

	storeInLValue(_assignment);
	return false;
}

void ExpressionCompiler::endVisit(UnaryOperation& _unaryOperation)
{
	//@todo type checking and creating code for an operator should be in the same place:
	// the operator should know how to convert itself and to which types it applies, so
	// put this code together with "Type::acceptsBinary/UnaryOperator" into a class that
	// represents the operator
	switch (_unaryOperation.getOperator())
	{
	case Token::NOT: // !
		m_context << eth::Instruction::ISZERO;
		break;
	case Token::BIT_NOT: // ~
		m_context << eth::Instruction::NOT;
		break;
	case Token::DELETE: // delete
	{
		// a -> a xor a (= 0).
		// @todo semantics change for complex types
		m_context << eth::Instruction::DUP1 << eth::Instruction::XOR;
		storeInLValue(_unaryOperation);
		break;
	}
	case Token::INC: // ++ (pre- or postfix)
	case Token::DEC: // -- (pre- or postfix)
		if (!_unaryOperation.isPrefixOperation())
			m_context << eth::Instruction::DUP1;
		m_context << u256(1);
		if (_unaryOperation.getOperator() == Token::INC)
			m_context << eth::Instruction::ADD;
		else
			m_context << eth::Instruction::SWAP1 << eth::Instruction::SUB; // @todo avoid the swap
		if (_unaryOperation.isPrefixOperation())
			storeInLValue(_unaryOperation);
		else
			moveToLValue(_unaryOperation);
		break;
	case Token::ADD: // +
		// unary add, so basically no-op
		break;
	case Token::SUB: // -
		m_context << u256(0) << eth::Instruction::SUB;
		break;
	default:
		assert(false); // invalid operation
	}
}

bool ExpressionCompiler::visit(BinaryOperation& _binaryOperation)
{
	Expression& leftExpression = _binaryOperation.getLeftExpression();
	Expression& rightExpression = _binaryOperation.getRightExpression();
	Type const& resultType = *_binaryOperation.getType();
	Token::Value const op = _binaryOperation.getOperator();

	if (op == Token::AND || op == Token::OR)
	{
		// special case: short-circuiting
		appendAndOrOperatorCode(_binaryOperation);
	}
	else if (Token::isCompareOp(op))
	{
		leftExpression.accept(*this);
		rightExpression.accept(*this);

		// the types to compare have to be the same, but the resulting type is always bool
		assert(*leftExpression.getType() == *rightExpression.getType());
		appendCompareOperatorCode(op, *leftExpression.getType());
	}
	else
	{
		leftExpression.accept(*this);
		cleanHigherOrderBitsIfNeeded(*leftExpression.getType(), resultType);
		rightExpression.accept(*this);
		cleanHigherOrderBitsIfNeeded(*rightExpression.getType(), resultType);
		appendOrdinaryBinaryOperatorCode(op, resultType);
	}

	// do not visit the child nodes, we already did that explicitly
	return false;
}

bool ExpressionCompiler::visit(FunctionCall& _functionCall)
{
	if (_functionCall.isTypeConversion())
	{
		//@todo we only have integers and bools for now which cannot be explicitly converted
		assert(_functionCall.getArguments().size() == 1);
		Expression& firstArgument = *_functionCall.getArguments().front();
		firstArgument.accept(*this);
		cleanHigherOrderBitsIfNeeded(*firstArgument.getType(), *_functionCall.getType());
	}
	else
	{
		// Calling convention: Caller pushes return address and arguments
		// Callee removes them and pushes return values
		m_currentLValue = nullptr;
		_functionCall.getExpression().accept(*this);
		FunctionDefinition const* function = dynamic_cast<FunctionDefinition*>(m_currentLValue);
		assert(function);

		eth::AssemblyItem returnLabel = m_context.pushNewTag();
		std::vector<ASTPointer<Expression>> const& arguments = _functionCall.getArguments();
		assert(arguments.size() == function->getParameters().size());
		for (unsigned i = 0; i < arguments.size(); ++i)
		{
			arguments[i]->accept(*this);
			cleanHigherOrderBitsIfNeeded(*arguments[i]->getType(),
										 *function->getParameters()[i]->getType());
		}

		m_context.appendJumpTo(m_context.getFunctionEntryLabel(*function));
		m_context << returnLabel;

		// callee adds return parameters, but removes arguments and return label
		m_context.adjustStackOffset(function->getReturnParameters().size() - arguments.size() - 1);

		// @todo for now, the return value of a function is its first return value, so remove
		// all others
		for (unsigned i = 1; i < function->getReturnParameters().size(); ++i)
			m_context << eth::Instruction::POP;
	}
	return false;
}

void ExpressionCompiler::endVisit(MemberAccess&)
{

}

void ExpressionCompiler::endVisit(IndexAccess&)
{

}

void ExpressionCompiler::endVisit(Identifier& _identifier)
{
	m_currentLValue = _identifier.getReferencedDeclaration();
	switch (_identifier.getType()->getCategory())
	{
	case Type::Category::BOOL:
	case Type::Category::INTEGER:
	case Type::Category::REAL:
	{
		//@todo we also have to check where to retrieve them from once we add storage variables
		unsigned stackPos = stackPositionOfLValue();
		if (stackPos >= 15) //@todo correct this by fetching earlier or moving to memory
			BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_identifier.getLocation())
												  << errinfo_comment("Stack too deep."));
		m_context << eth::dupInstruction(stackPos + 1);
		break;
	}
	default:
		break;
	}
}

void ExpressionCompiler::endVisit(Literal& _literal)
{
	switch (_literal.getType()->getCategory())
	{
	case Type::Category::INTEGER:
	case Type::Category::BOOL:
		m_context << _literal.getType()->literalValue(_literal);
		break;
	default:
		assert(false); // @todo
	}
}

void ExpressionCompiler::cleanHigherOrderBitsIfNeeded(Type const& _typeOnStack, Type const& _targetType)
{
	// If the type of one of the operands is extended, we need to remove all
	// higher-order bits that we might have ignored in previous operations.
	// @todo: store in the AST whether the operand might have "dirty" higher
	// order bits

	if (_typeOnStack == _targetType)
		return;
	if (_typeOnStack.getCategory() == Type::Category::INTEGER &&
			_targetType.getCategory() == Type::Category::INTEGER)
	{
		//@todo
	}
	else
	{
		// If we get here, there is either an implementation missing to clean higher oder bits
		// for non-integer types that are explicitly convertible or we got here in error.
		assert(!_typeOnStack.isExplicitlyConvertibleTo(_targetType));
		assert(false); // these types should not be convertible.
	}
}

void ExpressionCompiler::appendAndOrOperatorCode(BinaryOperation& _binaryOperation)
{
	Token::Value const op = _binaryOperation.getOperator();
	assert(op == Token::OR || op == Token::AND);

	_binaryOperation.getLeftExpression().accept(*this);
	m_context << eth::Instruction::DUP1;
	if (op == Token::AND)
		m_context << eth::Instruction::ISZERO;
	eth::AssemblyItem endLabel = m_context.appendConditionalJump();
	_binaryOperation.getRightExpression().accept(*this);
	m_context << endLabel;
}

void ExpressionCompiler::appendCompareOperatorCode(Token::Value _operator, Type const& _type)
{
	if (_operator == Token::EQ || _operator == Token::NE)
	{
		m_context << eth::Instruction::EQ;
		if (_operator == Token::NE)
			m_context << eth::Instruction::ISZERO;
	}
	else
	{
		IntegerType const* type = dynamic_cast<IntegerType const*>(&_type);
		assert(type);
		bool const isSigned = type->isSigned();

		// note that EVM opcodes compare like "stack[0] < stack[1]",
		// but our left value is at stack[1], so everyhing is reversed.
		switch (_operator)
		{
		case Token::GTE:
			m_context << (isSigned ? eth::Instruction::SGT : eth::Instruction::GT)
					  << eth::Instruction::ISZERO;
			break;
		case Token::LTE:
			m_context << (isSigned ? eth::Instruction::SLT : eth::Instruction::LT)
					  << eth::Instruction::ISZERO;
			break;
		case Token::GT:
			m_context << (isSigned ? eth::Instruction::SLT : eth::Instruction::LT);
			break;
		case Token::LT:
			m_context << (isSigned ? eth::Instruction::SGT : eth::Instruction::GT);
			break;
		default:
			assert(false);
		}
	}
}

void ExpressionCompiler::appendOrdinaryBinaryOperatorCode(Token::Value _operator, Type const& _type)
{
	if (Token::isArithmeticOp(_operator))
		appendArithmeticOperatorCode(_operator, _type);
	else if (Token::isBitOp(_operator))
		appendBitOperatorCode(_operator);
	else if (Token::isShiftOp(_operator))
		appendShiftOperatorCode(_operator);
	else
		assert(false); // unknown binary operator
}

void ExpressionCompiler::appendArithmeticOperatorCode(Token::Value _operator, Type const& _type)
{
	IntegerType const* type = dynamic_cast<IntegerType const*>(&_type);
	assert(type);
	bool const isSigned = type->isSigned();

	switch (_operator)
	{
	case Token::ADD:
		m_context << eth::Instruction::ADD;
		break;
	case Token::SUB:
		m_context << eth::Instruction::SWAP1 << eth::Instruction::SUB;
		break;
	case Token::MUL:
		m_context << eth::Instruction::MUL;
		break;
	case Token::DIV:
		m_context << eth::Instruction::SWAP1 << (isSigned ? eth::Instruction::SDIV : eth::Instruction::DIV);
		break;
	case Token::MOD:
		m_context << eth::Instruction::SWAP1 << (isSigned ? eth::Instruction::SMOD : eth::Instruction::MOD);
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::appendBitOperatorCode(Token::Value _operator)
{
	switch (_operator)
	{
	case Token::BIT_OR:
		m_context << eth::Instruction::OR;
		break;
	case Token::BIT_AND:
		m_context << eth::Instruction::AND;
		break;
	case Token::BIT_XOR:
		m_context << eth::Instruction::XOR;
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::appendShiftOperatorCode(Token::Value _operator)
{
	switch (_operator)
	{
	case Token::SHL:
		assert(false); //@todo
		break;
	case Token::SAR:
		assert(false); //@todo
		break;
	default:
		assert(false);
	}
}

void ExpressionCompiler::storeInLValue(Expression const& _expression)
{
	moveToLValue(_expression);
	unsigned stackPos = stackPositionOfLValue();
	if (stackPos > 16)
		BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_expression.getLocation())
											  << errinfo_comment("Stack too deep."));
	m_context << eth::dupInstruction(stackPos + 1);
}

void ExpressionCompiler::moveToLValue(Expression const& _expression)
{
	unsigned stackPos = stackPositionOfLValue();
	if (stackPos > 16)
		BOOST_THROW_EXCEPTION(CompilerError() << errinfo_sourceLocation(_expression.getLocation())
											  << errinfo_comment("Stack too deep."));
	else if (stackPos > 0)
		m_context << eth::swapInstruction(stackPos) << eth::Instruction::POP;
}

unsigned ExpressionCompiler::stackPositionOfLValue() const
{
	assert(m_currentLValue);
	return m_context.getStackPositionOfVariable(*m_currentLValue);
}

}
}
<|endoftext|>


<commit_before>/* Copyright 2020 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/lite/delegates/gpu/common/testing/feature_parity/utils.h"

#include <memory>
#include <optional>
#include <ostream>
#include <string>
#include <utility>

#include "absl/status/status.h"
#include "absl/strings/substitute.h"
#include "tensorflow/lite/interpreter.h"
#include "tensorflow/lite/kernels/register.h"
#include "tensorflow/lite/model.h"
#include "tensorflow/lite/string_type.h"

std::ostream& operator<<(std::ostream& os, const TfLiteTensor& tensor) {
  std::string shape;
  absl::optional<std::string> result = tflite::ShapeToString(tensor.dims);
  if (result.has_value()) {
    shape = std::move(result.value());
  } else {
    shape = "[error: unsupported number of dimensions]";
  }
  return os << "tensor of shape " << shape;
}

namespace tflite {

absl::optional<std::string> ShapeToString(TfLiteIntArray* shape) {
  std::string result;
  int* data = shape->data;
  switch (shape->size) {
    case 1:
      result = absl::Substitute("Linear=[$0]", data[0]);
      break;
    case 2:
      result = absl::Substitute("HW=[$0, $1]", data[0], data[1]);
      break;
    case 3:
      result = absl::Substitute("HWC=[$0, $1, $2]", data[0], data[1], data[2]);
      break;
    case 4:
      result = absl::Substitute("BHWC=[$0, $1, $2, $3]", data[0], data[1],
                                data[2], data[3]);
      break;
    default:
      // This printer doesn't expect shapes of more than 4 dimensions.
      return absl::nullopt;
  }
  return result;
}

absl::optional<std::string> CoordinateToString(TfLiteIntArray* shape,
                                               int linear) {
  std::string result;
  switch (shape->size) {
    case 1: {
      result = absl::Substitute("[$0]", linear);
      break;
    } break;
    case 2: {
      const int tensor_width = shape->data[1];
      const int h_coord = linear / tensor_width;
      const int w_coord = linear % tensor_width;
      result = absl::Substitute("[$0, $1]", h_coord, w_coord);
      break;
    } break;
    case 3: {
      const int tensor_width = shape->data[1];
      const int tensor_channels = shape->data[2];
      const int h_coord = linear / (tensor_width * tensor_channels);
      const int w_coord =
          (linear % (tensor_width * tensor_channels)) / tensor_channels;
      const int c_coord =
          (linear % (tensor_width * tensor_channels)) % tensor_channels;
      result = absl::Substitute("[$0, $1, $2]", h_coord, w_coord, c_coord);
      break;
    } break;
    case 4: {
      const int tensor_height = shape->data[1];
      const int tensor_width = shape->data[2];
      const int tensor_channels = shape->data[3];
      const int b_coord =
          linear / (tensor_height * tensor_width * tensor_channels);
      const int h_coord =
          (linear % (tensor_height * tensor_width * tensor_channels)) /
          (tensor_width * tensor_channels);
      const int w_coord =
          ((linear % (tensor_height * tensor_width * tensor_channels)) %
           (tensor_width * tensor_channels)) /
          tensor_channels;
      const int c_coord =
          ((linear % (tensor_height * tensor_width * tensor_channels)) %
           (tensor_width * tensor_channels)) %
          tensor_channels;
      result = absl::Substitute("[$0, $1, $2, $3]", b_coord, h_coord, w_coord,
                                c_coord);
      break;
    }
    default:
      // This printer doesn't expect shapes of more than 4 dimensions.
      return absl::nullopt;
  }
  return result;
}

// Builds interpreter for a model, allocates tensors.
absl::Status BuildInterpreter(const Model* model,
                              std::unique_ptr<Interpreter>* interpreter) {
  TfLiteStatus status =
      InterpreterBuilder(model, ops::builtin::BuiltinOpResolver())(interpreter);
  if (status != kTfLiteOk || !*interpreter) {
    return absl::InternalError(
        "Failed to initialize interpreter with model binary.");
  }
  return absl::OkStatus();
}

absl::Status AllocateTensors(std::unique_ptr<Interpreter>* interpreter) {
  if ((*interpreter)->AllocateTensors() != kTfLiteOk) {
    return absl::InternalError("Failed to allocate tensors.");
  }
  return absl::OkStatus();
}

absl::Status ModifyGraphWithDelegate(std::unique_ptr<Interpreter>* interpreter,
                                     TfLiteDelegate* delegate) {
  if ((*interpreter)->ModifyGraphWithDelegate(delegate) != kTfLiteOk) {
    return absl::InternalError("Failed modify graph with delegate.");
  }
  return absl::OkStatus();
}

void InitializeInputs(int left, int right,
                      std::unique_ptr<Interpreter>* interpreter) {
  for (int id : (*interpreter)->inputs()) {
    float* input_data = (*interpreter)->typed_tensor<float>(id);
    int input_size = (*interpreter)->input_tensor(id)->bytes;
    for (int i = 0; i < input_size; i++) {
      input_data[i] = left + i % right;
    }
  }
}

absl::Status Invoke(std::unique_ptr<Interpreter>* interpreter) {
  if ((*interpreter)->Invoke() != kTfLiteOk) {
    return absl::InternalError("Failed during inference.");
  }
  return absl::OkStatus();
}

std::ostream& operator<<(std::ostream& os, const TestParams& param) {
  return os << param.name;
}

}  // namespace tflite
<commit_msg>(lite) Fix grammar error in error message.<commit_after>/* Copyright 2020 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/lite/delegates/gpu/common/testing/feature_parity/utils.h"

#include <memory>
#include <optional>
#include <ostream>
#include <string>
#include <utility>

#include "absl/status/status.h"
#include "absl/strings/substitute.h"
#include "tensorflow/lite/interpreter.h"
#include "tensorflow/lite/kernels/register.h"
#include "tensorflow/lite/model.h"
#include "tensorflow/lite/string_type.h"

std::ostream& operator<<(std::ostream& os, const TfLiteTensor& tensor) {
  std::string shape;
  absl::optional<std::string> result = tflite::ShapeToString(tensor.dims);
  if (result.has_value()) {
    shape = std::move(result.value());
  } else {
    shape = "[error: unsupported number of dimensions]";
  }
  return os << "tensor of shape " << shape;
}

namespace tflite {

absl::optional<std::string> ShapeToString(TfLiteIntArray* shape) {
  std::string result;
  int* data = shape->data;
  switch (shape->size) {
    case 1:
      result = absl::Substitute("Linear=[$0]", data[0]);
      break;
    case 2:
      result = absl::Substitute("HW=[$0, $1]", data[0], data[1]);
      break;
    case 3:
      result = absl::Substitute("HWC=[$0, $1, $2]", data[0], data[1], data[2]);
      break;
    case 4:
      result = absl::Substitute("BHWC=[$0, $1, $2, $3]", data[0], data[1],
                                data[2], data[3]);
      break;
    default:
      // This printer doesn't expect shapes of more than 4 dimensions.
      return absl::nullopt;
  }
  return result;
}

absl::optional<std::string> CoordinateToString(TfLiteIntArray* shape,
                                               int linear) {
  std::string result;
  switch (shape->size) {
    case 1: {
      result = absl::Substitute("[$0]", linear);
      break;
    } break;
    case 2: {
      const int tensor_width = shape->data[1];
      const int h_coord = linear / tensor_width;
      const int w_coord = linear % tensor_width;
      result = absl::Substitute("[$0, $1]", h_coord, w_coord);
      break;
    } break;
    case 3: {
      const int tensor_width = shape->data[1];
      const int tensor_channels = shape->data[2];
      const int h_coord = linear / (tensor_width * tensor_channels);
      const int w_coord =
          (linear % (tensor_width * tensor_channels)) / tensor_channels;
      const int c_coord =
          (linear % (tensor_width * tensor_channels)) % tensor_channels;
      result = absl::Substitute("[$0, $1, $2]", h_coord, w_coord, c_coord);
      break;
    } break;
    case 4: {
      const int tensor_height = shape->data[1];
      const int tensor_width = shape->data[2];
      const int tensor_channels = shape->data[3];
      const int b_coord =
          linear / (tensor_height * tensor_width * tensor_channels);
      const int h_coord =
          (linear % (tensor_height * tensor_width * tensor_channels)) /
          (tensor_width * tensor_channels);
      const int w_coord =
          ((linear % (tensor_height * tensor_width * tensor_channels)) %
           (tensor_width * tensor_channels)) /
          tensor_channels;
      const int c_coord =
          ((linear % (tensor_height * tensor_width * tensor_channels)) %
           (tensor_width * tensor_channels)) %
          tensor_channels;
      result = absl::Substitute("[$0, $1, $2, $3]", b_coord, h_coord, w_coord,
                                c_coord);
      break;
    }
    default:
      // This printer doesn't expect shapes of more than 4 dimensions.
      return absl::nullopt;
  }
  return result;
}

// Builds interpreter for a model, allocates tensors.
absl::Status BuildInterpreter(const Model* model,
                              std::unique_ptr<Interpreter>* interpreter) {
  TfLiteStatus status =
      InterpreterBuilder(model, ops::builtin::BuiltinOpResolver())(interpreter);
  if (status != kTfLiteOk || !*interpreter) {
    return absl::InternalError(
        "Failed to initialize interpreter with model binary.");
  }
  return absl::OkStatus();
}

absl::Status AllocateTensors(std::unique_ptr<Interpreter>* interpreter) {
  if ((*interpreter)->AllocateTensors() != kTfLiteOk) {
    return absl::InternalError("Failed to allocate tensors.");
  }
  return absl::OkStatus();
}

absl::Status ModifyGraphWithDelegate(std::unique_ptr<Interpreter>* interpreter,
                                     TfLiteDelegate* delegate) {
  if ((*interpreter)->ModifyGraphWithDelegate(delegate) != kTfLiteOk) {
    return absl::InternalError("Failed to modify graph with delegate.");
  }
  return absl::OkStatus();
}

void InitializeInputs(int left, int right,
                      std::unique_ptr<Interpreter>* interpreter) {
  for (int id : (*interpreter)->inputs()) {
    float* input_data = (*interpreter)->typed_tensor<float>(id);
    int input_size = (*interpreter)->input_tensor(id)->bytes;
    for (int i = 0; i < input_size; i++) {
      input_data[i] = left + i % right;
    }
  }
}

absl::Status Invoke(std::unique_ptr<Interpreter>* interpreter) {
  if ((*interpreter)->Invoke() != kTfLiteOk) {
    return absl::InternalError("Failed during inference.");
  }
  return absl::OkStatus();
}

std::ostream& operator<<(std::ostream& os, const TestParams& param) {
  return os << param.name;
}

}  // namespace tflite
<|endoftext|>


<commit_before>#include "litesql.hpp"
#include "generator.hpp"
#include "litesql-gen-cpp.hpp"
#include "litesql-gen-graphviz.hpp"
#include "litesql-gen-ruby-activerecord.hpp"
#include "logger.hpp"
#include "objectmodel.hpp"

using namespace std;
using namespace litesql;

const char* help =
"Usage: litesql-gen [options] <my-database.xml>\n\n"
"Options:\n"
" -t, --target=TARGET                generate code for TARGET (default: c++)\n"
" -v, --verbose                      verbosely report code generation\n"
" --help                             print help\n"

" -t, --target=TARGET                generate code for TARGET (default: c++)\n"
" --output-dir=/path/to/src          output all files to directory \n"
" --output-sources=/path/to/src      output sources to directory \n"
" --output-include=/path/to/include  output includes to directory\n"
" --refresh                          refresh code of target\n"
" --overwrite                        overwrite code on generation\n"
"\n"
"Supported targets:\n"
"  'c++'        C++ target (.cpp,.hpp)\n"
"  'ruby-activerecord'          ruby target (.rb)\n"
//"  'objc'          Objective C (.m,.h)\n"
//"  'c'          C target (.c,.h)\n"
//"  'haskell'    Haskell target (.hs)\n"
//"  'sql'        SQL schema of database (.sql)\n"
//"  'php'        PHP target (.php)\n"
//"  'python'     Python target (.py)\n"
"  'graphviz'   Graphviz file (.dot)\n"
"\n\n"
;

struct options_t {
  string output_dir;
  string output_sources;
  string output_includes;
  bool refresh;
  bool printHelp;
  vector<string> targets;
};

options_t options = {"","","",true,false};

int parseArgs(int argc, char **argv)
{
  if(argc==1)
    return -1;

  for (int i = 1; i < argc; i++) {
    string arg = argv[i];
    if (arg == "-v" || arg == "--verbose") {
      Logger::verbose(true);
      continue;
    } else if (arg == "-t" || arg == "--target") {
      if (i+1 >= argc) {
        Logger::error("Error: missing target");
        return -1;
      }
      options.targets.push_back(argv[i+1]);
      i++;
      continue;
    } else if (litesql::startsWith(arg, "--target=")) {
      litesql::Split lang(arg, "=");
      options.targets.push_back(lang[1]);
      continue;
    }  else if (litesql::startsWith(arg, "--output-dir")) {
      litesql::Split lang(arg, "=");
      options.output_dir=lang[1];
      continue;
    } else if (litesql::startsWith(arg, "--output-sources")) {
      litesql::Split lang(arg, "=");
      options.output_sources=lang[1];
      continue;
    }  else if (litesql::startsWith(arg, "--output-include")) {
      litesql::Split lang(arg, "=");
      options.output_includes=lang[1];
      continue;
    }
    else if (arg == "--help") {
      options.printHelp = true;
      continue;
    } else if (i < argc - 1) {
      Logger::error("Error: invalid argument "+ arg);
      return -1;
    }
  }
  return 0;
}

int generateCode(ObjectModel& model)
{
    CompositeGenerator generator;

    generator.setOutputDirectory(options.output_dir);

    for (vector<string>::const_iterator target= options.targets.begin(); target!=options.targets.end();target++)
    {

      if (*target == "c++")
      {
        CppGenerator* pCppGen = new CppGenerator();
        pCppGen->setOutputSourcesDirectory(options.output_sources);
        pCppGen->setOutputIncludesDirectory(options.output_includes);

        generator.add(pCppGen);
      }
      else if (*target == "graphviz")
      {
        generator.add(new GraphvizGenerator());
      }
      else if (*target == "ruby-activerecord")
      {
        generator.add(new RubyActiveRecordGenerator());
      }
      else
      {
        throw litesql::Except("unsupported target: " + *target);
      }
    }

    return generator.generateCode(&model)? 0 : 1 ;
}

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

  int rc = parseArgs(argc,argv);
  if (rc!=0)
  {
    Logger::error(help);
    return -1;
  }

  if (options.printHelp) {
    cout << help << endl;
  }

  ObjectModel model;
  try {
    if (!model.loadFromFile(argv[argc-1]))
    {
      string msg = "could not load file '" + string(argv[argc-1]) + "'";
      Logger::error(msg);
      return -1;
    }
    else
    {
      return generateCode(model);
    }
  }
  catch (Except e) {
      Logger::error(e);
      return -1;
  }
}
<commit_msg>generic loop over registered codegenerators allows them all to be --target parameter<commit_after>#include "litesql.hpp"
#include "generator.hpp"
#include "litesql-gen-cpp.hpp"
#include "logger.hpp"
#include "objectmodel.hpp"

using namespace std;
using namespace litesql;

const char* help =
"Usage: litesql-gen [options] <my-database.xml>\n\n"
"Options:\n"
" -t, --target=TARGET                generate code for TARGET (default: c++)\n"
" -v, --verbose                      verbosely report code generation\n"
" --help                             print help\n"

" -t, --target=TARGET                generate code for TARGET (default: c++)\n"
" --output-dir=/path/to/src          output all files to directory \n"
" --output-sources=/path/to/src      output sources to directory \n"
" --output-include=/path/to/include  output includes to directory\n"
" --refresh                          refresh code of target\n"
" --overwrite                        overwrite code on generation\n"
"\n"
"Supported targets:\n"
"  'c++'        C++ target (.cpp,.hpp)\n"
"  'ruby-activerecord'          ruby target (.rb)\n"
//"  'objc'          Objective C (.m,.h)\n"
//"  'c'          C target (.c,.h)\n"
//"  'haskell'    Haskell target (.hs)\n"
//"  'sql'        SQL schema of database (.sql)\n"
//"  'php'        PHP target (.php)\n"
//"  'python'     Python target (.py)\n"
"  'graphviz'   Graphviz file (.dot)\n"
"\n\n"
;

struct options_t {
  string output_dir;
  string output_sources;
  string output_includes;
  bool refresh;
  bool printHelp;
  vector<string> targets;
};

options_t options = {"","","",true,false};

int parseArgs(int argc, char **argv)
{
  if(argc==1)
    return -1;

  for (int i = 1; i < argc; i++) {
    string arg = argv[i];
    if (arg == "-v" || arg == "--verbose") {
      Logger::verbose(true);
      continue;
    } else if (arg == "-t" || arg == "--target") {
      if (i+1 >= argc) {
        Logger::error("Error: missing target");
        return -1;
      }
      options.targets.push_back(argv[i+1]);
      i++;
      continue;
    } else if (litesql::startsWith(arg, "--target=")) {
      litesql::Split lang(arg, "=");
      options.targets.push_back(lang[1]);
      continue;
    }  else if (litesql::startsWith(arg, "--output-dir")) {
      litesql::Split lang(arg, "=");
      options.output_dir=lang[1];
      continue;
    } else if (litesql::startsWith(arg, "--output-sources")) {
      litesql::Split lang(arg, "=");
      options.output_sources=lang[1];
      continue;
    }  else if (litesql::startsWith(arg, "--output-include")) {
      litesql::Split lang(arg, "=");
      options.output_includes=lang[1];
      continue;
    }
    else if (arg == "--help") {
      options.printHelp = true;
      continue;
    } else if (i < argc - 1) {
      Logger::error("Error: invalid argument "+ arg);
      return -1;
    }
  }
  return 0;
}

int generateCode(ObjectModel& model)
{
    CompositeGenerator generator;

    generator.setOutputDirectory(options.output_dir);

    for (vector<string>::const_iterator target= options.targets.begin(); target!=options.targets.end();target++)
    {
        CodeGenerator* pGen = CodeGenerator::create(target->c_str());
        if (!pGen)
        {
            throw litesql::Except("unsupported target: " + *target);
        }
        else
        {
            generator.add(pGen);

            // special case for c++
            CppGenerator* pCppGen= dynamic_cast<CppGenerator*>(pGen);
            if (pCppGen)
            {
                pCppGen->setOutputSourcesDirectory(options.output_sources);
                pCppGen->setOutputIncludesDirectory(options.output_includes);
            }
        }

    }

    return generator.generateCode(&model)? 0 : 1 ;
}

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

  int rc = parseArgs(argc,argv);
  if (rc!=0)
  {
    Logger::error(help);
    return -1;
  }

  if (options.printHelp) {
    cout << help << endl;
  }

  ObjectModel model;
  try {
    if (!model.loadFromFile(argv[argc-1]))
    {
      string msg = "could not load file '" + string(argv[argc-1]) + "'";
      Logger::error(msg);
      return -1;
    }
    else
    {
      return generateCode(model);
    }
  }
  catch (Except e) {
      Logger::error(e);
      return -1;
  }
}
<|endoftext|>


<commit_before>#include <limits>
#include <type_traits>

#include "support.cuh"

namespace curng
{
	namespace detail
	{
		__device__ inline
		void sincos_(float aX, float* aSin, float* aCos)
		{
			sincosf(aX, aSin, aCos);
		}
		__device__ inline
		void sincos_(double aX, double* aSin, double* aCos)
		{
			sincos(aX, aSin, aCos);
		}
	}

	template< typename tReal > __device__ inline
	NormalBoxMuller::Distribution<tReal>::Distribution( unsigned, NormalBoxMuller::GlobalData<tReal> const& )
		: mCache( std::numeric_limits<tReal>::quiet_NaN() )
	{}

	template< typename tReal > template< class tRand, class tRandData > __device__ inline
	auto NormalBoxMuller::Distribution<tReal>::operator() (tRand& aRng, unsigned aTid, GlobalData<tReal>&, tRandData& aEngData ) -> result_type
	{
		constexpr tReal pi2 = tReal(2)*tReal(3.14159265357989);
		constexpr tReal eps = std::numeric_limits<tReal>::min();
		constexpr std::size_t digits_ = std::numeric_limits<tReal>::digits;

		if( !/*std::*/isnan( mCache ) )
		{
			auto ret = mCache;
			mCache = std::numeric_limits<tReal>::quiet_NaN();
			return ret;
		}

		tReal u1;
		do
		{
			u1 = generate_canonical<tReal,digits_>( aRng, aTid, aEngData );
		} while (u1 <= eps);

		tReal const lu2 = std::sqrt(tReal(-2) * std::log(u1));

		tReal u2 = generate_canonical<tReal,digits_>( aRng, aTid, aEngData );

		tReal s, c;
		detail::sincos_(pi2 * u2, &s, &c);

		mCache = lu2 * s;
		return lu2 * c;
	}

	template< typename tReal > __host__ inline
	auto NormalBoxMuller::initialize( Identity<tReal>, std::size_t ) -> GlobalData<tReal>
	{
		return {};
	}

	template< typename tReal > __host__ inline
	void NormalBoxMuller::cleanup( GlobalData<tReal>& )
	{}
}
<commit_msg>µopt: use intrinsics when generating float normal dist<commit_after>#include <limits>
#include <type_traits>

#include "support.cuh"

namespace curng
{
	namespace detail
	{
		__device__ inline
		void sincos_(float aX, float* aSin, float* aCos)
		{
			//sincosf(aX, aSin, aCos);
			__sincosf( aX, aSin, aCos );
		}
		__device__ inline
		void sincos_(double aX, double* aSin, double* aCos)
		{
			sincos(aX, aSin, aCos);
		}

		__device__ inline
		float log_( float aX )
		{
			//return logf( aX );
			return __logf( aX );
		}
		__device__ inline
		double log_( double aX )
		{
			return log( aX );
		}
	}

	template< typename tReal > __device__ inline
	NormalBoxMuller::Distribution<tReal>::Distribution( unsigned, NormalBoxMuller::GlobalData<tReal> const& )
		: mCache( std::numeric_limits<tReal>::quiet_NaN() )
	{}

	template< typename tReal > template< class tRand, class tRandData > __device__ inline
	auto NormalBoxMuller::Distribution<tReal>::operator() (tRand& aRng, unsigned aTid, GlobalData<tReal>&, tRandData& aEngData ) -> result_type
	{
		constexpr tReal pi2 = tReal(2)*tReal(3.14159265357989);
		constexpr tReal eps = std::numeric_limits<tReal>::min();
		constexpr std::size_t digits_ = std::numeric_limits<tReal>::digits;

		if( !/*std::*/isnan( mCache ) )
		{
			auto ret = mCache;
			mCache = std::numeric_limits<tReal>::quiet_NaN();
			return ret;
		}

		tReal u1;
		do
		{
			u1 = generate_canonical<tReal,digits_>( aRng, aTid, aEngData );
		} while (u1 <= eps);

		tReal const lu2 = std::sqrt(tReal(-2) * detail::log_(u1));

		tReal const u2 = generate_canonical<tReal,digits_>( aRng, aTid, aEngData );

		tReal s, c;
		detail::sincos_(pi2 * u2, &s, &c);

		mCache = lu2 * s;
		return lu2 * c;
	}

	template< typename tReal > __host__ inline
	auto NormalBoxMuller::initialize( Identity<tReal>, std::size_t ) -> GlobalData<tReal>
	{
		return {};
	}

	template< typename tReal > __host__ inline
	void NormalBoxMuller::cleanup( GlobalData<tReal>& )
	{}
}
<|endoftext|>


<commit_before>#include <base/gl/Window.h>
#include <framework/TestRunner.h>
#include <tests/test1/BallsSceneTests.h>
#include <tests/test2/TerrainSceneTests.h>
#include <tests/test3/ShadowMappingSceneTests.h>

#include <iostream>
#include <stdexcept>

namespace framework {
TestRunner::TestRunner(base::ArgumentParser argumentParser)
    : arguments(std::move(argumentParser))
{
}

int TestRunner::run()
{
    const int TESTS = 3;

    auto errorCallback = [&](const std::string& msg) -> int {
        std::cerr << "Invalid usage! " << msg << std::endl;
        std::cerr << "Usage: `" << arguments.getPath() << " -t T -api API [-m]`" << std::endl;
        std::cerr << "  -t T       - test number (in range [1, " << TESTS << "])" << std::endl;
        std::cerr << "  -api API   - API (`gl` or `vk`)" << std::endl;
        std::cerr << "  -m         - run multithreaded version (if exists)" << std::endl;
        return -1;
    };

    if (!arguments.hasArgument("t"))
        return errorCallback("Missing `-t` argument!");

    if (!arguments.hasArgument("api"))
        return errorCallback("Missing `-api` argument!");

    bool multithreaded = arguments.hasArgument("m");
    int testNum = -1;
    try {
        testNum = arguments.getIntArgument("t");
    } catch (...) {
        // ignore, will fail with proper message later
    }

    if (testNum < 1 || testNum > TESTS)
        return errorCallback("Invalid test number!");

    std::string api = arguments.getArgument("api");
    if (api != "gl" && api != "vk")
        return errorCallback("Invalid `-api` value!");

    if (api == "gl") {
        return run_gl(testNum, multithreaded);
    } else {
        return run_vk(testNum, multithreaded);
    }
}

int TestRunner::run_gl(int testNumber, bool multithreaded)
{
    std::unique_ptr<TestInterface> test;

    switch (testNumber) {
    case 1:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::MultithreadedBallsSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::SimpleBallsSceneTest());
        }
        break;

    case 2:
        if (multithreaded) {
            // N/A
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::TerrainSceneTest());
        }

    case 3:
        if (multithreaded) {
            // TODO:
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::ShadowMappingSceneTest());
        }
    }

    if (test) {
        return run_any(std::move(test));
    } else {
        std::cerr << "Unknown OpenGL test: " << testNumber << std::endl;
        return -1;
    }
}

int TestRunner::run_vk(int testNumber, bool multithreaded)
{
    std::unique_ptr<TestInterface> test;

    switch (testNumber) {
    case 1:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::MultithreadedBallsSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::SimpleBallsSceneTest());
        }
        break;

    case 2:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::MultithreadedTerrainSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::TerrainSceneTest());
        }

    case 3:
        if (multithreaded) {
            // TODO:
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::ShadowMappingSceneTest);
        }
    }

    if (test) {
        return run_any(std::move(test));
    } else {
        std::cerr << "Unknown Vulkan test: " << testNumber << std::endl;
        return -1;
    }
}

int TestRunner::run_any(std::unique_ptr<TestInterface> test)
{
    try {
        test->setup();
        test->run();
        test->teardown();

    } catch (const std::runtime_error& exception) {
        std::cerr << "Caught runtime exception during test execution!" << std::endl;
        std::cerr << exception.what() << std::endl;
        return -1;

    } catch (const std::exception& exception) {
        std::cerr << "Caught exception during test execution!" << std::endl;
        std::cerr << exception.what() << std::endl;
        return -1;

    } catch (...) {
        std::cerr << "Caught unknown exception during test execution!" << std::endl;
        return -1;
    }

    return 0;
}
}
<commit_msg>Fixed bug in testrunner preventing to run second test.<commit_after>#include <base/gl/Window.h>
#include <framework/TestRunner.h>
#include <tests/test1/BallsSceneTests.h>
#include <tests/test2/TerrainSceneTests.h>
#include <tests/test3/ShadowMappingSceneTests.h>

#include <iostream>
#include <stdexcept>

namespace framework {
TestRunner::TestRunner(base::ArgumentParser argumentParser)
    : arguments(std::move(argumentParser))
{
}

int TestRunner::run()
{
    const int TESTS = 3;

    auto errorCallback = [&](const std::string& msg) -> int {
        std::cerr << "Invalid usage! " << msg << std::endl;
        std::cerr << "Usage: `" << arguments.getPath() << " -t T -api API [-m]`" << std::endl;
        std::cerr << "  -t T       - test number (in range [1, " << TESTS << "])" << std::endl;
        std::cerr << "  -api API   - API (`gl` or `vk`)" << std::endl;
        std::cerr << "  -m         - run multithreaded version (if exists)" << std::endl;
        return -1;
    };

    if (!arguments.hasArgument("t"))
        return errorCallback("Missing `-t` argument!");

    if (!arguments.hasArgument("api"))
        return errorCallback("Missing `-api` argument!");

    bool multithreaded = arguments.hasArgument("m");
    int testNum = -1;
    try {
        testNum = arguments.getIntArgument("t");
    } catch (...) {
        // ignore, will fail with proper message later
    }

    if (testNum < 1 || testNum > TESTS)
        return errorCallback("Invalid test number!");

    std::string api = arguments.getArgument("api");
    if (api != "gl" && api != "vk")
        return errorCallback("Invalid `-api` value!");

    if (api == "gl") {
        return run_gl(testNum, multithreaded);
    } else {
        return run_vk(testNum, multithreaded);
    }
}

int TestRunner::run_gl(int testNumber, bool multithreaded)
{
    std::unique_ptr<TestInterface> test;

    switch (testNumber) {
    case 1:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::MultithreadedBallsSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::SimpleBallsSceneTest());
        }
        break;

    case 2:
        if (multithreaded) {
            // N/A
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::TerrainSceneTest());
        }
        break;

    case 3:
        if (multithreaded) {
            // TODO:
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_gl::ShadowMappingSceneTest());
        }
        break;
    }

    if (test) {
        return run_any(std::move(test));
    } else {
        std::cerr << "Unknown OpenGL test: " << testNumber << std::endl;
        return -1;
    }
}

int TestRunner::run_vk(int testNumber, bool multithreaded)
{
    std::unique_ptr<TestInterface> test;

    switch (testNumber) {
    case 1:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::MultithreadedBallsSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::SimpleBallsSceneTest());
        }
        break;

    case 2:
        if (multithreaded) {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::MultithreadedTerrainSceneTest());
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::TerrainSceneTest());
        }
        break;

    case 3:
        if (multithreaded) {
            // TODO:
        } else {
            test = std::unique_ptr<TestInterface>(new tests::test_vk::ShadowMappingSceneTest);
        }
        break;
    }

    if (test) {
        return run_any(std::move(test));
    } else {
        std::cerr << "Unknown Vulkan test: " << testNumber << std::endl;
        return -1;
    }
}

int TestRunner::run_any(std::unique_ptr<TestInterface> test)
{
    try {
        test->setup();
        test->run();
        test->teardown();

    } catch (const std::runtime_error& exception) {
        std::cerr << "Caught runtime exception during test execution!" << std::endl;
        std::cerr << exception.what() << std::endl;
        return -1;

    } catch (const std::exception& exception) {
        std::cerr << "Caught exception during test execution!" << std::endl;
        std::cerr << exception.what() << std::endl;
        return -1;

    } catch (...) {
        std::cerr << "Caught unknown exception during test execution!" << std::endl;
        return -1;
    }

    return 0;
}
}
<|endoftext|>


<commit_before>#include <vector>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point32.h>
#include <visualization_msgs/MarkerArray.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/moment_of_inertia_estimation.h>

using namespace std;
ros::Publisher pub;

struct Plane {
	pcl::PointXYZ min;
	pcl::PointXYZ max;
};

void printROSPoint(geometry_msgs::Point *p) {
	ROS_INFO("Point: %f %f %f", p->x, p->y, p->z);
}

void printPlane(struct Plane *plane) {
	ROS_INFO("AABB: %f %f %f -> %f %f %f", plane->min.x, plane->min.y, plane->min.z, plane->max.x, plane->max.z,
		plane->max.z);
}

void printPoint(pcl::PointXYZ *p) {
	ROS_INFO("Point: %f %f %f", p->x, p->y, p->z);
}

void getAABB(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, struct Plane *plane) {
	pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
	feature_extractor.setInputCloud(cloud);
	feature_extractor.compute();
	feature_extractor.getAABB(plane->min, plane->max);
}

void buildRosMarker(visualization_msgs::Marker *marker, struct Plane *plane, unsigned int id) {
	marker->header.frame_id = "base_link";
	marker->header.stamp = ros::Time::now();
	marker->ns = "hmmwv";
	marker->id = id;

	marker->type = visualization_msgs::Marker::LINE_STRIP;
	marker->action = visualization_msgs::Marker::ADD;

	marker->scale.x = 0.1f;

	marker->color.r = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.g = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.b = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.a = 1.0;

	marker->lifetime = ros::Duration();

	geometry_msgs::Point p1;
	p1.x = plane->min.z;
	p1.y = plane->min.x * (-1);
	p1.z = plane->min.y * (-1);

	geometry_msgs::Point p2;
	p2.x = plane->min.z;
	p2.y = plane->max.x * (-1);
	p2.z = plane->min.y * (-1);

	geometry_msgs::Point p3;
	p3.x = plane->min.z;
	p3.y = plane->max.x * (-1);
	p3.z = plane->max.y * (-1);

	geometry_msgs::Point p4;
	p4.x = plane->min.z;
	p4.y = plane->min.x * (-1);
	p4.z = plane->max.y * (-1);

	marker->points.push_back(p1);
	marker->points.push_back(p2);
	marker->points.push_back(p3);
	marker->points.push_back(p4);
	marker->points.push_back(p1);
}

void callback(const sensor_msgs::PointCloud2ConstPtr& input) {
	ROS_INFO("Callback!");

	// convert from ros::pointcloud2 to pcl::pointcloud2
	pcl::PCLPointCloud2* unfilteredCloud = new pcl::PCLPointCloud2;
	pcl::PCLPointCloud2ConstPtr unfilteredCloudPtr(unfilteredCloud);
	pcl_conversions::toPCL(*input, *unfilteredCloud);

	// create a voxelgrid to downsample the input data to speed things up.
	pcl::PCLPointCloud2::Ptr filteredCloud (new pcl::PCLPointCloud2);

	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(unfilteredCloudPtr);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*filteredCloud);

	// convert to pointcloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2(*filteredCloud, *cloud);

	// Does the parametric segmentation
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);

	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.01);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>),
		cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	unsigned int pointsAtStart = cloud->points.size(), id = 0;

	std::vector<struct Plane> planes;
	visualization_msgs::MarkerArray markerArray;

	// while 30% of the original cloud is still present
	while (cloud->points.size() > 0.3 * pointsAtStart) {

		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);

		if (inliers->indices.size() == 0) {
			ROS_WARN("Could not estimate a planar model for the given dataset.");
			break;
		}

		// extract the inliers
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud1);

		extract.setNegative(true);
		extract.filter(*cloud2);
		cloud.swap(cloud2);

		// calculate AABB and add to planes list
		struct Plane plane;
		getAABB(cloud1, &plane);
		printPlane(&plane);

		visualization_msgs::Marker marker;
		buildRosMarker(&marker, &plane, id);
		markerArray.markers.push_back(marker);

		id++;
	}

    pub.publish(markerArray);
}

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

	ros::init(argc, argv, "stairwaydetector");
	ros::NodeHandle nh;
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/points", 1, callback);
	pub = nh.advertise<visualization_msgs::MarkerArray>("/hmmwv/steps", 0);

	ros::spin();

	return 0;
}
<commit_msg>stairsdetection with PCL probably works.<commit_after>#include <vector>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point32.h>
#include <visualization_msgs/MarkerArray.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/moment_of_inertia_estimation.h>

using namespace std;
ros::Publisher pub;

struct Plane {
	pcl::PointXYZ min;
	pcl::PointXYZ max;
};

void printROSPoint(geometry_msgs::Point *p) {
	ROS_INFO("Point: %f %f %f", p->x, p->y, p->z);
}

void printPlane(struct Plane *plane) {
	ROS_INFO("AABB: %f %f %f -> %f %f %f", plane->min.x, plane->min.y, plane->min.z, plane->max.x, plane->max.z,
		plane->max.z);
}

void printPoint(pcl::PointXYZ *p) {
	ROS_INFO("Point: %f %f %f", p->x, p->y, p->z);
}

void getAABB(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, struct Plane *plane) {
	pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
	feature_extractor.setInputCloud(cloud);
	feature_extractor.compute();
	feature_extractor.getAABB(plane->min, plane->max);
}

/**
 * Transforms a point from PCL coordinate system to ROS coordinate system
 *
 * Documentation:
 * ROS: http://wiki.ros.org/geometry/CoordinateFrameConventions
 */
void transformPCLPointToROSPoint(pcl::PointXYZ *input, geometry_msgs::Point *output) {
	output->x = input->z;
	output->y = input->x * (-1.f);
	output->z = input->y * (-1.f);
}

void buildRosMarker(visualization_msgs::Marker *marker, struct Plane *plane, unsigned int id) {
	marker->header.frame_id = "camera_link";
	marker->header.stamp = ros::Time::now();
	marker->ns = "hmmwv";
	marker->id = id;
	marker->lifetime = ros::Duration();

	marker->type = visualization_msgs::Marker::LINE_STRIP;
	marker->action = visualization_msgs::Marker::ADD;

	marker->scale.x = 0.05f;

	marker->color.r = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.g = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.b = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
	marker->color.a = 1.0;

	/*
	 * Get vertices of the rectangle and transform them to ROS coordinates
	 *
	 *  p2-----------------p3
	 *  |                   |
	 *  |                   |
	 *  p1-----------------p4
	 *
	 */

	geometry_msgs::Point p1;
	transformPCLPointToROSPoint(&plane->min, &p1);

	geometry_msgs::Point p2;
	pcl::PointXYZ leftTop(plane->min.x, plane->max.y, plane->min.z);
	transformPCLPointToROSPoint(&leftTop, &p2);

	geometry_msgs::Point p3;
	transformPCLPointToROSPoint(&plane->max, &p3);

	geometry_msgs::Point p4;
	pcl::PointXYZ rightBottom(plane->max.x, plane->min.y, plane->max.z);
	transformPCLPointToROSPoint(&rightBottom, &p4);

	marker->points.push_back(p1);
	marker->points.push_back(p2);
	marker->points.push_back(p3);
	marker->points.push_back(p4);
	marker->points.push_back(p1);
}

void callback(const sensor_msgs::PointCloud2ConstPtr& input) {
	ROS_INFO("Callback!");

	// convert from ros::pointcloud2 to pcl::pointcloud2
	pcl::PCLPointCloud2* unfilteredCloud = new pcl::PCLPointCloud2;
	pcl::PCLPointCloud2ConstPtr unfilteredCloudPtr(unfilteredCloud);
	pcl_conversions::toPCL(*input, *unfilteredCloud);

	// create a voxelgrid to downsample the input data to speed things up.
	pcl::PCLPointCloud2::Ptr filteredCloud (new pcl::PCLPointCloud2);

	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(unfilteredCloudPtr);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*filteredCloud);

	// convert to pointcloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2(*filteredCloud, *cloud);

	// Does the parametric segmentation
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);

	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.01);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>),
		cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	unsigned int pointsAtStart = cloud->points.size(), id = 0;

	std::vector<struct Plane> planes;
	visualization_msgs::MarkerArray markerArray;

	// while 10% of the original cloud is still present
	while (cloud->points.size() > 0.1 * pointsAtStart) {

		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);

		if (inliers->indices.size() == 0) {
			ROS_WARN("Could not estimate a planar model for the given dataset.");
			break;
		}

		// extract the inliers
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud1);

		extract.setNegative(true);
		extract.filter(*cloud2);
		cloud.swap(cloud2);

		// calculate AABB and add to planes list
		struct Plane plane;
		getAABB(cloud1, &plane);

		// calculate the height of the plane and remove planes with less than 5 cm or more than 40 cm
		float height = plane.max.y - plane.min.y;
		ROS_INFO("Height: %f", height);
		if (height <= 0.4f && height >= 0.0f) {
			visualization_msgs::Marker marker;
			buildRosMarker(&marker, &plane, id);
			markerArray.markers.push_back(marker);
		}

		id++;
	}

    pub.publish(markerArray);
}

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

	ros::init(argc, argv, "stairwaydetector");
	ros::NodeHandle nh;
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/points", 1, callback);
	pub = nh.advertise<visualization_msgs::MarkerArray>("/hmmwv/steps", 0);

	ros::spin();

	return 0;
}
<|endoftext|>