Aria: ArRobotParams.cpp Source File

00001 /*
00002 MobileRobots Advanced Robotics Interface for Applications (ARIA)
00003 Copyright (C) 2004, 2005 ActivMedia Robotics LLC
00004 Copyright (C) 2006, 2007 MobileRobots Inc.
00005 
00006      This program is free software; you can redistribute it and/or modify
00007      it under the terms of the GNU General Public License as published by
00008      the Free Software Foundation; either version 2 of the License, or
00009      (at your option) any later version.
00010 
00011      This program is distributed in the hope that it will be useful,
00012      but WITHOUT ANY WARRANTY; without even the implied warranty of
00013      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014      GNU General Public License for more details.
00015 
00016      You should have received a copy of the GNU General Public License
00017      along with this program; if not, write to the Free Software
00018      Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019 
00020 If you wish to redistribute ARIA under different terms, contact 
00021 MobileRobots for information about a commercial version of ARIA at 
00022 robots@mobilerobots.com or 
00023 MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
00024 */
00025 
00026 #include "ArExport.h"
00027 
00028 
00029 #include "ariaOSDef.h"
00030 #include "ArRobotParams.h"
00031 #include "ariaInternal.h"
00032 
00033 AREXPORT ArRobotParams::ArRobotParams() :
00034   ArConfig(NULL, true),
00035   myIRUnitGetFunctor(this, &ArRobotParams::getIRUnits),
00036   myIRUnitSetFunctor(this, &ArRobotParams::parseIRUnit),
00037   mySonarUnitGetFunctor(this, &ArRobotParams::getSonarUnits),
00038   mySonarUnitSetFunctor(this, &ArRobotParams::parseSonarUnit)
00039 {
00040   sprintf(myClass, "Pioneer");
00041   mySubClass[0] = '\0';
00042   myRobotRadius = 250;
00043   myRobotDiagonal = 120;
00044   myRobotWidth = 400;
00045   myRobotLength = 500; 
00046   myRobotLengthFront = 0; 
00047   myRobotLengthRear = 0; 
00048   myHolonomic = true;
00049   myAbsoluteMaxVelocity = 0;
00050   myAbsoluteMaxRVelocity = 0;
00051   myHaveMoveCommand = true;
00052   myAngleConvFactor = 0.001534;
00053   myDistConvFactor = 0;
00054   myVelConvFactor = 1.0;
00055   myRangeConvFactor = 0;
00056   myVel2Divisor = 20;
00057   myGyroScaler = 1.626;
00058   myNumSonar = 0;
00059   myTableSensingIR = false;
00060   myNewTableSensingIR = false;
00061   myFrontBumpers = false;
00062   myNumFrontBumpers = 5;
00063   myRearBumpers = false;
00064   myNumRearBumpers = 5;
00065   myNumSonar = 0;
00066   myNumIR = 0;
00067   mySonarMap.clear();
00068   myIRMap.clear();
00069   myLaserPossessed = false;
00070   sprintf(myLaserPort, "COM3");
00071   myLaserFlipped = false;
00072   myLaserPowerControlled = true;
00073   myLaserX = 0;
00074   myLaserY = 0;
00075   myLaserTh = 0.0;
00076   myLaserIgnore[0] = '\0';
00077   
00078   myRequestIOPackets = false;
00079   myRequestEncoderPackets = false;
00080   mySwitchToBaudRate = 38400;
00081 
00082   mySettableVelMaxes = true;
00083   myTransVelMax = 0;
00084   myRotVelMax = 0;
00085 
00086   mySettableAccsDecs = true;
00087   myTransAccel = 0;
00088   myTransDecel = 0;
00089   myRotAccel = 0;
00090   myRotDecel = 0;
00091 
00092   addComment("Robot parameter file");
00093 //  addComment("");
00094   //addComment("General settings");
00095   std::string section;
00096   section = "General settings";
00097   addParam(ArConfigArg("Class", myClass, "general type of robot", 
00098          sizeof(myClass)), section.c_str(), ArPriority::TRIVIAL);
00099   addParam(ArConfigArg("Subclass", mySubClass, "specific type of robot", 
00100                sizeof(mySubClass)), section.c_str(), 
00101        ArPriority::TRIVIAL);
00102   addParam(ArConfigArg("RobotRadius", &myRobotRadius, "radius in mm"), 
00103        section.c_str(), ArPriority::NORMAL);
00104   addParam(ArConfigArg("RobotDiagonal", &myRobotDiagonal, 
00105          "half-height to diagonal of octagon"), "General settings",
00106        ArPriority::TRIVIAL);
00107   addParam(ArConfigArg("RobotWidth", &myRobotWidth, "width in mm"), 
00108        section.c_str(), ArPriority::NORMAL);
00109   addParam(ArConfigArg("RobotLength", &myRobotLength, "length in mm of the whole robot"),
00110        section.c_str(), ArPriority::NORMAL);
00111   addParam(ArConfigArg("RobotLengthFront", &myRobotLengthFront, "length in mm to the front of the robot (if this is 0 (or non existant) this value will be set to half of RobotLength)"),
00112        section.c_str(), ArPriority::NORMAL);
00113   addParam(ArConfigArg("RobotLengthRear", &myRobotLengthRear, "length in mm to the rear of the robot (if this is 0 (or non existant) this value will be set to half of RobotLength)"), 
00114        section.c_str(), ArPriority::NORMAL);
00115   addParam(ArConfigArg("Holonomic", &myHolonomic, "turns in own radius"), 
00116        section.c_str(), ArPriority::TRIVIAL);
00117   addParam(ArConfigArg("MaxRVelocity", &myAbsoluteMaxRVelocity, 
00118                "absolute maximum degrees / sec"), section.c_str(),
00119        ArPriority::TRIVIAL);
00120   addParam(ArConfigArg("MaxVelocity", &myAbsoluteMaxVelocity, 
00121          "absolute maximum mm / sec"), section.c_str(), 
00122        ArPriority::TRIVIAL);
00123   addParam(ArConfigArg("HasMoveCommand", &myHaveMoveCommand, 
00124          "has built in move command"), section.c_str(),
00125        ArPriority::TRIVIAL);
00126   addParam(ArConfigArg("RequestIOPackets", &myRequestIOPackets,
00127          "automatically request IO packets"), section.c_str(),
00128        ArPriority::NORMAL);
00129   addParam(ArConfigArg("RequestEncoderPackets", &myRequestEncoderPackets,
00130                "automatically request encoder packets"), 
00131        section.c_str(), ArPriority::NORMAL);
00132   addParam(ArConfigArg("SwitchToBaudRate", &mySwitchToBaudRate, 
00133          "switch to this baud if non-0 and supported on robot"), 
00134        section.c_str(), ArPriority::IMPORTANT);
00135   
00136   section = "Conversion factors";
00137   addParam(ArConfigArg("AngleConvFactor", &myAngleConvFactor,
00138              "radians per angular unit (2PI/4096)"), section.c_str(),
00139        ArPriority::TRIVIAL);
00140   addParam(ArConfigArg("DistConvFactor", &myDistConvFactor,
00141                "multiplier to mm from robot units"), section.c_str(),
00142        ArPriority::IMPORTANT);
00143   addParam(ArConfigArg("VelConvFactor", &myVelConvFactor,
00144              "multiplier to mm/sec from robot units"), 
00145        section.c_str(),
00146        ArPriority::NORMAL);
00147   addParam(ArConfigArg("RangeConvFactor", &myRangeConvFactor, 
00148                "multiplier to mm from sonar units"), section.c_str(),
00149        ArPriority::TRIVIAL);
00150   addParam(ArConfigArg("DiffConvFactor", &myDiffConvFactor, 
00151              "ratio of angular velocity to wheel velocity (unused in newer firmware that calculates and returns this)"), 
00152        section.c_str(),
00153        ArPriority::TRIVIAL);
00154   addParam(ArConfigArg("Vel2Divisor", &myVel2Divisor, 
00155                "divisor for VEL2 commands"), section.c_str(),
00156        ArPriority::TRIVIAL);
00157   addParam(ArConfigArg("GyroScaler", &myGyroScaler, 
00158              "Scaling factor for gyro readings"), section.c_str(),
00159        ArPriority::IMPORTANT);
00160 
00161   section = "Accessories the robot has";
00162   addParam(ArConfigArg("TableSensingIR", &myTableSensingIR,
00163                "if robot has upwards facing table sensing IR"), 
00164        section.c_str(),
00165        ArPriority::TRIVIAL);
00166   addParam(ArConfigArg("NewTableSensingIR", &myNewTableSensingIR,
00167          "if table sensing IR are sent in IO packet"), 
00168        section.c_str(),
00169        ArPriority::TRIVIAL);
00170   addParam(ArConfigArg("FrontBumpers", &myFrontBumpers, 
00171          "if robot has a front bump ring"), section.c_str(),
00172        ArPriority::IMPORTANT);
00173   addParam(ArConfigArg("NumFrontBumpers", &myNumFrontBumpers,
00174              "number of front bumpers on the robot"), 
00175        section.c_str(),
00176        ArPriority::TRIVIAL);
00177   addParam(ArConfigArg("RearBumpers", &myRearBumpers,
00178                "if the robot has a rear bump ring"), section.c_str(),
00179        ArPriority::IMPORTANT);
00180   addParam(ArConfigArg("NumRearBumpers", &myNumRearBumpers,
00181                "number of rear bumpers on the robot"), section.c_str(),
00182        ArPriority::TRIVIAL);
00183 
00184   section = "IR parameters";
00185   addParam(ArConfigArg("IRNum", &myNumIR, "number of IRs on the robot"), section.c_str(), ArPriority::NORMAL);
00186    addParam(ArConfigArg("IRUnit", &myIRUnitSetFunctor, &myIRUnitGetFunctor,
00187             "IRUnit <IR Number> <IR Type> <Persistance, cycles> <x position, mm> <y position, mm>"), 
00188         section.c_str(), ArPriority::TRIVIAL);
00189 
00190 
00191   section = "Sonar parameters";
00192   addParam(ArConfigArg("SonarNum", &myNumSonar, 
00193              "number of sonar on the robot"), section.c_str(),
00194        ArPriority::NORMAL);
00195   addParam(ArConfigArg("SonarUnit", &mySonarUnitSetFunctor, 
00196              &mySonarUnitGetFunctor,
00197              "SonarUnit <sonarNumber> <x position, mm> <y position, mm> <heading of disc, degrees>"), section.c_str(), ArPriority::TRIVIAL);
00198 
00199 
00200   section = "Laser parameters";
00201   addParam(ArConfigArg("LaserPossessed", &myLaserPossessed, 
00202              "if there is a laser on the robot"), section.c_str(),
00203        ArPriority::IMPORTANT);
00204   addParam(ArConfigArg("LaserPort", myLaserPort, "port the laser is on", 
00205              sizeof(myLaserPort)), section.c_str(),
00206        ArPriority::NORMAL);
00207   addParam(ArConfigArg("LaserFlipped", &myLaserFlipped,
00208              "if the laser is upside-down or not"), section.c_str(),
00209        ArPriority::NORMAL);
00210   addParam(ArConfigArg("LaserPowerControlled", &myLaserPowerControlled,
00211              "if the power to the laser is controlled by serial"), 
00212        section.c_str(),
00213        ArPriority::TRIVIAL);
00214   addParam(ArConfigArg("LaserX", &myLaserX, "x location of laser, mm"), 
00215        section.c_str(),
00216        ArPriority::NORMAL);
00217   addParam(ArConfigArg("LaserY", &myLaserY, "y location of laser, mm"), 
00218        section.c_str(),
00219        ArPriority::NORMAL);
00220   addParam(ArConfigArg("LaserTh", &myLaserTh, "rotation of laser, deg"), 
00221        section.c_str(),
00222        ArPriority::NORMAL);
00223   addParam(ArConfigArg("LaserIgnore", myLaserIgnore, "Readings within a degree of the listed degrees (separated by a space) will be ignored", sizeof(myLaserIgnore)), 
00224        section.c_str(),
00225        ArPriority::NORMAL);
00226 
00227   section = "Movement control parameters";
00228   setSectionComment(section.c_str(), "if these are 0 the parameters from robot flash will be used, otherwise these values will be used");
00229   addParam(ArConfigArg("SettableVelMaxes", &mySettableVelMaxes, "if TransVelMax and RotVelMax can be set"), section.c_str(),
00230        ArPriority::TRIVIAL);
00231   addParam(ArConfigArg("TransVelMax", &myTransVelMax, "maximum desired translational velocity for the robot"), section.c_str(), 
00232        ArPriority::IMPORTANT);
00233   addParam(ArConfigArg("RotVelMax", &myRotVelMax, "maximum desired rotational velocity for the robot"), section.c_str(),
00234        ArPriority::IMPORTANT);
00235   addParam(ArConfigArg("SettableAccsDecs", &mySettableAccsDecs, "if the accel and decel parameters can be set"), section.c_str(), ArPriority::TRIVIAL);
00236   addParam(ArConfigArg("TransAccel", &myTransAccel, "translational acceleration"), 
00237        section.c_str(), ArPriority::IMPORTANT);
00238   addParam(ArConfigArg("TransDecel", &myTransDecel, "translational deceleration"), 
00239 
00240        section.c_str(), ArPriority::IMPORTANT);
00241   addParam(ArConfigArg("RotAccel", &myRotAccel, "rotational acceleration"), 
00242        section.c_str());
00243   addParam(ArConfigArg("RotDecel", &myRotDecel, "rotational deceleration"),
00244        section.c_str(), ArPriority::IMPORTANT);
00245 
00246 }
00247 
00248 AREXPORT ArRobotParams::~ArRobotParams()
00249 {
00250 
00251 }
00252 
00253 
00254 AREXPORT bool ArRobotParams::parseIRUnit(ArArgumentBuilder *builder)
00255 {
00256   if (builder->getArgc() != 5 || !builder->isArgInt(0) || 
00257       !builder->isArgInt(1) || !builder->isArgInt(2) || 
00258       !builder->isArgInt(3) || !builder->isArgInt(4))
00259   {
00260     ArLog::log(ArLog::Terse, "ArRobotParams: IRUnit parameters invalid");
00261     return false;
00262   }
00263   myIRMap[builder->getArgInt(0)][IR_TYPE] = builder->getArgInt(1);
00264   myIRMap[builder->getArgInt(0)][IR_CYCLES] = builder->getArgInt(2);
00265   myIRMap[builder->getArgInt(0)][IR_X] = builder->getArgInt(3);
00266   myIRMap[builder->getArgInt(0)][IR_Y] = builder->getArgInt(4);
00267   return true;
00268 }
00269 
00270 AREXPORT const std::list<ArArgumentBuilder *> *ArRobotParams::getIRUnits(void)
00271 {
00272   std::map<int, std::map<int, int> >::iterator it;
00273   int num, type, cycles,  x, y;
00274   ArArgumentBuilder *builder;
00275 
00276   for (it = myIRMap.begin(); it != myIRMap.end(); it++)
00277   {
00278     num = (*it).first;
00279     type = (*it).second[IR_TYPE];
00280     cycles = (*it).second[IR_CYCLES];
00281     x = (*it).second[IR_X];
00282     y = (*it).second[IR_Y];
00283     builder = new ArArgumentBuilder;
00284     builder->add("%d %d %d %d %d", num, type, cycles, x, y);
00285     myGetIRUnitList.push_back(builder);
00286   }
00287   return &myGetIRUnitList;
00288 }
00289 
00290 AREXPORT void ArRobotParams::internalSetIR(int num, int type, int cycles, int x, int y)
00291 {
00292   myIRMap[num][IR_TYPE] = type;
00293   myIRMap[num][IR_CYCLES] = cycles;
00294   myIRMap[num][IR_X] = x;
00295   myIRMap[num][IR_Y] = y;
00296 }
00297 
00298 AREXPORT bool ArRobotParams::parseSonarUnit(ArArgumentBuilder *builder)
00299 {
00300   if (builder->getArgc() != 4 || !builder->isArgInt(0) || 
00301       !builder->isArgInt(1) || !builder->isArgInt(2) ||
00302       !builder->isArgInt(3))
00303   {
00304     ArLog::log(ArLog::Terse, "ArRobotParams: SonarUnit parameters invalid");
00305     return false;
00306   }
00307   mySonarMap[builder->getArgInt(0)][SONAR_X] = builder->getArgInt(1);
00308   mySonarMap[builder->getArgInt(0)][SONAR_Y] = builder->getArgInt(2);
00309   mySonarMap[builder->getArgInt(0)][SONAR_TH] = builder->getArgInt(3);
00310   return true;
00311 }
00312 
00313 
00314 AREXPORT const std::list<ArArgumentBuilder *> *ArRobotParams::getSonarUnits(void)
00315 {
00316   std::map<int, std::map<int, int> >::iterator it;
00317   int num, x, y, th;
00318   ArArgumentBuilder *builder;
00319 
00320   for (it = mySonarMap.begin(); it != mySonarMap.end(); it++)
00321   {
00322     num = (*it).first;
00323     x = (*it).second[SONAR_X];
00324     y = (*it).second[SONAR_Y];
00325     th = (*it).second[SONAR_TH];
00326     builder = new ArArgumentBuilder;
00327     builder->add("%d %d %d %d", num, x, y, th);
00328     myGetSonarUnitList.push_back(builder);
00329   }
00330   return &myGetSonarUnitList;
00331 }
00332 
00333 AREXPORT void ArRobotParams::internalSetSonar(int num, int x, 
00334                           int y, int th)
00335 {
00336   mySonarMap[num][SONAR_X] = x;
00337   mySonarMap[num][SONAR_Y] = y;
00338   mySonarMap[num][SONAR_TH] = th;
00339 }
00340 
00341 AREXPORT bool ArRobotParams::save(void)
00342 {
00343   char buf[10000];
00344   sprintf(buf, "%sparams/", Aria::getDirectory());
00345   setBaseDirectory(buf);
00346   sprintf(buf, "%s.p", getSubClassName());
00347   return writeFile(buf, false, NULL, false);
00348 }