// Various  codes for navigator client program
Listing of Client.java

/* this class handles socket communication 
required by both the thread receiving image from
 the image server and the thread controlling the robot motion 

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
 
*/

import java.net.*;
import java.io.*;

class Client
{
Socket s;//Socket object for tcp communication
InputStream in;//Reader for the socket
OutputStream out;//Writer for the socket

//Constructor takes a socket
public Client (Socket s)
{ this.s=s; }

public boolean set()
{
System.out.println ("Client. set () ");
try{ 
in = s.getInputStream();
out = s.getOutputStream();
}catch(IOException e){return(false); }
return(true);
}

//method to write data from the socket 
public synchronized void  write(String str)
{
try{
out.write(str.getBytes());
out.flush();
}catch(IOException e){System.out.println(e);}
}

//method to read data from the socket 
public synchronized String read()
{String str;
byte[] b=new byte[1024];
int n;
try{
n=in.read(b);
if(n<=0) return(null);
str=new String(b,0,n);
}catch(IOException e) 
{System.out.println("Read Error...");return(null); }
return(str);
} 
//Called before the object is garbage collected 
protected void finalize()
{	try{in.close();out.close();} catch(IOException e){}	
	try{ s.close(); } catch(IOException e) {}
}
}//Client

Listing of Camera.java
/* This class implements methods for connecting to the robots image server. The class is Runnable and runs on a separate thread to receive the rle encoded image form the image server and display it in the upper left corner of the client window */

import java.io.*;
import java.awt.*;
import java.awt.event.*;
import java.awt.image.*;
import javax.swing.*;
import java.util.*;
import java.lang.*;
import javax.swing.event.*;
import java.net .*;

