先锋的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();
}