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;
	string			sensor_label;
};
void SensorThread(TThreadParams params)
{
	try
	{
		string driver_name = params.cfgFile->read_string(params.sensor_label, "driver", "", true);

		CGenericSensorPtr	sensor = 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).");
		int		process_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();

			// Process
			sensor->doProcess();

			// Get new observations
			CGenericSensor::TListObservations	lstObjs;
			sensor->getObservations(lstObjs);

			{
				synch::CCriticalSectionLocker	lock(&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();
			double	At = 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");
	//TODO
	allThreadsMustExit = 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 server
	void safeDrive();
	/// Send a request to disable "safe drive" mode on the server
	void unsafeDrive();
	/// Request stop
	void 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 information
	bool 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 arrives
	void handleOutput(ArNetPacket *packet);
	/// This callback is called when an update on general robot state arrives
	void handleOutputNumbers(ArNetPacket *packet);
	/// This callback is called when an update on general robot state arrives
	void handleOutputStrings(ArNetPacket *packet);
	/// This callback is called when an update on the battery configuration changes
	void handleBatteryInfo(ArNetPacket *packet);
	/// This is called when the physical robot information comes back
	void handlePhysicalInfo(ArNetPacket *packet);
	/// This callback is called when an update on the temperature information changes
	void 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 printed
	bool myNeedToPrintHeader;
	/// Don't print any information until we get the battery info
	bool 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 that
	if (myClient->dataExists("updateNumbers") &&
		myClient->dataExists("updateStrings"))
	{
		printf("Using new updates\n");
		// get the numbers every 100 ms
		myClient->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 ms
		myClient->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;
	}
	else
		myVoltageIsStateOfCharge = (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()
{
	
	CTicTac								tictac, tictacGlobal, tictac_JH;
	CSimpleMap				            finalMap;      //记录PosePDF与SensorFrame映射关系的类，核心m_PosesObsPairs
	float								t_exec;
	COccupancyGridMap2D::TEntropyInfo	entropy;
	size_t								rawlogEntry = 0;
	char								strFil[1000];

	CMetricMapBuilderRBPF::TConstructionOptions		rbpfMappingOptions;

	rbpfMappingOptions.loadFromConfigFile(CConfigFile(Slam_Ini_File), "MappingApplication");
	rbpfMappingOptions.dumpToConsole();

	// ---------------------------------
	//		Constructor
	// ---------------------------------
	CMetricMapBuilderRBPF mapBuilder(rbpfMappingOptions);

	// handle the case of metric map continuation
	if (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;
		}
		else
			timeout_read_scans.Tic();

		observations->insert(observation);

		//TODO TODO TODO
		CPose2D 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 pair
		tictac.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_t		M = 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);

			float	x0 = 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:
		CPose3D			lastMeanPose;
		float			minDistBtwPoses = -1;
		std::deque<TPose3D>		dummyPath;
		mapBuilder.mapPDF.getPath(0, dummyPath);
		for (int k = (int)dummyPath.size() - 1; k >= 0; k--)
		{
			CPose3DPDFParticles	poseParts;
			mapBuilder.mapPDF.getEstimatedPosePDFAtTime(k, poseParts);

			CPose3D		meanPose;
			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");
	//TODO
	if (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 deleted
		bool 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-781
		int 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;
				else
					flag = 14;
			}
		}
		else
			flag = 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();
}