/////////////////////////////////////////////////// //Sample from Samples/devices/lidar /////////////////////////////////////////////////// #include "Modacpp.h" #include "conio.h" #define MODASERVER "localhost" #define WORLD "/" #define DEVICENAME "zone0/lidar" using namespace ModaCPP; //********************************************************************************* void DisplayScan(Moda::Commons::LidarScan *pScan) { //display directly on screen int dx=200,dy=200; HDC hdcecr=GetDC(NULL); HBRUSH hBrush=CreateSolidBrush(RGB(255,255,255)); HPEN hPen=CreatePen(PS_SOLID,1,RGB(0,0,0)); HBRUSH hSaveBrush; HPEN hSavePen; //save data hSaveBrush=(HBRUSH) SelectObject(hdcecr,hBrush); hSavePen=(HPEN) SelectObject(hdcecr,hPen); //paint data //Rectangle(hdcecr,0,0,dx,dy); if(pScan->ValuesCount) { int lines=pScan->ValuesCount/pScan->ValuesPerLine; float measureheight=float(dy)/float(lines); float measurewidth=float(dx)/float(pScan->ValuesPerLine); float minvalue=999.0f; float maxvalue=-999.0f; for(int i=0;i<pScan->ValuesCount;i++) { if(pScan->Values[i]>maxvalue) { //ignore out of range values if(pScan->Values[i]<pScan->RangeMax) maxvalue=pScan->Values[i]; } if(pScan->Values[i]<minvalue) minvalue=pScan->Values[i]; } //check //all values are out ouf range if(maxvalue<=0) { maxvalue=pScan->RangeMax; minvalue=0; } for(int line=0;line<lines;line++) { for(int index=0;index<pScan->ValuesPerLine;index++) { float measure=pScan->Values[line*pScan->ValuesPerLine+index]; if(measure>maxvalue) measure=maxvalue; if(measure<minvalue) measure=minvalue; int color=255-int( ((measure-minvalue)/(maxvalue-minvalue))*255.0f); HBRUSH hTemp=CreateSolidBrush(RGB(color,0,0)); SelectObject(hdcecr,hTemp); int x=dx-(int) ((index+1)*measurewidth); int y=(int) (line*measureheight); Rectangle(hdcecr,x,y,x+max(1,1+(int)measurewidth),y+max(1,1+(int)measureheight)); SelectObject(hdcecr,hSaveBrush); DeleteObject(hTemp); } } } //restore data SelectObject(hdcecr,hSaveBrush); SelectObject(hdcecr,hSavePen); DeleteObject(hBrush); DeleteObject(hPen); ReleaseDC(NULL,hdcecr); } //********************************************************************************* void EnumWorkingModes(DeviceLidar *pLidar) { xkode::lib::Array<Moda::Commons::LidarWorkingMode> _Modes; pLidar->GetWorkingModes(_Modes); _cprintf("Enum Working modes:\r\n"); for(int i=0;i<_Modes.Count();i++) { Moda::Commons::LidarWorkingMode &m=_Modes[i]; _cprintf("->%s\r\n",m.ToString().GetData()); } _cprintf("Enum end.\r\n"); } //********************************************************************************* int main(int argc, char* argv[]) { ModaCPP::Connection *connection=new ModaCPP::Connection(true); //Try connect to MODA server if(connection->Connect(MODASERVER)) { _cprintf("Connection ok to moda server\r\n"); //Find the robot ModaCPP::RobotPHX *pWorld=connection->QueryRobotPHX(WORLD); if(pWorld) { _cprintf("world found\r\n"); DeviceLidar *pLidar=pWorld->QueryDeviceLidar(DEVICENAME); if(pLidar) { _cprintf("device found\r\n"); //enum working modes EnumWorkingModes(pLidar); //scan int time=pWorld->GetTime(); int workingmode=0; while(!_kbhit()) { xkode::lib::Array<MU8> _Scan; M32 DeviceResponse; M32 SystemResponse=pLidar->GetScanWaitComplete(_Scan,&DeviceResponse); if(_Scan.Count()!=0) { //Data is valid if array is not empty Moda::Commons::LidarScan *pScan=(Moda::Commons::LidarScan *)_Scan.GetData(); DisplayScan(pScan); } else { _cprintf("error while getting scan data (system:%d device:%d)\r\n",SystemResponse,DeviceResponse); } } delete pLidar; } else { _cprintf("device %s not found\r\n",DEVICENAME); } delete pWorld; } else { _cprintf("world not found\r\n"); } } else { _cprintf("Unable to connect to moda server\r\n"); } //Disconnect & delete connection->Disconnect(); delete connection; _getch(); return 0; }