// Codes for Motion Server Program

/*
The client program sends the specific commands in the form of special strings. 
The program then interprets the commands and if a match is found the associated function is executed.
Listing of Socket.h
Author: Srikanta Patnaik and Team 
Machine Intelligence Laboratory, University College of Engineering, Burla, India
Date of Writing: April, 2003; Version: 1.0
Book: Robot Cognition and Navigation: An Experiment with Mobile Robots
Publisher: Springer
ISBN: 978-3-540-23446-3
URL: http://www.springer.com/3-540-23446-2
 
*/


//This file defines the Socket class.


#ifndef Socket_class
#define Socket_class
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <string>
#include <arpa/inet.h>
const int MAXHOSTNAME = 200;
const int MAXCONNECTIONS = 5;
const int MAXRECV = 500;

class Socket
{
public:
Socket();
virtual ~Socket();

// Server initialization
bool create();
bool bind ( const int port );
bool listen() const;
bool accept ( Socket& ) const;

// Client initialization
bool connect( const std::string host, const int port);

// Data Transimission
bool send ( const std::string ) const;
int recv ( std::string& ) const;

void set_non_blocking ( const bool );
void close(void);
bool is_valid() const { return m_sock != -1; }

private:

int m_sock;
sockaddr_in m_addr;
};
#endif

Listing of Socket.cpp

//This program implements the Socket class, which is //defined by the Socket.h.

#include "Socket.h"
#include "string.h"
#include <string.h>
#include <errno.h>
#include <fcntl.h>

Socket::Socket() :  m_sock ( -1 )
{  
memset ( &m_addr,  0,  sizeof ( m_addr ) );
}
Socket::~Socket()
{
if ( is_valid() )  ::close ( m_sock );
}
bool Socket::create()
{
m_sock = socket ( AF_INET, SOCK_STREAM, 0 );

if ( ! is_valid() ) return false;
// TIME_WAIT - argh
int on = 1;
if ( setsockopt ( m_sock, SOL_SOCKET, SO_REUSEADDR, ( const char* ) 
&on, sizeof ( on ) ) == -1 )
return false;
return true;
}

bool Socket::bind ( const int port )
{
if ( ! is_valid() )
{
return false;
}
m_addr.sin_family = AF_INET;
m_addr.sin_addr.s_addr = INADDR_ANY;
m_addr.sin_port = htons ( port );
int bind_return = ::bind ( m_sock,( struct sockaddr * ) &m_addr,
sizeof ( m_addr ) );
if ( bind_return == -1 )
{
return false;
}

return true;
}
bool Socket::listen() const
{
if ( ! is_valid() )
{
return false;
}
int listen_return = ::listen (m_sock, MAXCONNECTIONS );
if ( listen_return == -1 )
{
return false;
}
return true;
}
bool Socket::accept ( Socket& new_socket ) const
{
int addr_length = sizeof ( m_addr );
new_socket.m_sock = ::accept ( m_sock, ( sockaddr * ) &m_addr, ( socklen_t * ) &addr_length );
if ( new_socket.m_sock <= 0 )
return false;
else
return true;
}
bool Socket::send ( const std::string s ) const
{
int status = ::send ( m_sock, s.c_str(), s.size(), MSG_NOSIGNAL );
if ( status == -1 )
{
return false;
}
else
{
return true;
}
}

int Socket::recv ( std::string& s ) const
{
char buf [ MAXRECV + 1 ];
s = "";
memset ( buf, 0, MAXRECV + 1 );

int status = ::recv ( m_sock, buf, MAXRECV, 0 );

if ( status == -1 )
{
if (errno==EAGAIN) { s="null"; return(1);}
std::cout << "status == -1   errno == " << errno << "  in 
Socket::recv\n";
return 0;
}
else if ( status == 0 )
{
return 0;
}
else
{
s = buf;
return status;
}
}

