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;
}
ArnlSystem::Error ArnlSystem::setup()
{
// Note, various objects are allocated here which are never deleted (freed),
// if you want to destroy and create new ArnlSystem objects in the future
// these need to be stored and deleted in the constructor. If you use it once
// and keep it for the lifetime of the process, its ok.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder();
ArArgumentParser *argparser = new ArArgumentParser(args);
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
argparser->addDefaultArgument("-connectLaser");
ArRobotConnector *robotConnector = new ArRobotConnector(argparser, robot);
if(!robotConnector->connectRobot())
{
ArLog::log(ArLog::Normal, "%sError: could not connect to robot.", logprefix);
return RobotConnectError;
}
robot->addPacketHandler(new ArRetFunctor1C<bool, ArnlSystem, ArRobotPacket*>(this, &ArnlSystem::handleDebugMessage));
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
//robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
char fileDir[1024];
ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
"examples");
ArLog::addToConfig(Aria::getConfig());
ArAnalogGyro *gyro = new ArAnalogGyro(robot); // for old robots with separate gyro communication
serverBase = new ArServerBase;
ArServerSimpleOpener *simpleOpener = new ArServerSimpleOpener(argparser);
ArLaserConnector *laserConnector = new ArLaserConnector(argparser, robot, robotConnector);
if(!Aria::parseArgs() || !argparser->checkHelpAndWarnUnparsed())
{
return ParseArgumentsError;
}
ArSonarDevice *sonarDev = new ArSonarDevice;
robot->addRangeDevice(sonarDev);
ArRobotConfig *robotConfig = new ArRobotConfig(robot);
robotConfig->addAnalogGyro(gyro);
robot->runAsync(true);
ArLog::log(ArLog::Normal, "%sConnecting to laser(s) configured in parameters...", logprefix);
if (!laserConnector->connectLasers())
{
ArLog::log(ArLog::Terse, "%sError: Could not connect to laser(s). Exiting.", logprefix);
return LaserConnectError;
}
ArLog::log(ArLog::Normal, "%sDone connecting to laser(s).", logprefix);
robot->lock();
ArLaser *firstLaser = robot->findLaser(1);
assert(firstLaser);
if (firstLaser == NULL || !firstLaser->isConnected())
{
ArLog::log(ArLog::Terse, "%sDid not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting", logprefix);
return LaserConnectError;
}
robot->unlock();
map = new ArMap(fileDir);
map->setIgnoreEmptyFileName(true);
map->setIgnoreCase(true);
//map->setIgnoreBadFile(true);
pathTask = new ArPathPlanningTask (robot, sonarDev, map);
locTask = new ArLocalizationTask (robot, firstLaser, map);
std::map<int, ArLaser *>::iterator laserIt;
for (laserIt = robot->getLaserMap()->begin();
laserIt != robot->getLaserMap()->end();
laserIt++)
{
int laserNum = (*laserIt).first;
ArLaser *laser = (*laserIt).second;
if(!laser->isConnected())
continue;
laser->setCumulativeBufferSize(200);
pathTask->addRangeDevice(laser, ArPathPlanningTask::BOTH);
laser->setCumulativeCleanOffset(laserNum * 100);
laser->resetLastCumulativeCleanTime();
std::string laserPacketCountName;
laserPacketCountName = laser->getName();
laserPacketCountName += " Packet Count";
Aria::getInfoGroup()->addStringInt(
laserPacketCountName.c_str(), 10,
new ArRetFunctorC<int, ArLaser>(laser,
&ArLaser::getReadingCount));
}
if (!simpleOpener->open(serverBase, fileDir, 240))
{
ArLog::log(ArLog::Normal, "%sError: Could not open server.", logprefix);
exit(2);
}
ArBumpers *bumpers = new ArBumpers;
robot->addRangeDevice(bumpers);
pathTask->addRangeDevice(bumpers, ArPathPlanningTask::CURRENT);
ArForbiddenRangeDevice *forbidden = new ArForbiddenRangeDevice(map);
robot->addRangeDevice(forbidden);
pathTask->addRangeDevice(forbidden, ArPathPlanningTask::CURRENT);
robot->unlock();
ArActionSlowDownWhenNotCertain *actionSlowDown = new ArActionSlowDownWhenNotCertain(locTask);
pathTask->getPathPlanActionGroup()->addAction(actionSlowDown, 140);
ArActionLost *actionLostPath = new ArActionLost(locTask, pathTask);
pathTask->getPathPlanActionGroup()->addAction(actionLostPath, 150);
ArGlobalReplanningRangeDevice *replanDev = new ArGlobalReplanningRangeDevice(pathTask);
ArServerInfoDrawings *drawings = new ArServerInfoDrawings(serverBase);
drawings->addRobotsRangeDevices(robot);
drawings->addRangeDevice(replanDev);
drawings->addDrawing(
new ArDrawingData("polyLine", ArColor(200,200,200), 1, 75),
"Local Plan Area",
new ArFunctor2C<ArPathPlanningTask, ArServerClient*, ArNetPacket*>(pathTask, &ArPathPlanningTask::drawSearchRectangle)
);
drawings->addDrawing(
new ArDrawingData("polySegments", ArColor(166, 166, 166), 1, 60, 100, "DefaultOn"),
"Path Planning Clearances",
new ArFunctor2C<ArPathPlanningTask, ArServerClient*, ArNetPacket*>(pathTask, &ArPathPlanningTask::drawRobotBounds)
);
drawings->addDrawing(
new ArDrawingData("polyDots", ArColor(0, 255, 0), 100, 75),
"Localization Points",
new ArFunctor2C<ArLocalizationTask, ArServerClient*, ArNetPacket*>(locTask, &ArLocalizationTask::drawRangePoints)
);
ArServerHandlerCommands *commands = new ArServerHandlerCommands(serverBase);
new ArServerInfoRobot(serverBase, robot);
new ArServerInfoSensor(serverBase, robot);
ArServerInfoPath *serverInfoPath = new ArServerInfoPath(serverBase, robot, pathTask);
serverInfoPath->addSearchRectangleDrawing(drawings);
serverInfoPath->addControlCommands(commands);
new ArServerInfoLocalization(serverBase, robot, locTask);
ArServerHandlerLocalization *serverLocHandler = new ArServerHandlerLocalization(serverBase, robot, locTask);
new ArServerHandlerMap(serverBase, map);
new ArServerSimpleComUC(commands, robot);
new ArServerSimpleComMovementLogging(commands, robot);
new ArServerSimpleComLogRobotConfig(commands, robot);
new ArServerSimpleServerCommands(commands, serverBase);
new ArServerHandlerCommMonitor(serverBase);
new ArServerHandlerConfig (serverBase, Aria::getConfig(),
Arnl::getTypicalDefaultParamFileName(),
Aria::getDirectory());
ArServerHandlerPopup *popupServer = new ArServerHandlerPopup(serverBase);
new RobotMonitor(robot, popupServer);
modeGoto = new ArServerModeGoto (serverBase, robot, pathTask, map);
modeStop = new ArServerModeStop(serverBase, robot);
new ArSonarAutoDisabler(robot);
ArServerModeRatioDrive *modeRatioDrive = new ArServerModeRatioDrive(serverBase, robot);
ArActionLost *actionLostRatioDrive = new ArActionLost(locTask, pathTask, modeRatioDrive);
modeRatioDrive->getActionGroup()->addAction(actionLostRatioDrive, 110);
modeRatioDrive->addToConfig(Aria::getConfig(), "Teleop settings");
modeRatioDrive->addControlCommands(commands);
//Wander Mode//
modeWander = new ArServerModeWander(serverBase, robot);
ArActionLost *actionLostWander = new ArActionLost(locTask,pathTask,modeWander);
modeWander->getActionGroup()->addAction(actionLostWander, 110);
//Wander Mode//
// Tool to log data periodically to a file
//ArDataLogger dataLogger(&robot, "datalog.txt");
//dataLogger.addToConfig(Aria::getConfig()); // make it configurable through ArConfig
// Automatically add anything from the global info group to the data logger.
//Aria::getInfoGroup()->addAddStringCallback(dataLogger.getAddStringFunctor());
ArServerInfoStrings *stringInfo = new ArServerInfoStrings(serverBase);
Aria::getInfoGroup()->addAddStringCallback(stringInfo->getAddStringFunctor());
Aria::getInfoGroup()->addStringInt(
"Motor Packet Count", 10,
new ArConstRetFunctorC<int, ArRobot>(robot,
&ArRobot::getMotorPacCount));
Aria::getInfoGroup()->addStringDouble(
"Laser Localization Score", 8,
new ArRetFunctorC<double, ArLocalizationTask>(
locTask, &ArLocalizationTask::getLocalizationScore),
"%.03f");
Aria::getInfoGroup()->addStringInt(
"Laser Loc Num Samples", 8,
new ArRetFunctorC<int, ArLocalizationTask>(
locTask, &ArLocalizationTask::getCurrentNumSamples),
"%4d");
ArSystemStatus::startPeriodicUpdate(1000); // update every 1 second
Aria::getInfoGroup()->addStringDouble("CPU Use", 10, ArSystemStatus::getCPUPercentFunctor(), "% 4.0f%%");
Aria::getInfoGroup()->addStringInt("Wireless Link Quality", 9, ArSystemStatus::getWirelessLinkQualityFunctor(), "%d");
Aria::getInfoGroup()->addStringInt("Wireless Link Noise", 9, ArSystemStatus::getWirelessLinkNoiseFunctor(), "%d");
Aria::getInfoGroup()->addStringInt("Wireless Signal", 9, ArSystemStatus::getWirelessLinkSignalFunctor(), "%d");
modeDock = NULL;
modeDock = ArServerModeDock::createDock(serverBase, robot, locTask, pathTask);
if (modeDock != NULL)
{
modeDock->checkDock();
modeDock->addAsDefaultMode();
modeDock->addToConfig(Aria::getConfig());
modeDock->addControlCommands(commands);
}
modeStop->addAsDefaultMode();
ArServerHandlerMapping *handlerMapping = new ArServerHandlerMapping(serverBase, robot, firstLaser,
fileDir, "", true);
// make laser localization stop while mapping
handlerMapping->addMappingStartCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(locTask, &ArLocalizationTask::setIdleFlag, true));
// and then make it start again when we're doine
handlerMapping->addMappingEndCallback(
new ArFunctor1C<ArLocalizationTask, bool>
(locTask, &ArLocalizationTask::setIdleFlag, false));
// Make it so our "lost" actions don't stop us while mapping
handlerMapping->addMappingStartCallback(actionLostPath->getDisableCB());
handlerMapping->addMappingStartCallback(actionLostRatioDrive->getDisableCB());
// And then let them make us stop as usual when done mapping
handlerMapping->addMappingEndCallback(actionLostPath->getEnableCB());
handlerMapping->addMappingEndCallback(actionLostRatioDrive->getEnableCB());
// don't let forbidden lines show up as obstacles while mapping
// (they'll just interfere with driving while mapping, and localization is off anyway)
handlerMapping->addMappingStartCallback(forbidden->getDisableCB());
// let forbidden lines show up as obstacles again as usual after mapping
handlerMapping->addMappingEndCallback(forbidden->getEnableCB());
// create a pose storage class, this will let the program keep track
// of where the robot is between runs... after we try and restore
// from this file it will start saving the robot's pose into the
// file
ArPoseStorage *poseStorage = new ArPoseStorage(robot);
/// if we could restore the pose from then set the sim there (this
/// won't do anything to the real robot)... if we couldn't restore
/// the pose then just reset the position of the robot (which again
/// won't do anything to the real robot)
if (poseStorage->restorePose("robotPose"))
serverLocHandler->setSimPose(robot->getPose());
//else
// robot->com(ArCommands::SIM_RESET);
/* File transfer services: */
#ifdef WIN32
// Not implemented for Windows yet.
ArLog::log(ArLog::Terse, "%sfile upload/download services are not implemented for Windows; not enabling them.", logprefix);
#else
new ArServerFileLister (serverBase, fileDir);
new ArServerFileToClient (serverBase, fileDir);
new ArServerFileFromClient (serverBase, fileDir, "/tmp");
new ArServerDeleteFileOnServer (serverBase, fileDir);
#endif
// When parsing the configuration file, also look at the program's command line options
// from the command-line argument parser as well as the configuration file.
// (So you can use any argument on the command line, namely -map.)
Aria::getConfig()->useArgumentParser(argparser);
// Read in parameter files.
ArLog::log(ArLog::Normal, "%sLoading config file %s%s into ArConfig...", logprefix, Aria::getDirectory(), Arnl::getTypicalParamFileName());
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "%sCould not load ARNL configuration file. Set ARNL environment variable to use non-default installation directory.", logprefix);
return ConfigError;
}
if (!simpleOpener->checkAndLog() || !argparser->checkHelpAndWarnUnparsed())
{
return ParseArgumentsError;
}
// Warn if there is no map
if (map->getFileName() == NULL || strlen(map->getFileName()) <= 0)
{
ArLog::log(ArLog::Terse, "%s### No map file is set up, you can make a map with the following procedure", logprefix);
ArLog::log(ArLog::Terse, "%s 0) You can find this information in README.txt or docs/Mapping.txt", logprefix);
ArLog::log(ArLog::Terse, "%s 1) Connect to this server with MobileEyes", logprefix);
ArLog::log(ArLog::Terse, "%s 2) Go to Tools->Map Creation->Start Scan", logprefix);
ArLog::log(ArLog::Terse, "%s 3) Give the map a name and hit okay", logprefix);
ArLog::log(ArLog::Terse, "%s 4) Drive the robot around your space (see docs/Mapping.txt", logprefix);
ArLog::log(ArLog::Terse, "%s 5) Go to Tools->Map Creation->Stop Scan", logprefix);
ArLog::log(ArLog::Terse, "%s 6) Start up Mapper3", logprefix);
ArLog::log(ArLog::Terse, "%s 7) Go to File->Open on Robot", logprefix);
ArLog::log(ArLog::Terse, "%s 8) Select the .2d you created", logprefix);
ArLog::log(ArLog::Terse, "%s 9) Create a .map", logprefix);
ArLog::log(ArLog::Terse, "%s 10) Go to File->Save on Robot", logprefix);
ArLog::log(ArLog::Terse, "%s 11) In MobileEyes, go to Tools->Robot Config", logprefix);
ArLog::log(ArLog::Terse, "%s 12) Choose the Files section", logprefix);
ArLog::log(ArLog::Terse, "%s 13) Enter the path and name of your new .map file for the value of the Map parameter.", logprefix);
ArLog::log(ArLog::Terse, "%s 14) Press OK and your new map should become the map used", logprefix);
}
// Print a log message notifying user of the directory for map files
ArLog::log(ArLog::Normal, "%sDirectory for maps and file serving: %s", logprefix, fileDir);
ArLog::log(ArLog::Normal, "%sSee the ARNL README.txt for more information", logprefix);
// Do an initial localization of the robot-> ARNL and SONARNL try all the home points
// in the map, as well as the robot's current odometric position, as possible
// places the robot is likely to be at startup. If successful, it will
// also save the position it found to be the best localized position as the
// "Home" position, which can be obtained from the localization task (and is
// used by the "Go to home" network request).
// MOGS instead just initializes at the current GPS position.
// (You will stil have to drive the robot so it can determine the robot's
// heading, however. See GPS Mapping instructions.)
locTask->localizeRobotAtHomeBlocking();
// Start the networking server's thread
serverBase->runAsync();
ArLog::log(ArLog::Normal, "%sARNL server is now running. You may connect with MobileEyes and Mapper3.", logprefix);
robot->lock();
robot->enableMotors();
robot->disableSonar();
robot->unlock();
return OK;
}
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;
}
