/**
* Search all locations for argument defaults and parse them.
* These locations may be environment variables to read argument varues
* from, or files to read.
* @sa addDefaultArgumentLocation
*
* @note If you use this function your normal argc (passed into main()) will
* have been modified, and won't reflect reality anymore. You'll have to use
* getArgc() to get the actual original argument count. This is a little wierd but is
* this way so lots of people don't have to change lots of code.
*/
AREXPORT void ArArgumentParser::loadDefaultArguments(int position)
{
std::list<std::string>::iterator it;
std::list<bool>::iterator bIt;
const char *str;
char *argumentsPtr;
char arguments[1024];
if (!myUsingBuilder)
{
myBuilder = new ArArgumentBuilder;
myBuilder->addStringsAsIs(*myArgc, myArgv);
myOwnBuilder = true;
myUsingBuilder = true;
}
for (it = ourDefaultArgumentLocs.begin(),
bIt = ourDefaultArgumentLocIsFile.begin();
it != ourDefaultArgumentLocs.end();
it++, bIt++)
{
str = (*it).c_str();
// see if its an environmental variable
if (!(*bIt) && (argumentsPtr = getenv(str)) != NULL)
{
ArArgumentBuilder compressed;
compressed.add(argumentsPtr);
compressed.compressQuoted(true);
myBuilder->addStringsAsIs(compressed.getArgc(), compressed.getArgv(),
position);
ArLog::log(ArLog::Normal,
"Added arguments from environmental variable '%s'", str);
}
// see if we have a file
else if ((*bIt) &&
ArUtil::getStringFromFile(str, arguments, sizeof(arguments)))
{
ArArgumentBuilder compressed;
compressed.add(arguments);
compressed.compressQuoted(true);
myBuilder->addStringsAsIs(compressed.getArgc(), compressed.getArgv(),
position);
ArLog::log(ArLog::Normal, "Added arguments from file '%s'",
str);
}
// the file or env didn't exit
// this'll return true otherwise it'll return false)
else
{
ArLog::log(ArLog::Verbose,
"Could not load from environmental variable or file '%s'",
str);
}
}
}
AREXPORT ArArgumentBuilder::ArArgumentBuilder(const ArArgumentBuilder & builder)
{
size_t i;
myFullString = builder.myFullString;
myExtraString = builder.myExtraString;
myArgc = builder.getArgc();
myArgvLen = builder.getArgvLen();
myOrigArgc = myArgc;
myArgv = new char *[myArgvLen];
for (i = 0; i < myArgc; i++)
myArgv[i] = strdup(builder.getArg(i));
}
AREXPORT ArArgumentBuilder::ArArgumentBuilder(const ArArgumentBuilder & builder)
{
size_t i;
myFullString = builder.myFullString;
myExtraString = builder.myExtraString;
myArgc = builder.getArgc();
myArgvLen = builder.getArgvLen();
myOrigArgc = myArgc;
myArgv = new char *[myArgvLen];
for (i = 0; i < myArgc; i++)
myArgv[i] = cppstrdup(builder.getArg(i));
//myArgv[i] = strdup(builder.getArg(i));
myIsQuiet = builder.myIsQuiet;
myExtraSpace = builder.myExtraSpace;
myIgnoreNormalSpaces = builder.myIgnoreNormalSpaces;
myIsPreCompressQuotes = builder.myIsPreCompressQuotes;
}
AREXPORT const std::list<ArArgumentBuilder *> *ArRobotParams::getSonarUnits(void)
{
std::map<int, std::map<int, int> >::iterator it;
int num, x, y, th;
ArArgumentBuilder *builder;
for (it = mySonarMap.begin(); it != mySonarMap.end(); it++)
{
num = (*it).first;
x = (*it).second[SONAR_X];
y = (*it).second[SONAR_Y];
th = (*it).second[SONAR_TH];
builder = new ArArgumentBuilder;
builder->add("%d %d %d %d", num, x, y, th);
myGetSonarUnitList.push_back(builder);
}
return &myGetSonarUnitList;
}
AREXPORT const std::list<ArArgumentBuilder *> *ArRobotParams::getIRUnits(void)
{
std::map<int, std::map<int, int> >::iterator it;
int num, type, cycles, x, y;
ArArgumentBuilder *builder;
for (it = myIRMap.begin(); it != myIRMap.end(); it++)
{
num = (*it).first;
type = (*it).second[IR_TYPE];
cycles = (*it).second[IR_CYCLES];
x = (*it).second[IR_X];
y = (*it).second[IR_Y];
builder = new ArArgumentBuilder;
builder->add("%d %d %d %d %d", num, type, cycles, x, y);
myGetIRUnitList.push_back(builder);
}
return &myGetIRUnitList;
}
int RosAriaNode::Setup()
{
ArArgumentBuilder *args;
args = new ArArgumentBuilder();
args->add("-rp"); //pass robot's serial port to Aria
args->add(serial_port.c_str());
conn = new ArSimpleConnector(args);
robot = new ArRobot();
robot->setCycleTime(1);
ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);
//parse all command-line arguments
if (!Aria::parseArgs())
{
Aria::logOptions();
return 1;
}
// Connect to the robot
if (!conn->connectRobot(robot)) {
ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
return 1;
}
//Sonar sensor
sonar.setMaxRange(Max_Range);
robot->addRangeDevice(&sonar);
robot->enableSonar();
// Enable the motors
robot->enableMotors();
robot->runAsync(true);
return 0;
}
const std::list<ArArgumentBuilder *> *getList(void)
{
std::list<ArArgumentBuilder *>::iterator argIt;
std::list<std::string>::iterator listIt;
ArArgumentBuilder *builder;
if (myArgList.size() != 0)
{
while ((argIt = myArgList.begin()) != myArgList.end())
{
delete (*argIt);
myArgList.pop_front();
}
}
for (listIt = myList.begin(); listIt != myList.end(); listIt++)
{
builder = new ArArgumentBuilder;
builder->add((*listIt).c_str());
myArgList.push_front(builder);
}
return &myArgList;
}
AREXPORT void ArServerHandlerMapping::addStringsForStartOfLogToMap(
ArMap *arMap)
{
std::list<std::string>::iterator it;
std::string str;
ArArgumentBuilder *builder;
for (it = myStringsForStartOfLog.begin();
it != myStringsForStartOfLog.end();
it++)
{
str = (*it);
builder = new ArArgumentBuilder;
builder->add(str.c_str());
if (strcasecmp(builder->getArg(0), "LogInfoStrip:") == 0)
{
builder->removeArg(0, true);
printf("lis %s\n", builder->getFullString());
}
std::list<std::string> infoNames = arMap->getInfoNames();
for (std::list<std::string>::iterator iter = infoNames.begin();
iter != infoNames.end();
iter++)
{
const char *curName = (*iter).c_str();
if (strcasecmp(builder->getArg(0), curName) == 0)
{
builder->removeArg(0, true);
arMap->getInfo(curName)->push_back(builder);
builder = NULL;
break;
}
}
if (builder != NULL)
delete builder;
}
}
AREXPORT void ArClientHandlerConfig::handleGetConfigDefaults(
ArNetPacket *packet)
{
ArLog::log(ArLog::Normal, "%sreceived default config %s",
myLogPrefix.c_str(),
((myHaveRequestedDefaultCopy) ? "(copy)" : "(reset)"));
char param[1024];
char argument[1024];
char errorBuffer[1024];
myDataMutex.lock();
ArConfig *config = NULL;
// If the config (or a section) is being reset to its default values,
// then we don't want to remove any parameters that are not set -- i.e.
// any parameters that are not contained in the default config.
bool isClearUnsetValues = false;
if (myHaveRequestedDefaults) {
config = &myConfig;
}
else if (myHaveRequestedDefaultCopy) {
// If we have requested a copy of the default configuration, then we
// will want to remove any parameters that haven't been explicitly set.
// (This is because of the next line, which copies the current config
// to the default config.)
isClearUnsetValues = true;
// The default config is transmitted in an "abbreviated" form -- just
// the section/param names and values. Copy the current config to the
// default before processing the packet so that the parameter types, etc.
// can be preserved.
if (myDefaultConfig == NULL) {
myDefaultConfig = new ArConfig(myConfig);
myDefaultConfig->setConfigName("Default", myRobotName.c_str());
myDefaultConfig->setQuiet(myIsQuiet);
}
else {
*myDefaultConfig = myConfig;
}
config = myDefaultConfig;
}
// if we didn't ask for any of these, then just return since the
// data is for someone else
else
{
myDataMutex.unlock();
return;
}
if (config == NULL) {
ArLog::log(ArLog::Normal,
"%serror determining config to populate with default values",
myLogPrefix.c_str());
myDataMutex.unlock();
return;
}
ArArgumentBuilder *builder = NULL;
ArLog::log(ArLog::Normal, "%sGot defaults", myLogPrefix.c_str());
errorBuffer[0] = '\0';
//myDataMutex.lock();
if (isClearUnsetValues) {
config->clearAllValueSet();
}
while (packet->getDataReadLength() < packet->getDataLength())
{
packet->bufToStr(param, sizeof(param));
packet->bufToStr(argument, sizeof(argument));
builder = new ArArgumentBuilder;
builder->setQuiet(myIsQuiet);
builder->setExtraString(param);
builder->add(argument);
if ((strcasecmp(param, "Section") == 0 &&
!config->parseSection(builder, errorBuffer, sizeof(errorBuffer))) ||
(strcasecmp(param, "Section") != 0 &&
!config->parseArgument(builder, errorBuffer, sizeof(errorBuffer))))
{
ArLog::log(ArLog::Terse, "%shandleGetConfigDefaults: Hideous problem getting defaults, couldn't parse '%s %s'",
myLogPrefix.c_str(), param, argument);
}
else {
IFDEBUG(if (strlen(param) > 0) {
ArLog::log(ArLog::Normal, "%shandleGetConfigDefaults: added default '%s %s'",
myLogPrefix.c_str(), param, argument); } );
}
delete builder;
builder = NULL;
}
bool ArServerHandlerConfig::internalSetConfig(ArServerClient *client,
ArNetPacket *packet)
{
char param[1024];
char argument[1024];
char errorBuffer[1024];
char firstError[1024];
ArNetPacket retPacket;
ArConfig *config;
bool ret = true;
if (client != NULL)
config = myConfig;
else
config = myDefault;
if (client != NULL)
lockConfig();
ArArgumentBuilder *builder = NULL;
if (client != NULL)
ArLog::log(ArLog::Normal, "Got new config from client %s", client->getIPString());
else
ArLog::log(ArLog::Verbose, "New default config");
errorBuffer[0] = '\0';
firstError[0] = '\0';
while (packet->getDataReadLength() < packet->getDataLength())
{
packet->bufToStr(param, sizeof(param));
packet->bufToStr(argument, sizeof(argument));
builder = new ArArgumentBuilder;
builder->setExtraString(param);
builder->add(argument);
ArLog::log(ArLog::Verbose, "Config: %s %s", param, argument);
// if the param name here is "Section" we need to parse sections,
// otherwise we parse the argument
if ((strcasecmp(param, "Section") == 0 &&
!config->parseSection(builder, errorBuffer, sizeof(errorBuffer))) ||
(strcasecmp(param, "Section") != 0 &&
!config->parseArgument(builder, errorBuffer, sizeof(errorBuffer))))
{
if (firstError[0] == '\0')
strcpy(firstError, errorBuffer);
}
delete builder;
builder = NULL;
}
if (firstError[0] == '\0')
{
if (config->callProcessFileCallBacks(true,
errorBuffer,
sizeof(errorBuffer)))
{
if (client != NULL)
ArLog::log(ArLog::Normal, "New config from client %s was fine.",
client->getIPString());
else
ArLog::log(ArLog::Verbose, "New default config was fine.");
retPacket.strToBuf("");
writeConfig();
}
else // error processing config callbacks
{
ret = false;
if (firstError[0] == '\0')
strcpy(firstError, errorBuffer);
// if its still empty it means we didn't have anything good in the errorBuffer
if (firstError[0] == '\0')
strcpy(firstError, "Error processing");
if (client != NULL)
ArLog::log(ArLog::Normal,
"New config from client %s had errors processing ('%s').",
client->getIPString(), firstError);
else
ArLog::log(ArLog::Normal,
"New default config had errors processing ('%s').",
firstError);
retPacket.strToBuf(firstError);
}
}
else
{
ret = false;
if (client != NULL)
ArLog::log(ArLog::Normal,
"New config from client %s had at least this problem: %s",
client->getIPString(), firstError);
else
ArLog::log(ArLog::Normal,
"New default config had at least this problem: %s",
firstError);
retPacket.strToBuf(firstError);
}
//printf("Sending ");
//retPacket.log();
if (client != NULL)
client->sendPacketTcp(&retPacket);
if (client != NULL)
unlockConfig();
if (client != NULL)
configUpdated(client);
return ret;
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
robot->lock();
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
//arbitrary non-zero values so dynamic reconfigure isn't STUPID
dynConf_min.trans_vel_max = 0.1;
dynConf_min.rot_vel_max = 0.1;
dynConf_min.trans_accel = 0.1;
dynConf_min.trans_decel = 0.1;
dynConf_min.rot_accel = 0.1;
dynConf_min.rot_decel = 0.1;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0;
dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0;
dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0;
dynConf_default.trans_accel = robot->getTransAccel() / 1000.0;
dynConf_default.trans_decel = robot->getTransDecel() / 1000.0;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180.0;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180.0;
/* ROS_ERROR("ON ROBOT NOW\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());
ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max, dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
RevCount = robot->getOrigRobotConfig()->getRevCount();
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_default);
for(int i = 0; i < 16; i++)
{
sonar_tf_array[i].header.frame_id = frame_id_base_link;
std::stringstream _frame_id;
_frame_id << "sonar" << i;
sonar_tf_array[i].child_frame_id = _frame_id.str();
ArSensorReading* _reading = NULL;
_reading = robot->getSonarReading(i);
sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
sonar_tf_array[i].transform.translation.z = 0.19;
sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
}
for (int i=0;i<16;i++) {
sensor_msgs::Range r;
ranges.data.push_back(r);
}
int i=0,j=0;
if (sonars__crossed_the_streams) {
i=8;
j=8;
}
for(; i<16; i++) {
//populate the RangeArray msg
std::stringstream _frame_id;
_frame_id << "sonar" << i;
ranges.data[i].header.frame_id = _frame_id.str();
ranges.data[i].radiation_type = 0;
ranges.data[i].field_of_view = 0.2618f;
ranges.data[i].min_range = 0.03f;
ranges.data[i].max_range = 5.0f;
}
// Enable the motors
robot->enableMotors();
robot->disableSonar();
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
robot->unlock();
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
for(int i =0; i < 16; i++) {
std::stringstream topic_name;
topic_name << "range" << i;
range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
}
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
veltime = ros::Time::now();
sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this);
sonar_tf_timer.stop();
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArLog::init(ArLog::StdOut, ArLog::Verbose);
ArMap testMap;
ArTime timer;
ArGlobalFunctor mapChangedCB(&mapChanged);
testMap.addMapChangedCB(&mapChangedCB);
if (argc <= 1)
{
printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]);
Aria::exit(1);
}
timer.setToNow();
if (!testMap.readFile(argv[1]))
{
printf("mapTest: Could not read map '%s'\n", argv[1]);
Aria::exit(2);
}
printf("mapTest: Took %ld ms to read file\n", timer.mSecSince());
/*
printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
testMap.getMapObjectsChanged().mSecSince(),
testMap.getPointsChanged().mSecSince(),
testMap.getMapInfoChanged().mSecSince());
*/
timer.setToNow();
if(!testMap.writeFile("mapTest.map"))
{
printf("mapTest: Error could not write new map to mapTest.map");
Aria::exit(3);
}
printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince());
std::list<ArMapObject *>::iterator objIt;
ArMapObject *obj;
for (objIt = testMap.getMapObjects()->begin();
objIt != testMap.getMapObjects()->end();
objIt++)
{
obj = (*objIt);
printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh());
if(obj->hasFromTo())
printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY());
printf("mapTest: \n");
/*
if (strcasecmp(obj->getType(), "Goal") == 0 ||
strcasecmp(obj->getType(), "GoalWithHeading") == 0)
{
printf("mapTest: Map object: Goal %s\n", obj->getName());
}
else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 &&
obj->hasFromTo())
{
printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n",
obj->getFromPose().getX(), obj->getFromPose().getY(),
obj->getToPose().getX(), obj->getToPose().getY());
}
*/
}
std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo();
for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin();
i != objInfo->end();
i++)
{
printf("mapTest: MapInfo object definition:\n----\n");
(*i)->log();
printf("mapTest: ----\n");
}
printf("mapTest: First 5 data points:\n");
std::vector<ArPose>::iterator pIt;
ArPose pose;
int n = 0;
for (pIt = testMap.getPoints()->begin();
pIt != testMap.getPoints()->end();
pIt++)
{
pose = (*pIt);
if (n > 5)
exit(0);
printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY());
n++;
// the points are gone through
}
if (argc >= 3)
{
timer.setToNow();
if (!testMap.readFile(argv[2]))
{
printf("mapTest: Could not read map '%s'\n", argv[2]);
}
printf("mapTest: Took %ld ms to read file2\n", timer.mSecSince());
/*
ArUtil::sleep(30);
printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
testMap.getMapObjectsChanged().mSecSince(),
testMap.getPointsChanged().mSecSince(),
testMap.getMapInfoChanged().mSecSince());
*/
timer.setToNow();
testMap.writeFile("mapTest2.map");
printf("mapTest: Took %ld ms to write file2\n", timer.mSecSince());
}
// now test it with the config stuff
ArArgumentBuilder builder;
builder.setExtraString("Map");
builder.add(argv[1]);
printf("mapTest: Trying config with map (%s)\n", argv[1]);
Aria::getConfig()->parseArgument(&builder);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
printf("mapTest: Trying config again with same map (%s)\n", argv[1]);
Aria::getConfig()->parseArgument(&builder);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
if (argc >= 3)
{
ArArgumentBuilder builder2;
builder2.setExtraString("Map");
builder2.add(argv[2]);
printf("mapTest: Trying config with map2 (%s)\n", argv[2]);
Aria::getConfig()->parseArgument(&builder2);
Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
}
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
//ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() );
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
//ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() );
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
readParameters();
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
/*
* 横向速度和纵向速度单位为米
*/
/*
* 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760
*/
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
// 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
// 初始化参数的默认值
rosaria::RosAriaConfig dynConf_default;
dynConf_default.trans_accel = robot->getTransAccel() / 1000;
dynConf_default.trans_decel = robot->getTransDecel() / 1000;
dynConf_default.lat_accel = robot->getLatAccel() / 1000;
dynConf_default.lat_decel = robot->getLatDecel() / 1000;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180;
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_max);
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// Enable the motors
robot->enableMotors();
// disable sonars on startup
robot->disableSonar();
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
AREXPORT bool ArFileParser::parseLine(char *line,
char *errorBuffer, size_t errorBufferLen)
{
char keyword[512];
char *choppingPos;
char *valueStart=NULL;
size_t textStart=0;
size_t len;
size_t i;
bool noArgs;
std::map<std::string, HandlerCBType *, ArStrCaseCmpOp>::iterator it;
HandlerCBType *handler;
myLineNumber++;
noArgs = false;
// chop out the comments
if ((choppingPos = strstr(line, ";")) != NULL)
line[choppingPos-line] = '\0';
if ((choppingPos = strstr(line, "#")) != NULL)
line[choppingPos-line] = '\0';
// chop out the new line if its there
if ((choppingPos = strstr(line, "\n")) != NULL)
line[choppingPos-line] = '\0';
// chop out the windows new line if its there
while ((choppingPos = strstr(line, "\r")) != NULL)
memmove(choppingPos, choppingPos + 1, strlen(line));
// see how long the line is
len = strlen(line);
// find the keyword
// if this is 0 then we have an empty line so we continue
if (len == 0)
{
ArLog::log(ArLog::Verbose, "line %d: empty line", myLineNumber);
return true;
}
// first find the start of the text
for (i = 0; i < len; i++)
{
// if its not a space we're done
if (!isspace(line[i]))
{
textStart = i;
break;
};
}
// if we reached the end of the line then continue
if (i == len)
{
ArLog::log(ArLog::Verbose, "line %d: just white space at start of line", myLineNumber);
return true;
}
// now we chisel out the keyword
// adding it so that if the text is quoted it pulls the whole keyword
bool quoted = false;
for (i = textStart;
i < len && i < sizeof(keyword) + textStart - 3;
i++)
{
// if we're on the start and its a quote just note that and continue
if (!quoted && i == textStart && line[i] == '"')
{
// set quoted to true since we're going to move textStart ahead
// and don't want to loop this
quoted = true;
// note that our text starts on the next char really
textStart++;
continue;
}
// if we're not looking for the end quote and its a space we're done
if (!quoted && isspace(line[i]))
{
break;
}
// if we are looking for the end quote and its a quote we're done
// (so put the null terminator in the keyword and advance the line
// iterator beyond the end quote
else if (quoted && line[i] == '"')
{
keyword[i-textStart] = '\0';
i++;
break;
}
// if not its part of the keyword
else
keyword[i-textStart] = line[i];
}
keyword[i-textStart] = '\0';
//ArLog::log(ArLog::Verbose, "line %d: keyword %s", lineNumber, keyword);
// now find the start of the value (first non whitespace)
for (; i < len; i++)
{
// if its not a space we're done
if (!isspace(line[i]))
{
valueStart = &line[i];
break;
};
}
// lower that keyword
ArUtil::lower(keyword, keyword, 512);
// a variable for if we're using the remainder handler or not (don't
// do a test just because someone could set the remainder handler to
// some other handler they're using)
bool usingRemainder = false;
// see if we have a handler for the keyword
if ((it = myMap.find(keyword)) != myMap.end())
{
//printf("have handler for keyword %s\n", keyword);
// we have a handler, so pull that out
handler = (*it).second;
// valueStart was set above but make sure there's an argument
if (i == len)
noArgs = true;
}
// if we don't then check for a remainder handler
else
{
//printf("no handler for keyword %s\n", keyword);
// if we have one set it
if (myRemainderHandler != NULL)
{
usingRemainder = true;
handler = myRemainderHandler;
// reset the value to the start of the text
valueStart = &line[textStart];
}
// if we don't just keep going
else
{
ArLog::log(ArLog::Verbose,
"line %d: unknown keyword '%s' line '%s', continuing",
myLineNumber, keyword, &line[textStart]);
return true;
}
}
/*
if (noArgs)
ArLog::log(ArLog::Verbose, "line %d: firstword '%s' no argument",
myLineNumber, keyword);
else
ArLog::log(ArLog::Verbose, "line %d: firstword '%s' argument '%s'",
myLineNumber, keyword, valueStart);
*/
// now toss the rest of the argument into an argument builder then
// form it up to send to the functor
ArArgumentBuilder builder;
// if we have arguments add them
if (!noArgs)
builder.add(valueStart);
// if not we still set the name of whatever we parsed (unless we
// didn't have a param of course)
if (!usingRemainder)
builder.setExtraString(keyword);
// make sure we don't overwrite any errors
if (errorBuffer != NULL && errorBuffer[0] != '\0')
{
errorBuffer = NULL;
errorBufferLen = 0;
}
// call the functor and see if there are errors;
// if we had an error and aren't continuing on errors then we keep going
if (!handler->call(&builder, errorBuffer, errorBufferLen))
{
// put the line number in the error message (this won't overwrite
// anything because of the check above
if (errorBuffer != NULL)
{
std::string errorString = errorBuffer;
snprintf(errorBuffer, errorBufferLen, "Line %d: %s", myLineNumber,
errorString.c_str());
}
return false;
}
return true;
}
bool ArSoundPlayer::playWavFile(const char* filename, const char* params)
{
ArArgumentBuilder builder;
//builder.addPlain("sleep .35; play");
builder.addPlain("play");
builder.add("-v %.2f", ourVolume);
builder.addPlain(filename);
builder.addPlain(params);
ArLog::log(ArLog::Normal, "ArSoundPlayer: Playing file \"%s\" with \"%s\"",
filename, builder.getFullString());
int ret;
if ((ret = system(builder.getFullString())) != -1)
{
ArLog::log(ArLog::Normal, "ArSoundPlayer: Played file \"%s\" with \"%s\" (got %d)",
filename, builder.getFullString(), ret);
return true;
}
else
{
ArLog::logErrorFromOS(ArLog::Normal,
"ArSoundPlayer::playWaveFile: system call failed");
return false;
}
/*
This was an old mechanism for doing a fork then exec in order to
mimic a system call, so that it could have the child PID for
killing... however in our embedded linux killing the play command
doesn't also kill the sox command, so the kill doesn't work at
all... and the fork() would also reserve a lot of memory for the new process
...
so it was replaced with the above system call
const char* prog = NULL;
prog = getenv("PLAY_WAV");
if(prog == NULL)
prog = "play";
char volstr[4];
if(strcmp(prog, "play") == 0)
{
snprintf(volstr, 4, "%f", ourVolume);
}
ArLog::log(ArLog::Normal, "ArSoundPlayer: Playing file \"%s\" using playback program \"%s\" with argument: -v %s", filename, prog, volstr);
ourPlayChildPID = fork();
//ourPlayChildPID = vfork(); // XXX rh experimental, avoids the memory copy cost of fork()
// NOTE: after vfork() you can ONLY safely use an exec function or _exit() in the
// child process. Any other call, if it modifies memory still shared by the
// parent, will result in problems.
if(ourPlayChildPID == -1)
{
ArLog::log(ArLog::Terse, "ArSoundPlayer: error forking! (%d: %s)", errno,
(errno == EAGAIN) ? "EAGAIN reached process limit, or insufficient memory to copy page tables" :
( (errno == ENOMEM) ? "ENOMEM out of kernel memory" : "unknown error" ) );
ArLog::logErrorFromOS(ArLog::Normal,
"ArSoundPlayer::playWaveFile: fork failed");
return false;
}
if(ourPlayChildPID == 0)
{
// child process: execute sox
int r = -1;
r = execlp(prog, prog, "-v", volstr, filename, (char*)0);
if(r < 0)
{
int err = errno;
const char *errstr = strerror(err);
printf("ArSoundPlayer (child process): Error executing Wav file playback program \"%s %s\" (%d: %s)\n", prog, filename, err, errstr);
//_exit(-1); // need to use _exit with vfork
exit(-1);
}
}
// parent process: wait for child to finish
ArLog::log(ArLog::Verbose, "ArSoundPlayer: created child process %d to play wav file \"%s\".",
ourPlayChildPID, filename);
int status;
waitpid(ourPlayChildPID, &status, 0);
if(WEXITSTATUS(status) != 0) {
ArLog::log(ArLog::Terse, "ArSoundPlayer: Error: Wav file playback program \"%s\" with file \"%s\" exited with error code %d.", prog, filename, WEXITSTATUS(status));
ourPlayChildPID = -1;
return false;
}
ArLog::log(ArLog::Verbose, "ArSoundPlayer: child process %d finished.", ourPlayChildPID);
ourPlayChildPID = -1;
return true;
*/
}
AREXPORT void ArNetServer::runOnce(void)
{
ArSocket acceptingSocket;
ArSocket *socket;
char *str;
std::list<ArSocket *> removeList;
std::list<ArSocket *>::iterator it;
ArArgumentBuilder *args = NULL;
std::string command;
if (!myOpened)
{
return;
}
lock();
// get any new sockets that want to connect
while (myServerSocket.accept(&acceptingSocket) && acceptingSocket.getFD() >= 0)
{
acceptingSocket.setNonBlock();
// see if we want more sockets
if (!myMultipleClients && (myConns.size() > 0 ||
myConnectingConns.size() > 0))
{
// we didn't want it, so politely tell it to go away
acceptingSocket.writeString("Conn refused.");
acceptingSocket.writeString(
"Only client allowed and it is already connected.");
acceptingSocket.close();
ArLog::log(ArLog::Terse, "ArNetServer not taking multiple clients and another client tried to connect from %s.", acceptingSocket.getIPString());
}
else
{
// we want the client so we put it in our list of connecting
// sockets, which means that it is waiting to give its password
socket = new ArSocket;
socket->setLogWriteStrings(myLoggingDataSent);
socket->transfer(&acceptingSocket);
socket->writeString("Enter password:"******"Client connecting from %s.",
socket->getIPString());
}
}
// now we read in data from our connecting sockets and see if
// they've given us the password
for (it = myConnectingConns.begin(); it != myConnectingConns.end(); ++it)
{
socket = (*it);
// read in what the client has to say
if ((str = socket->readString()) != NULL)
{
if (str[0] == '\0')
continue;
// now see if the word matchs the password
if (myPassword == str)
{
ArLog::log(ArLog::Normal,
"Client from %s gave password and connected.",
socket->getIPString());
myConns.push_front(socket);
removeList.push_front(socket);
internalGreeting(socket);
}
else
{
socket->close();
myDeleteList.push_front(socket);
ArLog::log(ArLog::Terse,
"Client from %s gave wrong password and is being disconnected.",
socket->getIPString());
}
}
// if we couldn't read a string it means we lost a connection
else
{
ArLog::log(ArLog::Normal,
"Connection to %s lost.", socket->getIPString());
socket->close();
myDeleteList.push_front(socket);
}
}
// now we clear out the ones we want to remove from our connecting
// clients list
while ((it = removeList.begin()) != removeList.end())
{
socket = (*it);
myConnectingConns.remove(socket);
removeList.pop_front();
}
// now we read in data from our connecting sockets and see if
// they've given us the password
for (it = myConns.begin(); it != myConns.end() && myOpened; ++it)
{
socket = (*it);
// read in what the client has to say
while ((str = socket->readString()) != NULL)
{
// if this is null then there wasn't anything said
if (str[0] == '\0')
break;
// make sure we read something
// set up the arguments and then call the function for the
// argument
args = new ArArgumentBuilder;
args->addPlain(str);
//args->log();
parseCommandOnSocket(args, socket);
delete args;
args = NULL;
}
// if str was NULL we lost connection
if (str == NULL)
{
ArLog::log(ArLog::Normal,
"Connection to %s lost.", socket->getIPString());
socket->close();
myDeleteList.push_front(socket);
}
}
// now we delete the ones we want to delete (we could do this above
// but then it wouldn't be symetrical with above)
while ((it = myDeleteList.begin()) != myDeleteList.end())
{
socket = (*it);
myConnectingConns.remove(socket);
myConns.remove(socket);
socket->close();
delete socket;
myDeleteList.pop_front();
}
if (myWantToClose)
{
close();
}
unlock();
}
int main(int argc, char **argv)
{
Aria::init();
char *worldName;
char *mapName;
if (argc != 3)
{
ArLog::log(ArLog::Normal, "Usage: %s <WorldFile> <MapFile>", argv[0]);
ArLog::log(ArLog::Normal, "Example: %s columbia.wld columbia.map", argv[0]);
exit(1);
}
worldName = argv[1];
mapName = argv[2];
FILE *file;
if ((file = ArUtil::fopen(worldName, "r")) == NULL)
{
ArLog::log(ArLog::Normal, "Could not open world file '%s' to convert", worldName);
exit(1);
}
char line[10000];
std::vector<ArLineSegment> lines;
bool haveHome = false;
ArPose homePose;
// read until the end of the file
while (fgets(line, sizeof(line), file) != NULL)
{
ArArgumentBuilder builder;
builder.add(line);
// Four ints is a line
if (builder.getArgc() == 4 && builder.isArgInt(0) &&
builder.isArgInt(1) && builder.isArgInt(2) &&
builder.isArgInt(3))
{
lines.push_back(
ArLineSegment(builder.getArgInt(0), builder.getArgInt(1),
builder.getArgInt(2), builder.getArgInt(3)));
}
// "position X Y Th" becomes a RobotHome
if( !strcmp(builder.getArg(0), "position") &&
builder.getArgc() == 4 && builder.isArgInt(1) &&
builder.isArgInt(2) && builder.isArgInt(3) )
{
haveHome = true;
homePose.setX(builder.getArgInt(1));
homePose.setY(builder.getArgInt(2));
homePose.setTh(builder.getArgInt(3));
printf("Will make a Home point out of start position: ");
homePose.log();
}
}
ArMap armap;
armap.setLines(&lines);
ArPose nopose;
ArMapObject home("RobotHome", homePose, NULL, "ICON", "Home", false, nopose, nopose);
std::list<ArMapObject*> objects;
if(haveHome)
{
objects.push_back(&home);
armap.setMapObjects(&objects);
}
if (!armap.writeFile(mapName))
{
ArLog::log(ArLog::Normal, "Could not save map file '%s'", mapName);
exit(1);
}
ArLog::log(ArLog::Normal, "Converted %s world file to %s map file.", worldName, mapName);
exit(0);
}
