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; }