00001 #include "ParticleFilter.H" 00002 00003 #include "Component/ModelParam.H" 00004 #include "Component/ModelOptionDef.H" 00005 #include "Image/DrawOps.H" 00006 #include "LocalizationUtil.h" 00007 //#include "Raster/Raster.H" 00008 #include <time.h> 00009 #include <iostream> 00010 #include <fstream> 00011 #include <sstream> 00012 #include <stdexcept> 00013 00014 #ifndef ParticleFilter_C 00015 #define ParticleFilter_C 00016 00017 using namespace std; 00018 00019 const string mConfigFileURI = "/home/uscr/.seabee/ParticleFilterParams.txt"; 00020 const string mSavedStateURI = "/home/uscr/.seabee/ParticleFilterState.txt"; 00021 00022 const ModelOptionCateg MOC_SeaBeeIIIParticleFilter = { 00023 MOC_SORTPRI_3, "Options" }; 00024 00025 const ModelOptionDef OPT_DisableGraphics = 00026 { MODOPT_ARG(int), "DisableGraphics", &MOC_SeaBeeIIIParticleFilter, OPTEXP_CORE, 00027 "Prevent the particle filter from performing graphics calculations", 00028 "disable-graphics", '\0', "<int>", "0" }; 00029 00030 // ###################################################################### 00031 ParticleFilter::ParticleFilter(int id, OptionManager& mgr, XBox360RemoteControlI * controller, 00032 const std::string& descrName, const std::string& tagName) : 00033 RobotBrainComponent(mgr, descrName, tagName), itsOfs(new OutputFrameSeries( 00034 mgr)), 00035 mDisableGraphics(&OPT_DisableGraphics, this, 0) 00036 { 00037 addSubComponent(itsOfs); 00038 00039 itsJSValues.resize(8); 00040 itsButValues.resize(20); 00041 00042 mSimulationState = SimulationState::idle; 00043 mInitializationState = InitializationState::uninitialized; 00044 00045 mController = controller; 00046 } 00047 00048 ParticleFilter::~ParticleFilter() 00049 { 00050 delete mController; 00051 delete mBuoySensor; 00052 delete mCompassSensor; 00053 delete mPipeSensor; 00054 delete mRectangleSensor; 00055 } 00056 00057 void ParticleFilter::stop2() 00058 { 00059 resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::NONE); 00060 } 00061 00062 bool ParticleFilter::tryToRecoverState(string uri, LocalizationParticle::State &state) 00063 { 00064 ifstream ifs(uri.c_str(), ifstream::in); 00065 if (!ifs.good()) 00066 { 00067 LERROR("Couldn't find file.\n"); 00068 return false; 00069 } 00070 else 00071 { 00072 map<std::string, float> theParams; 00073 00074 bool go = ifs.good(); 00075 int pos2 = 0; 00076 int pos3 = -1; 00077 00078 while (go) 00079 { 00080 float theValue; 00081 string theTag; 00082 stringstream ss; 00083 string s; 00084 ifs >> s; 00085 if (ifs.good()) 00086 { 00087 int pos1 = pos3 + 1; 00088 pos2 = s.find("=", pos1); 00089 pos3 = s.find("\n", pos2); 00090 theTag = s.substr(pos1, pos2 - pos1); 00091 pos2 ++; 00092 ss << s.substr(pos2, pos3 - pos2); 00093 ss >> theValue; 00094 theParams[theTag] = theValue; 00095 } 00096 else 00097 { 00098 go = false; 00099 } 00100 } 00101 00102 ifs.close(); 00103 00104 state.mAngle = theParams["heading"]; 00105 state.mPoint.i = theParams["p_x"]; 00106 state.mPoint.j = theParams["p_y"]; 00107 state.mWeight = 0; 00108 mWaypointController.mCurrentWaypointIndex = theParams["wp_current_index"]; 00109 mWaypointController.mState = theParams["wp_state"]; 00110 mWaypointController.mLoopState = theParams["wp_loopstate"]; 00111 mWaypointController.mMovementState = theParams["wp_movstate"]; 00112 int numWaypoints = theParams["num_waypoints"]; 00113 stringstream ss; 00114 string s0, s1, s2, s3, s4; 00115 for(int i = 0; i < numWaypoints; i ++) 00116 { 00117 ss << "wp" << i << "_p_x"; s0 = ss.str(); ss.str(""); 00118 ss << "wp" << i << "_p_y"; s1 = ss.str(); ss.str(""); 00119 ss << "wp" << i << "_depth"; s2 = ss.str(); ss.str(""); 00120 ss << "wp" << i << "_heading"; s3 = ss.str(); ss.str(""); 00121 ss << "wp" << i << "_rad"; s4 = ss.str(); ss.str(""); 00122 LocalizationWaypointController::Waypoint wp(Point2D<float> (theParams[s0], theParams[s1]), theParams[s2], theParams[s3], theParams[s4]); 00123 mWaypointController.addWaypoint(wp); 00124 } 00125 } 00126 return true; 00127 } 00128 00129 bool ParticleFilter::tryToSaveState(string uri, LocalizationParticle::State s) 00130 { 00131 ofstream ofs(uri.c_str()); 00132 if (ofs.good()) 00133 { 00134 map<std::string, float> state; 00135 00136 state["heading"] = s.mAngle; 00137 state["p_x"] = s.mPoint.i; 00138 state["p_y"] = s.mPoint.j; 00139 state["wp_current_index"] = mWaypointController.mCurrentWaypointIndex; 00140 state["wp_state"] = mWaypointController.mState; 00141 state["wp_loopstate"] = mWaypointController.mLoopState; 00142 state["wp_movstate"] = mWaypointController.mMovementState; 00143 state["num_waypoints"] = mWaypointController.mWaypoints.size(); 00144 stringstream ss; 00145 string s0, s1, s2, s3, s4; 00146 for(unsigned int i = 0; i < mWaypointController.mWaypoints.size(); i ++) 00147 { 00148 ss << "wp" << i << "_p_x"; s0 = ss.str(); ss.str(""); 00149 ss << "wp" << i << "_p_y"; s1 = ss.str(); ss.str(""); 00150 ss << "wp" << i << "_depth"; s2 = ss.str(); ss.str(""); 00151 ss << "wp" << i << "_heading"; s3 = ss.str(); ss.str(""); 00152 ss << "wp" << i << "_rad"; s4 = ss.str(); ss.str(""); 00153 state[s0] = mWaypointController[i].mPoint.i; 00154 state[s1] = mWaypointController[i].mPoint.j; 00155 state[s2] = mWaypointController[i].mDepth; 00156 state[s3] = mWaypointController[i].mOrientation; 00157 state[s4] = mWaypointController[i].mRadius; 00158 } 00159 00160 map<std::string, float>::iterator it; 00161 for(it = state.begin(); it != state.end(); it ++) 00162 { 00163 ofs << it->first << "=" << it->second << endl; 00164 } 00165 } 00166 else 00167 { 00168 LERROR("Couldn't write to file.\n"); 00169 return false; 00170 } 00171 ofs.close(); 00172 return true; 00173 } 00174 00175 00176 00177 void ParticleFilter::Init() 00178 { 00179 if(mInitializationState == InitializationState::initialized) 00180 return; 00181 00182 /*RobotSimEvents::BeeStemMotorControllerMessagePtr mcMsg = new RobotSimEvents::BeeStemMotorControllerMessage; 00183 getMotorControllerMsg(mcMsg, 0,0,0,0,0,0,100,0,0); 00184 publish("BeeStemMotorControllerMessageTopic", mcMsg); 00185 sleep(1); 00186 getMotorControllerMsg(mcMsg, 0,0,0,0,0,0,0,100,0); 00187 publish("BeeStemMotorControllerMessageTopic", mcMsg); 00188 sleep(1); 00189 getMotorControllerMsg(mcMsg, 0,0,0,0,0,0,0,0,100); 00190 publish("BeeStemMotorControllerMessageTopic", mcMsg); 00191 sleep(1); 00192 getMotorControllerMsg(mcMsg, 0,0,0,0,0,0,0,0,0); 00193 publish("BeeStemMotorControllerMessageTopic", mcMsg);*/ 00194 00195 cout << "Initing..." << endl; 00196 00197 ParamData defaultParamData = LocalizationUtil::getDefaultParamData(mController); 00198 00199 bool readAttempt1, writeAttempt1, readAttempt2 = false; 00200 readAttempt1 = LocalizationUtil::readConfigFile(mConfigFileURI, mParamData); 00201 00202 if(!readAttempt1) 00203 { 00204 writeAttempt1 = LocalizationUtil::writeConfigFile(mConfigFileURI, defaultParamData); 00205 00206 if(writeAttempt1) 00207 { 00208 readAttempt2 = LocalizationUtil::readConfigFile(mConfigFileURI, mParamData); 00209 } 00210 00211 if(!readAttempt2) 00212 { 00213 LERROR("Using default hard-coded parameters!\n"); 00214 mParamData = defaultParamData; 00215 } 00216 } 00217 00218 LocalizationParticle::State startingState; 00219 00220 if(tryToRecoverState(mSavedStateURI, startingState)) 00221 { 00222 cout << "Successfully recovered state" << endl; 00223 } 00224 else 00225 { 00226 LERROR("Unable to recover state! Using default state...\n"); 00227 00228 startingState.mPoint = Point2D<float> (mParamData["startingstate_x"], mParamData["startingstate_y"]); 00229 startingState.mAngle = mParamData["startingstate_angle"]; 00230 00231 LocalizationWaypointController::Waypoint wp1 (Point2D<float>(-15, -30), 0, 90, 0); 00232 LocalizationWaypointController::Waypoint wp2 (Point2D<float>(-30, -15), 0, 45, 0); 00233 LocalizationWaypointController::Waypoint wp3 (Point2D<float>(-15, -15), 0, 270, 0); 00234 LocalizationWaypointController::Waypoint wp4 (Point2D<float>(-30, -30), 0, 90, 0); 00235 mWaypointController.addWaypoint(wp1); 00236 mWaypointController.addWaypoint(wp2); 00237 mWaypointController.addWaypoint(wp3); 00238 mWaypointController.addWaypoint(wp4); 00239 mWaypointController.start(); 00240 } 00241 00242 startingState.mWeight = 1.0f / (float) mParamData["num_particles"]; 00243 00244 //printf("%f|%f|%f|%d|%d\n", startingState.mAngle, startingState.mPoint.i, startingState.mPoint.j, mWaypointController.mMode, mWaypointController.mMovementMode); 00245 00246 mBuoySensor = new BuoySensor(1.0f); 00247 mCompassSensor = new CompassSensor(1.0f); 00248 mPipeSensor = new PipeSensor(1.0f); 00249 mRectangleSensor = new RectangleSensor(1.0f); 00250 00251 itsMap = Image<PixRGB<byte> > ((int)mParamData["display_width"], (int)mParamData["display_height"], ZEROS); 00252 00253 mCam.mCenter = Point2D<float> (mParamData["cam_center_x"], mParamData["cam_center_y"]); 00254 mCam.setZoom(mParamData["cam_zoom"]); 00255 00256 LocalizationParticle::State indicatorState = {Point2D<float>(-50, 0), 0, 0}; 00257 realParticle = LocalizationParticle(indicatorState); 00258 00259 avgParticle = LocalizationParticle(startingState); 00260 00261 particles.resize((int)mParamData["num_particles"]); 00262 for (int i = 0; i < (int)mParamData["num_particles"]; i++) 00263 { 00264 particles[i] = LocalizationParticle(startingState); 00265 } 00266 00267 if(mController && mController->isEnabled()) 00268 { 00269 stopSimulation(); 00270 } 00271 else 00272 { 00273 startSimulation(); 00274 } 00275 00276 lastTime = clock(); 00277 motorSpeedX = 0; 00278 motorSpeedY = 0; 00279 dt = 0.01; 00280 timescale = 1.0; 00281 00282 LocalizationMapEntity theBoundary(Point2D<float> (0.0, 0.0), 90.0, Point2D< 00283 float> (mParamData["pool_dim_x"], mParamData["pool_dim_y"]), //25 yd by 25 yd 00284 LocalizationMapEntity::ShapeType::rect, PixRGB<byte>(255, 255, 255), LocalizationMapEntity::ObjectType::boundary, LocalizationMapEntity::InteractionType::external); 00285 00286 LocalizationMapEntity pipe1 (Point2D<float> (-22.5, -15), 0.0, Point2D<float> (0.5, 2.0), LocalizationMapEntity::ShapeType::rect, PixRGB<byte>(255, 127, 0), LocalizationMapEntity::ObjectType::pipe); 00287 LocalizationMapEntity pipe2 (Point2D<float> (-22.5, -22.5), 135.0, Point2D<float> (0.5, 2.0), LocalizationMapEntity::ShapeType::rect, PixRGB<byte>(255, 127, 0), LocalizationMapEntity::ObjectType::pipe); 00288 LocalizationMapEntity pipe3 (Point2D<float> (-22.5, -30), 0.0, Point2D<float> (0.5, 2.0), LocalizationMapEntity::ShapeType::rect, PixRGB<byte>(255, 127, 0), LocalizationMapEntity::ObjectType::pipe); 00289 00290 mCurrentPose = LocalizationWaypointController::Waypoint(avgParticle.mState.mPoint, avgParticle.mState.mAngle, 0, 0); 00291 00292 mLocMap = LocalizationMap(theBoundary); 00293 mLocMap.addMapEntity(pipe1); 00294 mLocMap.addMapEntity(pipe2); 00295 mLocMap.addMapEntity(pipe3); 00296 00297 mInitializationState = InitializationState::initialized; 00298 cout << "Init finished." << endl; 00299 } 00300 00301 // ###################################################################### 00302 // ###################################################################### 00303 void ParticleFilter::registerTopics() 00304 { 00305 registerPublisher("LocalizationMessageTopic"); 00306 registerPublisher("XBox360RemoteControlMessageTopic"); 00307 registerPublisher("BeeStemConfigMessageTopic"); 00308 if(mDisableGraphics.getVal() != 0) //if graphics are disabled, display our messages instead 00309 { 00310 //registerSubscription("LocalizationMessageTopic"); 00311 } 00312 registerPublisher("BeeStemMotorControllerMessageTopic"); 00313 00314 registerSubscription("XBox360RemoteControlMessageTopic"); 00315 registerSubscription("IMUDataServerMessageTopic"); 00316 registerSubscription("BeeStemMotorControllerMessageTopic"); 00317 registerSubscription("VisionRectangleMessageTopic"); 00318 registerSubscription("PipeColorSegmentMessageTopic"); 00319 } 00320 00321 void ParticleFilter::getMotorControllerMsg(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, int mc0, int mc1, int mc2, int mc3, int mc4, int mc5, int mc6, int mc7, int mc8) 00322 { 00323 vector<int> values; 00324 values.resize(BeeStem3::NUM_MOTOR_CONTROLLERS); 00325 values[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] = mc0; 00326 values[BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER] = mc1; 00327 values[BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER] = mc2; 00328 values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER] = mc3; 00329 values[BeeStem3::MotorControllerIDs::DEPTH_LEFT_THRUSTER] = mc4; 00330 values[BeeStem3::MotorControllerIDs::DEPTH_RIGHT_THRUSTER] = mc5; 00331 values[BeeStem3::MotorControllerIDs::SHOOTER] = mc6; 00332 values[BeeStem3::MotorControllerIDs::DROPPER_STAGE1] = mc7; 00333 values[BeeStem3::MotorControllerIDs::DROPPER_STAGE2] = mc8; 00334 msg->values = values; 00335 vector<int> mask; 00336 mask.assign(9, 1); 00337 msg->mask = mask; 00338 } 00339 00340 void ParticleFilter::simulateMovement(int state, int movementState, RobotSimEvents::BeeStemMotorControllerMessagePtr & msg) 00341 { 00342 if(state != LocalizationWaypointController::State::remote) 00343 { 00344 if(state == LocalizationWaypointController::State::idle) 00345 { 00346 movementState = state; 00347 } 00348 else 00349 { 00350 cout << "{ " << state << "|" << movementState << " } "; 00351 } 00352 switch(movementState) 00353 { 00354 case LocalizationWaypointController::MovementState::rotate: 00355 motorSpeedX = -msg->values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER];//-value; 00356 motorSpeedY = 0; 00357 strafeValue = 0; 00358 break; 00359 case LocalizationWaypointController::MovementState::rotate_moving: 00360 motorSpeedX = -msg->values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER];//-value; 00361 motorSpeedY = msg->values[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] * msg->mask[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER]; 00362 strafeValue = 0; 00363 break; 00364 case LocalizationWaypointController::MovementState::translate: 00365 motorSpeedX = -msg->values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER];//-value; 00366 motorSpeedY = msg->values[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] * msg->mask[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER]; 00367 strafeValue = 0; 00368 break; 00369 case LocalizationWaypointController::MovementState::rotate2: 00370 motorSpeedX = -msg->values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER];//-value; 00371 motorSpeedY = 0; 00372 strafeValue = 0; 00373 break; 00374 case LocalizationWaypointController::MovementState::idle: 00375 motorSpeedX = 0; 00376 motorSpeedY = 0; 00377 strafeValue = 0; 00378 break; 00379 } 00380 } 00381 int speedValue = -motorSpeedY; 00382 int theAngle = 0; 00383 00384 if(abs(motorSpeedY) < abs(strafeValue)) 00385 { 00386 speedValue = -strafeValue; 00387 theAngle += 90; 00388 } 00389 00390 if(speedValue < 0) 00391 { 00392 theAngle -= 180; 00393 } 00394 00395 //float da = LocalizationParticle::getChangeInRotation((float)motorSpeedX / 100.0, timescale * dt / (float)numSteps, mParamData); 00396 00397 /*float m = LocalizationParticle::getDistance( 00398 (float) speedValue / 100.0, 00399 timescale * dt / (float) numSteps, 00400 mParamData);*/ 00401 00402 particles = LocalizationParticle::stepParticles(particles, timescale * dt / (float) numSteps, motorSpeedX, speedValue, theAngle, mParamData); 00403 } 00404 00405 void ParticleFilter::step() 00406 { 00407 if(mInitializationState != InitializationState::initialized) 00408 { 00409 cout << "waiting for initialization" << endl; 00410 return; 00411 } 00412 static int myTimer1 = -1; //reweigh 00413 static int myTimer2 = -1; //resample 00414 static int myTimer3 = -1; //save state 00415 00416 //update loop 00417 00418 for (int i = 0; i < (int) ceil(timescale); i++) 00419 { 00420 if(reWeighDelay > 0) 00421 { 00422 myTimer1++; 00423 } 00424 if(resampleDelay > 0) 00425 { 00426 myTimer2++; 00427 } 00428 00429 if (myTimer1 >= reWeighDelay) 00430 { 00431 reweighParticles(particles); 00432 00433 myTimer1 = -1; 00434 } 00435 if (myTimer2 >= resampleDelay) 00436 { 00437 particles = resampleParticles(particles); 00438 00439 myTimer2 = -1; 00440 } 00441 00442 /*if(mController && mController->isEnabled()) 00443 { 00444 simulateMovement(LocalizationWaypointController::MovementState::full, 0); 00445 } 00446 else if(mWaypointController.mMode == LocalizationWaypointController::State::idle) 00447 { 00448 //just sit there and drift 00449 simulateMovement(LocalizationWaypointController::MovementState::idle, 0); 00450 }*/ 00451 00452 /*int speedValue = -motorSpeedY; 00453 int theAngle = 0; 00454 00455 if(abs(motorSpeedY) < abs(strafeValue)) 00456 { 00457 speedValue = -strafeValue; 00458 theAngle += 90; 00459 } 00460 00461 if(speedValue < 0) 00462 { 00463 theAngle -= 180; 00464 } 00465 00466 float da = LocalizationParticle::getChangeInRotation((float)motorSpeedX / 100.0, timescale * dt / (float)numSteps, mParamData); 00467 00468 float m = LocalizationParticle::getDistance( 00469 (float) speedValue / 100.0, timescale * dt / (float) numSteps, 00470 mParamData); 00471 */ 00472 //realParticle.updatePosition(m, da + theAngle + realParticle.mState.mAngle); 00473 //realParticle.updatePosition(m, da + theAngle + realParticle.mState.mAngle); 00474 //--->realParticle.mState.mAngle += da; 00475 float theCompassAngle = realParticle.mState.mAngle; 00476 if(mSensorFlags.CompassSensor) 00477 { 00478 theCompassAngle = RobotSimEvents::IMUDataServerMessagePtr::dynamicCast(mCompassSensor->mRealMsg)->orientation[2] + 90; 00479 if(theCompassAngle < 0) 00480 { 00481 theCompassAngle += 360; 00482 } 00483 } 00484 realParticle.mState.mAngle = theCompassAngle; 00485 //particles = LocalizationParticle::stepParticles(particles, timescale * dt / (float) numSteps, motorSpeedX, speedValue, theAngle, mParamData); 00486 00487 //realParticle.mState.mAngle = IMUCompass.mData[0] + mParamData["startingstate_angle"]; 00488 00489 //printf("%f, %f, %f\n", avgParticle.mState.mPoint.i, avgParticle.mState.mPoint.j, avgParticle.mState.mAngle); 00490 calculateAvgParticle(); 00491 00492 if(saveStateDelay > 0) 00493 { 00494 myTimer3++; 00495 } 00496 if(myTimer3 >= saveStateDelay) 00497 { 00498 cout << "saving state..."; 00499 if(tryToSaveState(mSavedStateURI, avgParticle.mState)) 00500 { 00501 cout << "success" << endl; 00502 } 00503 else 00504 { 00505 cout << "failed" << endl; 00506 } 00507 00508 myTimer3 = -1; 00509 } 00510 } 00511 } 00512 00513 void ParticleFilter::updateGraphics() 00514 { 00515 static int display_width = (int)mParamData["display_width"]; 00516 static int display_height = (int)mParamData["display_height"]; 00517 itsMap = Image<PixRGB<byte> > (display_width, display_height, ZEROS); 00518 00519 mLocMap.drawMe(itsMap, mCam); 00520 00521 switch(mGraphicsFlags.mSnapState) 00522 { 00523 case GraphicsFlags::SnapState::disable: 00524 break; 00525 case GraphicsFlags::SnapState::snapToRealParticle: 00526 mCam.mCenter.i = realParticle.mState.mPoint.i; 00527 mCam.mCenter.j = -realParticle.mState.mPoint.j; 00528 break; 00529 case GraphicsFlags::SnapState::snapToAvgParticle: 00530 mCam.mCenter.i = avgParticle.mState.mPoint.i; 00531 mCam.mCenter.j = -avgParticle.mState.mPoint.j; 00532 break; 00533 case GraphicsFlags::SnapState::snapToCurrentWaypoint: 00534 if(mWaypointController.mState > LocalizationWaypointController::State::idle) 00535 { 00536 mCam.mCenter.i = mWaypointController[-1].mPoint.i; 00537 mCam.mCenter.j = -mWaypointController[-1].mPoint.j; 00538 } 00539 break; 00540 } 00541 00542 Point2D<int> thePoint(0, 0); 00543 static PixRGB<byte> theColor; 00544 static PixRGB<byte> theActiveWaypointColor(0, 255, 0); 00545 float base_rad; 00546 00547 for (unsigned int i = 0; i < particles.size(); i++) 00548 { 00549 thePoint = Point2D<int> (round((particles[i].mState.mPoint.i - mCam.mCenter.i) * mCam.mScale), -round((particles[i].mState.mPoint.j + mCam.mCenter.j) * mCam.mScale)); 00550 thePoint += Point2D<int> (itsMap.getWidth() / 2, itsMap.getHeight() / 2); 00551 00552 theColor = LocalizationUtil::RGBFromHSV(0.33f * 360.0f * (1.0 / particleColor.spectrum) * (particles[i].mState.mWeight - particleColor.minValue), 1.0, 1.0); 00553 00554 if (mGraphicsFlags.drawLines) 00555 { 00556 drawLine(itsMap, thePoint, particles[i].mState.mAngle * D_DEGREE, (6.0 / 12.0) * mCam.mScale, theColor, 1); 00557 } 00558 else 00559 { 00560 Dims theDim = Dims(mCam.mScale * (12.0 / 12.0), mCam.mScale * (24.0 / 12.0)); 00561 Rectangle theRect = Rectangle(Point2D<int> (thePoint.i - theDim.w() / 2, thePoint.j - theDim.h() / 2), theDim); 00562 drawRectOR(itsMap, theRect, theColor, 1, D_DEGREE * (particles[i].mState.mAngle + 90)); 00563 } 00564 } 00565 00566 theColor = PixRGB<byte>(255, 255, 255); 00567 00568 for(unsigned int i = 0; i < mWaypointController.mWaypoints.size(); i ++) 00569 { 00570 PixRGB<byte> color = theColor; 00571 if(i == mWaypointController.mCurrentWaypointIndex) 00572 { 00573 color = theActiveWaypointColor; 00574 } 00575 base_rad = mWaypointController[i].mRadius; 00576 if(base_rad <= 0) 00577 { 00578 base_rad = 1; 00579 } 00580 thePoint = Point2D<int> (round((mWaypointController[i].mPoint.i - mCam.mCenter.i) * mCam.mScale), -round((mWaypointController[i].mPoint.j + mCam.mCenter.j) * mCam.mScale)); 00581 thePoint += Point2D<int> (itsMap.getWidth() / 2, itsMap.getHeight() / 2); 00582 00583 drawLine(itsMap, thePoint, mWaypointController[i].mOrientation * D_DEGREE, (12.0 / 12.0) * (2 * base_rad) * mCam.mScale, color, 1); 00584 00585 drawCircle(itsMap, thePoint, (12 / 12.0) * (mWaypointController[i].mRadius + LocalizationWaypointController::translateThresholdRadius) * mCam.mScale, PixRGB<byte> (255, 0, 0), 1); 00586 drawCircle(itsMap, thePoint, (12 / 12.0) * (mWaypointController[i].mRadius + LocalizationWaypointController::translateThresholdRadius2) * mCam.mScale, PixRGB<byte> (255, 255, 0), 1); 00587 00588 drawCircle(itsMap, thePoint, (12 / 12.0) * base_rad * mCam.mScale, color, 1); 00589 00590 thePoint += Point2D<int> ((12.0 / 12.0) * base_rad * mCam.mScale * cos(D_DEGREE * mWaypointController[i].mOrientation), -(12.0 / 12.0) * base_rad * mCam.mScale * sin(D_DEGREE * mWaypointController[i].mOrientation)); 00591 drawCircle(itsMap, thePoint, (3.0 / 12.0) * mCam.mScale, color, 1); 00592 } 00593 base_rad = mWaypointController[-1].mRadius; 00594 if(base_rad <= 0) 00595 { 00596 base_rad = 1; 00597 } 00598 thePoint = Point2D<int> (round((mCurrentPose.mPoint.i - mCam.mCenter.i) * mCam.mScale), -round((mCurrentPose.mPoint.j + mCam.mCenter.j) * mCam.mScale)); 00599 thePoint += Point2D<int> (itsMap.getWidth() / 2, itsMap.getHeight() / 2); 00600 thePoint += Point2D<int> ((12.0 / 12.0) * 0.5f * (mWaypointController.distance - base_rad) * mCam.mScale * cos(D_DEGREE * mCurrentPose.mOrientation), -(12.0 / 12.0) * 0.5f * (mWaypointController.distance - base_rad) * mCam.mScale * sin(D_DEGREE * mCurrentPose.mOrientation)); 00601 drawLine(itsMap, thePoint, mCurrentPose.mOrientation * D_DEGREE, (12.0 / 12.0) * (mWaypointController.distance - base_rad) * mCam.mScale, theActiveWaypointColor, 1); 00602 00603 thePoint = Point2D<int> (round((avgParticle.mState.mPoint.i - mCam.mCenter.i) * mCam.mScale), -round((avgParticle.mState.mPoint.j + mCam.mCenter.j) * mCam.mScale)); 00604 thePoint += Point2D<int> (itsMap.getWidth() / 2, itsMap.getHeight() / 2); 00605 00606 drawLine(itsMap, thePoint, avgParticle.mState.mAngle * D_DEGREE, (24.0 / 12.0) * mCam.mScale, PixRGB<byte> (0, 0, 255), 1); 00607 00608 drawCircle(itsMap, thePoint, (12.0 / 12.0) * mCam.mScale, PixRGB<byte> (0, 0, 255), 1); 00609 thePoint += Point2D<int> ((12.0 / 12.0) * mCam.mScale * cos(D_DEGREE * avgParticle.mState.mAngle), -(12.0 / 12.0) * mCam.mScale * sin(D_DEGREE * avgParticle.mState.mAngle)); 00610 drawCircle(itsMap, thePoint, (3.0 / 12.0) * mCam.mScale, PixRGB<byte> (0, 0, 255), 1); 00611 00612 thePoint = Point2D<int> (round((realParticle.mState.mPoint.i - mCam.mCenter.i) * mCam.mScale), -round((realParticle.mState.mPoint.j + mCam.mCenter.j) * mCam.mScale)); 00613 thePoint += Point2D<int> (itsMap.getWidth() / 2, itsMap.getHeight() / 2); 00614 00615 drawLine(itsMap, thePoint, realParticle.mState.mAngle * D_DEGREE, (24.0 / 12.0) * mCam.mScale, PixRGB<byte> (0, 255, 0), 2); 00616 drawCircle(itsMap, thePoint, (12.0 / 12.0) * mCam.mScale, PixRGB<byte> (0, 255, 0), 1); 00617 00618 itsOfs->writeRGB(itsMap, "Particle Map"); 00619 } 00620 00621 void ParticleFilter::calculateAvgParticle() 00622 { 00623 avgParticle.mState.mPoint.i = 0.0; 00624 avgParticle.mState.mPoint.j = 0.0; 00625 avgParticle.mState.mAngle = 0.0; 00626 00627 for (unsigned int i = 0; i < particles.size(); i++) 00628 { 00629 avgParticle.mState.mPoint.i += particles[i].mState.mPoint.i * particles[i].mState.mWeight; 00630 avgParticle.mState.mPoint.j += particles[i].mState.mPoint.j * particles[i].mState.mWeight; 00631 avgParticle.mState.mAngle += particles[i].mState.mAngle * particles[i].mState.mWeight; 00632 } 00633 00634 avgParticle.mState.mAngle = LocalizationParticle::normalizeAngle(avgParticle.mState.mAngle); 00635 00636 mCurrentPose.mPoint = avgParticle.mState.mPoint; 00637 mCurrentPose.mOrientation = avgParticle.mState.mAngle; 00638 } 00639 00640 bool ParticleFilter::dynamicsEnabled() 00641 { 00642 return mInitializationState == InitializationState::initialized && (mSensorFlags.BinFinder || mSensorFlags.BuoyFinder || mSensorFlags.CompassSensor || mSensorFlags.Hydrohpones || mSensorFlags.PipeSensor || mSensorFlags.RectangleSensor); 00643 } 00644 00645 // Use a roulette wheel to probabilistically duplicate particles with high weights, 00646 // and discard those with low weights. A ‘Particle’ is some structure that has 00647 // a weight element w. The sum of all w’s in oldParticles should equal 1. 00648 vector<LocalizationParticle> ParticleFilter::resampleParticles(vector< 00649 LocalizationParticle> oldParticles) 00650 { 00651 if(!mNeedsResampling) 00652 return oldParticles; 00653 00654 LINFO("\n\n\nresampling...\n\n\n"); 00655 00656 vector<LocalizationParticle> newParticles; 00657 newParticles.resize(oldParticles.size()); 00658 /// Calculate a Cumulative Distribution Function for our particle weights 00659 vector<double> CDF; 00660 00661 CDF.resize(oldParticles.size()); 00662 00663 CDF[0] = oldParticles[0].mState.mWeight; 00664 00665 for (uint i = 1; i < CDF.size(); i++) 00666 { 00667 CDF[i] = CDF[i - 1] + oldParticles[i].mState.mWeight; 00668 } 00669 00670 // Loop through our particles as if spinning a roulette wheel. 00671 // The random u that we start with determines the initial offset 00672 // and each particle will have a probability of getting landed on and 00673 // saved proportional to it’s posterior probability. If a particle has a very large 00674 // posterior, then it may get landed on many times. If a particle has a very low 00675 // posterior, then it may not get landed on at all. By incrementing by 00676 // 1/(NUM_PARTICLES) we ensure that we don’t change the number of particles in our 00677 // returned set. 00678 uint i = 0; 00679 double u = randomDouble() * 1.0 / double(oldParticles.size()); 00680 00681 for (uint j = 0; j < oldParticles.size(); j++) 00682 { 00683 while (u > CDF[i]) 00684 { 00685 i++; 00686 } 00687 LocalizationParticle p = oldParticles[i]; 00688 p.mState.mWeight = 1.0 / double(oldParticles.size()); //all resampled particles will have the same weight... 00689 newParticles[j] = p; 00690 u += 1.0 / double(oldParticles.size()); 00691 } 00692 mNeedsResampling = false; 00693 return newParticles; 00694 } 00695 00696 //use sensor data here to adjust particle weights 00697 void ParticleFilter::reweighParticles(vector<LocalizationParticle> &p) 00698 { 00699 if(!dynamicsEnabled()) 00700 return; 00701 00702 //printf("\n\nreweighing :D\n\n"); 00703 00704 for (uint i = 0; i < p.size(); i++) 00705 { 00706 float totalScale = 0.0; 00707 //cout << p.size() << endl; 00708 p[i].mState.mWeight = 0.0; 00709 00710 //if(mSensorFlags.BinFinder && mBinSensor->inited); 00711 //--do this for each sensor 00712 if(mSensorFlags.BuoyFinder && mBuoySensor->inited) 00713 { 00714 VirtualSensorMessage::BuoyColorSegmentMessage * msgptr = static_cast<VirtualSensorMessage::BuoyColorSegmentMessage*>(BuoySensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00715 mBuoySensor->takeVirtualReading(msgptr); 00716 p[i].mState.mWeight += mBuoySensor->getScaledVote(); 00717 totalScale += mBuoySensor->mScale; 00718 delete msgptr; 00719 } 00720 //-- 00721 if(mSensorFlags.CompassSensor && mCompassSensor->inited) 00722 { 00723 VirtualSensorMessage::IMUDataServerMessage * msgptr = static_cast<VirtualSensorMessage::IMUDataServerMessage*>(CompassSensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00724 mCompassSensor->takeVirtualReading(msgptr); 00725 p[i].mState.mWeight += mCompassSensor->getScaledVote(); 00726 totalScale += mCompassSensor->mScale; 00727 delete msgptr; 00728 } 00729 //if(mSensorFlags.Hydrohpones && mHydrophonesSensor->inited); 00730 if(mSensorFlags.PipeSensor && mPipeSensor->inited && mSensorFlags.RectangleSensor && mRectangleSensor->inited) 00731 { 00732 printf("pipe and rectangle sensors enabled\n"); 00733 VirtualSensorMessage::PipeColorSegmentMessage * msgptr1 = static_cast<VirtualSensorMessage::PipeColorSegmentMessage*>(PipeSensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00734 VirtualSensorMessage::VisionRectangleMessage * msgptr2 = static_cast<VirtualSensorMessage::VisionRectangleMessage*>(RectangleSensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00735 mPipeSensor->takeVirtualReading(msgptr1); 00736 //p[i].mState.mWeight = mPipeSensor->getScaledVote(); 00737 //totalScale += mPipeSensor->mScale; 00738 mRectangleSensor->takeVirtualReading(msgptr2); 00739 float temp = mPipeSensor->getScaledVote() * mRectangleSensor->getScaledVote(); //we need to see orange AND see a rectangle 00740 if(temp != 0) //only change it if it's not zero, otherwise we trigger resampling 00741 { 00742 printf("\n\n\n\n\nparticle can see an orange rectangle\n\n\n\n\n"); 00743 p[i].mState.mWeight = 2 * temp; 00744 totalScale += mPipeSensor->mScale + mRectangleSensor->mScale; 00745 } 00746 delete msgptr1; 00747 //delete msgptr2; 00748 } 00749 /*if(mSensorFlags.PipeSensor && mPipeSensor->inited) 00750 { 00751 VirtualSensorMessage::PipeColorSegmentMessage * msgptr1 = static_cast<VirtualSensorMessage::PipeColorSegmentMessage*>(PipeSensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00752 mPipeSensor->takeVirtualReading(msgptr1); 00753 p[i].mState.mWeight += mPipeSensor->getScaledVote(); 00754 totalScale += mPipeSensor->mScale; 00755 delete msgptr1; 00756 } 00757 if(mSensorFlags.RectangleSensor && mRectangleSensor->inited) 00758 { 00759 VirtualSensorMessage::VisionRectangleMessage * msgptr2 = static_cast<VirtualSensorMessage::VisionRectangleMessage*>(RectangleSensor::generatetVirtualSensorMessage(p[i], mLocMap)); 00760 mRectangleSensor->takeVirtualReading(msgptr2); 00761 p[i].mState.mWeight += mRectangleSensor->getScaledVote(); 00762 totalScale += mRectangleSensor->mScale; 00763 delete msgptr2; 00764 }*/ 00765 if(abs(totalScale) > 0) 00766 { 00767 p[i].mState.mWeight /= totalScale; 00768 } 00769 00770 //sum += p[i].mState.mWeight; 00771 00772 } 00773 00774 float largest = p[0].mState.mWeight; 00775 float smallest = p[0].mState.mWeight; 00776 00777 //calculate the largest and smallest values 00778 for (uint i = 0; i < p.size(); i++) 00779 { 00780 if (p[i].mState.mWeight > largest) 00781 { 00782 largest = p[i].mState.mWeight; 00783 } 00784 if (p[i].mState.mWeight < smallest) 00785 { 00786 smallest = p[i].mState.mWeight; 00787 } 00788 } 00789 00790 float sum = 0.0f; 00791 00792 if(smallest == largest) //likely all zeros; regardless, prepare to set all weights to 1/p.size() 00793 { 00794 sum = p.size(); 00795 } 00796 /*else //set sum to what it will be after stretching the data 00797 { 00798 sum = (sum - (smallest * p.size())) / (largest - smallest); 00799 }*/ 00800 00801 //reorganize particles so that small changes can be amplified 00802 //stretch data so that smallest->0 and largest->1 00803 //calculate sum if necessary 00804 for(uint i = 0; i < p.size(); i ++) 00805 { 00806 if(smallest == largest) 00807 { 00808 p[i].mState.mWeight = 1.0f / sum; 00809 } 00810 else 00811 { 00812 p[i].mState.mWeight -= smallest; 00813 p[i].mState.mWeight /= (largest - smallest); 00814 p[i].mState.mWeight = pow(p[i].mState.mWeight, 1); 00815 sum += p[i].mState.mWeight; 00816 } 00817 //p[i].mState.mWeight /= sum; 00818 } 00819 00820 //normalize if necessary 00821 if(smallest != largest) 00822 { 00823 for(uint i = 0; i < p.size(); i ++) 00824 { 00825 p[i].mState.mWeight /= sum; 00826 } 00827 } 00828 00829 //float largestValue = 1.0f / sum; 00830 //float smallestValue = 0; 00831 mNeedsResampling = true; 00832 if(smallest == largest) //no resampling needs to get done on particles that all have the same weight 00833 { 00834 mNeedsResampling = false; 00835 } 00836 if(mDisableGraphics.getVal() == 0) 00837 { 00838 //this will suffice regardless of the situation 00839 particleColor.setSpectrum(1.0f / sum); 00840 particleColor.minValue = 0; 00841 if(smallest == largest) 00842 { 00843 particleColor.minValue = 1.0f / sum; 00844 } 00845 } 00846 } 00847 00848 void ParticleFilter::processControllerInput(ParamData pd) 00849 { 00850 itsJSMutex.lock(); 00851 00852 static int jsvals_r_x = pd["jsvals_r_x"]; 00853 static int jsvals_l_x = pd["jsvals_l_x"]; 00854 static int jsvals_l_y = pd["jsvals_l_y"]; 00855 static int jsvals_dpad_x = pd["jsvals_dpad_x"]; 00856 static int jsvals_dpad_y = pd["jsvals_dpad_y"]; 00857 static int bvals_back = pd["bvals_back"]; 00858 static int bvals_start = pd["bvals_start"]; 00859 static int bvals_x = pd["bvals_x"]; 00860 static int bvals_a = pd["bvals_a"]; 00861 static int bvals_b = pd["bvals_b"]; 00862 static int bvals_y = pd["bvals_y"]; 00863 static int bvals_l_bumper = pd["bvals_l_bumper"]; 00864 static int bvals_r_bumper = pd["bvals_r_bumper"]; 00865 00866 motorSpeedX = -itsJSValues[jsvals_r_x]; //speed 00867 strafeValue = -itsJSValues[jsvals_l_x]; //strafe 00868 motorSpeedY = itsJSValues[jsvals_l_y]; //heading 00869 00870 mCam.mCenter.i += 0.25 * (float) itsJSValues[jsvals_dpad_x] 00871 / mCam.mScale; 00872 mCam.mCenter.j += 0.25 * (float) itsJSValues[jsvals_dpad_y] 00873 / mCam.mScale; 00874 00875 static int last_bvals_start = -1; 00876 static int last_bvals_back = -1; 00877 static int last_bvals_b = -1; 00878 static int last_bvals_y = -1; 00879 static int last_bvals_x = -1; 00880 static int last_bvals_a = -1; 00881 static int last_bvals_l_bumper = -1; 00882 static int last_bvals_r_bumper = -1; 00883 00884 if (itsButValues[bvals_back] == 1) 00885 { 00886 if (last_bvals_back == 0) 00887 { 00888 mGraphicsFlags.drawLines = !mGraphicsFlags.drawLines; 00889 } 00890 } 00891 00892 if (itsButValues[bvals_start] == 1) 00893 { 00894 if (last_bvals_start == 0) 00895 { 00896 if(mSimulationState == SimulationState::running) 00897 { 00898 stopSimulation(); 00899 } 00900 else 00901 { 00902 startSimulation(); 00903 } 00904 } 00905 } 00906 00907 /*if (itsButValues[pd.bvals_b] == 1) 00908 { 00909 if (last_bvals_b == 0) 00910 { 00911 timescaleExp -= 1; 00912 timescale = pow(1.25f, (float) timescaleExp); 00913 cout << "a^[k] dt: " << timescale << endl; 00914 } 00915 }*/ 00916 00917 /*if (itsButValues[pd.bvals_y] == 1) 00918 { 00919 if (last_bvals_y == 0) 00920 { 00921 timescaleExp += 1; 00922 timescale = pow(1.25f, (float) timescaleExp); 00923 cout << "a^[k] dt: " << timescale << endl; 00924 } 00925 }*/ 00926 00927 if (itsButValues[bvals_x] == 1) //use this to cycle through snapping options 00928 { 00929 if (last_bvals_x == 0) 00930 { 00931 if(mGraphicsFlags.mSnapState < GraphicsFlags::SnapState::snapToCurrentWaypoint) 00932 { 00933 mGraphicsFlags.mSnapState ++; 00934 } 00935 else 00936 { 00937 mGraphicsFlags.mSnapState = GraphicsFlags::SnapState::disable; 00938 } 00939 } 00940 } 00941 00942 /*if (itsButValues[bvals_a] == 1) 00943 { 00944 if (last_bvals_a == 0) 00945 { 00946 toggleFollowRealParticle = !toggleFollowRealParticle; 00947 toggleFollowAvgParticle = false; 00948 } 00949 }*/ 00950 00951 if (itsButValues[bvals_l_bumper] == 1) 00952 { 00953 if (last_bvals_l_bumper == 0) 00954 { 00955 mCam.offsetZoom(-50); 00956 } 00957 } 00958 00959 if (itsButValues[bvals_r_bumper] == 1) 00960 { 00961 if (last_bvals_r_bumper == 0) 00962 { 00963 mCam.offsetZoom(50); 00964 } 00965 } 00966 00967 if (abs(motorSpeedX) < 1) 00968 { 00969 motorSpeedX = 0; 00970 } 00971 if (abs(motorSpeedY) < 1) 00972 { 00973 motorSpeedY = 0; 00974 } 00975 00976 last_bvals_start = itsButValues[bvals_start]; 00977 last_bvals_back = itsButValues[bvals_back]; 00978 last_bvals_b = itsButValues[bvals_b]; 00979 last_bvals_y = itsButValues[bvals_y]; 00980 last_bvals_x = itsButValues[bvals_x]; 00981 last_bvals_a = itsButValues[bvals_a]; 00982 last_bvals_l_bumper = itsButValues[bvals_l_bumper]; 00983 last_bvals_r_bumper = itsButValues[bvals_r_bumper]; 00984 00985 itsJSMutex.unlock(); 00986 } 00987 00988 // ###################################################################### 00989 void ParticleFilter::evolve() 00990 { 00991 if(mInitializationState < InitializationState::controllerChecked) 00992 { 00993 if(mController && mController->isEnabled()) 00994 { 00995 cout << "controller enabled" << endl; 00996 } 00997 else 00998 { 00999 cout << "controller disabled" << endl; 01000 } 01001 mInitializationState = InitializationState::controllerChecked; 01002 } 01003 01004 /* 01005 if (askToCalibrate){ 01006 int choice = 0; 01007 cout << "Enter 1 To Calibrate, 0 for default: "; 01008 cint >> choice; 01009 if (choice == 1) 01010 controller.calibrate(); 01011 askToCalibrate = false; 01012 controller.setCalibration(askToCalibrate); 01013 }*/ 01014 01015 lastTimeTemp = clock(); 01016 dt = (float) (lastTimeTemp - lastTime) / (float) CLOCKS_PER_SEC; 01017 lastTime = lastTimeTemp; 01018 01019 01020 01021 if(mInitializationState < InitializationState::initialized) 01022 { 01023 if( !(mController) || (mController && mController->isEnabled() && mController->isCalibrated()) ) 01024 { 01025 Init(); 01026 } 01027 else 01028 { 01029 return; 01030 } 01031 } 01032 01033 if(mController && mController->isEnabled()) 01034 { 01035 processControllerInput(mParamData); 01036 //simulateMovement(LocalizationWaypointController::State::remote, 0, 0); 01037 } 01038 01039 if (mSimulationState == SimulationState::running) 01040 { 01041 step(); 01042 01043 static RobotSimEvents::BeeStemMotorControllerMessagePtr motorMsg = new RobotSimEvents::BeeStemMotorControllerMessage; 01044 resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::SPEED, motorMsg); 01045 //motorMsg->values.assign(BeeStem3::NUM_MOTOR_CONTROLLERS, 0); 01046 //motorMsg->mask.assign(BeeStem3::NUM_MOTOR_CONTROLLERS, 0); 01047 //resetMotorsExceptFor("", motorMsg); 01048 //joyStickMsg->axis = -1; //we only want BeeStemI to use this, so make everything else think it's an invalid message 01049 //joyStickMsg->button = -1; //we don't care about the buttons 01050 //joyStickMsg->axisVal = 0; 01051 01052 RobotSimEvents::BeeStemConfigMessagePtr depthMsg;// = new RobotSimEvents::BeeStemMotorControllerMessage; 01053 01054 if(mWaypointController.step(motorMsg, depthMsg, mCurrentPose)) 01055 { 01056 //cout << "trying to publish 360 message..."; 01057 //resetMotorsExceptFor(motorMsg->axisName); 01058 publish("BeeStemMotorControllerMessageTopic", motorMsg); 01059 //cout << "done" << endl; 01060 } 01061 else 01062 { 01063 if(depthMsg && depthMsg->updateDepthPID && depthMsg->updateDepthPID == 1) 01064 { 01065 //publish("BeeStemConfigMessageTopic", depthMsg); 01066 depthMsg->updateDepthPID = 0; 01067 depthMsg->enablePID = 1; 01068 //publish("BeeStemConfigMessageTopic", depthMsg); 01069 } 01070 //joyStickMsg->axisVal = 0; 01071 //resetMotorsExceptFor(""); 01072 } 01073 01074 cout << "simulating movement..."; 01075 //simulateMovement(LocalizationWaypointController::State::move_to_waypoint, LocalizationWaypointController::MovementMode::rotate2, 25); 01076 simulateMovement(mWaypointController.mState, mWaypointController.mMovementState, motorMsg); 01077 cout << "done" << endl; 01078 } 01079 01080 if(mDisableGraphics.getVal() == 0) 01081 { 01082 updateGraphics(); 01083 } 01084 01085 RobotSimEvents::LocalizationMessagePtr msg = new RobotSimEvents::LocalizationMessage; 01086 msg->mode = mWaypointController.mState; 01087 msg->currentWaypoint.x = mWaypointController[-1].mPoint.i; 01088 msg->currentWaypoint.y = mWaypointController[-1].mPoint.j; 01089 msg->currentWaypoint.depth = mWaypointController[-1].mDepth; 01090 msg->currentWaypoint.heading = mWaypointController[-1].mOrientation; 01091 msg->pos.i = avgParticle.mState.mPoint.i; 01092 msg->pos.j = avgParticle.mState.mPoint.j; 01093 msg->heading = avgParticle.mState.mAngle; 01094 01095 publish("LocalizationMessageTopic", msg); 01096 } 01097 01098 void ParticleFilter::resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::Ids actionId) 01099 { 01100 static RobotSimEvents::BeeStemMotorControllerMessagePtr msg = new RobotSimEvents::BeeStemMotorControllerMessage; 01101 resetMotorsExceptFor(actionId, msg); 01102 publish("BeeStemMotorControllerMessageTopic", msg); 01103 } 01104 01105 void ParticleFilter::resetMotorsExceptFor(XBox360RemoteControl::Keys::Actions::Ids actionId, RobotSimEvents::BeeStemMotorControllerMessagePtr & msg) 01106 { 01107 msg->values.assign(BeeStem3::NUM_MOTOR_CONTROLLERS, 0); 01108 msg->mask.assign(BeeStem3::NUM_MOTOR_CONTROLLERS, 0); 01109 01110 msg->mask[BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER] = 1; 01111 msg->mask[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER] = 1; 01112 msg->mask[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] = 1; 01113 msg->mask[BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER] = 1; 01114 if(actionId == XBox360RemoteControl::Keys::Actions::SPEED) 01115 { 01116 msg->mask[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] = 0; 01117 msg->mask[BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER] = 0; 01118 } 01119 else if(actionId == XBox360RemoteControl::Keys::Actions::HEADING) 01120 { 01121 msg->mask[BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER] = 0; 01122 msg->mask[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER] = 0; 01123 } 01124 //msg->mask[BeeStem3::MotorControllerIDs::DROPPER_STAGE1] = 0; 01125 //msg->mask[BeeStem3::MotorControllerIDs::DROPPER_STAGE2] = 0; 01126 //msg->mask[BeeStem3::MotorControllerIDs::SHOOTER] = 0; 01127 //msg->mask[BeeStem3::MotorControllerIDs::DEPTH_LEFT_THRUSTER] = 0; 01128 //msg->mask[BeeStem3::MotorControllerIDs::DEPTH_RIGHT_THRUSTER] = 0; 01129 } 01130 01131 // ###################################################################### 01132 void ParticleFilter::updateMessage( 01133 const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&) 01134 { 01135 static int BinFinderCounter = 0; 01136 static int BuoyFinderCounter = 0; 01137 static int CompassSensorCounter = 0; 01138 static int HydrophonesCounter = 0; 01139 static int PipeSensorCounter = 0; 01140 static int RectangleSensorCounter = 0; 01141 const static int sensorMsgTimeout = 5; //timeout if no msg after this many updates 01142 01143 if(mSensorFlags.BinFinder) 01144 BinFinderCounter ++; 01145 if(mSensorFlags.BuoyFinder) 01146 BuoyFinderCounter ++; 01147 if(mSensorFlags.CompassSensor) 01148 CompassSensorCounter ++; 01149 if(mSensorFlags.Hydrohpones) 01150 HydrophonesCounter ++; 01151 if(mSensorFlags.PipeSensor) 01152 PipeSensorCounter ++; 01153 if(mSensorFlags.RectangleSensor) 01154 RectangleSensorCounter ++; 01155 01156 if(BinFinderCounter > sensorMsgTimeout) 01157 mSensorFlags.BinFinder = false; 01158 if(BuoyFinderCounter > sensorMsgTimeout) 01159 mSensorFlags.BuoyFinder = false; 01160 if(CompassSensorCounter > sensorMsgTimeout) 01161 mSensorFlags.CompassSensor = false; 01162 if(HydrophonesCounter > sensorMsgTimeout) 01163 mSensorFlags.Hydrohpones = false; 01164 if(PipeSensorCounter > sensorMsgTimeout) 01165 mSensorFlags.PipeSensor = false; 01166 if(RectangleSensorCounter > sensorMsgTimeout) 01167 mSensorFlags.RectangleSensor = false; 01168 01169 if (eMsg->ice_isA("::RobotSimEvents::JoyStickControlMessage")) 01170 { 01171 RobotSimEvents::JoyStickControlMessagePtr msg = RobotSimEvents::JoyStickControlMessagePtr::dynamicCast(eMsg); 01172 01173 itsJSMutex.lock(); 01174 if (msg->axis >= 0) 01175 { 01176 itsJSValues[msg->axis] = msg->axisVal; 01177 } 01178 if (msg->button >= 0) 01179 itsButValues[msg->button] = msg->butVal; 01180 01181 itsJSMutex.unlock(); 01182 } 01183 01184 /* need working bin finder first 01185 else if (eMsg->ice_isA("::RobotSimEvents::BinFinderMessage")) 01186 { 01187 mSensorFlags.BinFinder = true; 01188 BinFinderCounter = 0; 01189 }*/ 01190 01191 else if (eMsg->ice_isA("::RobotSimEvents::BuoyColorSegmentMessage")) 01192 { 01193 mSensorFlags.BuoyFinder = true; 01194 BuoyFinderCounter = 0; 01195 RobotSimEvents::BuoyColorSegmentMessagePtr msg = RobotSimEvents::BuoyColorSegmentMessagePtr::dynamicCast(eMsg); 01196 mBuoySensor->takeReading(eMsg); 01197 } 01198 01199 else if (eMsg->ice_isA("::RobotSimEvents::IMUDataServerMessage")) 01200 { 01201 mSensorFlags.CompassSensor = true; 01202 CompassSensorCounter = 0; 01203 //cout << "IMU" << endl; 01204 RobotSimEvents::IMUDataServerMessagePtr msg = RobotSimEvents::IMUDataServerMessagePtr::dynamicCast(eMsg); 01205 if(msg->angleMode == IMUDataServer::Mode::euler) 01206 { 01207 mCompassSensor->takeReading(eMsg); 01208 } 01209 } 01210 01211 /* need working hydrophones first 01212 else if (eMsg->ice_isA("::RobotSimEvents::HydrophoneMessage")) 01213 { 01214 mSensorFlags.Hydrohpones = true; 01215 HydrophonesSensorCounter = 0; 01216 }*/ 01217 01218 else if (eMsg->ice_isA("::RobotSimEvents::PipeColorSegmentMessage")) 01219 { 01220 mSensorFlags.PipeSensor = true; 01221 PipeSensorCounter = 0; 01222 //printf("got a pipe color segmenter message!\n"); 01223 RobotSimEvents::PipeColorSegmentMessagePtr msg = RobotSimEvents::PipeColorSegmentMessagePtr::dynamicCast(eMsg); 01224 if(msg->size > 1500) 01225 { 01226 //printf("message contains large orange blob xD\n"); 01227 mPipeSensor->takeReading(eMsg); 01228 } 01229 } 01230 01231 else if (eMsg->ice_isA("::RobotSimEvents::VisionRectangleMessage")) 01232 { 01233 //printf("got a rectangle message!\n"); 01234 mSensorFlags.RectangleSensor = true; 01235 RectangleSensorCounter = 0; 01236 RobotSimEvents::VisionRectangleMessagePtr msg = RobotSimEvents::VisionRectangleMessagePtr::dynamicCast(eMsg); 01237 mRectangleSensor->takeReading(eMsg); 01238 } 01239 01240 else if(eMsg->ice_isA("::RobotSimEvents::BeeStemMotorControllerMessage")) 01241 { 01242 cout << "MotorController" << endl; 01243 RobotSimEvents::BeeStemMotorControllerMessagePtr msg = RobotSimEvents::BeeStemMotorControllerMessagePtr::dynamicCast(eMsg); 01244 cout << "{[" << msg->mask[0] << "]" << msg->values[0]; 01245 for(unsigned int i = 1; i < msg->values.size(); i ++) 01246 { 01247 cout << ",[" << msg->mask[i] << "]" << msg->values[i]; 01248 } 01249 cout << "}" << endl; 01250 } 01251 01252 else if(eMsg->ice_isA("::RobotSimEvents::LocalizationMessage")) 01253 { 01254 cout << "localization" << endl; 01255 RobotSimEvents::LocalizationMessagePtr msg = RobotSimEvents::LocalizationMessagePtr::dynamicCast(eMsg); 01256 cout << "----------" << endl; 01257 cout << "(" << msg->pos.i << "," << msg->pos.j << ")" << endl; 01258 cout << msg->heading << endl; 01259 cout << msg->mode << endl; 01260 if(msg->mode == LocalizationWaypointController::State::move_to_waypoint || msg->mode == LocalizationWaypointController::State::at_waypoint) 01261 { 01262 cout << "<" << msg->currentWaypoint.x << "," << msg->currentWaypoint.y << "," << msg->currentWaypoint.depth << ">" << endl; 01263 cout << "@ " << msg->currentWaypoint.heading << endl; 01264 } 01265 /*cout << "(" << avgParticle.mState.mPoint.i << "," << avgParticle.mState.mPoint.j << ")" << endl; 01266 cout << avgParticle.mState.mAngle << endl; 01267 cout << mWaypointController.mMode << endl; 01268 if(mWaypointController.mMode == LocalizationWaypointController::State::move_to_waypoint || mWaypointController.mMode == LocalizationWaypointController::State::at_waypoint) 01269 { 01270 cout << "<" << mWaypointController.mCurrentWaypoint.mPoint.x << "," << mWaypointController.mCurrentWaypoint.mPoint.y << "," << mWaypointController.mCurrentWaypoint.mPoint.z << ">" << endl; 01271 cout << mWaypointController.mCurrentWaypoint.mOrientation << endl; 01272 }*/ 01273 } 01274 } 01275 01276 #endif