int main(int argc, char* argv[])
{
// Whether to print verbose output - default off
bool verb = false;
// Whether to manually load Libraries - default off
bool init = false;
// Name of desired library
std::string libName = "bodyinfo";
// Name of desired module in library
std::string moduleName = "BodyInfo";
// Explicit library path
std::string libPath = "";
// Set broker name, ip and port, finding first available port from 54000
const std::string brokerName = "MotionDerivativeBroker";
int brokerPort = qi::os::findAvailablePort(54000);
const std::string brokerIp = "0.0.0.0";
// Default parent port and ip
int pport = 9559;
std::string pip = "127.0.0.1";
// Default time to run in seconds
int timeToRun = 10;
// Get any arguments
while (true)
{
static int index = 0;
// Struct of options
// Columns are:
// {Option name, Option required?, flag(not needed here), return value/character}
static const struct option longopts[] =
{
{"pip", 1, 0, 'i'},
{"pport", 1, 0, 'p'},
{"path", 1, 0, 'x'},
{"time", 0, 0, 't'},
{"verb", 0, 0, 'v'},
{"help", 0, 0, 'h'},
{0, 0, 0, 0 }
};
// Get next option, and check return value
switch(index = getopt_long(argc, argv, "i:p:t:vh", longopts, &index))
{
// Print usage and quit
case 'h':
argErr();
break;
// Set parent IP
case 'i':
if (optarg)
pip = std::string(optarg);
else
argErr();
break;
// Set parent port
case 'p':
if (optarg)
pport = atoi(optarg);
else
argErr();
break;
case 'x':
if (optarg)
libPath = std::string(optarg);
else
argErr();
break;
case 't':
if (optarg)
timeToRun = atoi(optarg);
else
argErr();
break;
case 'v':
verb = true;
break;
}
if (index == -1)
break;
}
// Need this for SOAP serialisation of floats to work
setlocale(LC_NUMERIC, "C");
// Create a broker
if(verb)
std::cout << bold_on << "Creating broker..." << bold_off << std::endl;
boost::shared_ptr<AL::ALBroker> broker;
try
{
broker = AL::ALBroker::createBroker(
brokerName,
brokerIp,
brokerPort,
pip,
pport,
0);
}
// Throw error and quit if a broker could not be created
catch(...)
{
std::cerr << "Failed to connect broker to: "
<< pip
<< ":"
<< pport
<< std::endl;
AL::ALBrokerManager::getInstance()->killAllBroker();
AL::ALBrokerManager::kill();
return 1;
}
// Add the broker to NAOqi
AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
AL::ALBrokerManager::getInstance()->addBroker(broker);
// Create an instance of the desired module
if (libPath == "")
CreateModule(libName, moduleName, broker, verb, true);
else
CreateModule(libPath, moduleName, broker, verb, false);
// Create a proxy to the module
if(verb)
std::cout << bold_on << "Creating proxy to module..." << bold_off << std::endl;
AL::ALProxy bodyProxy(moduleName, pip, pport);
// Get COM angles as vector of [upper body, lower body]
std::vector<float> COMAngles = bodyProxy.genericCall("getSittingCOMAngles", 0);
// Get start time of simulation and store it
qi::os::timeval startTime;
qi::os::timeval currentTime;
qi::os::gettimeofday(&startTime);
qi::os::gettimeofday(¤tTime);
// Run for time "timeToRun"
while (currentTime.tv_sec - startTime.tv_sec < timeToRun)
{
std::cout << std::setw(15) << std::left << "Upper Body:" << COMAngles[0] << std::endl;
std::cout << std::setw(15) << std::left << "Lower Body:" << COMAngles[1] << std::endl;
qi::os::sleep(1);
qi::os::gettimeofday(¤tTime);
COMAngles = bodyProxy.genericCall("getSittingCOMAngles", 0);
}
// Get a handle to the module and close it
{
boost::shared_ptr<AL::ALModuleCore> module = broker->getModuleByName(moduleName);
if(verb)
std::cout << bold_on << "Closing module " << moduleName << "..." << bold_off << std::endl;
module->exit();
}
// Check module has closed
if(verb)
{
std::cout << bold_on << "Module " << moduleName << " is ";
if (!(broker->isModulePresent(moduleName)))
{
std::cout << "not ";
}
std::cout << "present" << bold_off << std::endl;
}
// Close the broker
if(verb)
std::cout << bold_on << "Closing broker..." << bold_off << std::endl;
broker->shutdown();
// Exit program
if(verb)
std::cout << bold_on << "Exiting..." << bold_off << std::endl;
return 0;
}
/**
* @ingroup shell
*
* Shell command (timeserver).
* @param nargs number of arguments in args array
* @param args array of arguments
* @return 0 for success, 1 for error
*/
shellcmd xsh_timeserver(int nargs, char *args[])
{
int descrp, port, i;
/* parse arguments to find port number */
if ((2 == nargs) && (strcmp(args[1], "--help") == 0))
{
printf("Usage: %s [-d device] [-p port]\n\n", args[0]);
printf("Description:\n");
printf("\tSpawns a time server, providing remote system time\n");
printf("Options:\n");
printf
("\t-d device\tdevice to listen for traffic. (default: ETH0)\n");
printf
("\t-p port\t\tport on which the server listens. (default: %d)\n",
UDP_PORT_RDATE);
printf("\t--help\t\tdisplay this help information and exit\n");
return 0;
}
if (nargs > 4)
{
fprintf(stderr, "%s: too many arguments\n", args[0]);
fprintf(stderr, "Try %s --help for usage\n", args[0]);
return SHELL_ERROR;
}
/* assume default device and port */
descrp = (ethertab[0].dev)->num;
port = UDP_PORT_RDATE;
/* set user options if specified */
for (i = 1; i < nargs; i++)
{
if (strcmp(args[i], "-d") == 0)
{
i++;
if (i >= nargs)
return argErr(args[0], "");
descrp = getdev(args[i]);
}
else if (strcmp(args[i], "-p") == 0)
{
i++;
if (i >= nargs)
return argErr(args[0], "");
port = atoi(args[i]);
}
else
{
return argErr(args[0], args[i]);
}
}
/* verify the device is valid */
if (isbaddev(descrp))
{
fprintf(stderr, "%s: invalid device.\n", args[0]);
fprintf(stderr, "Try %s --help for usage\n", args[0]);
return SHELL_ERROR;
}
/* verify the provided port is a valid number */
if (port <= 0)
{
fprintf(stderr, "%s: invalid port\n", args[0]);
fprintf(stderr, "Try %s --help for usage\n", args[0]);
return SHELL_ERROR;
}
ready(create((void *)timeServer, INITSTK, INITPRIO, "TimeServer",
2, descrp, port), RESCHED_YES);
return SHELL_OK;
}