bool Socket::connect ( const std::string host, const int port )
{
if ( ! is_valid() ) return false;
m_addr.sin_family = AF_INET;
m_addr.sin_port = htons ( port );
int status = inet_pton ( AF_INET, host.c_str(), &m_addr.sin_addr );
if ( errno == EAFNOSUPPORT ) return false;
status = ::connect( m_sock,( sockaddr * )&m_addr,sizeof( m_addr ));
if ( status == 0 )
return true;
else
return false;
}
void Socket::set_non_blocking ( const bool b )
{
int opts;
opts = fcntl ( m_sock, F_GETFL );
if ( opts < 0 )
{
return;
}
if ( b )
opts = ( opts | O_NONBLOCK );
else
opts = ( opts & ~O_NONBLOCK );
fcntl ( m_sock, F_SETFL,opts );
}
void Socket::close(void)
{  ::close(m_sock); }


Listing of ServerSocket.h

//This file defines the ServerSocket class.

#ifndef ServerSocket_class
#define ServerSocket_class
#include "Socket.h"
class ServerSocket : private Socket
{
public:
ServerSocket ( int port );
ServerSocket (){};
virtual ~ServerSocket();
const ServerSocket& operator << (const std::string&) const;
const ServerSocket& operator >> ( std::string& ) const;
void accept ( ServerSocket& );
void set_non_blocking ( const bool b );
void close(void);
};
#endif

Listing of ServerSocket.cpp

/*This program implements the ServerSocket class, which is defined in the last section. */

#include "ServerSocket.h"
#include "SocketException.h"
ServerSocket::ServerSocket ( int port )
{
  if ( ! Socket::create() )
{
throw SocketException ( "Could not create server socket." );
}
  if ( ! Socket::bind ( port ) )
{
throw SocketException ( "Could not bind to port." );
}
  if ( ! Socket::listen() )
{
throw SocketException ( "Could not listen to socket." );
}
}
ServerSocket::~ServerSocket()
{
}

const ServerSocket& ServerSocket::operator << ( const std::string& s ) const
{
if ( ! Socket::send ( s ) )
{
throw SocketException ( "Could not write to socket." );
}
return *this;
}
const ServerSocket& ServerSocket::operator >> ( std::string& s ) const
{
int state=Socket::recv ( s );
if ( !state  )
{
throw SocketException ( "Could not read from socket." );
}
return *this;
}

void ServerSocket::accept ( ServerSocket& sock )
{
if ( ! Socket::accept ( sock ) )
{
throw SocketException ( "Could not accept socket." );
}
}

void ServerSocket::set_non_blocking ( const bool b )
{
Socket::set_non_blocking ( b );
}

void ServerSocket::close(void)
{
  Socket::close();
}

Listing of SocketException.h

//This file describes the SocketException class.

#ifndef SocketException_class
#define SocketException_class
#include <string>

class SocketException
{
 public:
 SocketException ( std::string s ) : m_s ( s ) {};
 ~SocketException (){};
 std::string description() { return m_s; }
 private:
 std::string m_s;
};
#endif





Listing of Simple_Server_main.cpp

/*This is the main program for controlling the motion commands for the robot. */ 

#include "ServerSocket.h"
#include "SocketException.h"
#include <string>
#include<stdio.h>
#include<stdlib.h>
#include <signal.h>
#include"Aria.h"
extern "C" {
#include"BotSpeak.h"
}
int process(std::string s);
bool sound=true;
ArSerialConnection serConn;
ArTcpConnection tcpConn;
ArRobot robot;
ArSonarDevice sonar;
ArGripper grip(&robot);
double head=0.0;
ArActionStallRecover recover;
ArActionBumpers bumpers;
ArActionAvoidFront avoidFrontNear("Avoid Front Near", 325, 0);
ArActionAvoidFront avoidFrontFar;
ArActionConstantVelocity constantVelocity("Constant Velocity", 300);
bool wander=false;
ServerSocket new_sock,*server;
bool mhead=false,mmove=false;//Flags for mapper
bool gripflag=false;

