Please find below the following programs:

 

1)  Master

2)  Client

3)  Configuration file

4)  player output

5)  playerv output

Can someone help with the effort to communicate with two or more robots.  The sample code is listed below.  The problem that I'm having is with both robots communcating on the same port (port 6665).  It listens on 6665, accepts client 0 on port 6665,  fd 5 ,      

Connected to TSU_230, a Pioneer p3dx-sh
accepted client 1 on port 6665, fd 7
warning : other error on client 1
closing connection to client 1 on port 6665


As a result of this,  one robot will just wanders around and the other one just sits  there.  Why is client 1 not moving?

Can someone be of assistance in telling me what I am doing wrong or what the problem is?


 

 
 

 **************************
===== Master Program =====

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <cstring>

using namespace std;

int main(int argc, char *argv[]) // argc = argument count,  gcc -o myprog myprog.c,  argc = 4
{                                // argv = argument vector, argv[0] = gcc  argv[1] = -o ....
   using namespace PlayerCc;

if (argc < 2)
    {
    cout << "Usage: ./sonar <port>\n\n";
    cout << "Port 6665 is typical\n\n";
    exit(1);
    }

//atoi - convert a string to an integer
PlayerClient robot("localhost", atoi(argv[1]));
//PlayerClient robot("cooprobot8");
//This proxy gets sonar readings from the server
SonarProxy sp(&robot,0);

//The position proxy gives us information about our
//x,y position and heading

Position2dProxy pp(&robot,0);

//flags indicate which side an obstacle is on
int right, left;

int turn_count = 0;

double newrotation, newspeed;

//srand - seed (initalize) the random number generator
//a small amount of randomness prevents oscillation that gets us stuck

time_t seconds;       //Get the current calendar time as a time_t object
srand((unsigned int) seconds);

pp.SetMotorEnable (true);    // Enable the motors


for(;;)  
    {
    //get latest batch of sensor readings from the proxy

    robot.Read();


    //  print out sonars for fun
    std::cout << sp << std::endl;

      newrotation = 0;
    newspeed = .25;
    right = 0;
    left = 0;

    //check for obstacles on right and left sonar sensors.
    //allows obstacles fairly close to edges, but not in front
    //this permits passing narrow doors

    if ((sp[1] < 2) || (sp[2] < 2) || (sp[3] < 3))
        right = 1;

    if ((sp[4] < 3) || (sp[5] < 2) || (sp[6] < 2))
        left = 1;

    if (left)
        {
        newspeed = .1;
        newrotation = .5;
        }

    if (right)
        {
        newspeed = .1;
        newrotation = -.5;
        }

    //if the obstacle is in front, tend to turn left mostly
    //but 30% of the time turn right to prevent oscillation

    if (left && right)
        {
        if ((rand() % 10 + 1) > 7)  // rand() % 10 + 1 is in the range from 1 to 10
            newrotation = -.5;
        else
            newrotation = .5;

        newspeed = -.2;

        //in this case, turn a while... don't want to oscillate

        turn_count = 18;
        pp.SetSpeed(newspeed, newrotation);
        }

    //only update if we aren't turning from a front obstacle
    if (turn_count == 0)
        pp.SetSpeed(newspeed, newrotation);

    else
        {
        turn_count --;
        }
  }
}


**************************
===== Client Program =====

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <cstring>
#include <math.h>

using namespace std;

struct point {

double x;
double y;

};

//computes the bearing from a robot's point to it's target
double compute_bearing(point, point);

//computes distance from a robot's point to a target
double compute_distance(point, point);

//computes a robot's position in the diamond formation
//relative to the master's position

point compute_destination(point, double);

//the port we're running on. Used to select our offset and place
//in the diamond
int port;

