goalclient.cpp

sonarnl localization and path planning by mobile robots

/*

MobileRobots Advanced Robotics Navigation and Localization (ARNL)
Copyright (C) 2004, 2005, ActivMedia Robotics LLC.
Copyright (C) 2006, 2007, MobileRobots Inc.
All Rights Reserved

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

The license for this software is distributed as LICENSE.txt in the top
level directory.

[e-mail]
MobileRobots
19 Columbia Drive
Amherst, NH 03031
800-639-9481

*/



#include "Aria.h"
#include "ArNetworking.h"



void handlePathPlannerStatus(ArNetPacket *packet)
{
  char buf[64];
  packet->bufToStr(buf, 63);
  printf(".. Path planner status: \"%s\"\n", buf);
}


void handleGoalName(ArNetPacket* packet)
{
  char buf[64];
  packet->bufToStr(buf, 63);
  printf(".. Current goal: \"%s\"\n", buf);
}

void handleRobotUpdate(ArNetPacket* packet)
{
  char buf[64];
  packet->bufToStr(buf, 63);
  printf(".. Robot server status: \"%s\"\n", buf);
  packet->bufToStr(buf, 63);
  printf(".. Robot server mode: \"%s\"\n", buf);
}


void handleGoalList(ArNetPacket *packet)
{
  printf(".. Server has these goals:\n");
  char goal[256];
  for(int i = 0; packet->getReadLength() < packet->getLength(); i++)
  {
    packet->bufToStr(goal, 255);
    if(strlen(goal) == 0)
        return;
    printf("      %s\n", goal);
  }
}
    
int main(int argc, char **argv)
{
  Aria::init();
  ArClientBase client;
  ArArgumentParser parser(&argc, argv);
  ArClientSimpleConnector clientConnector(&parser);
  parser.loadDefaultArguments();

  if (!clientConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    clientConnector.logOptions();
    exit(0);
  }
  
  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server rejected connection, exiting\n");
    else
      printf("Could not connect to server, exiting\n");
    
    exit(1);
  } 

  printf("Connected to server.\n");
  client.addHandler("pathPlannerStatus", new ArGlobalFunctor1<ArNetPacket*>(&handlePathPlannerStatus));
  client.addHandler("update", new ArGlobalFunctor1<ArNetPacket*>(&handleRobotUpdate));
  client.addHandler("goalName", new ArGlobalFunctor1<ArNetPacket*>(&handleGoalName));
  client.addHandler("getGoals", new  ArGlobalFunctor1<ArNetPacket*>(&handleGoalList));
  client.runAsync();
  client.requestOnce("getGoals");
  client.request("pathPlannerStatus", 5000);
  client.request("goalName", 5000);
  client.request("update", 5000);
  while(client.getRunningWithLock())
  {
    char goal[128];
    printf("=> Enter a goal name.\n");
    if( fgets(goal, 127, stdin) == NULL )
    {
        // EOF
        printf("goodbye.\n");
        Aria::shutdown();
        return 0;
    }
    if(goal[0] == '?')
    {
      client.requestOnce("getGoals");
      printf("=> Enter a goal name.\n");
      continue;
    }
    goal[strlen(goal)-1] = '\0';  // remove newline
    printf("=> Sending goal \"%s\" to server...\n", goal);
    client.requestOnceWithString("gotoGoal", goal);
  }
  printf("Server disconnected.\n");
  Aria::shutdown();
  return 0;
}