class Camera extends Canvas implements Runnable
{
private Image image;
public MemoryImageSource source;/* this generates a displayable 
image form an array of pixels */
public int pixel[];
public int height;
public int width;
public Socket client=null;
public OutputStream out=null;

public void init(int w,int h)
{//initialises the cameras image object
height=h;
width=w;
pixel=new int[h*w];
int size=w*h;
for(int i=0;i<size;i++) pixel[i]=0;
source= new MemoryImageSource(width,height,pixel,0,width);
source.setAnimated(true);
image=createImage(source);
}

public void paint(Graphics g)
{//this method is required for display of images
	g.drawImage(image,0,0,Color.gray,this);
}

public void setPixel(int x,int y,int red,int green,int blue)
{//set the pixel values of the image array
int offset = y * width + x;
pixel[offset]=(255 << 24)|(red << 16)|(green << 8)|blue;
}
// updates the display after receiving a complete image
public void updateImage(){source.newPixels(0, 0, width, height);}
public void update(Graphics g) { paint(g); }

// the running thread executes this method
public void run()
{/* The thread for receiving images form the image server 
running on the robot */
int count=0;
try
{
client=new Socket("192.168.0.9",4325);
client.setTcpNoDelay(true);
System.out.println("Client connected to the image
 server at port 4324.");
InputStream	in = client.getInputStream();
out=client.getOutputStream();
System.out.println("Opening input stream.");
while(true)
{
int val=0,prev=0,kcount=0,k=0,col=0,row=0;
while(true)
{	try{
prev=val;
val=in.read();
kcount=in.read(); 
if((kcount==0)&&(val==0))
{ row++;col+=320;
if(row==240)	break;
}
else
for(int i=0;i<kcount;i++)
{
pixel[k]=(255<<24)|(val<<16)|(val<<8)|val;
k++;
}
}catch(Exception e){}
}
updateImage();
if (val==-1) break;
}//while loop
in.close(); 
System.out.println("End OF Stream.");
client.close();
System.out.println("Server shutdown...closing.");
System.exit(0);
}catch(Exception e) {System.out.println(e);}

updateImage();
}//run
}


Listing of Map.java
/* This class handles displaying of the robots position in the grid display and also the sonar range information */
import java.io.*;
import java.awt.*;
import java.awt.event.*;
import java.awt.image.*;
import javax.swing.*;
import java.util.*;
import java.lang.*;
import javax.swing.event.*;
import java.net .*;
import java.awt.geom.*;


class Map extends Canvas 
{
private Image image;
private boolean init=true;
private Robot robot;
public Color color=Color.red;
public Vector points;
public void setRobot(Robot r){ robot=r;}
public void setPoints(Vector r){ points=r ;}
	
// display for the robot position and sonar sensor in//formation
public void paint(Graphics g)
{
image=createImage(getWidth(),getHeight());
Graphics grph=image.getGraphics();
Graphics2D g2D=(Graphics2D)grph;
g2D.setColor(Color.darkGray);
for(int i=0;i<getSize().width;i+=5) 
  g2D.drawLine(i,0,i,getSize().height-1);
for(int i=0;i<getSize().height;i+=5) 
  g2D.drawLine(0,i,getSize().width-1,i);
grph.setColor(Color.red);
grph.drawLine(0,0,0,getSize().height-1);
//grph.drawString(getSize().toString(),50,50);
grph.setColor(Color.green);
if (robot.sonar.size()!=0)
{
grph.setColor(color);
for (int i=0;i<robot.sonar.size();i++)
{
Rectangle2D 
gen=(Rectangle2D)robot.sonar.get(i);
((Graphics2D)grph).draw(gen);
}
}
if (!points.isEmpty())
{	grph.setColor(Color.yellow);
Line2D line=new 
Line2D.Double(((Point)points.firstElement()).x,((Point)points.firstElement()).y,robot.getPosition().x,robot.getPosition().y);
 ((Graphics2D)grph).draw(line);
for (int i=0;i<points.size()-1;i++)
{
line=new Line2D.Double(((Point)points.get(i)).x,((Point)points.get(i)).y,
((Point)points.get(i+1)).x,((Point)points.get(i+1)).y);
((Graphics2D)grph).draw(line);
}
}

if (robot.first) 
{	robot.setPosition(getSize().width/2-
10,getSize().height/2-10);
robot.setO(getSize().width/2-
10,getSize().height/2-10);
robot.first=false;
}
grph.drawImage(robot.getImage(),robot.getX(),robot.getY(),null);
grph.setColor((Color)robot.traceColor);
grph.dispose();
Graphics2D g2=(Graphics2D)g;
g2.drawImage((BufferedImage)image,null,0,0);
}//paint

public void update(Graphics g) { paint(g); }
}

Listing of Robot.java
/* This class implements methods for drawing the robot and the sonar sensor reading on the display */
import java.awt.*;
import java.awt.event.*;
import java.awt.image.*;
import java.awt.geom.*;
import java.lang.*;
import java.util.*;

class Robot
{
private Image body;
private Map disp=null;
private double curX=100.0,curY=100.0,oX,oY;
public String name;
public Color color=Color.yellow;
public static int count=1;
public double direction=0.0;
public boolean status = false;
public boolean first=true;
public boolean pathTrace=true;
public Color traceColor=Color.green;
public Vector pointStore=new Vector();
public int pathLength=20;
public Vector sonar=new Vector();
 
public Robot()
{
name=new String("Robot "+count);
count++;
for(int i=0;i<16;i++)
sonar.add(new Rectangle2D.Double(9,9,2,2));
body=draw();
}
Robot(String n,Color c)
{
name=n;
color=c;
body=draw();
}

public int getX(){ return((int)(curX-10.0));}
public int getY(){ return((int)(curY-10.0));}
public Image getImage() {return(body);}
public Image updateImage() {return(body=draw());}

public boolean isOn(){return(status);}
public void setDisp(Map p){ disp=p;}
public void setPosition(double x,double y)
{
curX=x; curY=y;
if(disp!=null) disp.repaint();
}
public void setO(double x,double y)
{
oX=x; oY=y;
}
public Point getPosition()
{    return(new Point((int)curX,(int)curY));	}
 
public void setDirection(double d)
{
direction=d;
body=draw();
if(disp!=null) disp.repaint();
}
public double getDirection(){return(direction);}

public void updateStatus(double y,double x,double th)
{
x=x/30.0;y=y/30.0;
double maxx=474.0;
double maxy=575.0;
x=(maxx/2)-x;y=(maxy/2)-y;
while(x<0){ x+=maxx;}
while(y<0){ y+=maxy;}
while(x>maxx){x-=maxx;}
while(y>maxy){y-=maxy;}
setPosition(x,y);
setDirection(-th);
if(disp!=null) disp.repaint();
}
public void setSonar(int num,double y,double x)
{  
if(num>=16) return;
x=x/30.0;y=y/30.0;
double maxx=474.0;
double maxy=575.0;
x=(maxx/2)-x;y=(maxy/2)-y;
while(x<0){ x+=maxx;}
while(y<0){ y+=maxy;}
while(x>maxx){ x-=maxx;}
while(y>maxy){y-=maxy;}
((Rectangle2D)sonar.get(num)).setRect(x-1,y-1,2,2);
}

public void rotate(double angle)
{
direction+=angle;
body=draw();
if (disp!=null) disp.repaint();
}
public void rotate(double angle,double step)
{
while(true)
{
angle-=step;
if (Math.abs(angle)>Math.abs(step))  { rotate(step); }
else break;
delay(5,0);
Thread.yield();
}
move(angle+step);
}

public void move(double r)
{
if (pathTrace)
{
Point p=new Point((int)curX,(int)curY);
pointStore.add(p);
if(pointStore.size()>pathLength) pointStore.remove(0);
}
double dx=(r * Math.sin(Math.toRadians(direction)));
double dy=(r * Math.cos(Math.toRadians(direction)));
curX+=dx;
curY-=dy;
if (disp!=null) disp.repaint();
}
public void move(double r,double step)
{
while(true)
{
r-=step;
if (r>step)  { move(step); }
else break;
delay(5,0);
Thread.yield();
}
move(r+step);
}

public void delay(int millsec,int nanosec)
{
try { Thread.sleep(millsec,nanosec); }
catch (InterruptedException e) {  }
}//delay