int main(int argc, char *argv[]) // argc = argument count
{                                // argv = argument vector

if (argc < 2)
    {
    cout << "Usage: ./sonar <port>\n\n";
    cout << "Port 6666=6667 is typical\n\n";
    exit(1);
    }

//atoi - convert a string to an integer
port = atoi(argv[1]);     // argv[1] = assigns the second input argument to port

//our connection to the world server
PlayerClient robot("cooprobot5", port);

//This connects to the master robot, always on 6665
PlayerClient master_robot("cooprobot8", 6665);

//This proxy gets sonar readings from the server
SonarProxy sp(&robot,0);

//The position proxy gives us information about our
//x,y position and heading

PositionProxy pp(&robot,0);

//The position proxy gives us information about the
//master's position and heading

PositionProxy mpp(&master_robot);

//points holding our current position and our current target position
point me, target;

//assorted movement information
double newrotation, newspeed, bearing, heading, distance, target_speed, target_theta;

//our offset from the master's starting position at startup

double my_x_offset, my_y_offset;

//These offsets are the values needed to adjust the robot's coordinate system
//to that of the master, based on their starting condition.
//MUST MATCH THE WORLD FILE or confusion will ensue

switch (port)
    {
    case 6666: my_y_offset = 1.0;
           my_x_offset = 0.0;
           break;

    case 6667: my_y_offset = 2.0;
           my_x_offset = 0.0;
           break;

    case 6668: my_y_offset = 3.0;
           my_x_offset = 0.0;
           break;    
    };

while(1)  
    {
    //get latest batch of sensor readings from the proxy

    if(robot.Peek())
              exit(1);

    //get position proxy info from master for updated position
    if(master_robot.Peek())
              {
        cout << "Failed to connect to master robot's odometry proxy.\n";
        exit(1);
        }

    //assign our position
    me.x = pp.xpos;
    me.y = pp.ypos;

    //set master's position and heading
    target.x = mpp.xpos;
    target.y = mpp.ypos;
    target_theta = mpp.theta;

    //figure out our new target point
    target = compute_destination(target, mpp.theta);
    

    //adjust to match our coordinate system
    target.x -= my_x_offset;
    target.y -= my_y_offset;

    //we'll set speed relative to how far out we are
    distance = compute_distance(me, target);

    //get the heading we must drive to reach target
    bearing = compute_bearing(me, target);

    //figure out which way we are currently heading
    heading = pp.theta;

    //turn at variable rate to match heading to bearing
    //turn faster if they are far apart, slowly to avoid
    //overshooting if close

    if (bearing < heading)
        {
        newrotation = -1.0 * ((3 * fabs(bearing - heading)));
        }

    if (bearing > heading)
        {
        newrotation = ((3 * fabs(bearing - heading)));
        }

    //if distance within tolerance, stop. Else, speed is set
    //relative to distance + base rate of .2 to match master
    //prevents oscillation and allows pulling to a smooth halt

    if (distance > .05)
        {
        newspeed = .2 + (distance * .5);
        }
    else
        newspeed = 0;

    pp.SetSpeed(newspeed, newrotation);
  }
}

double compute_bearing(point me, point target)
{

double x,y, theta;

x = target.x - me.x;
y = target.y - me.y;

theta = atan2(y,x);

return theta;
}

double compute_distance(point me, point target)
{

double x, y;

x = target.x - me.x;
y = target.y - me.y;

return sqrt((x * x) + (y * y));
}

point compute_destination(point target, double theta)
{

double x, y;
double new_x, new_y;
point new_target;

//assume target at the origin. Calculate x,y offsets for a rotation of a
//point about the origin.

//our target point is 1 unit below the master (target.y - 1)
//if we are rotating around origin, x = 0, y = -1

//set up for 4 part diamond

switch (port)
    {
    case 6666: x = -1.0;
           y = -1.0;
           break;

    case 6667: x = -1.0;
           y = 1.0;
           break;

    case 6668: x = -2.0;
           y = 0.0;
           break;

    };


new_x =  x * cos(theta) - y * sin(theta);
new_y = x * sin(theta) + y * cos(theta);

//shift back from origin to original position

new_target.x = new_x + target.x;
new_target.y = new_y + target.y;

return new_target;

}



******************************
===== Configuration File =====   (on both machines)


driver
(
  name "p2os"
  provides ["odometry:::position2d:0"
            "compass:::position2d:1"
            "gyro:::position2d:2"
            "sonar:0"
            "power:0"]
  port "/dev/ttyUSB0"
)




**************************************
===== Output from Running player =====

[root@cooprobot8 ps]# player -d 9 pioneer3_2.cfg

* Part of the Player/Stage/Gazebo Project [http://playerstage.sourceforge.net].
* Copyright (C) 2000 - 2006 Brian Gerkey, Richard Vaughan, Andrew Howard,
* Nate Koenig, and contributors. Released under the GNU General Public License.
* Player comes with ABSOLUTELY NO WARRANTY.  This is free software, and you
* are welcome to redistribute it under certain conditions; see COPYING
* for details.

Listening on ports: 6665
accepted client 0 on port 6665, fd 5
P2OS connection opening serial port /dev/ttyUSB0...Connected to robot device, handshaking with P2OS...turning off NONBLOCK mode...
Done.
   Connected to TSU_2390, a Pioneer p3dx-sh
accepted client 1 on port 6665, fd 7
warning : other error on client 1
closing connection to client 1 on port 6665
accepted client 1 on port 6665, fd 7
warning : other error on client 1
closing connection to client 1 on port 6665
P2OS has been shutdown
read() read zero bytes
failed to read from client 0
closing connection to client 0 on port 6665
Quitting.


===== Output from Running playerv =====
[rcole@cooprobot8 ps]$ playerv --position2d:0 --sonar:0 --power:0
PlayerViewer 2.0.3
Connecting to [localhost:6665]
calling connect
done
Available devices: localhost:6665
position2d:0     p2os                                    subscribed
position2d:1     p2os                                    ready
position2d:2     p2os                                    ready
sonar:0          p2os                                    subscribed
power:0          p2os                                    subscribed