#include "Aria.h"//Server可以處理的命令字#define HelloToServer  1#define RobotStatus    2#define ShutdownServer 3#define RobotMove      4#define RobotStop      5#define RobotVel       6#define RobotRotVel    7#define SetHeading     8#define RobotRotate    9#define	Setting        10struct MsgRFC {//機器人上的Server從PC上Client接受到的信息msg1	double	coordx;	double	coordy;	double	th;	double	distance;	double  robRotAngle;	double	vel;	int		comm;	//請求命令};struct MsgSTC {//PC上Client從機器人上的Server接受到的信息msg2	double	posX;	double	posy;	double	th;	double	vel;	double	rotVel;	double  front_range;	double  left_range;	double  right_range;	char	buffer[20];};ArRobot robot;ArSonarDevice sonar;ArKeyHandler keyHandler;ArSerialConnection serConn;ArTcpConnection tcpConn;class ConnectHandler{public:	// Constructor	ConnectHandler(ArRobot *robot);	// Destructor, its just empty	~ConnectHandler(void) {};	// to be called if the connection was made	void connected(void);	// to call if the connection failed	void connFail(void);	// to be called if the connection was lost	void disconnected(void);	protected:	// robot pointer	ArRobot *myRobot;	ArFunctorC<ConnectHandler> myConnectedCB;	ArFunctorC<ConnectHandler> myConnFailCB;	ArFunctorC<ConnectHandler> myDisconnectedCB;};ConnectHandler::ConnectHandler(ArRobot *robot) :myConnectedCB(this, &ConnectHandler::connected),  myConnFailCB(this, &ConnectHandler::connFail),myDisconnectedCB(this, &ConnectHandler::disconnected){	// set the robot pointer	myRobot = robot;		myRobot->addConnectCB(&myConnectedCB, ArListPos::FIRST);	myRobot->addFailedConnectCB(&myConnFailCB, ArListPos::FIRST);	myRobot->addDisconnectNormallyCB(&myDisconnectedCB, ArListPos::FIRST);	myRobot->addDisconnectOnErrorCB(&myDisconnectedCB, ArListPos::FIRST);}// just exit if the connection failedvoid ConnectHandler::connFail(void){	printf("Failed to connect.\n");	myRobot->stopRunning();	Aria::shutdown();	return;}// turn on motors, and off sonar, and off amigobot sounds, when connectedvoid ConnectHandler::connected(void){	printf("Connected\n");	// turn off the sonar, enable the motors, turn off amigobot sounds	myRobot->comInt(ArCommands::SONAR, 0);	myRobot->comInt(ArCommands::ENABLE, 1);	myRobot->comInt(ArCommands::SOUNDTOG, 0);}// lost connection, so just exitvoid ConnectHandler::disconnected(void){	printf("Lost connection\n");	myRobot->stopRunning();	ArThread::stopAll();//	exit(0);	return;}class mythread : public ArASyncTask{public:	// Constructor	mythread(ArMutex *mStatue, ArMutex *mReq, MsgRFC *pMsgR, MsgSTC *pMsgS,		bool *pbSendReady, bool *pbNewOrder);	// Destructor	~mythread(){};		void *runThread(void *arg)	{		// The string to send to the client. Done here as a char array so that its		// easier to figure out its length.		char strToSend[]="Hello Client1\n";		// The buffer in which to recieve the hello from the client		char buff[100];		// The size of the string the client sent		size_t  strSize;		bool	bExit;		bExit = false;		// The socket object		ArSocket serverSock, clientSock;		ArRobot  robot;		// Lets open the server socket		if (serverSock.open(7171, ArSocket::TCP))		{			printf("Opened the server port\n");		}		else		{			printf("Failed to open the server port: %s\n",serverSock.getErrorStr().c_str());			return(NULL);		}				while (!bExit) 		{			// Lets wait for the client to connect to us.			if (serverSock.accept(&clientSock))				printf("Client has connected\n");			else				printf("Error in accepting a connection from the client: %s\n",serverSock.getErrorStr().c_str());			strSize=clientSock.read(buff, sizeof(buff));			if (strSize == sizeof(MsgRFC))			{				if (((MsgRFC *)buff)->comm == HelloToServer) 				{								//say hello to client					MsgSTC submsgS;					printf("send msg size %d",sizeof(MsgSTC));					for(int i=0;i<sizeof(strToSend);i++) submsgS.buffer[i]=strToSend[i];										if (clientSock.write(&submsgS, sizeof(MsgSTC)) == sizeof(MsgSTC))					{						printf("Said hello to the client\n");						goto end;					}					else					{						printf("Error in sending hello string to the client\n");						return(NULL);					}				}								if (((MsgRFC *)buff)->comm == RobotStatus) 				{							//say hello to client					MsgSTC submsgS;										mutexRobotStatue->lock();					submsgS.posX   = pMsgS->posX;					submsgS.posy   = pMsgS->posy;					submsgS.rotVel = pMsgS->rotVel;					submsgS.th     = pMsgS->th;					submsgS.vel    = pMsgS->vel;					submsgS.front_range=pMsgS->front_range;					submsgS.left_range=pMsgS->left_range;					submsgS.right_range=pMsgS->right_range;					mutexRobotStatue->unlock();										if (clientSock.write(&submsgS, sizeof(MsgSTC)) == sizeof(MsgSTC))					{						printf("had sent the robot statue info to client\n");						goto end;					}					else					{						printf("Error in sending hello string to the client\n");						return(NULL);					}				}				if (((MsgRFC *)buff)->comm == ShutdownServer)					bExit = true;								mutexRequire->lock();				pMsgR->comm = ((MsgRFC *)buff)->comm;				pMsgR->coordx = ((MsgRFC *)buff)->coordx;				pMsgR->coordy = ((MsgRFC *)buff)->coordy;				pMsgR->distance = ((MsgRFC *)buff)->distance;				pMsgR->robRotAngle = ((MsgRFC *)buff)->robRotAngle;				pMsgR->th = ((MsgRFC *)buff)->th;				pMsgR->vel = ((MsgRFC *)buff)->vel;				printf("get the order from client!!");				mutexRequire->unlock();								*pbGetNewOrder = true;				printf("get the order from client!");			}						else 			{				printf("the order length not right!");				goto end;			}						printf("receive size= %d \n",strSize);			// Now lets close the connection to the clientend:	    clientSock.close();			printf("Socket to client closed\n");		}		serverSock.close();		printf("Server socket closed\n");		printf("%d\n", clientSock.getFD());		return(NULL);			}protected:private:	ArMutex		*mutexRobotStatue;	ArMutex		*mutexRequire;	MsgRFC		*pMsgR;	MsgSTC		*pMsgS;	bool		*pbmsgSReady;	bool		*pbGetNewOrder;};mythread::mythread(ArMutex *mStatue, ArMutex *mReq, MsgRFC *pMsgR, MsgSTC *pMsgS,				   bool *pbSendReady, bool *pbNewOrder):mutexRobotStatue(mStatue),mutexRequire(mReq),pMsgR(pMsgR),pMsgS(pMsgS),pbmsgSReady(pbSendReady),pbGetNewOrder(pbNewOrder){};void quit(void){    printf("quit\n");    printf("exiting\n");    keyHandler.restore();    ArLog::log(ArLog::Terse, "Quit was pressed, program is Quitting.");	robot.stopRunning();	robot.disconnect();	ArThread::stopAll();	tcpConn.close();	//    Aria::shutdown();//加入此句將不能以"press any key to continue"退出程序	Aria::uninit();    exit(0);	//	return;}int main(){	MsgRFC  MsgR;	MsgSTC	MsgS;	bool	bmsgSReady;	bool	bGetNewOrder;	bool	bShutDown;	bool    usingSim;	ArMutex mStatue;	ArMutex mReq;	MsgRFC  subMsgR;		//ArRobot robot;	//	ArSonarDevice sonar;	//	ArKeyHandler keyHandler;			bGetNewOrder = false;	bShutDown    = false;	bmsgSReady   = false;	mythread serverthread(&mStatue, &mReq, &MsgR, &MsgS , &bmsgSReady, &bGetNewOrder);	ConnectHandler ch(&robot);	//ArSerialConnection serConn;	//	ArTcpConnection tcpConn;	 		//Aria::init();		// init area with a dedicated signal handling thread	Aria::init();	ArGlobalFunctor quitCB(&quit);	keyHandler.addKeyHandler('q', &quitCB);	Aria::setKeyHandler(&keyHandler);	robot.attachKeyHandler(&keyHandler,false);//false表示按下esc不斷開連接	// add the sonar to the robot	robot.addRangeDevice(&sonar);	//	Aria::setKeyHandler(&keyHandler);//	robot.attachKeyHandler(&keyHandler);		// modify this next line if you're not using default tcp connection	tcpConn.setPort();		// see if we can get to the simulator  (true is success)	if (tcpConn.openSimple())	{		// we could get to the sim, so set the robots device connection to the sim		printf("Connecting to simulator through tcp.\n");		robot.setDeviceConnection(&tcpConn);		usingSim = true;	}	else	{		// we couldn't get to the sim, so set the port on the serial		// connection and then set the serial connection as the robots		// device				// modify the next line if you're not using the first serial port		// to talk to your robot		serConn.setPort();		printf(			"Could not connect to simulator, connecting to robot through serial.\n");		robot.setDeviceConnection(&serConn);		usingSim = false;	}		// try to connect, if we fail exit	if (!robot.blockingConnect())	{		printf("Could not connect to robot... exiting\n");		Aria::shutdown();		return 1;	}		robot.comInt(ArCommands::SONAR, 1);    robot.comInt(ArCommands::SOUNDTOG, 0);	serverthread.create();	robot.runAsync(true);	sonar.lockDevice();	sonar.setMaxRange(2000);	sonar.unlockDevice();	while (!bShutDown) 	{		mStatue.lock();		MsgS.posX = robot.getX();		MsgS.posy = robot.getY();		MsgS.th = robot.getTh();		MsgS.rotVel = robot.getRotVel();		MsgS.front_range=sonar.currentReadingPolar(-30,30);		MsgS.left_range=sonar.currentReadingPolar(50,90);		MsgS.right_range=sonar.currentReadingPolar(-90,-50);		mStatue.unlock();				bmsgSReady = true;		if(bGetNewOrder == true)		{			mReq.lock();			subMsgR = MsgR;			mReq.unlock();			ArPose setPose(subMsgR.coordx,subMsgR.coordy,subMsgR.th);			switch(subMsgR.comm) 			{			case Setting:					robot.moveTo(setPose, true);				printf("send the setpos order");				break;			case RobotMove:				robot.lock();				robot.move(subMsgR.distance);				robot.unlock();				break;			case RobotStop:				robot.lock();				robot.stop();				robot.unlock();				break;			case RobotVel:				robot.lock();				robot.setVel(subMsgR.vel);				robot.unlock();				break;			case RobotRotVel:				robot.lock();				robot.setRotVel(subMsgR.vel);				robot.unlock();				break;			case SetHeading:				robot.lock();				robot.setHeading(subMsgR.th);				robot.unlock();				break;			case RobotRotate:				robot.lock();				robot.setDeltaHeading(subMsgR.robRotAngle);				robot.unlock();				break;			case ShutdownServer:				bShutDown = true;				robot.lock();				robot.stop();				robot.unlock();				break;			default:				ArUtil::sleep(5);				break;			}			bGetNewOrder=false;		}		ArUtil::sleep(500);	}	serverthread.stopRunning();//	Aria::uninit();	Aria::shutdown();//	return(0);}