server.cpp

Linux下多移动机器人服务器端通讯程序

#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        10

struct 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 failed
void ConnectHandler::connFail(void)
{
	printf("Failed to connect.\n");
	myRobot->stopRunning();
	Aria::shutdown();
	return;
}

// turn on motors, and off sonar, and off amigobot sounds, when connected
void 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 exit
void 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 client

end:	    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);
}