00001 #include <IceUtil/Thread.h> 00002 00003 #include "Robots/RobotBrain/RobotBrainComponent.H" 00004 00005 #include "Component/ModelManager.H" 00006 #include "Component/OptionManager.H" 00007 #include "Component/ModelComponent.H" 00008 #include "Component/ModelParam.H" 00009 00010 #include "GUI/XWinManaged.H" 00011 #include "GUI/ImageDisplayStream.H" 00012 00013 #include "Ice/RobotBrainObjects.ice.H" 00014 #include "Ice/RobotSimEvents.ice.H" 00015 #include "Ice/SeaBeeSimEvents.ice.H" 00016 #include "Ice/IceImageUtils.H" 00017 00018 #include "Image/PixelsTypes.H" 00019 #include "Image/Image.H" 00020 #include "Image/Pixels.H" 00021 00022 #include "Media/FrameSeries.H" 00023 00024 #include "Raster/GenericFrame.H" 00025 00026 #include "Transport/FrameInfo.H" 00027 00028 #include "Controller.h" 00029 00030 #include "LocalizationParticle.h" 00031 #include "LocalizationUtil.h" 00032 #include "LocalizationSensorReader.h" 00033 #include "LocalizationMapEntity.h" 00034 #include "LocalizationMap.h" 00035 #include "IMUDataServer/IMUDataServer.H" 00036 #include "BeeStemI.H" 00037 00038 #ifndef ParticleFilter_H 00039 #define ParticleFilter_H 00040 00041 using namespace std; 00042 00043 enum EXCEPTION_ID 00044 { 00045 OUT_OF_RANGE 00046 }; 00047 00048 const int numSteps = 1; //number of steps to make each update cycle for visualization 00049 //static float angDrift = 2.5; //degrees/sec 00050 //static float transDrift = 1.5 / 12.0; //ft/sec 00051 //static int numParticles = 2500; 00052 const int resampleDelay = 6; //number of iterations to wait before resampling 00053 const int reWeighDelay = 3; 00054 const int saveStateDelay = 10; 00055 00056 class LocalizationWaypointController 00057 { 00058 public: 00059 const static float rotateThresholdRadius = 2; // min offset before going to TRANSLATE 00060 const static float rotateThresholdRadius2 = 10; //max offset before stopping to adjust angle during TRANSLATE 00061 const static float translateThresholdRadius = 1; // radius to stop; feet? 00062 const static float translateThresholdRadius2 = 3; //radius to slow down; feet? 00063 const static float depthThresholdRadius = 2; // arbitrary units, handled by BeeStem's PID 00064 00065 float desiredAngle; 00066 float distance; 00067 00068 struct State 00069 { 00070 const static int idle = 0; 00071 const static int move_to_waypoint = 1; 00072 const static int at_waypoint = 2; 00073 const static int remote = 3; 00074 }; 00075 struct LoopState 00076 { 00077 const static int disabled = -1; 00078 const static int stopped = 0; 00079 const static int started = 1; 00080 const static int running = 2; 00081 }; 00082 struct MovementState 00083 { 00084 const static int idle = 0; 00085 const static int set_depth = 1; 00086 const static int rotate = 2; //rotate to face the waypoint 00087 const static int rotate_moving = 3; //rotate while moving 00088 const static int translate = 4; 00089 const static int rotate2 = 5; //rotate into position at the waypoint 00090 }; 00091 00092 struct Waypoint 00093 { 00094 Waypoint() 00095 { 00096 mPoint = Point2D<float>(0, 0); 00097 mDepth = 0; 00098 mOrientation = 0; 00099 mRadius = 0; 00100 } 00101 Waypoint(Point2D<float> point, float depth, float orientation, float radius) 00102 { 00103 mPoint = point; 00104 mDepth = depth; 00105 mOrientation = orientation; 00106 mRadius = radius; 00107 } 00108 Point2D<float> mPoint; //(x on map, y on map) 00109 float mDepth; 00110 float mOrientation; //orientation angle -> UP; N = 90 00111 float mRadius; 00112 }; 00113 00114 LocalizationWaypointController() 00115 { 00116 mLoopState = LoopState::stopped; 00117 mState = State::idle; 00118 mMovementState = MovementState::idle; 00119 mCurrentWaypointIndex = -1; 00120 }; 00121 00122 Waypoint operator[](int i) 00123 { 00124 if(mWaypoints.size() == 0) 00125 { 00126 Waypoint result; 00127 return result; 00128 } 00129 if(i == -1) 00130 { 00131 return mWaypoints[mCurrentWaypointIndex]; 00132 } 00133 if(i == -2) 00134 { 00135 return mWaypoints[mWaypoints.size() - 1]; 00136 } 00137 return mWaypoints[i]; 00138 } 00139 00140 void addWaypoint(Waypoint wp) 00141 { 00142 mWaypoints.resize(mWaypoints.size() + 1); 00143 mWaypoints[mWaypoints.size() - 1] = wp; 00144 } 00145 00146 void resume() 00147 { 00148 if(mLoopState == LoopState::started) 00149 { 00150 mLoopState = LoopState::running; 00151 mState = State::move_to_waypoint; 00152 mMovementState = MovementState::rotate; 00153 } 00154 } 00155 00156 void start() 00157 { 00158 if(mLoopState == LoopState::stopped) 00159 { 00160 mLoopState = LoopState::started; 00161 resume(); 00162 mCurrentWaypointIndex = 0; 00163 } 00164 } 00165 00166 void pause() 00167 { 00168 if(mLoopState == LoopState::running) 00169 { 00170 mLoopState = LoopState::started; 00171 mState = State::idle; 00172 } 00173 } 00174 00175 void stop() 00176 { 00177 pause(); 00178 if(mLoopState != LoopState::stopped) 00179 { 00180 mLoopState = LoopState::stopped; 00181 mMovementState = MovementState::idle; 00182 } 00183 } 00184 00185 void disable() 00186 { 00187 mLoopState = LoopState::disabled; 00188 } 00189 00190 void enable() 00191 { 00192 mLoopState = LoopState::stopped; 00193 } 00194 00195 bool step(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, RobotSimEvents::BeeStemConfigMessagePtr & depthMsg, Waypoint currentPose) 00196 { 00197 if(mState == State::idle) 00198 { 00199 return false; 00200 } 00201 else if(mState == State::at_waypoint) 00202 { 00203 if(mCurrentWaypointIndex < mWaypoints.size()) 00204 { 00205 mState = State::move_to_waypoint; 00206 mMovementState = MovementState::set_depth; 00207 } 00208 else 00209 { 00210 stop(); 00211 disable(); 00212 return false; 00213 } 00214 } 00215 00216 if(mMovementState == MovementState::set_depth) 00217 { 00218 //make sure to do this only once per waypoint; the PID loop will handle the rest 00219 //set depth in pid 00220 if(operator[](-1).mDepth != 0) 00221 { 00222 depthMsg = new RobotSimEvents::BeeStemConfigMessage; 00223 depthMsg->desiredDepth = operator[](-1).mDepth; 00224 depthMsg->updateDepthPID = 1; 00225 depthMsg->depthK; 00226 depthMsg->depthP; 00227 depthMsg->depthI; 00228 depthMsg->depthD; 00229 } 00230 mMovementState = MovementState::rotate; 00231 return false; 00232 } 00233 00234 //step as long as mState == move_to_waypoint 00235 00236 //do checks to see if mode needs to be stepped down to correct angle or translate 00237 desiredAngle = LocalizationUtil::vectorTo(currentPose.mPoint, mWaypoints[mCurrentWaypointIndex].mPoint).j; 00238 distance = sqrt(pow(currentPose.mPoint.i - mWaypoints[mCurrentWaypointIndex].mPoint.i, 2) + pow(currentPose.mPoint.j - mWaypoints[mCurrentWaypointIndex].mPoint.j, 2)) - operator[](-1).mRadius; 00239 if(distance < 0) 00240 { 00241 distance = 0; 00242 } 00243 float angleDiff = 180.0 * LocalizationUtil::linearAngleDiffRatio(currentPose.mOrientation, desiredAngle); 00244 if(mMovementState == MovementState::rotate || mMovementState == MovementState::translate || mMovementState == MovementState::rotate_moving) 00245 { 00246 //cout << "desired angle to waypoint: " << desiredAngle << endl; 00247 //cout << "current heading: " << currentPose.mOrientation << endl; 00248 if(distance <= translateThresholdRadius || (mMovementState == MovementState::translate && angleDiff < rotateThresholdRadius2) || !getMsgToFaceAngle(msg, currentPose.mOrientation, desiredAngle)) 00249 { 00250 mMovementState = MovementState::translate; 00251 } 00252 else if(mMovementState == MovementState::translate || mMovementState == MovementState::rotate_moving) 00253 { 00254 mMovementState = MovementState::rotate_moving; 00255 } 00256 else 00257 { 00258 mMovementState = MovementState::rotate; 00259 } 00260 } 00261 if(mMovementState == MovementState::translate || mMovementState == MovementState::rotate_moving) 00262 { 00263 if(distance <= translateThresholdRadius) 00264 { 00265 mMovementState = MovementState::rotate2; 00266 } 00267 else 00268 { 00269 int val = -85; 00270 //msg->axisName = XBox360RemoteControl::Keys::Actions::toString[XBox360RemoteControl::Keys::Actions::SPEED]; 00271 //msg->axisVal = -85; 00272 if(distance <= translateThresholdRadius2) 00273 { 00274 val *= (distance - translateThresholdRadius) / (translateThresholdRadius2 - translateThresholdRadius); 00275 } 00276 val -= 15; 00277 00278 setMotorVal(msg, BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER, -val); 00279 setMotorVal(msg, BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER, val); 00280 } 00281 } 00282 if(mMovementState == MovementState::rotate2) 00283 { 00284 //cout << "desired angle (of waypoint): " << mWaypoints[mCurrentWaypointIndex].mOrientation << endl; 00285 //cout << "current heading: " << currentPose.mOrientation << endl; 00286 if(!getMsgToFaceAngle(msg, currentPose.mOrientation, mWaypoints[mCurrentWaypointIndex].mOrientation)) 00287 { 00288 mMovementState = MovementState::idle; 00289 mState = State::at_waypoint; //allow everyone to see that we're at the waypoint; this will be changed to idle during the next step() 00290 mCurrentWaypointIndex ++; //get ready to go to the next waypoint 00291 return false; 00292 } 00293 else 00294 { 00295 setMotorVal(msg, BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER, 0); 00296 setMotorVal(msg, BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER, 0); 00297 } 00298 } 00299 00300 return true; 00301 } 00302 00303 void setMotorVal(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, int i, int val) 00304 { 00305 msg->values[i] = val; 00306 msg->mask[i] = 1; 00307 } 00308 00309 bool getMsgToFaceAngle(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, float currentAngle, float desiredAngle) 00310 { 00311 float tempAngle = desiredAngle - currentAngle; 00312 if(abs(tempAngle) <= rotateThresholdRadius) //we're already close enough to the specified angle 00313 { 00314 return false; 00315 } 00316 else 00317 { 00318 int val = round(95.0f * LocalizationUtil::linearAngleDiffRatio(currentAngle, desiredAngle)) + 5; 00319 //msg->axisName = XBox360RemoteControl::Keys::Actions::toString[XBox360RemoteControl::Keys::Actions::HEADING]; 00320 //msg->axisVal = round(95.0f * LocalizationUtil::linearAngleDiffRatio(currentAngle, desiredAngle)) + 5; 00321 //cout << ">>" << tempAngle << "|" << msg->axisVal << endl; 00322 if((abs(tempAngle) < 180 && tempAngle >= 0) || (abs(tempAngle) >= 180 && tempAngle < 0)) //conditions for turning left 00323 { 00324 val *= -1; 00325 } 00326 val *= -1; 00327 setMotorVal(msg, BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER, -val); 00328 setMotorVal(msg, BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER, val); 00329 //cout << ">>>" << msg->axisVal << endl; 00330 } 00331 return true; 00332 } 00333 00334 int mLoopState; 00335 int mID; 00336 int mState; 00337 int mMovementState; 00338 int mCurrentWaypointIndex; 00339 vector<Waypoint> mWaypoints; 00340 }; 00341 00342 class ParticleFilter: public RobotBrainComponent 00343 { 00344 00345 public: 00346 struct SimulationState 00347 { 00348 const static int disabled = -1; 00349 const static int idle = 0; 00350 const static int running = 1; 00351 }; 00352 struct InitializationState 00353 { 00354 const static int uninitialized = 0; 00355 const static int controllerChecked = 1; 00356 const static int initialized = 2; 00357 }; 00358 struct GraphicsFlags 00359 { 00360 struct SnapState 00361 { 00362 const static int disable = 0; 00363 const static int snapToAvgParticle = 1; 00364 const static int snapToRealParticle = 2; 00365 const static int snapToCurrentWaypoint = 3; 00366 }; 00367 00368 bool drawLines; 00369 int mSnapState; 00370 00371 GraphicsFlags() 00372 { 00373 drawLines = true; 00374 mSnapState = SnapState::snapToCurrentWaypoint; 00375 } 00376 }; 00377 struct SensorFlags //signals whether these sensors are enabled; set automatically based on detection of related ICE messages 00378 { 00379 bool BinFinder; 00380 bool BuoyFinder; 00381 bool CompassSensor; 00382 bool Hydrohpones; 00383 bool PipeSensor; 00384 bool RectangleSensor; 00385 00386 SensorFlags() 00387 { 00388 BinFinder = false; 00389 BuoyFinder = false; 00390 CompassSensor = false; 00391 Hydrohpones = false; 00392 PipeSensor = false; 00393 RectangleSensor = false; 00394 } 00395 }; 00396 00397 //!Constructor 00398 ParticleFilter(int id, OptionManager& mgr, XBox360RemoteControlI * controller, const std::string& descrName = 00399 "ParticleFilter", const std::string& tagName = "ParticleFilter"); 00400 00401 //!Destructor 00402 ~ParticleFilter(); 00403 00404 //!Main run loop 00405 virtual void evolve(); 00406 00407 //!Get a message 00408 virtual void updateMessage(const RobotSimEvents::EventMessagePtr& eMsg, 00409 const Ice::Current&); 00410 00411 //!Component registers itself as publisher/subscriber to topic(s) 00412 virtual void registerTopics(); 00413 00414 void simulateMovement(int mode, int movementMode, RobotSimEvents::BeeStemMotorControllerMessagePtr & msg); 00415 void startSimulation() 00416 { 00417 if(mSimulationState == SimulationState::idle) 00418 { 00419 mSimulationState = SimulationState::running; 00420 } 00421 } 00422 00423 void stopSimulation() 00424 { 00425 if(mSimulationState == SimulationState::running) 00426 { 00427 mSimulationState = SimulationState::idle; 00428 } 00429 } 00430 00431 void disableSimulation() 00432 { 00433 mSimulationState = SimulationState::disabled; 00434 } 00435 00436 static void getMotorControllerMsg(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, int mc0, int mc1, int mc2, int mc3, int mc4, int mc5, int mc6, int mc7, int mc8); 00437 00438 private: 00439 void Init(); 00440 bool tryToRecoverState(string uri, LocalizationParticle::State &s); 00441 bool tryToSaveState(string uri, LocalizationParticle::State s); 00442 bool readDefaultConfigFile(bool firstAttempt); 00443 bool writeDefaultConfigFile(bool firstAttempt); 00444 void step(); 00445 void updateGraphics(); 00446 void calculateAvgParticle(); 00447 void resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::Ids actionId); 00448 void resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::Ids actionId, RobotSimEvents::BeeStemMotorControllerMessagePtr & msg); 00449 bool dynamicsEnabled(); 00450 vector<LocalizationParticle> resampleParticles( 00451 vector<LocalizationParticle> oldParticles); 00452 void reweighParticles(vector<LocalizationParticle> &p); 00453 void processControllerInput(ParamData pd); 00454 void stop2(); 00455 00456 int mSimulationState; 00457 int mInitializationState; 00458 00459 GraphicsFlags mGraphicsFlags; 00460 SensorFlags mSensorFlags; 00461 00462 ParamData mParamData; //for configuration 00463 Camera mCam; 00464 ColorSpectrum particleColor; 00465 00466 LocalizationMap mLocMap; 00467 LocalizationParticle realParticle; 00468 LocalizationParticle avgParticle; 00469 LocalizationWaypointController::Waypoint mCurrentPose; 00470 LocalizationWaypointController mWaypointController; 00471 00472 vector<LocalizationParticle> particles; 00473 00474 CompassSensor * mCompassSensor; 00475 PipeSensor * mPipeSensor; 00476 RectangleSensor * mRectangleSensor; 00477 BuoySensor * mBuoySensor; 00478 00479 long lastTimeTemp; 00480 float dt; 00481 float lastTime; 00482 int motorSpeedX; 00483 int strafeValue; 00484 int motorSpeedY; 00485 bool mNeedsResampling; 00486 00487 //int timescaleExp; // timescale for visualization 00488 float timescale; 00489 00490 XBox360RemoteControlI * mController; 00491 IceUtil::Mutex itsJSMutex; 00492 vector<int> itsJSValues; 00493 vector<int> itsButValues; 00494 00495 nub::ref<OutputFrameSeries> itsOfs; 00496 Image<PixRGB<byte> > itsMap; 00497 OModelParam<int> mDisableGraphics; 00498 }; 00499 00500 #endif