Pioneer3-AT + Hokuyo UTM30LX + MRPT 搭建遥控地面移动SLAM平台

先锋的P3-AT地面移动平台采用ActivMedia库，而MRPT对此库只进行了一点微小的开发，将二者结合起来使用是一大挑战。

在我的设计中，ActivMedia库用于移动平台的控制和位姿获取，MRPT用于SLAM，二者之间可以做到不冲突。

一、开发环境

1. Win 7 / 8.1， 64 bit。

2. visual studio 2013， Qt 5.5.0。

3. 其他配置见我另外两篇博客，这里不再进一步阐述。

二、程序设计

1. 使用Qt多线程，主要分为四个线程。

有一个读取Hokuyo激光数据的线程SensorThread没有采用QThread，用MRPT读取传感器数据的代码改的。

与机器人的通信采用CS模式，我不会写CS模式的东西，所以这部分是用的demo改的。ROBOT_CLIENT_THREAD是一个机器人客户端线程，用于与机器人服务器连接，主要是控制机器人，并不停地更新机器人位姿。

SLAM_THREAD用于进行SLAM，实时获取激光数据，根据位姿序列得到移动平台的Movement2D。

AUTORUN_THREAD是一个自主探索的线程，目前我刚刚开始研究这一问题，已经能够实现实时避障，但其本质是一个随机游走的过程，并不是自主探索，还有很长的路要走。

三、读取Hokuyo激光数据

