多机器人操作平台

今天实际测试了一个自己以前在仿真环境下写好的多机器人操作，内容是一个控制，一个漫游。又一次小有成就感。哈哈~~

 

代码如下：

#include "Aria.h"

ArActionGroup *teleop;

 

int main(int argc, char** argv)

{

  int ret;

 

  std::string str;

 

  // get hostnames and port numbers for connecting to the robots.

  ArArgumentParser argParser(&argc, argv);

  char* host1 = argParser.checkParameterArgument("-rh1");

  if(!host1) host1 = "localhost";

  char* host2 = argParser.checkParameterArgument("-rh2");

  if(!host2) host2 = "localhost";

 

  int port1 = 8101;

  int port2 = 8101;

  if(strcmp(host1, host2) == 0 )

  {

    // same host, it must be using two ports (but can still override below with -rp2)

    port2++;

  }

 

  bool argSet = false;

  argParser.checkParameterArgumentInteger("-rp1", &port1, &argSet); 

  if(!argSet) argParser.checkParameterArgumentInteger("-rrtp1", &port1);

  argSet = false;

  argParser.checkParameterArgumentInteger("-rp2", &port2, &argSet);

  if(!argSet) argParser.checkParameterArgumentInteger("-rrtp2", &port2);

 

  if(!argParser.checkHelpAndWarnUnparsed())

  {

    ArLog::log(ArLog::Terse, "Usage: twoRobotWander [-rh1 <hostname1>] [-rh2 <hostname2>] [-rp1 <port1>] [-rp2 <port2>]/n"/

      "/t<hostname1> Is the network host name of the first robot."/

          " Default is localhost (for the simulator)./n"/

      "/t<hostname2> Is the network host name of the second robot."/

          " Default is localhost (for the simulator)./n"/

      "/t<port1> Is the TCP port number of the first robot. Default is 8101./n"/

      "/t<port2> Is the TCP port number of the second robot. Default is 8102 if"/

        " both robots have the same hostname, or 8101 if they differ./n/n");

    return 1;

  }

 

 

 

  //

  // The first robot's objects

  //

  // the first robot connection

  ArTcpConnection con1;

  // the first robot

  ArRobot robot1;

  // sonar, must be added to the first robot

  ArSonarDevice sonar1;

  ArPose pose1;

 

 

 

  //

  // The second robot's objects

  //

  // the second robot connection

  ArTcpConnection con2;

  // the second robot

  ArRobot robot2;

  // sonar, must be added to the second robot

  ArSonarDevice sonar2;

  // the actions we'll use to wander for the second robot

  ArActionStallRecover recover2;

  ArActionBumpers bumpers2;

  ArActionAvoidFront avoidFront2;

  ArActionConstantVelocity constantVelocity2("Constant Velocity", 400);

 

  // mandatory init

  Aria::init();

 

  //

  // Lets get robot 1 going

  //

 

  // open the connection, if this fails exit

  ArLog::log(ArLog::Normal, "Connecting to first robot at %s:%d...", host1, port1);

  if ((ret = con1.open(host1, port1)) != 0)

  {

    str = con1.getOpenMessage(ret);

    printf("Open failed to robot 1: %s/n", str.c_str());

    Aria::shutdown();

    return 1;

  }

 

  // add the sonar to the robot

  robot1.addRangeDevice(&sonar1);

 

  // set the device connection on the robot

  robot1.setDeviceConnection(&con1);

 

  // try to connect, if we fail exit

  if (!robot1.blockingConnect())

  {

    printf("Could not connect to robot 1... exiting/n");

    Aria::shutdown();

    return 1;

  }

 

  // turn on the motors, turn off amigobot sounds

  robot1.comInt(ArCommands::ENABLE, 1);

  robot1.comInt(ArCommands::SOUNDTOG, 0);

 

    /* - the action group for teleoperation actions: */

  teleop = new ArActionGroup(&robot1);

 

  // don't hit any tables (if the robot has IR table sensors)

  teleop->addAction(new ArActionLimiterTableSensor, 100);

 

  // limiter for close obstacles

  teleop->addAction(new ArActionLimiterForwards("speed limiter near",

                        300, 600, 250), 95);

 

  // limiter for far away obstacles

  teleop->addAction(new ArActionLimiterForwards("speed limiter far",

                           300, 1100, 400), 90);

 

  // limiter so we don't bump things backwards

  teleop->addAction(new ArActionLimiterBackwards, 85);

 

  // the keydrive action (drive from keyboard)

  teleop->addAction(new ArActionKeydrive, 45);

 

 

 

 

  /* - connect to the robot, then enter teleoperation mode.  */

 

  robot1.addRangeDevice(&sonar1);

 

 

  robot1.enableMotors();

  teleop->activate();

 

 

 

 

  //

  // Lets get robot 2 going

  //

 

  // open the connection, if this fails exit

  ArLog::log(ArLog::Normal, "Connecting to second robot at %s:%d...", host2, port2);

  if ((ret = con2.open(host2, port2)) != 0)

  {

    str = con2.getOpenMessage(ret);

    printf("Open failed to robot 2: %s/n", str.c_str());

    Aria::shutdown();

    return 1;

  }

 

  // add the sonar to the robot

  robot2.addRangeDevice(&sonar2);

 

  // set the device connection on the robot

  robot2.setDeviceConnection(&con2);

 

  // try to connect, if we fail exit

  if (!robot2.blockingConnect())

  {

    printf("Could not connect to robot 2... exiting/n");

    Aria::shutdown();

    return 1;

  }

 

  // turn on the motors, turn off amigobot sounds

  robot2.comInt(ArCommands::ENABLE, 1);

  robot2.comInt(ArCommands::SOUNDTOG, 0);

 

  // add the actions

  robot2.addAction(&recover2, 100);

  robot2.addAction(&bumpers2, 75);

  robot2.addAction(&avoidFront2, 50);

  robot2.addAction(&constantVelocity2, 25);

 

  // start the robots running. true so that if we lose connection to either

  // robot, the run stops.

  robot1.runAsync(true);

  robot2.runAsync(true);

  ArUtil::sleep(1000000);

 

 

  // now exit

  Aria::shutdown();

  return 0;

}