 public String toString(){  return(name);}
 private Image draw()
{//positive angle => clock wise rotation.
Image img=new BufferedImage(20,20,BufferedImage.TYPE_INT_ARGB);
Graphics2D g=(Graphics2D)img.getGraphics();
AffineTransform t=new AffineTransform();
t.setToRotation(Math.toRadians(direction),9.5,9.5);
g.setTransform(t);
g.setColor(Color.red);
g.draw(new Line2D.Float(4,16,14,16));
g.setColor(Color.blue);
g.fill(new Rectangle2D.Float(2,6,3,10));
g.fill(new Rectangle2D.Float(15,6,3,10));
GeneralPath p=new GeneralPath();
p.moveTo(10,5);
p.lineTo(7,15);
p.lineTo(13,15);
p.lineTo(10,5);
g.setColor(color);
g.fill(p);
g.dispose();
return(img);
}//draw()
}


Listing of Navigator.java
/* This is the main class and brings together all the functionalities of the classes. This program spawns two threads one to receive the image from the image server and the other for receiving the state information of the robot. On the main thread the program listens to user events and accordingly commands the motion server */

import java.io.*;
import java.awt.*;
import java.awt.event.*;
import java.awt.image.*;
import javax.swing.*;
import java.util.*;
import java.lang.*;
import javax.swing.event.*;
import java.net .*;

class Navigator extends Frame implements ActionListener,ItemListener,Runnable
{
Button gUp=new Button("Up");
Button gDown=new Button("Down");
Button gClose=new Button("Close");
Button gOpen=new Button("Open");
Button gStop=new Button("Stop");
Button gReady=new Button("Ready");
Button connect=new Button("Halt");
Button go=new Button("Go");
Button say=new Button("Say");
Socket sckt=null;
public Vector mapper=new Vector();
Client robotConn;

double angle=0,dist=0;
		
Checkbox selSpk=new Checkbox("Commentary",true);
TextField txtSpk=new TextField("Type here to activate robot's 
speech synthesis system",30);
Label lblX=new Label("0");
Label lblY=new Label("0");
Label lblHead=new Label("0");
Label paddleState=new Label("Closed");
Label lblVel=new Label("1000");
Label lblRvel=new Label("100");
Label lblBatt=new Label("11.8");
ImageIcon red=new ImageIcon("red.gif");
ImageIcon blue=new ImageIcon("blue.gif");
ImageIcon yellow=new ImageIcon("yellow.gif");
ImageIcon green=new ImageIcon("green.gif");
JLabel gBulb=new JLabel(red);
JProgressBar velBar,batt_vol;
JProgressBar rvelBar;
Robot robot=new Robot();

JComboBox nOption;

Camera camera=new Camera();
Map map=new Map();

public Navigator()
{
Box a=new Box(BoxLayout.Y_AXIS);//left panel
Box b=new Box(BoxLayout.Y_AXIS);//camera
Box c=new Box(BoxLayout.X_AXIS);
Box d=new Box(BoxLayout.Y_AXIS);//button panel
Box temp;
camera.setSize(320,240);
camera.init(320,240);
c.add(Box.createVerticalStrut(240));c.add(camera);
b.add(Box.createHorizontalStrut(320));b.add(c);
		
d.add(Box.createVerticalStrut(5));
d.add(new Label("Gripper Controls"));
temp=new Box(BoxLayout.X_AXIS);
temp.add(gUp);temp.add(Box.createHorizontalStrut(5));
  gUp.addActionListener(this);
temp.add(gDown);temp.add(Box.createHorizontalStrut(5));
  gDown.addActionListener(this);
temp.add(gOpen);temp.add(Box.createHorizontalStrut(5));
  gOpen.addActionListener(this);
temp.add(gClose);temp.add(Box.createHorizontalStrut(5));
gClose.addActionListener(this);
temp.add(gReady);temp.add(Box.createHorizontalStrut(5));
gReady.addActionListener(this);
temp.add(gStop);gStop.addActionListener(this);
d.add(temp);
d.add(Box.createVerticalStrut(5));
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Navigate"));
String str[]={ "Teleoperate","Wander","Mapper"};
nOption=new JComboBox(str);
temp.add(nOption);nOption.addActionListener(this);
d.add(temp);
d.add(Box.createVerticalStrut(10));
temp=new Box(BoxLayout.X_AXIS);
temp.add(connect);temp.add(Box.createHorizontalStrut(50));
  temp.add(go);
d.add(temp);connect.addActionListener(this);
  go.addActionListener(this);
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Speech"));
temp.add(selSpk);selSpk.addItemListener(this);
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Speak"));
temp.add(txtSpk);txtSpk.addActionListener(this);
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Robot Cooridinates (x,y):"));
temp.add(lblX);temp.add(new Label(","));temp.add(lblY);
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Robot Heading:"));
temp.add(lblHead);
temp.add(new Label("degrees"));
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
temp.add(new Label("Gripper Status:"));
temp.add(paddleState);
temp.add(gBulb);
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
velBar = new JProgressBar(0,4000);
velBar.setValue(50);
velBar.setStringPainted(true);
temp.add(new Label("Velocity:"));
temp.add(velBar);temp.add(lblVel);
temp.add(new Label("mm/s"));		
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
rvelBar = new JProgressBar(0,500);
rvelBar.setValue(50);
rvelBar.setStringPainted(true);
temp.add(new Label("Rot.Vel:"));temp.add(rvelBar);
temp.add(lblRvel);
temp.add(new Label("deg/s"));		
d.add(temp);
temp=new Box(BoxLayout.X_AXIS);
batt_vol = new JProgressBar(110,140);
batt_vol.setValue(120);
batt_vol.setStringPainted(true);
temp.add(new Label("Batt volt:"));temp.add(batt_vol);
temp.add(lblBatt);
temp.add(new Label("V"));
d.add(temp);
temp=new Box(BoxLayout.X_AXIS); 
temp.add(Box.createRigidArea(new Dimension(10,20)));
temp.add(d);
temp.add(Box.createRigidArea(new Dimension(10,20)));
a.add(b);a.add(Box.createGlue());a.add(temp);
add(a,BorderLayout.WEST);
add(map,BorderLayout.CENTER);
map.setBackground(Color.black);

setTitle("Robot Navigator Module...");
setBackground(Color.black);
gUp.setForeground(Color.black);
gDown.setForeground(Color.black);
gClose.setForeground(Color.black);
gOpen.setForeground(Color.black);
gReady.setForeground(Color.black);
gStop.setForeground(Color.black);
connect.setForeground(Color.black);
go.setForeground(Color.black);
say.setForeground(Color.black);
txtSpk.setForeground(Color.black);
setForeground(Color.white);
setResizable(false);
Dimension dm=Toolkit.getDefaultToolkit().getScreenSize();
setSize(dm.width,dm.height);
addWindowListener(new WindowAdapter()
{  public void windowClosing(WindowEvent we)
{
System.exit(0);
}
});
map.addKeyListener(new KeyAdapter()
{ public void keyPressed(KeyEvent e)
{   map.repaint();
int key=e.getKeyCode();
String label= (String) 
nOption.getSelectedItem();
if(label.equals("Teleoperate"))
{
if(key==KeyEvent.VK_LEFT) 
robotConn.write("TLFT");
if(key==KeyEvent.VK_RIGHT) 
robotConn.write("TRGT");
if(key==KeyEvent.VK_UP) 
robotConn.write("FRWD");
if(key==KeyEvent.VK_DOWN) 
robotConn.write("MBCK");
}
if(key==79) robotConn.write("GOPN");
if(key==67) robotConn.write("GCLS");
if(key==85) robotConn.write("UMOV");
if(key==68) robotConn.write("DMOV");
}
});
map.addMouseListener(new MouseAdapter()
{ public void mousePressed(MouseEvent e)
{ 
String label = (String) 
nOption.getSelectedItem();
if(label.equals("Mapper"))
{   Point p=new Point();
if 
(e.getButton()==MouseEvent.BUTTON3)
{p=new Point(robot.getPosition().x, robot.getPosition().y);
mapper.add(p);}
else if 
(e.getButton()==MouseEvent.BUTTON1)
{ p=new 
Point(near5(e.getX()),near5(e.getY()));mapper.add(p);}
map.repaint();
}
}
});
map.setRobot(robot);
map.setPoints(mapper);
robot.setDisp(map);
try	
{ 
sckt=new Socket("192.168.0.9",4000);
robotConn=new Client(sckt);
robotConn.set();
Thread t=new Thread(this);
t.start();
}
catch (Exception e){System.out.println(e);sckt=null;	}
Thread t=new Thread(camera);
t.start();		
}

/* this method is executed by the running thread which receives state information from the motion server */
public void run()
{
robotConn.write("Pioneer robot is up, and running");
while(true)
{try{
String s=robotConn.read();
if (s==null) continue;
if (s.indexOf("errordone")!=-1)
{
mapper.clear();
}
if (s.indexOf("headdone")!=-1)
{
String si=new String("MOVE|"+dist);
robotConn.write(si);
System.out.println("headdone");				
}
if (s.indexOf("movedone")!=-1)
{
if(mapper.size()>0)
{	Point p=(Point)mapper.firstElement();
mapper.remove(p);
}
if(mapper.size()>0)
{
Point p=(Point)mapper.firstElement();
Point r=robot.getPosition();
double d=Math.toDegrees(Math.atan2(r.x-p.x,r.y-p.y));
double rv=30.0*Math.sqrt((r.x-p.x)*(r.x-p.x)+(r.y-p.y)*(r.y-p.y));
angle=d;
dist=rv;
String si=new String("HEAD|"+angle);
robotConn.write(si);
System.out.println("movedone");
}
continue;
}
StringTokenizer t=new StringTokenizer(s,"|");
String sx,sy,sth;
String v,rv,btt;
sx=t.nextToken();sy=t.nextToken();sth=t.nextToken();
v=t.nextToken(); rv=t.nextToken(); btt=t.nextToken();
double x,y,th;
try{
x=Double.parseDouble(sx);
y=Double.parseDouble(sy);
th=Double.parseDouble(sth);
robot.updateStatus(x,y,th);
}catch(Exception e){}
int vel,rvel,batt;
vel=(int)(Double.parseDouble(v));
rvel=(int)(Double.parseDouble(rv));
batt=(int)(Double.parseDouble(btt)*10.0);
if ((batt/10.0)>13.5) continue;

lblX.setText(sx);
lblY.setText(sy);
lblHead.setText(sth);			
lblVel.setText(v);
lblRvel.setText(rv);
lblBatt.setText(btt);
velBar.setValue(vel);
rvelBar.setValue(rvel);
batt_vol.setValue(batt);
int i,j;
try{
i=Integer.parseInt(t.nextToken());
j=Integer.parseInt(t.nextToken());
}catch(NumberFormatException nfe){i=0;j=0;}
switch(i)
{ case 0:	paddleState.setText("Moving");
break;
case 1:   paddleState.setText("Open");
break;
case 2:	paddleState.setText("Closed");
break;
}
switch(j)
{	
case 0:	gBulb.setIcon(red); break;
case 1:	gBulb.setIcon(blue); break; 
case 2:	gBulb.setIcon(yellow); break;
case 3:	gBulb.setIcon(green); break;
}
for(i=0;i<16;i++)
{
double tx=Double.parseDouble(t.nextToken());
double ty=Double.parseDouble(t.nextToken());
robot.setSonar(i,tx,ty);
}
}catch(Exception e){ System.out.println("Incorrect execution of thread");}
try{ Thread.sleep(300); } catch(Exception e) {}
}//while
}
/* Handle user events generated on the Client Widgets like buttons, drop down combo, check box etc */
	
public void actionPerformed(ActionEvent e)
{
if(e.getActionCommand().equals("comboBoxChanged")) 
{}
else if(e.getActionCommand().equals("Up")) 
robotConn.write("UMOV");
else if(e.getActionCommand().equals("Down")) 
robotConn.write("DMOV");
else if(e.getActionCommand().equals("Open")) 
robotConn.write("GOPN");
else if(e.getActionCommand().equals("Close")) 
robotConn.write("GCLS");
else if(e.getActionCommand().equals("Ready")) 
robotConn.write("GPRD");
else if(e.getActionCommand().equals("Stop")) 
robotConn.write("STOP");
else if(e.getActionCommand().equals("Go"))
{
if (((String)nOption.getSelectedItem()).equals("Wander"))
		robotConn.write("WNDR");
if (((String)nOption.getSelectedItem()).equals("Mapper"))
{	if(mapper.size()>0)
{
Point p=(Point)mapper.firstElement();
Point r=robot.getPosition();
double d=Math.toDegrees(Math.atan2(r.x-p.x,r.y-p.y));
double rv=30.0*Math.sqrt((r.x-p.x)*(r.x-p.x)+(r.y-p.y)*(r.y-p.y));
angle=d;
dist=rv;
String s=new String("HEAD|"+angle);
robotConn.write(s);
}
}
}
else if(e.getActionCommand().equals("Halt"))
{
if (((String)nOption.getSelectedItem()).equals("Wander"))
robotConn.write("SWND");
if 
(((String)nOption.getSelectedItem()).equals("Mapper"))
{robotConn.write("HALT"); mapper.clear();}}
else robotConn.write(e.getActionCommand());
}
public void itemStateChanged(ItemEvent e)
{
if(selSpk.getState()) robotConn.write("SONN"); else 
robotConn.write("SOFF");
}

//Truncates a number to nearest multiple of 5
public static int near5(int n)
{
int fp=n/10;
int dec=n%10;
int add=0;
switch (dec)
{
case 0:
case 1:
case 2:break;
case 3:
case 4:
case 5:
case 6:
case 7:add=5;break;
case 8:
case 9:add=10;           
}
return(fp*10+add);
}
/* This is the entry point of the program. 
	public static void main(String[] args) 
{
Navigator x=new Navigator();
x.pack();
x.show();		
}
}