void shutdown(int signum)
{
printf("MyShutDown procedure...\n");
new_sock.close();
server->close();
robot.stopRunning();
Aria::shutdown();
exit(0);
}

int main ( int argc, int argv[] )
{
char data[1024];

std::cout << "running....\n";
  
try
{
// Create the socket
server = new ServerSocket(4000);
bsInit();
Aria::init(Aria::SIGHANDLE_THREAD);
//terminate (Ctrl+C) signal handler

struct sigaction sa;
memset(&sa,0,sizeof(sa));
sa.sa_handler = &shutdown;
sigaction(SIGINT,&sa,NULL);

tcpConn.setPort();
if (tcpConn.openSimple())
{ robot.setDeviceConnection(&tcpConn);}
else 
{ serConn.setPort();
robot.setDeviceConnection(&serConn);
}
robot.addRangeDevice(&sonar);
		
if (!robot.blockingConnect())
{
Aria::shutdown();
return(1);
}
robot.runAsync(true);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SONAR, 1);

while ( true )
{
server->accept ( new_sock );
new_sock.set_non_blocking(true);
	  	 
try
{
head=0.0;
while ( true )
{
std::string data;
		  
new_sock >> data;
process(data);
std::string sample;
char cpacket[120],csonar[120];
robot.lock();  
int num=robot.getNumSonar();
for(int i=0;i<num;i++)
{
ArSensorReading *r=robot.getSonarReading(i);
sprintf(csonar,"%.2f|%.2f|",r->getX(),r->getY());
sample+=csonar;
}	  sprintf(cpacket,"%.2f|%.2f|%.2f|%.2f|%.2f|%.1f|%d|%d|",robot.getX(),robot.getY(),robot.getTh(),robot.getVel(),robot.getRotVel(),robot.getBatteryVoltage(),grip.getGripState(),grip.getBreakBeamState());
robot.unlock();
std::string packet(cpacket);
packet+=sample;
new_sock << packet;

//std::cout << data <<endl;
//std::cout << packet <<endl;
ArUtil::sleep(300);
robot.lock();
if((mhead)&&(robot.isHeadingDone(2.0)))
{ new_sock << "headdone"; mhead=false;
robot.stop();robot.setRotVel(0); ArUtil::sleep(300);
if(sound) bsSpeak("Heading Done.");
}
if(mmove)
{
if (robot.isMoveDone())
{ new_sock << "movedone"; mmove=false;
robot.stop();robot.setVel(0); ArUtil::sleep(300);
if(sound) bsSpeak("Move Done.");
}
double r = sonar.currentReadingPolar(-60,60) 
robot.getRobotRadius();

if((r>0)&&(r<400))
{ new_sock << "errordone"; mmove=false;mhead=false;
robot.stop();robot.setVel(0); ArUtil::sleep(300);
if(sound) bsSpeak("Unvoidable Obstacle, terminating 
map action.");
}
}
robot.unlock();
if (gripflag)
{
int gstate=grip.getBreakBeamState();
if (gstate!=0)
{
robot.lock(); robot.setVel(0); 
robot.setRotVel(0);robot.unlock();
if (sound) bsSpeak("Found Object between 
paddles, stopping to grab it");
while(!grip.gripClose()){ }
ArUtil::sleep(100);
bool gmove=true;
while(gmove)
{
ArUtil::sleep(100);
gmove=grip.isGripMoving();
}
while(!grip.liftUp()){ }
ArUtil::sleep(100);
while((grip.isGripMoving())||(grip.isLiftMoving( )))
{ ArUtil::sleep(50); }
gripflag=false;
robot.lock();
robot.clearDirectMotion();
robot.unlock();
 }
}
}//while
}
catch ( SocketException& ) { new_sock.close(); }

}
}
catch ( SocketException& e )
{
std::cout << "Exception was caught:" << e.description() << 
"\nExiting.\n";
}
//robot.stopRunning();
Aria::shutdown();

return 0;
}

int process(std::string s)
{
int ret=0;
for(int i=0;i<s.length();i+=4)
{
string data=s.substr(i,4);
if(data=="HEAD") 
{ 
char *p;
float angle=0;
p = strtok((char*)s.c_str(), "|");
p = strtok(NULL, "|");
sscanf(p,"%f",&angle);
head=angle;
robot.lock();
robot.setMaxRotVel(10.0);
robot.setHeading(head);
robot.unlock();
mhead=true;
if(sound) bsSpeak("Activating Rotate");
ret++;
}
else if(data=="MOVE")
{
char *p;
float dist=0;
p = strtok((char*)s.c_str(), "|");
p = strtok(NULL, "|");
sscanf(p,"%f",&dist);
robot.lock();
robot.setMaxTransVel(200.0);
robot.move(dist);
robot.unlock();
mmove=true;
if(sound) bsSpeak("Activating Move");
ret++;
}
else if(data=="HALT")
{ robot.lock();robot.stop();
robot.setVel(0);robot.setRotVel(0);
robot.unlock();mhead=false;mmove=false;
if(sound) bsSpeak("Stopping Motion");
}
else if(data=="UMOV") { grip.liftUp();if(sound) bsSpeak("Lift moving Up");ret++;}
else if(data=="DMOV") {grip.liftDown();if (sound) bsSpeak("Lift moving Down");ret++;}
else if(data=="GOPN") {grip.gripOpen();if (sound) bsSpeak("Gripper Opening");ret++;}
else if(data=="GCLS") {grip.gripClose();if (sound) bsSpeak("Gripper Closing");ret++;}
else if(data=="STOP") {grip.gripperHalt();if (sound) bsSpeak("Gripper Stopped");ret++;}
else if(data=="SOFF") {sound=false;ret++;}
else if(data=="SONN") {sound=true;ret++;}
else if(data=="FRWD")
{
robot.lock();robot.move(150.0);robot.unlock();
if ((sound)&&(ret==0)) bsSpeak("Moving Forward");
ret++;
}
else if(data=="MBCK")
{
robot.lock();robot.move(-100.0);robot.unlock();
if ((sound)&&(ret==0)) bsSpeak("Moving Back");
ret++;
}
else if(data=="TLFT")
{	if (ret>2) break;
head+=15.0;
robot.lock();robot.setHeading(head); robot.unlock();
if (sound) bsSpeak("Turn Left");
ret++;
}
else if(data=="TRGT")
{
if (ret>2) break;
head-=15.0;
robot.lock();robot.setHeading(head); robot.unlock();
if (sound) bsSpeak("Turn Right");
ret++;
}
else if(data=="WNDR")
{
if(wander==false)
{	if (sound) bsSpeak("Starting to wander.");
robot.lock();
robot.clearDirectMotion();
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 75);
robot.addAction(&avoidFrontNear, 50);
robot.addAction(&avoidFrontFar, 49);
robot.addAction(&constantVelocity, 25);
robot.unlock();
wander=true;
ret++;
}
}
else if(data=="SWND")
{
if (wander==true)
{	if (sound) bsSpeak("Stopping wander action.");
robot.lock();
robot.remAction(&recover);
robot.remAction(&bumpers);
robot.remAction(&avoidFrontNear);
robot.remAction(&avoidFrontFar);
robot.remAction(&constantVelocity);
robot.stop();
robot.unlock();
wander=false;
ret++;
}
}
else if(data=="GPRD")
{
if (gripflag==false)
{	
if (sound) bsSpeak("Getting ready to grip.");
robot.lock();
while(!grip.liftDown()){ }
while((grip.isGripMoving())||(grip.isLiftMoving()))
{ArUtil::sleep(50); }
while(!grip.gripOpen()){ }
while((grip.isGripMoving())||(grip.isLiftMoving()))
{ ArUtil::sleep(50); }
robot.unlock();
gripflag=true;
ret++;
}
}
bsFinishSpeaking();
}
if ((ret==0)&&(s!="null"))  bsSpeak((char *)s.c_str());
bsFinishSpeaking();
return(ret);
}
