樹莓派採集HOKUYO激光雷達數據

前一篇講到在樹莓派上安裝mrpt，其實我安裝mrpt的最終目的是利用樹莓派採集Hokuyo的數據。

連接Hokuyo後，要設置ip 掩碼 和 網關：

下面是採集Hokuyo數據並顯示的代碼：

#include <mrpt/hwdrivers/CHokuyoURG.h>
#include <mrpt/hwdrivers/CSerialPort.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/system/threads.h>
#include <mrpt/system/os.h>

void Test_HOKUYO()
{
	mrpt::hwdrivers::CHokuyoURG		laser;
	std::string			ip = "192.168.0.10";
	unsigned int	port = 10940;

	laser.setIPandPort( ip, port );

	if (!laser.turnOn())
	{
		printf("Initialization failed!\n");
		return;
	}

	mrpt::gui::CDisplayWindowPlots		win("Laser scans");

	std::cout << "Press any key to stop capturing..." << std::endl;

	mrpt::utils::CTicTac     tictac;
	tictac.Tic();

	while (!mrpt::system::os::kbhit())
	{
		bool thereIsObservation,hardError;
		mrpt::obs::CObservation2DRangeScan obs;

		laser.doProcessSimple( thereIsObservation, obs, hardError );

		if (hardError)
			printf("Hardware error=true!!\n");

		if (thereIsObservation)
		{
			double FPS = 1.0 / tictac.Tac();

			printf("Scan received: %u ranges, FOV: %.02fdeg, %.03fHz: mid rang=%fm\n",
				(unsigned int)obs.scan.size(),
				mrpt::utils::RAD2DEG(obs.aperture),
				FPS,
				obs.scan[obs.scan.size()/2]);

			obs.sensorPose = mrpt::poses::CPose3D(0,0,0);

			mrpt::maps::CSimplePointsMap		theMap;
			theMap.insertionOptions.minDistBetweenLaserPoints	= 0;
			theMap.insertObservation( &obs );

			std::vector<float>	xs,ys,zs;
			theMap.getAllPoints(xs,ys,zs);
			win.plot(xs,ys,".b3");
			win.axis_equal();

			tictac.Tic();
		}

		mrpt::system::sleep(15);
	};

	laser.turnOff();
}

int main(int argc, char **argv)
{
	try
	{
		Test_HOKUYO();
		return 0;

	} catch (std::exception &e)
	{
		std::cout << "EXCEPCION: " << e.what() << std::endl;
		return -1;
	}
	catch (...)
	{
		printf("Another exception!!");
		return -1;
	}
}

CMakeLists.txt文件：

SET(sampleName HOKUYO_laser_test)
SET(PRJ_NAME "${sampleName}")

PROJECT(${PRJ_NAME})

CMAKE_MINIMUM_REQUIRED(VERSION 2.4)

SET(EXECUTABLE_OUTPUT_PATH ".")

ADD_EXECUTABLE(${sampleName} HOKUYO_laser_test.cpp  ) 

SET_TARGET_PROPERTIES(
	${sampleName} 
	PROPERTIES 
	PROJECT_LABEL "(EXAMPLE) ${sampleName}")

TARGET_LINK_LIBRARIES(${sampleName} 
	${MRPT_LIBS}
	)

IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug")
	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
ENDIF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug")

來張效果圖~