struct TThreadParams{CConfigFile*cfgFile;stringsensor_label;};void SensorThread(TThreadParams params){try{string driver_name = params.cfgFile->read_string(params.sensor_label, "driver", "", true);CGenericSensorPtrsensor = CGenericSensor::createSensorPtr(driver_name);if (!sensor){//message_signal("***ERROR***: Class name not recognized CHokuyoURG...\n");allThreadsMustExit = true;}// Load common & sensor specific parameters:sensor->loadConfig(*params.cfgFile, params.sensor_label);cout << format("[thread_%s] Starting...", params.sensor_label.c_str()) << " at " << sensor->getProcessRate() << " Hz" << endl;//message_To_Text("laser thread starting...\n");ASSERTMSG_(sensor->getProcessRate()>0, "process_rate must be set to a valid value (>0 Hz).");intprocess_period_ms = round(1000.0 / sensor->getProcessRate());// For imaging sensors, set external storage directory://sensor->setPathForExternalImages(rawlog_ext_imgs_dir);// Init device:sensor->initialize();while (!allThreadsMustExit){TTimeStamp t0 = now();// Processsensor->doProcess();// Get new observationsCGenericSensor::TListObservationslstObjs;sensor->getObservations(lstObjs);{synch::CCriticalSectionLockerlock(&cs_global_list_obs);global_list_obs.insert(lstObjs.begin(), lstObjs.end());global_list_obs2.insert(lstObjs.begin(), lstObjs.end());}lstObjs.clear();// wait until the process period:TTimeStamp t1 = now();doubleAt = timeDifference(t0, t1);int At_rem_ms = process_period_ms - At * 1000;if (At_rem_ms>0)sleep(At_rem_ms);}sensor.clear();cout << format("[thread_%s] Closing...", params.sensor_label.c_str()) << endl;//message_To_Text("laser thread closing...\n");}catch (std::exception &e){//cerr << e.what() << endl;//message_To_Text(e.what());allThreadsMustExit = true;}catch (...){//cerr << "Untyped exception!!" << endl;//message_To_Text("Untyped exception!!...\n");allThreadsMustExit = true;}}void zed_slam::on_Button_Laser_clicked(){sendMessage("try to connect laser scanner...\n");//TODOallThreadsMustExit = false;ASSERT_FILE_EXISTS_(Laser_Ini_File);CConfigFile iniFile(Laser_Ini_File);MRPT_LOAD_CONFIG_VAR(time_between_launches, int, iniFile, GLOBAL_SECTION_NAME);MRPT_LOAD_CONFIG_VAR(SF_max_time_span, float, iniFile, GLOBAL_SECTION_NAME);MRPT_LOAD_CONFIG_VAR(use_sensoryframes, bool, iniFile, GLOBAL_SECTION_NAME);MRPT_LOAD_CONFIG_VAR(GRABBER_PERIOD_MS, int, iniFile, GLOBAL_SECTION_NAME);MRPT_LOAD_CONFIG_VAR(rawlog_GZ_compress_level, int, iniFile, GLOBAL_SECTION_NAME);mrpt::system::TThreadHandle hSensorThread;try{TThreadParams threParms;threParms.cfgFile = &iniFile;threParms.sensor_label = "LASER_2D";hSensorThread = mrpt::system::createThread(SensorThread, threParms);sendMessage("sensor thread running...\n");}catch (std::exception e){sendMessage(e.what());allThreadsMustExit = true;}catch (...){sendMessage("untyped exception!!!\n");allThreadsMustExit = true;}mrpt::system::sleep(1000);}

四、连接到地面移动平台

class InputHandler{public:InputHandler(ArClientBase *client);InputHandler(ArClientBase *client, ArKeyHandler *keyHandler);virtual ~InputHandler(void);void up(void);void down(void);void left(void);void right(void);void autorun(void);void lateralLeft(void);void lateralRight(void);void sendInput(void);public:/// Send a request to enable "safe drive" mode on the servervoid safeDrive();/// Send a request to disable "safe drive" mode on the servervoid unsafeDrive();/// Request stopvoid space();void listData();void logTrackingTerse();void logTrackingVerbose();void resetTracking();void toggleDebug();ArClientBase *myClient;ArKeyHandler *myKeyHandler;/// Set this to true in the constructor to print out debugging informationbool myPrinting;/// Object that continuously sends driving requests in the background. ArClientRatioDrive myDriveClient;/** Functor objects, given to the key handler, which then call our handler* methods above *////@{ArFunctorC<InputHandler> myUpCB;ArFunctorC<InputHandler> myDownCB;ArFunctorC<InputHandler> myLeftCB;ArFunctorC<InputHandler> myRightCB;ArFunctorC<InputHandler> myLateralLeftCB;ArFunctorC<InputHandler> myLateralRightCB;ArFunctorC<InputHandler> mySafeDriveCB;ArFunctorC<InputHandler> myUnsafeDriveCB;ArFunctorC<InputHandler> myListDataCB;ArFunctorC<InputHandler> myLogTrackingTerseCB;ArFunctorC<InputHandler> myLogTrackingVerboseCB;ArFunctorC<InputHandler> myResetTrackingCB;ArFunctorC<InputHandler> mySpaceCB;ArFunctorC<InputHandler> myToggleDebugCB;ArFunctorC<InputHandler> myAutoRunCB;///@}};InputHandler::InputHandler(ArClientBase *client) :myClient(client), myPrinting(false), myDriveClient(client){}InputHandler::InputHandler(ArClientBase *client, ArKeyHandler *keyHandler) :myClient(client), myKeyHandler(keyHandler),myPrinting(false), myDriveClient(client),/* Initialize functor objects with pointers to our handler methods: */myUpCB(this, &InputHandler::up),myDownCB(this, &InputHandler::down),myLeftCB(this, &InputHandler::left),myRightCB(this, &InputHandler::right),myLateralLeftCB(this, &InputHandler::lateralLeft),myLateralRightCB(this, &InputHandler::lateralRight),mySafeDriveCB(this, &InputHandler::safeDrive),myUnsafeDriveCB(this, &InputHandler::unsafeDrive),myListDataCB(this, &InputHandler::listData),myLogTrackingTerseCB(this, &InputHandler::logTrackingTerse),myLogTrackingVerboseCB(this, &InputHandler::logTrackingVerbose),myResetTrackingCB(this, &InputHandler::resetTracking),mySpaceCB(this, &InputHandler::space),myToggleDebugCB(this, &InputHandler::toggleDebug),myAutoRunCB(this, &InputHandler::autorun){/* Add our functor objects to the key handler, associated with the appropriate* keys: */myKeyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB);myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB);myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);myKeyHandler->addKeyHandler('q', &myLateralLeftCB);myKeyHandler->addKeyHandler('e', &myLateralRightCB);myKeyHandler->addKeyHandler('s', &mySafeDriveCB);myKeyHandler->addKeyHandler('u', &myUnsafeDriveCB);myKeyHandler->addKeyHandler('l', &myListDataCB);myKeyHandler->addKeyHandler('t', &myLogTrackingTerseCB);myKeyHandler->addKeyHandler('v', &myLogTrackingVerboseCB);myKeyHandler->addKeyHandler('r', &myResetTrackingCB);myKeyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB);myKeyHandler->addKeyHandler('d', &myToggleDebugCB);}InputHandler::~InputHandler(void){}void InputHandler::space(void){myDriveClient.stop();tempVV = 0;tempRV = 0;}void InputHandler::up(void){if (flag == 1){tempVV += 15;myDriveClient.setTransVelRatio(tempVV);flag = 50;}if (flag == 11){myDriveClient.setRotVelRatio(0);tempRV = 0;myDriveClient.setTransVelRatio(30);tempVV = 30;}}void InputHandler::down(void){if (flag == 2){tempVV -= 15;myDriveClient.setTransVelRatio(tempVV);flag = 50;}if (flag == 12){myDriveClient.setTransVelRatio(-30);tempVV = -30;}}void InputHandler::left(void){if (flag == 3){if (tempRV == 0){myDriveClient.setRotVelRatio(25);tempRV = 25;int count = 0;while (flag == 3 && count != 60){count++;ArUtil::sleep(100);}myDriveClient.setRotVelRatio(0);tempRV = 0;flag = 50;}else{myDriveClient.setRotVelRatio(0);tempRV = 0;flag = 50;}}if (flag == 13){myDriveClient.setTransVelRatio(20);tempVV = 20;myDriveClient.setRotVelRatio(20);tempRV = 20;int count = 0;while (flag == 13 && count != 30){count++;ArUtil::sleep(100);}myDriveClient.setRotVelRatio(0);tempRV = 0;}}void InputHandler::right(void){if (flag == 4){if (tempRV == 0){myDriveClient.setRotVelRatio(-25);tempRV = -25;int count = 0;while (flag == 4 && count != 60){count++;ArUtil::sleep(100);}myDriveClient.setRotVelRatio(0);tempRV = 0;flag = 50;}else{myDriveClient.setRotVelRatio(0);tempRV = 0;flag = 50;}}if (flag == 14){myDriveClient.setTransVelRatio(20);tempVV = 20;myDriveClient.setRotVelRatio(-20);tempRV = -20;int count = 0;while (flag == 14 && count != 30){count++;ArUtil::sleep(100);}myDriveClient.setRotVelRatio(0);tempRV = 0;}}void InputHandler::autorun(){}void InputHandler::lateralLeft(void){tempRV += VEL_AMOUNT;myDriveClient.setLatVelRatio(VEL_AMOUNT);}void InputHandler::lateralRight(void){tempRV -= VEL_AMOUNT;myDriveClient.setLatVelRatio(-VEL_AMOUNT);}void InputHandler::safeDrive(){myDriveClient.safeDrive();}void InputHandler::unsafeDrive(){myDriveClient.unsafeDrive();}void InputHandler::toggleDebug(){myPrinting = !myPrinting;myDriveClient.setDebugPrint(myPrinting);}void InputHandler::listData(){myClient->logDataList();}void InputHandler::logTrackingTerse(){myClient->logTracking(true);}void InputHandler::logTrackingVerbose(){myClient->logTracking(false);}void InputHandler::resetTracking(){myClient->resetTracking();}/** This class requests continual data updates from the server and prints them* out.*/class OutputHandler{public:OutputHandler(ArClientBase *client);virtual ~OutputHandler(void);/// This callback is called when an update on general robot state arrivesvoid handleOutput(ArNetPacket *packet);/// This callback is called when an update on general robot state arrivesvoid handleOutputNumbers(ArNetPacket *packet);/// This callback is called when an update on general robot state arrivesvoid handleOutputStrings(ArNetPacket *packet);/// This callback is called when an update on the battery configuration changesvoid handleBatteryInfo(ArNetPacket *packet);/// This is called when the physical robot information comes backvoid handlePhysicalInfo(ArNetPacket *packet);/// This callback is called when an update on the temperature information changesvoid handleTemperatureInfo(ArNetPacket *packet);/// Called when the map on the server changes.void handleMapUpdated(ArNetPacket *packet);protected:/// The results from the data update are stored in these variables//@{double myX;double myY;double myTh;double myVel;double myRotVel;double myLatVel;bool myVoltageIsStateOfCharge;char myTemperature;double myVoltage;char myStatus[256];char myMode[256];//@}ArClientBase *myClient;/** These functor objects are given to the client to receive updates when they* arrive from the server.*///@{ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputNumbersCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputStringsCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandleBatteryInfoCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandlePhysicalInfoCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandleTemperatureInfoCB;ArFunctor1C<OutputHandler, ArNetPacket *> myHandleMapUpdatedCB;//@}/// A header for the columns in the data printout is sometimes printedbool myNeedToPrintHeader;/// Don't print any information until we get the battery infobool myGotBatteryInfo;};OutputHandler::OutputHandler(ArClientBase *client) :myClient(client),myHandleOutputCB(this, &OutputHandler::handleOutput),myHandleOutputNumbersCB(this, &OutputHandler::handleOutputNumbers),myHandleOutputStringsCB(this, &OutputHandler::handleOutputStrings),myHandleBatteryInfoCB(this, &OutputHandler::handleBatteryInfo),myHandlePhysicalInfoCB(this, &OutputHandler::handlePhysicalInfo),myHandleTemperatureInfoCB(this, &OutputHandler::handleTemperatureInfo),myHandleMapUpdatedCB(this, &OutputHandler::handleMapUpdated),myNeedToPrintHeader(false),myGotBatteryInfo(false){/* Add a handler for battery info, and make a single request for it  */myClient->addHandler("physicalInfo", &myHandlePhysicalInfoCB);myClient->requestOnce("physicalInfo");/* Add a handler for battery info, and make a single request for it  */myClient->addHandler("batteryInfo", &myHandleBatteryInfoCB);myClient->requestOnce("batteryInfo");/* If it exists add a handler for temperature info, and make a* single request for it  */if (myClient->dataExists("temperatureInfo")){myClient->addHandler("temperatureInfo", &myHandleTemperatureInfoCB);myClient->requestOnce("temperatureInfo");}// if we have the new way of broadcasting that only pushes strings// when they change then use thatif (myClient->dataExists("updateNumbers") &&myClient->dataExists("updateStrings")){printf("Using new updates\n");// get the numbers every 100 msmyClient->addHandler("updateNumbers", &myHandleOutputNumbersCB);myClient->request("updateNumbers", 100);// and the strings whenever they change (and are broadcast)myClient->addHandler("updateStrings", &myHandleOutputStringsCB);myClient->request("updateStrings", -1);}else{printf("Using old updates\n");// For the old way, just Add a handler for general info, and// request it to be called every 100 msmyClient->addHandler("update", &myHandleOutputCB);myClient->request("update", 100);}if (myClient->dataExists("mapUpdated")){myClient->addHandler("mapUpdated", &myHandleMapUpdatedCB);myClient->request("mapUpdated", -1);}}OutputHandler::~OutputHandler(void){/* Halt the request for data updates */myClient->requestStop("update");}void OutputHandler::handleOutput(ArNetPacket *packet){/* Extract the data from the update packet. Its format is status and* mode (null-terminated strings), then 6 doubles for battery voltage,* x position, y position and orientation (theta) (from odometry), current* translational velocity, and current rotational velocity. Translation is* always milimeters, rotation in degrees.*/memset(myStatus, 0, sizeof(myStatus));memset(myMode, 0, sizeof(myMode));packet->bufToStr(myStatus, sizeof(myStatus));packet->bufToStr(myMode, sizeof(myMode));myVoltage = ((double)packet->bufToByte2()) / 10.0;myX = (double)packet->bufToByte4();myY = (double)packet->bufToByte4();myTh = (double)packet->bufToByte2();myVel = (double)packet->bufToByte2();myRotVel = (double)packet->bufToByte2();myLatVel = (double)packet->bufToByte2();myTemperature = (double)packet->bufToByte();odomPose.m_coords[0] = myX / 1000.0;odomPose.m_coords[1] = myY / 1000.0;odomPose.m_coords[2] = myTh * M_PI / 180.0;}void OutputHandler::handleOutputNumbers(ArNetPacket *packet){/* Extract the data from the updateNumbers packet. Its format is 6* doubles for battery voltage, x position, y position and* orientation (theta) (from odometry), current translational* velocity, and current rotational velocity. Translation is always* milimeters, rotation in degrees.*/myVoltage = ((double)packet->bufToByte2()) / 10.0;myX = (double)packet->bufToByte4();myY = (double)packet->bufToByte4();myTh = (double)packet->bufToByte2();myVel = (double)packet->bufToByte2();myRotVel = (double)packet->bufToByte2();myLatVel = (double)packet->bufToByte2();myTemperature = (double)packet->bufToByte();odomPose = CPose2D(myX / 1000.0, myY / 1000.0, myTh * M_PI / 180.0);}void OutputHandler::handleOutputStrings(ArNetPacket *packet){/* Extract the data from the updateStrings packet. Its format is* status and mode (null-terminated strings).*/memset(myStatus, 0, sizeof(myStatus));memset(myMode, 0, sizeof(myMode));packet->bufToStr(myStatus, sizeof(myStatus));packet->bufToStr(myMode, sizeof(myMode));}void OutputHandler::handleBatteryInfo(ArNetPacket *packet){/* Get battery configuration parameters: when the robot will begin beeping and* warning about low battery, and when it will automatically disconnect and* shutdown. */double lowBattery = packet->bufToDouble();double shutdown = packet->bufToDouble();myNeedToPrintHeader = true;myGotBatteryInfo = true;if (packet->getDataReadLength() == packet->getDataLength()){//printf("Packet is too small so its an old server, though you could just get to the bufToUByte anyways, since it'd be 0 anyhow\n");myVoltageIsStateOfCharge = false;}elsemyVoltageIsStateOfCharge = (packet->bufToUByte() == 1);}void OutputHandler::handlePhysicalInfo(ArNetPacket *packet){/* Get phyiscal configuration parameters: */char robotType[512];char robotSubtype[512];int width;int lengthFront;int lengthRear;packet->bufToStr(robotType, sizeof(robotType));packet->bufToStr(robotSubtype, sizeof(robotSubtype));width = packet->bufToByte2();lengthFront = packet->bufToByte2();lengthRear = packet->bufToByte2();}void OutputHandler::handleTemperatureInfo(ArNetPacket *packet){char warning = packet->bufToByte();char shutdown = packet->bufToByte();myNeedToPrintHeader = true;}void OutputHandler::handleMapUpdated(ArNetPacket *packet){myNeedToPrintHeader = true;}//---------------////---------- robotInfo ----------//void robotInfo(ArNetPacket* packet){/* Extract the data from the update packet. Its format is status and* mode (null-terminated strings), then 6 doubles for battery voltage,* x position, y position and orientation (theta) (from odometry), current* translational velocity, and current rotational velocity. Translation is* always milimeters, rotation in degrees.*/double myX_1;double myY_1;double myTh_1;double myVel_1;double myRotVel_1;double myVoltage_1;char myStatus_1[256];char myMode_1[32];memset(myStatus_1, 0, sizeof(myStatus_1));memset(myMode_1, 0, sizeof(myMode_1));packet->bufToStr(myStatus_1, sizeof(myStatus_1));packet->bufToStr(myMode_1, sizeof(myMode_1));myVoltage_1 = ((double)packet->bufToByte2()) / 10.0;myX_1 = (double)packet->bufToByte4();myY_1 = (double)packet->bufToByte4();myTh_1 = (double)packet->bufToByte2();myVel_1 = (double)packet->bufToByte2();myRotVel_1 = (double)packet->bufToByte2();tempX = myX_1;tempY = myY_1;tempA = myTh_1;tempB = myVoltage_1;tempV = myVel_1;odomPose = CPose2D(myX_1 / 1000.0, myY_1 / 1000.0, myTh_1 * M_PI / 180.0);}//---- gets localization state ----//void getLocState(ArNetPacket* packet){double state = 0, score = 0;state = (double)packet->bufToUByte();score = (double)packet->bufToUByte2();localScore = score / 10;}//----- Get MAP from server ------//void getMap(ArNetPacket* packet){//using namespace std;char mapData[256], *mapString;std::ofstream write1;mapString = mapData;memset(mapData, 0, sizeof(mapData));packet->bufToStr(mapData, sizeof(mapData));write1.open("2D_MAP.map", std::ios::app);write1 << mapString;write1 << std::endl;write1.close();}//--------------////----- Get localization points -----//void getLocPoints(ArNetPacket* packet){int locPoint_num = 0, locPoint_x[200], locPoint_y[200];locPoint_num = packet->bufToByte2();for (int l = 0; l<locPoint_num; l++){locPoint_x[l] = packet->bufToByte4();locPoint_y[l] = packet->bufToByte4();}}void ROBOT_CLIENT_THREAD::run(){Aria::init();ArClientBase client;ArArgumentBuilder builder;ArArgumentParser parser(&builder);ArClientSimpleConnector clientConnector(&parser);parser.loadDefaultArguments();if (clientConnector.connectClient(&client))linkFlag = 1;if (linkFlag != 1){linkFlag = 99;ArUtil::sleep(3000);exit(1);}ArKeyHandler keyHandler;Aria::setKeyHandler(&keyHandler);InputHandler inputHandler(&client, &keyHandler);OutputHandler outputHandler(&client);inputHandler.safeDrive();ArNetPacket requestPacket;ArGlobalFunctor1<ArNetPacket *> robotInfoCB(&robotInfo);client.addHandler("update", &robotInfoCB);client.request("update", TM - 1);client.runAsync();while (client.getRunningWithLock()){client.requestOnce("update");switch (flag){case 1:inputHandler.up(); Sleep(300); break;  //if(GetAsyncKeyState(VK_LBUTTON)&0x8000)case 2:inputHandler.down(); Sleep(300); break;case 3:inputHandler.left(); Sleep(300); break;case 4:inputHandler.right(); Sleep(300); break;case 5:inputHandler.space(); Sleep(300); break;case 11:inputHandler.up(); Sleep(300); break;case 13:inputHandler.left(); Sleep(300); break;case 14:inputHandler.right(); Sleep(300); break;case 99:client.disconnect(); flag = 0; break; // disconnect from server}ArUtil::sleep(25); }client.disconnect();ArUtil::sleep(500);}void zed_slam::keyPressEvent(QKeyEvent *event){if (!robot_client_thread->isRunning())return;switch (event->key()){case Qt::Key_Up: flag = 1; sleep(100);  break;case Qt::Key_Down: flag = 2; sleep(100); break;case Qt::Key_Left: flag = 3; sleep(100); break;case Qt::Key_Right: flag = 4; sleep(100); break;case Qt::Key_Space: autodrive = false; flag = 5;  sleep(100); break;case Qt::Key_G: autodrive = true; sleep(100); break;case Qt::Key_Escape: flag = 99; sleep(100); break;}}void zed_slam::on_Button_Robot_clicked(){sendMessage("client is trying to connect server...\n");robot_client_thread = new ROBOT_CLIENT_THREAD();QObject::connect(robot_client_thread, SIGNAL(sendMessage(QString)), ui.textEdit, SLOT(append(QString)));robot_client_thread->start();}

五、开始SLAM

SLAM_THREAD::SLAM_THREAD(){outfile.open(output_rawlog);stopped = false;}SLAM_THREAD::~SLAM_THREAD(){outfile.close();}void SLAM_THREAD::run(){CTicTactictac, tictacGlobal, tictac_JH;CSimpleMap            finalMap;      //记录PosePDF与SensorFrame映射关系的类，核心m_PosesObsPairsfloatt_exec;COccupancyGridMap2D::TEntropyInfoentropy;size_trawlogEntry = 0;charstrFil[1000];CMetricMapBuilderRBPF::TConstructionOptionsrbpfMappingOptions;rbpfMappingOptions.loadFromConfigFile(CConfigFile(Slam_Ini_File), "MappingApplication");rbpfMappingOptions.dumpToConsole();// ---------------------------------//Constructor// ---------------------------------CMetricMapBuilderRBPF mapBuilder(rbpfMappingOptions);// handle the case of metric map continuationif (1){CSimpleMap       dummySimpleMap;CPosePDFGaussian startPose;startPose.mean.x(0);startPose.mean.y(0);startPose.mean.phi(0);startPose.cov.setZero();mrpt::maps::COccupancyGridMap2D gridmap;mapBuilder.initialize(dummySimpleMap, &startPose);for (CMultiMetricMapPDF::CParticleList::iterator it = mapBuilder.mapPDF.m_particles.begin(); it != mapBuilder.mapPDF.m_particles.end(); ++it) {CRBPFParticleData* part_d = it->d;CMultiMetricMap &mmap = part_d->mapTillNow;mrpt::maps::COccupancyGridMap2DPtr it_grid = mmap.getMapByClass<mrpt::maps::COccupancyGridMap2D>();ASSERTMSG_(it_grid.present(), "No gridmap in multimetric map definition, but metric map continuation was set (!)");it_grid->copyMapContentFrom(gridmap);}}mapBuilder.options.verbose = true;mapBuilder.options.enableMapUpdating = true;mapBuilder.options.debugForceInsertion = false;randomGenerator.randomize();CDisplayWindow3D *win3D = NULL;win3D = new CDisplayWindow3D("RT-SLAM @ CARTOLAB", 500, 400);win3D->setCameraZoom(50);win3D->setCameraAzimuthDeg(-50);win3D->setCameraElevationDeg(70);CActionCollectionPtr action = CActionCollection::Create();CSensoryFramePtr observations = CSensoryFrame::Create();CPose3D odoPose = CPose3D(0, 0, 0);CTicTac timeout_read_scans;int64_t step = 0;CSensoryFrame curSF;CObservationOdometryPtr odom = CObservationOdometry::Create();CObservationOdometry last_odo;while (!stopped){if (os::kbhit()){char c = os::getch();if (c == 27)break;}CObservation2DRangeScanPtr observation;CGenericSensor::TListObservations obs_copy;mrpt::synch::CCriticalSectionLocker csl(&cs_global_list_obs);obs_copy = global_list_obs;global_list_obs.clear();for (CGenericSensor::TListObservations::reverse_iterator it = obs_copy.rbegin(); !observation && it != obs_copy.rend(); ++it){if (it->second.present() && IS_CLASS(it->second, CObservation2DRangeScan))observation = CObservation2DRangeScanPtr(it->second);}if (!observation){if (timeout_read_scans.Tac() > 1.0)timeout_read_scans.Tic();mrpt::system::sleep(1);continue;}elsetimeout_read_scans.Tic();observations->insert(observation);//TODO TODO TODOCPose2D curPose;curPose = odomPose;odom->timestamp = observation->timestamp;odom->odometry = curPose;CActionRobotMovement2DPtr act = CActionRobotMovement2D::Create();act->timestamp = observation->timestamp;CActionRobotMovement2D::TMotionModelOptions odomOpts;static bool last_odo_first = true;CPose2D odo_incr;int64_t lticks_incr, rticks_incr;if (last_odo_first){last_odo_first = false;odo_incr = CPose2D(0, 0, 0);lticks_incr = rticks_incr = 0;}else{odo_incr = odom->odometry - last_odo.odometry;lticks_incr = odom->encoderLeftTicks - last_odo.encoderLeftTicks;rticks_incr = odom->encoderRightTicks - last_odo.encoderRightTicks;last_odo = *odom;}act->computeFromOdometry(odo_incr, odomOpts);act->hasEncodersInfo = true;act->encoderLeftTicks = lticks_incr;act->encoderRightTicks = rticks_incr;act->hasVelocities = true;act->velocityLin = odom->velocityLin;act->velocityAng = odom->velocityAng;action->insert(*act);// Save all of them to rawlog for optional post-processing:if (outfile.fileOpenCorrectly()){outfile << *action;outfile << *observations;}QString str = QString("odomPose: X:%1, Y:%2, THETA:%3").arg(odomPose.x()).arg(odomPose.y()).arg(odomPose.phi());sendMessage(str);//process action & observation pairtictac.Tic();mapBuilder.processActionObservation(*action, *observations);t_exec = tictac.Tac();COpenGLScenePtr scene;scene = COpenGLScene::Create();// The ground:mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);groundPlane->setColor(0.4, 0.4, 0.4);scene->insert(groundPlane);mrpt::opengl::CSetOfObjectsPtr objs = mrpt::opengl::CSetOfObjects::Create();CMultiMetricMap*mostLikMap = mapBuilder.mapPDF.getCurrentMostLikelyMetricMap();mostLikMap->getAs3DObject(objs);scene->insert(objs);// Draw the robot particles:size_tM = mapBuilder.mapPDF.particlesCount();mrpt::opengl::CSetOfLinesPtr objLines = mrpt::opengl::CSetOfLines::Create();objLines->setColor(0, 1, 1);for (size_t i = 0; i<M; i++){std::deque<TPose3D>path;mapBuilder.mapPDF.getPath(i, path);floatx0 = 0, y0 = 0, z0 = 0;for (size_t k = 0; k<path.size(); k++){objLines->appendLine(x0, y0, z0 + 0.001,path[k].x, path[k].y, path[k].z + 0.001);x0 = path[k].x;y0 = path[k].y;z0 = path[k].z;}}scene->insert(objLines);// An ellipsoid:CPose3DlastMeanPose;floatminDistBtwPoses = -1;std::deque<TPose3D>dummyPath;mapBuilder.mapPDF.getPath(0, dummyPath);for (int k = (int)dummyPath.size() - 1; k >= 0; k--){CPose3DPDFParticlesposeParts;mapBuilder.mapPDF.getEstimatedPosePDFAtTime(k, poseParts);CPose3DmeanPose;CMatrixDouble66 COV;poseParts.getCovarianceAndMean(COV, meanPose);if (meanPose.distanceTo(lastMeanPose)>minDistBtwPoses){CMatrixDouble33 COV3 = COV.block(0, 0, 3, 3);minDistBtwPoses = 6 * sqrt(COV3(0, 0) + COV3(1, 1));opengl::CEllipsoidPtr objEllip = opengl::CEllipsoid::Create();objEllip->setLocation(meanPose.x(), meanPose.y(), meanPose.z() + 0.001);objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);objEllip->setColor(0, 0, 1);objEllip->enableDrawSolid3D(false);scene->insert(objEllip);lastMeanPose = meanPose;}}COpenGLScenePtr &scenePtr = win3D->get3DSceneAndLock();scenePtr = scene;win3D->unlockAccess3DScene();win3D->forceRepaint();step++;observations->clear();action->clear();}}void zed_slam::on_Button_Start_clicked(){sendMessage("start slam...\n");//TODOif (allThreadsMustExit){sendMessage("laser scanner failed...\n");return;}slam_thread = new SLAM_THREAD();QObject::connect(slam_thread, SIGNAL(sendMessage(QString)), ui.textEdit, SLOT(append(QString)));slam_thread->start();}

六、随机游走避障

AUTORUN_THREAD::AUTORUN_THREAD(){stopped = false;}void AUTORUN_THREAD::run(){if (flag == 99)stopped = true;while (!stopped){if (!autodrive)continue;CTicTac timeout_read_scans;CObservation2DRangeScanPtr observation;CGenericSensor::TListObservations obs_copy;mrpt::synch::CCriticalSectionLocker csl(&cs_global_list_obs);obs_copy = global_list_obs2;global_list_obs2.clear();    //test code, to be deletedbool isblocked = false;CGenericSensor::TListObservations::iterator it = obs_copy.begin(); if (it!=obs_copy.end() && it->second.present() && IS_CLASS(it->second, CObservation2DRangeScan))observation = CObservation2DRangeScanPtr(it->second);if (!observation){//flag = 5;continue;}vector<float> scan = observation->scan;vector<char> valid = observation->validRange;// 30-70:300-460 70-110:461-620 110-150:621-781int i;for (i = 300; i < 781; i++){if (scan[i] > 1.0 || valid[i] == 0){continue;}else{isblocked = true;break;}}if (isblocked){if (i <= 461)flag = 13;else if (i >= 621)flag = 14;else{float right_laser_amount = 0, left_laser_amount = 0;for (int j = 300; j < 460; j++){if (valid[j])right_laser_amount += scan[j];if (valid[1080-j])left_laser_amount += scan[1080 - j];}if (right_laser_amount <= left_laser_amount)flag = 13;elseflag = 14;}}elseflag = 11;//QString str = QString("odomPose: X:%1, Y:%2, THETA:%3").arg(odomPose.x()).arg(odomPose.y()).arg(odomPose.phi());QString str = QString("flag: %1").arg(flag);sendMessage(str);ArUtil::sleep(500);}}void zed_slam::on_Button_Autorun_clicked(){if (robot_client_thread == NULL)return;if (allThreadsMustExit)return;autorun_thread = new AUTORUN_THREAD();QObject::connect(autorun_thread, SIGNAL(sendMessage(QString)), ui.textEdit, SLOT(append(QString)));autodrive = true;autorun_thread->start();}