前一篇讲到在树莓派上安装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")