[TeamTalk 344]: [880] trunk/usarsim: USARSim update from sourceforge.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Tue Dec 4 15:52:26 EST 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20071204/b8fd3609/attachment-0001.html
-------------- next part --------------
Modified: trunk/usarsim/System/USARBot.ini
===================================================================
--- trunk/usarsim/System/USARBot.ini 2007-12-04 20:37:42 UTC (rev 879)
+++ trunk/usarsim/System/USARBot.ini 2007-12-04 20:52:26 UTC (rev 880)
@@ -449,8 +449,8 @@
Sensors=(ItemClass=class'USARBot.GroundTruth',ItemName="GroundTruth",Position=(X=0.0,Y=0.0,Z=-0.0),Direction=(Y=0.0,Z=0.0,X=0.0))
Sensors=(ItemClass=class'USARBot.INSSensor',ItemName="INS",Position=(X=0.0,Y=0.0,Z=-0.0),Direction=(x=0.0,y=0.0,z=0.0))
Sensors=(ItemClass=class'USARModels.SICKLMS',ItemName="Scanner1",Position=(X=0.875,Y=0.0,Z=0.1),Direction=(Y=0.0,Z=0.0,X=0.0))
-Sensors=(ItemClass=class'USARModels.SICKLMS',ItemName="Scanner2",Position=(X=-0.75,Y=0.0,Z=-0.3),Direction=(Y=0.0,Z=3.145,X=0.0))
-Effecters=(ItemClass=class'USARBot.Roller',ItemName="Roller",Parent="UnitLoaderTop_Link1",Position=(Y=0.0,X=0.0,Z=-0.1),Direction=(Y=0.0,Z=0.0,X=0.0))
+;Sensors=(ItemClass=class'USARModels.SICKLMS',ItemName="Scanner2",Position=(X=-0.75,Y=0.0,Z=-0.3),Direction=(Y=0.0,Z=3.145,X=0.0))
+Effecters=(ItemClass=class'USARModels.UnitLoaderTableEffecter',ItemName="Roller",Parent="UnitLoaderTop_Link1",Position=(Y=0.0,X=-0.0,Z=-0.1),Direction=(Y=0.0,Z=0.0,X=0.0))
[USARBot.Hummer]
bDebug=False
Modified: trunk/usarsim/Tools/SimpleUI/ControlDlg.cpp
===================================================================
--- trunk/usarsim/Tools/SimpleUI/ControlDlg.cpp 2007-12-04 20:37:42 UTC (rev 879)
+++ trunk/usarsim/Tools/SimpleUI/ControlDlg.cpp 2007-12-04 20:52:26 UTC (rev 880)
@@ -11,6 +11,7 @@
#include "FreeImage/FIIO_Mem.cpp"
#include ".\controldlg.h"
#include <math.h> // sqrt()
+#include "Tree.h"
#ifdef _DEBUG
#define new DEBUG_NEW
@@ -50,11 +51,12 @@
#define MAX_MSGS 512
#define BLOCK 2048
+#define PATH_LENGTH 512
#define arrayof(x) (sizeof(x)/sizeof(x[0]))
-char _dllPath[512];
-char _utPath[512];
+char _dllPath[PATH_LENGTH];
+char _utPath[PATH_LENGTH];
/////////////////////////////////////////////////////////////////////////////
// CControlDlg dialog
@@ -70,8 +72,6 @@
//}}AFX_DATA_INIT
if (pParent)
m_parent = (CSimpleUIDlg*)pParent;
- m_cmdFile = NULL;
- m_arOut = m_arIn = NULL;
m_cmdSocket = NULL;
m_videoSocket = NULL;
m_videoState = 0;
@@ -79,8 +79,9 @@
m_utclient = NULL;
m_hookDLL = NULL;
m_pFrameData = NULL;
- strcpy(_utPath,UTCAPP);
- strcpy(_dllPath,DLLFILE);
+ strcpy_s(_utPath,PATH_LENGTH,UTCAPP);
+ strcpy_s(_dllPath,PATH_LENGTH,DLLFILE);
+ m_vehicle = NULL;
}
void CControlDlg::DoDataExchange(CDataExchange* pDX)
@@ -123,6 +124,11 @@
ON_BN_CLICKED(IDC_TURNMORE, OnTurnmore)
//}}AFX_MSG_MAP
ON_NOTIFY(TVN_SELCHANGED, IDC_MSG_TREE, OnTvnSelchangedMsgTree)
+ ON_BN_CLICKED(IDC_BUTTON1, &CControlDlg::OnBnClickedButton1)
+ ON_BN_CLICKED(IDC_BUTTON2, &CControlDlg::OnBnClickedButton2)
+ ON_BN_CLICKED(IDC_BUTTON3, &CControlDlg::OnBnClickedButton3)
+ ON_BN_CLICKED(IDC_BUTTON4, &CControlDlg::OnBnClickedButton4)
+ ON_BN_CLICKED(IDC_BUTTON5, &CControlDlg::OnBnClickedButton5)
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
@@ -132,8 +138,6 @@
{
CDialog::OnInitDialog();
- pLog = fopen ("usar_client.log", "w");
-
m_videoState = 0;
m_speed = 0.5;
m_turn = 0;
@@ -144,42 +148,34 @@
m_frameCount = 0;
m_drMode = DR_STOP;
m_pan = m_tilt = 0;
- m_pens = false;
+ CString position = m_parent->m_position;
+ m_pTree = new CSensed(m_parent->m_model, position, &m_dMsgTree, &m_cMsgTree);
+ m_vehicle = new CRobot(m_pTree);
+ // Get the dimensions of the bounding rectangle.
+ GetDlgItem(IDC_VIDEO2)->GetWindowRect(&(m_pTree->m_LidarRec));
+ ScreenToClient(&(m_pTree->m_LidarRec));
+ m_pTree->m_pDC = GetDC();
+
m_cmdSocket = new CCmdSocket(this);
- if (!m_cmdSocket->Create()) {
+ if (!m_cmdSocket->Create())
+ {
MessageBox("Can't create command socket","Socket",MB_OK);
- return TRUE;
}
- if (!m_cmdSocket->Connect(m_parent->m_cHost,m_parent->m_cPort)) {
- MessageBox("Can't connect to UT server","Connection",MB_OK);
+ if (!m_cmdSocket->Connect(m_parent->m_cHost,m_parent->m_cPort))
+ {
m_cmdSocket->Close();
- return TRUE;
+ MessageBox("Can't connect to UT server","Connection",MB_OK);
}
- m_cmdFile = new CSocketFile(m_cmdSocket);
- m_arIn = new CArchive(m_cmdFile, CArchive::load);
- m_arOut = new CArchive(m_cmdFile, CArchive::store);
-
- SetRobot(m_parent->m_model);
- CString position = m_parent->m_position;
- CorrectPosition(position);
- if (m_arOut) {
- CString cmd;
- cmd.Format("INIT {ClassName USARBot.%s} {Location %s}\r\n",
- m_parent->m_model, position);
- LogData(cmd);
- try {
- m_arOut->WriteString(cmd);
- m_arOut->Flush();
- }catch (CException* e) {
- e->Delete();
- MessageBox("Cannot write data!","Error",MB_OK|MB_ICONSTOP);
- //exit(1);
- }
- }
- SetMsgTree();
+ int result = m_vehicle->OpenFiles(m_parent->m_model, position,
+ m_cmdSocket);
+ if (result & 0x10)
+ MessageBox("Can't create output archive","Output",MB_OK);
+ else if (result & 0x20)
+ MessageBox("Cannot write data!","Error",MB_OK|MB_ICONSTOP);
+
if (!m_parent->m_remote) {
// Remote mode
@@ -227,9 +223,6 @@
m_UTCOffsetY = posW.bottom - posW.top - posC.bottom;
Sleep(100);
// Move and scale Unreal Client to desired position
-/* ut->SetWindowPos(&wndTop, posW.left, posW.top,
- posW.right - posW.left, posW.bottom - posW.top,
- SWP_FRAMECHANGED|SWP_NOACTIVATE); */
m_utclient->SetWindowPos(GetNextWindow(GW_HWNDPREV),
posW.left, posW.top,
posW.right - posW.left, posW.bottom - posW.top,
@@ -255,166 +248,14 @@
m_bmpInfo.bmiHeader.biCompression = BI_RGB;
m_bmpInfo.bmiHeader.biSizeImage = 0;
- if (m_cap.sonar || m_cap.range)
- UpdateLaser("", RANGE_SCALE );
+ if (m_pTree->m_cap.sonar || m_pTree->m_cap.range)
+ m_vehicle->UpdateGraphics("", RANGE_SCALE );
UpdateData(FALSE);
return TRUE; // return TRUE unless you set the focus to a control
// EXCEPTION: OCX Property Pages should return FALSE
}
//=============================================================================
-void CControlDlg::CorrectPosition(CString str)
-{
- float float3[3];
- sscanf(str,"%f,%f,%f",float3,float3+1,float3+2);
- float3[2] += m_cap.startHeight;
- str.Format("%04.2f,%04.2f,%04.2f",float3[0],float3[1],float3[2]);
-}
-//=============================================================================
-void CControlDlg::SetRobot(CString name)
-// better to do this from the ini file.
-{
- m_cap.drive = DRIVE_SKID;
- m_cap.sonar = true;
- m_cap.range = RANGE_SCAN;
- m_cap.odometry = true;
- m_cap.inu = true;
- m_cap.encoder = false;
- m_cap.touch = false;
- m_cap.rfid = false;
- m_cap.sound = false;
- m_cap.motion = false;
- m_cap.camera = true;
- m_cap.gps = false;
- m_cap.gripper = false;
- for (int i = 0; i<LIDAR_ROW; i++)
- m_depth[i][0] = 0;
- m_cap.index_sonar = 0;
- m_cap.index_range = 1;
- m_cap.start_sonar = 0;
- m_cap.end_sonar = 16;
- m_cap.start_range = 20;
- m_cap.end_range = 36;
- m_cap.start_scale = 50;
- m_cap.end_scale = 66;
-
-
- if (name=="P2AT")
- {
- m_cap.rfid = true;
- m_cap.startHeight = 0.55F;
- }
- else if (name=="P2DX")
- {
- m_cap.encoder = true;
- m_cap.startHeight = 0.62F;
- }
- else if (name=="ATRVJr")
- {
- m_cap.startHeight = 0.43F;
- }
- else if (name=="Hummer")
- {
- m_cap.drive = DRIVE_ACKERMAN;
- m_cap.sonar = false;
- m_cap.odometry = false;
- m_cap.inu = false;
- m_cap.startHeight = 0.82F;
- }
- else if (name=="Sedan")
- {
- m_cap.drive = DRIVE_ACKERMAN;
- m_cap.sonar = false;
- m_cap.range = RANGE_NONE;
- m_cap.odometry = false;
- m_cap.inu = false;
- m_cap.startHeight = 0.84F;
- }
- else if (name=="SnowStorm")
- {
- m_cap.drive = DRIVE_ACKERMAN;
- m_cap.sonar = false;
- m_cap.range = RANGE_3D;
- m_cap.gps = true;
- m_cap.startHeight = 0.83F;
- m_cap.index_range = 0;
- m_cap.start_range = 0;
- m_cap.end_range = 59;
- m_cap.start_scale = 61;
- m_cap.end_scale = 70;
- }
- else if (name=="PapaGoose")
- {
- m_cap.rfid = true;
- m_cap.startHeight = 0.64F;
- }
- else if (name=="Rover")
- {
- m_cap.drive = DRIVE_ACKERMAN;
- m_cap.range = RANGE_NONE;
- m_cap.odometry = false;
- m_cap.inu = false;
- m_cap.startHeight = 0.67F;
- }
- m_cap.start_sonar = 0;
- m_cap.end_sonar = 16;
- m_cap.start_range = 20;
- m_cap.end_range = 36;
- m_cap.start_scale = 60;
- m_cap.end_scale = 76;
-}
-//=============================================================================
-void CControlDlg::SetMsgTree()
-{
-// Output to the main screen
- m_dMsgTree.mt_time = m_cMsgTree.InsertItem(_T("Time:"), 0, TVI_LAST);
- m_dMsgTree.mt_state = m_cMsgTree.InsertItem(_T("Robot State"),0, TVI_LAST);
- m_dMsgTree.mt_chassis =m_cMsgTree.InsertItem(_T("Chassis"), m_dMsgTree.mt_state, TVI_LAST);
- m_dMsgTree.mt_ch_rot = m_cMsgTree.InsertItem(_T("Rotation:"),m_dMsgTree.mt_chassis,TVI_LAST);
- m_dMsgTree.mt_ch_loc = m_cMsgTree.InsertItem(_T("Location:"),m_dMsgTree.mt_chassis,TVI_LAST);
- m_dMsgTree.mt_ch_vel = m_cMsgTree.InsertItem(_T("Velocity:"),m_dMsgTree.mt_chassis,TVI_LAST);
- if (m_cap.camera)
- {
- m_dMsgTree.mt_camera = m_cMsgTree.InsertItem(_T("Camera"), m_dMsgTree.mt_state, TVI_LAST);
- m_dMsgTree.mt_pan_loc= m_cMsgTree.InsertItem(_T("Pan:"), m_dMsgTree.mt_camera, TVI_LAST);
-// m_dMsgTree.mt_pan_rot= m_cMsgTree.InsertItem(_T("Rotation:"),m_dMsgTree.mt_camera, TVI_LAST);
- m_dMsgTree.mt_tilt_loc=m_cMsgTree.InsertItem(_T("Tilt:"), m_dMsgTree.mt_camera, TVI_LAST);
-// m_dMsgTree.mt_tilt_rot=m_cMsgTree.InsertItem(_T("Rotation:"),m_dMsgTree.mt_camera, TVI_LAST);
- }
- m_dMsgTree.mt_light = m_cMsgTree.InsertItem(_T("Light:"), m_dMsgTree.mt_state, TVI_LAST);
- m_dMsgTree.mt_battery =m_cMsgTree.InsertItem(_T("Battery:"), m_dMsgTree.mt_state, TVI_LAST);
- if (m_cap.odometry)
- {
- m_dMsgTree.mt_odometry = m_cMsgTree.InsertItem(_T("Odometry"), 0, TVI_LAST);
- m_dMsgTree.mt_od_data = m_cMsgTree.InsertItem(_T("Position:"),m_dMsgTree.mt_odometry,TVI_LAST);
- }
- if (m_cap.gps)
- {
- m_dMsgTree.mt_gps = m_cMsgTree.InsertItem(_T("GPS"), 0, TVI_LAST);
- m_dMsgTree.mt_gps_data = m_cMsgTree.InsertItem(_T("Position:"),m_dMsgTree.mt_gps, TVI_LAST);
- }
- if (m_cap.inu)
- {
- m_dMsgTree.mt_inu = m_cMsgTree.InsertItem(_T("INU"), 0, TVI_LAST);
- m_dMsgTree.mt_inu_data = m_cMsgTree.InsertItem(_T("Orientation:"),m_dMsgTree.mt_inu, TVI_LAST);
- }
- m_dMsgTree.mt_other = m_cMsgTree.InsertItem(_T("Other"),0,TVI_LAST);
- if (m_cap.sonar)
- {
- m_dMsgTree.mt_sonar = m_cMsgTree.InsertItem(_T("Sonar"),0,TVI_LAST);
- m_dMsgTree.mt_so_count = 0;
- m_dMsgTree.mt_sonars = NULL;
- }
- if (m_cap.range > 0)
- {
- m_dMsgTree.mt_laser = m_cMsgTree.InsertItem(_T("Laser"),0,TVI_LAST);
- m_dMsgTree.mt_ls_count = 0;
- m_dMsgTree.mt_ls_data = NULL;
- }
- m_cMsgTree.Expand(m_dMsgTree.mt_state,TVE_EXPAND);
-}
-//=============================================================================
-
void CControlDlg::ProcessVideoData()
{
int count;
@@ -524,35 +365,8 @@
m_videoSocket->AsyncSelect(FD_READ|FD_CLOSE);
}
-void CControlDlg::ProcessMsgData()
-{
- CString str;
- int process=1;
- //int tmp;
-
- do {
- m_arIn->ReadString(str);
-
- //m_msgList.InsertString(0,str);
- //if (m_msgList.GetCount()>=MAX_MSGS)
- // m_msgList.DeleteString(MAX_MSGS);
-
- UpdateMsgTree(str);
- LogData(str);
-
- //tmp = str.GetLength()*6;
- //if (tmp>maxMsgLen) {
- // maxMsgLen = tmp;
- // m_msgList.SetHorizontalExtent(tmp);
- //}
- Sleep(1);
- }while(!m_arIn->IsBufferEmpty());
- UpdateData(false);
-}
-
void CControlDlg::OnClose()
{
- fclose(pLog);
if (m_parent->m_remote) {
// free(m_pFrameData); // can't free since it is linked to CRT; TCF 8/15/06
KillTimer(IDC_TIMER);
@@ -561,73 +375,38 @@
m_utclient->SendMessage(WM_CLOSE);
}
}
+ if (m_pTree)
+ {
+ if (m_pTree->m_pDC)
+ ReleaseDC(m_pTree->m_pDC);
+ m_pTree->Close();
+ delete m_pTree;
+ m_pTree = NULL;
+ }
+ if (m_vehicle)
+ {
+ m_vehicle->Close();
+ delete m_vehicle;
+ m_vehicle = NULL;
+ }
if (m_hookDLL!=NULL)
FreeLibrary(m_hookDLL);
- if (m_arIn) {
- delete m_arIn;
- m_arIn=NULL;
+ if (m_videoSocket) {
+ m_videoSocket->Close();
+ delete m_videoSocket;
+ m_videoSocket = NULL;
}
- if (m_arOut) {
- delete m_arOut;
- m_arOut=NULL;
- }
- if (m_cmdFile) {
- delete m_cmdFile;
- m_cmdFile=NULL;
- }
if (m_cmdSocket) {
m_cmdSocket->Close();
delete m_cmdSocket;
m_cmdSocket = NULL;
}
- if (m_videoSocket) {
- m_videoSocket->Close();
- delete m_videoSocket;
- m_videoSocket = NULL;
- }
-
- m_cMsgTree.DeleteAllItems();
-// if (m_dMsgTree.mt_sonars!=NULL)
-// free(m_dMsgTree.mt_sonars);
-// if (m_dMsgTree.mt_ls_data!=NULL)
-// free(m_dMsgTree.mt_ls_data);
- CleanUpGraphics();
-
CDialog::OnClose();
}
void CControlDlg::UpdateVideo()
{
- /*
- ////////////////////////////////////////////////////
- // Save to file
- BITMAPINFO bi;
- bi.bmiHeader.biSize = sizeof(bi.bmiHeader);
- bi.bmiHeader.biWidth = m_FrameData->width;
- bi.bmiHeader.biHeight = m_FrameData->height;
- bi.bmiHeader.biPlanes = 1;
- bi.bmiHeader.biBitCount = 24;
- bi.bmiHeader.biCompression = BI_RGB;
- bi.bmiHeader.biSizeImage = m_FrameData->size;
- bi.bmiHeader.biXPelsPerMeter = 0;
- bi.bmiHeader.biYPelsPerMeter = 0;
- bi.bmiHeader.biClrUsed = 0;
- bi.bmiHeader.biClrImportant = 0;
-
- BITMAPFILEHEADER bmfh;
- bmfh.bfType = 'MB';
- bmfh.bfSize = sizeof(bmfh) + sizeof(BITMAPINFOHEADER) + bi.bmiHeader.biSizeImage;
- bmfh.bfReserved1 = bmfh.bfReserved2 = 0;
- bmfh.bfOffBits = sizeof(bmfh) + sizeof(BITMAPINFOHEADER);
-
- FILE* f = fopen("test.bmp", "wb");
- fwrite(reinterpret_cast<void*>(&bmfh), sizeof(bmfh), 1, f);
- fwrite(reinterpret_cast<void*>(&(bi.bmiHeader)), sizeof(BITMAPINFOHEADER), 1, f);
- fwrite(reinterpret_cast<void*>(m_FrameData->data), sizeof(BYTE), bi.bmiHeader.biSizeImage, f);
- fclose(f);
- */
-
if (m_utclient && !IsIconic() && m_utclient->IsIconic()) {
::OpenIcon(m_utclient->m_hWnd);
return;
@@ -680,29 +459,25 @@
}
}
-
-
void CControlDlg::ShowUTC(BOOL show)
{
m_show = show;
if (!show) {
HDC hScrDC = CreateDC("DISPLAY", NULL, NULL, NULL);
m_utclient->SetWindowPos(this,
- GetDeviceCaps(hScrDC, HORZRES)-10,
- GetDeviceCaps(hScrDC, VERTRES)-10,
- m_UTCOffsetX+m_imgWidth, m_UTCOffsetY+m_imgHeight,
- SWP_FRAMECHANGED|SWP_NOACTIVATE);
+ GetDeviceCaps(hScrDC, HORZRES)-10,
+ GetDeviceCaps(hScrDC, VERTRES)-10,
+ m_UTCOffsetX+m_imgWidth, m_UTCOffsetY+m_imgHeight,
+ SWP_FRAMECHANGED|SWP_NOACTIVATE);
DeleteDC(hScrDC);
}
else {
RECT posW;
GetWindowRect(&posW);
m_utclient->SetWindowPos(this,
-// posW.left, posW.top,
-// posW.right - posW.left, posW.bottom - posW.top,
- posW.left-m_UTCOffsetX-m_imgWidth, posW.top,
- m_UTCOffsetX+m_imgWidth, m_UTCOffsetY+m_imgHeight,
- SWP_FRAMECHANGED|SWP_NOACTIVATE);
+ posW.left-m_UTCOffsetX-m_imgWidth, posW.top,
+ m_UTCOffsetX+m_imgWidth, m_UTCOffsetY+m_imgHeight,
+ SWP_FRAMECHANGED|SWP_NOACTIVATE);
}
}
@@ -730,6 +505,7 @@
m_height = m_pFrameData->height;
}
((CButton *)GetDlgItem(IDC_UPDATE))->SetCheck(1);
+ m_vehicle->ProcessMsgData();
UpdateData(FALSE);
UpdateVideo();
Sleep(2);
@@ -767,16 +543,19 @@
AfxMessageBox( IDS_OPEN_UT2004, MB_OK|MB_ICONSTOP );
errno = 0;
CFileDialog * pcf = new CFileDialog(true,".exe","ut2004",
- OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT,
- "Executable Files (*.exe)|*.exe");
+ OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT,
+ "Executable Files (*.exe)|*.exe");
pcf->DoModal();
CString utPath = pcf->GetPathName();
CString file = pcf->GetFileName();
delete pcf;
utPath.Replace("\\","\\\\");
strncpy(_utPath,(LPCSTR)utPath,512);
+
+
utPath.Replace((LPCSTR)file,"Hook.dll");
strncpy(_dllPath,(LPCSTR)utPath,512);
+// strncpy_s(_dllPath,PATH_LENGTH,(LPCSTR)utPath);
if (!DetourCreateProcessWithDll(_utPath,
UTCCMD, NULL, NULL, TRUE,
CREATE_DEFAULT_ERROR_MODE, NULL, NULL,
@@ -839,18 +618,18 @@
void CControlDlg::OnCup()
{
m_drMode = DR_FORWARD;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnCdown()
{
m_drMode = DR_BACKWARD;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnCleft()
{
- if(m_cap.drive == DRIVE_ACKERMAN)
+ if(m_pTree->m_cap.drive == DRIVE_ACKERMAN)
{
if (m_turn <= TURN_STEP && -TURN_STEP <= m_turn)
m_turn += TURN_STEP;
@@ -860,12 +639,12 @@
}
else
m_drMode = DR_LEFT;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnCright()
{
- if(m_cap.drive == DRIVE_ACKERMAN)
+ if(m_pTree->m_cap.drive == DRIVE_ACKERMAN)
{
if (m_turn <= TURN_STEP && -TURN_STEP <= m_turn)
m_turn -= TURN_STEP;
@@ -875,39 +654,37 @@
}
else
m_drMode = DR_RIGHT;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnStop()
{
m_drMode = DR_STOP;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnSpeedUp()
{
m_speed += SPEED_STEP;
m_speed = m_speed>SPEED_MAX?SPEED_MAX:m_speed;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnSpeedDown()
{
m_speed -= SPEED_STEP;
m_speed = m_speed<SPEED_MIN?SPEED_MIN:m_speed;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnStraight()
{
m_turn = 0;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
-
-
void CControlDlg::OnTurnless()
{
m_turn *= 0.5;
m_turn = m_turn<TURN_MIN?TURN_MIN:m_turn;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
void CControlDlg::OnTurnmore()
@@ -917,91 +694,35 @@
else
m_turn += m_turn;
m_turn = m_turn>TURN_MAX?TURN_MAX:m_turn;
- Drive();
+ m_vehicle->Drive(m_pTree->m_cap.drive, m_drMode, m_speed, m_turn);
}
-void CControlDlg::Drive()
-{
- CString cmd;
- if (m_arOut==NULL) return;
- switch (m_cap.drive){
- case DRIVE_ACKERMAN:
- switch (m_drMode){
- case DR_FORWARD:
- cmd.Format("DRIVE {Speed %f} {FrontSteer %f}\r\n",m_speed, m_turn);
-// cmd.Format("DRIVE {Speed %f}\r\n",m_speed);
- break;
- case DR_BACKWARD:
- cmd.Format("DRIVE {Speed -%f} {FrontSteer -%f}\r\n",m_speed, m_turn);
-// cmd.Format("DRIVE {Speed -%f}\r\n",m_speed);
- break;
- case DR_LEFT:
- cmd.Format("DRIVE {Speed %f} {FrontSteer %f}\r\n",m_speed, m_turn);
- break;
- case DR_RIGHT:
- cmd.Format("DRIVE {Speed %f} {FrontSteer -%f}\r\n",m_speed,m_turn);
- break;
- case DR_STOP:
- cmd = "DRIVE {Speed 0} \r\n";
- break;
- default:
- cmd = "";
- }
- break;
- default:
- case DRIVE_SKID:
- switch (m_drMode){
- case DR_FORWARD:
- cmd.Format("DRIVE {Left %f} {Right %f}\r\n",m_speed,m_speed);
- break;
- case DR_BACKWARD:
- cmd.Format("DRIVE {Left -%f} {Right -%f}\r\n",m_speed,m_speed);
- break;
- case DR_LEFT:
- cmd.Format("DRIVE {Left -%f} {Right %f}\r\n",m_speed,m_speed);
- break;
- case DR_RIGHT:
- cmd.Format("DRIVE {Left %f} {Right -%f}\r\n",m_speed,m_speed);
- break;
- case DR_STOP:
- cmd = "DRIVE {Left 0} {Right 0}\r\n";
- break;
- default:
- cmd = "";
- }
- break;
- }
- m_arOut->WriteString(cmd);
- LogData(cmd);
- m_arOut->Flush();
-}
-
void CControlDlg::OnVleft()
{
m_pan += PAN_STEP;
m_pan = m_pan<PAN_MIN?PAN_MIN:m_pan;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
void CControlDlg::OnVright()
{
m_pan -= PAN_STEP;
m_pan = m_pan<PAN_MIN?PAN_MIN:m_pan;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
void CControlDlg::OnVup()
{
m_tilt += TILT_STEP;
m_tilt = m_tilt<TILT_MIN?TILT_MIN:m_tilt;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
void CControlDlg::OnVdown()
{
m_tilt -= TILT_STEP;
m_tilt = m_tilt<TILT_MIN?TILT_MIN:m_tilt;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
void CControlDlg::OnZoomIn() // Labeled "Straight"
@@ -1010,495 +731,75 @@
m_zoom = m_zoom<ZOOM_MIN?ZOOM_MIN:m_zoom;
m_pan = 0;
m_tilt = 0;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
-
void CControlDlg::OnZoomOut()
{
m_zoom += ZOOM_STEP;
m_zoom = m_zoom>ZOOM_MAX?ZOOM_MAX:m_zoom;
m_pan = 0;
m_tilt = 0;
- Camera();
+ m_vehicle->Camera(m_pan, m_tilt);
}
-
-void CControlDlg::Camera()
-{
- CString cmd;
-
- if (m_arOut==NULL) return;
-// cmd.Format("CAMERA {Rotation %d,%d,0} {Zoom %d}\r\n",m_pan,m_tilt,m_zoom);
-// MISPKG {Name CameraPanTilt} {Link1 3.1415} {Link2 -0.1}
- cmd.Format("MISPKG {Name CameraPanTilt} {Link1 %f} {Link2 %f}\r\n",m_pan,m_tilt);
- m_arOut->WriteString(cmd);
- LogData(cmd);
- m_arOut->Flush();
-}
-
//=============================================================================
-void CControlDlg::UpdateMsgTree(CString str)
-{
- if (str.Find("STA",0)==0) {
-
- UpdateStatus( str );
-
- }
- else if (str.Find("MISSTA", 0)==0) {
- if (str.Find("{Name CameraPanTilt}",0)>=0) {
- UpdateCamera( str );
- }}
- else if (str.Find("SEN",0)==0)
- {
- if (str.Find("{Type LIDAR}",0)>=0) {
- UpdateRangeScan( str );
- UpdateLaser( str, RANGE_3D );
- }
- else if(str.Find("{Type RangeScanner}",0)>=0) {
- UpdateRangeScan( str );
- UpdateLaser( str, RANGE_SCAN );
- }
- else if (str.Find("{Type Sonar}",0)>=0) {
- UpdateSonar( str );
- UpdateLaser( str, RANGE_SONAR );
- }
- else if (str.Find("{Type IR}",0)>=0) {
- UpdateSonar( str );
- UpdateLaser( str, RANGE_IR );
- }
- else if (str.Find("{Type Odometry}",0)>=0) {
- UpdateOdometry( str );
- }
- else if (str.Find("{Type INS}",0)>=0) {
- UpdateINU( str );
- }
- else if (str.Find("{Type GPS}",0)>=0) {
- UpdateGPS( str );
- }
- else
- m_cMsgTree.SetItemText(m_dMsgTree.mt_other,(LPCSTR)str);
- }
- else {
- CString tmp = "Other: " + str;
- m_cMsgTree.SetItemText(m_dMsgTree.mt_other,(LPCSTR)tmp);
- }
-
-}
-//=============================================================================
-void CControlDlg::UpdateStatus(CString str)
-{
- CString tmp;
- CString rot, loc, vel;
- int count=3;
- float float3[3];
- float speed, speed_kph;
-// float wheels, wheels_degrees;
-
- tmp = "Time:"+CMessageParser::GetString("Time",str);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_time,(LPCSTR)tmp);
-
- CMessageParser::GetFloats("Location",str,',',&count,float3);
- tmp.Format("Location(m): X=%6.2f Y=%6.2f Z=%6.2f",float3[0],float3[1],float3[2]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_ch_loc,(LPCSTR)tmp);
- loc.Format("Location: (%6.2f, %6.2f)",float3[0],float3[1]);
-
- CMessageParser::GetFloats("Orientation",str,',',&count,float3);
- tmp.Format("Rotation(Rad): Pitch=%6.2f Roll=%6.2f Yaw=%6.2f",float3[0],float3[1],float3[2]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_ch_rot,(LPCSTR)tmp);
-
- CMessageParser::GetFloats("Velocity",str,',',&count,float3);
- tmp.Format("Velocity(m/s): X=%6.2f Y=%5.2f Z=%5.2f",float3[0],float3[1],float3[2]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_ch_vel,(LPCSTR)tmp);
- speed = (float)sqrt(float3[0]*float3[0]+float3[1]*float3[1]+float3[2]*float3[2]);
- speed_kph = speed*(float)3.6;
- vel.Format(" Speed = %6.2fm/s = %6.1fkm/h",speed, speed_kph);
-
- tmp.Format("Light: On=%s Intensity=%d",
- (LPCSTR)CMessageParser::GetString("LightToggle",str),
- CMessageParser::GetInt("LightIntensity",str));
- m_cMsgTree.SetItemText(m_dMsgTree.mt_light,(LPCSTR)tmp);
-
- tmp.Format("Fuel(%%):%d",(int)CMessageParser::GetFloat("Battery",str));
- m_cMsgTree.SetItemText(m_dMsgTree.mt_battery,(LPCSTR)tmp);
-
- tmp = "Chassis: " + loc + vel;
- m_cMsgTree.SetItemText(m_dMsgTree.mt_chassis,(LPCSTR)tmp);
-
-}
-
-//=============================================================================
-void CControlDlg::UpdateCamera(CString str)
-{
- /* MISSTA {Time 4341.34} {Name CameraPanTilt} {Link1 0.00,-20.00} {Link2 0.00,-20.00}
-
- Had been:
- MIS {Type PanTilt} {Name Cam1}
- {Part CameraPan Location 0.0001,0.0006,0.1249 Orientation 0.0038,-0.0014,0.0000}
- {Part CameraTilt Location 0.0001,0.0005,0.2962 Orientation 0.0000,0.0000,0.0000} */
- CString tmp;
- CString pan, tilt;
- int count=2;
- float float2[2];
-
- CMessageParser::GetFloats("Link1",str,',',&count,float2);
- tmp.Format("Pan =%6.2f (Rad); torque=%6.2f",float2[0],float2[1]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_pan_loc,(LPCSTR)tmp);
- pan.Format(" Pan =%6.2f", float2[0]);
-
- CMessageParser::GetFloats("Link2",str,',',&count,float2);
- tmp.Format(" Tilt =%6.2f (Rad); torque=%6.2f",float2[0],float2[1]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_tilt_loc,(LPCSTR)tmp);
- tilt.Format("Tilt =%6.2f", float2[0]);
-
- tmp = "Camera: " + pan + ", " + tilt;
- m_cMsgTree.SetItemText(m_dMsgTree.mt_camera,(LPCSTR)tmp);
-}
-
-//=============================================================================
-void CControlDlg::UpdateSonar(CString str)
-{
- CString tmp[32], sonar="Sonar:";
- const char *pChar;
- int pos1=0, pos2=0, count=0;
- while (pos1>=0 && count<32) {
- tmp[count++] = CMessageParser::GetString("Name",str, pos1, pos2);
- pos1 = pos2;
- }
- if (pos1<0) count--;
- m_depth[m_cap.index_sonar][0] = (float) count;
- if (m_dMsgTree.mt_sonars==NULL) {
- m_dMsgTree.mt_so_count = count;
- m_dMsgTree.mt_sonars = (HTREEITEM *)malloc(sizeof(HTREEITEM)*count);
- for (int i=0;i<count;i++)
- m_dMsgTree.mt_sonars[i] = m_cMsgTree.InsertItem(_T("Name:"),m_dMsgTree.mt_sonar,TVI_LAST);
- }
- for (int i=0;i<count;i++) {
-// tmp[i].Replace("Name ","");
- pos1 = tmp[i].Find("Range");
- // if (p1<0) return "";
- pos1 += 5;
- pChar = (LPCTSTR) tmp[i];
- pChar += pos1;
- m_depth[m_cap.index_sonar][i+1] = (float)atof(pChar);
- tmp[i].Replace("Range ",":");
- sonar += tmp[i] + " ";
- m_cMsgTree.SetItemText(m_dMsgTree.mt_sonars[i],(LPCSTR)tmp[i]);
- }
- m_cMsgTree.SetItemText(m_dMsgTree.mt_sonar,(LPCSTR)sonar);
-
-}
-//=============================================================================
-void CControlDlg::UpdateRangeScan(CString str)
-{
- /*
- SEN {Time 511.94} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174}
- {FOV 3.1415} {Range 20.0000,19.9903,20.0000,20.0000,20.0000,20.0000,
- 19.9868,19.9946,20.0000,20.0000,19.9969,20.0000,20.0000,20.0000,20.0000,
- 20.0000,19.9875,20.0000,19.9954,20.0000,19.9973,19.9973,20.0000,19.9809,
- 19.9935,19.9819,20.0000,20.0000,19.9899,19.9973,19.9823,19.9940,19.9938,
- 19.9856,20.0000,20.0000,20.0000,19.9878,20.0000,20.0000,19.9891,19.9952,
- 19.9905,19.9976,19.9959,20.0000,19.9930,19.9943,20.0000,20.0000,20.0000,
- 19.9811,19.9906,20.0000,20.0000,19.9937,19.9911,20.0000,19.9831,19.9973,
- 19.9861,19.9826,19.9924,20.0000,20.0000,19.9959,20.0000,19.9993,19.9826,
- 19.9816,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,19.9990,20.0000,
- 20.0000,19.9828,19.9903,20.0000,20.0000,20.0000,20.0000,20.0000,19.9831,
- 20.0000,20.0000,20.0000,20.0000,19.9989,19.9833,19.9958,19.9865,20.0000,
- 19.9867,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,
- 19.9831,19.9839,19.9975,19.9827,19.9872,19.9979,20.0000,19.9877,20.0000,
- 20.0000,19.9915,19.9887,19.9903,20.0000,20.0000,19.9950,20.0000,19.9846,
- 19.9802,20.0000,19.9882,20.0000,20.0000,19.9873,18.9922,15.4983,13.1021,
- 11.3225,10.0221,8.9622,8.1160,7.4080,6.8297,6.3202,5.8914,5.5086,5.1866,
- 4.9034,4.6398,4.4134,4.2047,4.0152,3.8472,3.6852,3.5415,3.4175,3.2956,
- 3.1792,3.0765,2.9832,2.8936,2.8057,2.7298,2.6573,2.5860,2.5266,2.4643,
- 2.4061,2.3549,2.3038,2.2541,2.2111,2.1658,2.1286,2.0886,2.0511,2.0191,
- 1.9859,1.9557,1.9218,1.8937,1.8702,1.8425,1.8201,1.7939,1.7732}
- */
-
- CString dataStr = "Ranges(";
- dataStr += CMessageParser::GetString("Name",str) +"):" + CMessageParser::GetString("Range",str);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_laser,(LPCSTR)dataStr);
-
- int count = 256;
- CMessageParser::GetFloats("Range",str,',',&count,&m_depth[m_cap.index_range][1]);
- m_depth[m_cap.index_range][0] = (float) count;
- int lines = count/8;
- int lastLn = count%8;
- if (lastLn) lines+=1;
- if (m_dMsgTree.mt_ls_data==NULL) {
- m_dMsgTree.mt_ls_count = lines;
- m_dMsgTree.mt_ls_data = (HTREEITEM *)malloc(sizeof(HTREEITEM)*lines);
- for (int i=0;i<lines;i++)
- m_dMsgTree.mt_ls_data[i] = m_cMsgTree.InsertItem(_T("Data:"),m_dMsgTree.mt_laser,TVI_LAST);
- }
- lines-=1;
- for (int i=0;i<lines;i++) {
- dataStr.Format("Data: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f",
- m_depth[m_cap.index_range][i*8+1], m_depth[m_cap.index_range][i*8+2],
- m_depth[m_cap.index_range][i*8+3], m_depth[m_cap.index_range][i*8+4],
- m_depth[m_cap.index_range][i*8+5], m_depth[m_cap.index_range][i*8+6],
- m_depth[m_cap.index_range][i*8+7], m_depth[m_cap.index_range][i*8+8]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_ls_data[i],(LPCSTR)dataStr);
- }
- if (lines>=0) {
- if (lastLn==0) lastLn = 8;
- CString data;
- dataStr = "Data: ";
- for (int i=0;i<lastLn;i++) {
- data.Format("%6.2f ",m_depth[m_cap.index_range][lines*8+i+1]);
- dataStr += data;
- }
- m_cMsgTree.SetItemText(m_dMsgTree.mt_ls_data[lines],(LPCSTR)dataStr);
- }
-
-}
-//=============================================================================
-void CControlDlg::UpdateLaser(CString str, int range)
-{
- CString name;
- // float depth[LIDAR_ROW][LIDAR_COL];
- int index, start, end;
- unsigned int i, c, r, lidarCol; // temporary indices
- unsigned int row = 0, col = 0; // number of items in lidar scan
- unsigned int row_box, col_box, chunk; // size of each item in pixels
- unsigned int imgHeight, imgWidth; // pixel size available for drawing
- unsigned int r_pixels, c_pixels; // pixels used for drawing
- float threshold[QUANTIZATION] = {2, 3, 4, 5, 6, 7.3F, 8.8F, 10.6F, 12.6F, 14.8F, 17, 19.8F, (float)10e20};
-
- // Get the dimensions of the bounding rectangle.
- GetDlgItem(IDC_VIDEO2)->GetWindowRect(&m_LidarRec);
- ScreenToClient(&m_LidarRec);
- CDC* pDC = GetDC();
- // Calculate sizes of Lidar areas
- imgWidth = m_LidarRec.right - m_LidarRec.left;
- imgHeight = m_LidarRec.bottom- m_LidarRec.top;
-
- switch (range) {
- case RANGE_3D:
- index = m_cap.index_range;
- start = m_cap.start_range;
- end = m_cap.end_range;
- for (r = 0; r < LIDAR_ROW; r++)
- {
- name.Format("Range%-i", r);
- int count = LIDAR_COL;
-
- // Capture the Lidar readings
- CMessageParser::GetFloats(name,str,',',&count,&m_depth[index][1]);
- m_depth[index][0] = (float) count;
- if (count > 1)
- {
- col = (count-col)? count: col;
- row++;
- }
- }
- col--;
- break;
- case RANGE_SCAN:
- index = m_cap.index_range;
- start = m_cap.start_range;
- end = m_cap.end_range;
- break;
- default:
- index = m_cap.index_sonar;
- start = m_cap.start_sonar;
- end = m_cap.end_sonar;
- break;
- }
- // Range scale
- if (!m_pens)
- {
- InitializeGraphics(m_cap.end_scale-m_cap.start_scale);
- pDC->FillSolidRect(&m_LidarRec, GRAY);
- }
- // Draw the color key
- r = m_cap.start_scale + m_LidarRec.top;
- chunk = (imgWidth - 65)/QUANTIZATION;
- c=m_LidarRec.left+40;
- pDC->MoveTo(c, r);
- for (i = 0; i < QUANTIZATION; i++) {
- c+=chunk;
- pDC->SelectObject(m_hPen[i]);
- pDC->LineTo(c, r);
- }
- pDC->DrawText("Close", -1, &m_LidarRec, DT_BOTTOM | DT_LEFT | DT_SINGLELINE);
- pDC->DrawText("Far", -1, &m_LidarRec, DT_BOTTOM | DT_RIGHT | DT_SINGLELINE);
-
- if (range != RANGE_SCALE)
- {
- if (m_depth[index][0] == 0 && range != RANGE_SCALE)
- return;
-
- start += m_LidarRec.top;
- end += m_LidarRec.top;
- col = (unsigned int) m_depth[index][0];
- col_box = imgWidth / col;
- if (col_box < 1) col_box = 1;
- c_pixels = col_box * col;
- if (c_pixels > imgWidth) c_pixels = imgWidth;
- row_box = end-start;
- if (range == RANGE_3D)
- row_box /= row;
- else
- row = 1;
- if (row_box < 1) row_box = 1;
- if (!m_pens)
- InitializeGraphics(row_box/2);
- r_pixels = row_box * row;
- if (r_pixels > imgHeight) r_pixels = imgHeight;
- // Draw the Lidar map
- r = start;
- for (r += row_box; r <= (unsigned)end; r += row_box)
- {
- c = (imgWidth-c_pixels)/2+m_LidarRec.left;
- pDC->MoveTo(c, r);
- c += col_box;
- for (lidarCol=col; lidarCol>0; c += col_box,lidarCol--)
- {
- for (i=0; m_depth[index][lidarCol] > threshold[i]; i++)
- { }
- pDC->SelectObject(m_hPen[i]);
- pDC->LineTo(c, r);
- }
- if (range == RANGE_3D)
- index++;
-
- }
- }
-
- if (pDC)
- {
- ReleaseDC(pDC);
- }
-
-}
-//=============================================================================
-void CControlDlg::InitializeGraphics(int pen_width)
-{
- m_hPen[0] = CreatePen( PS_SOLID, pen_width, WHITE);
- m_hPen[1] = CreatePen( PS_SOLID, pen_width, YELLOW);
- m_hPen[2] = CreatePen( PS_SOLID, pen_width, OFF_WHITE);
- m_hPen[3] = CreatePen( PS_SOLID, pen_width, TAN);
- m_hPen[4] = CreatePen( PS_SOLID, pen_width, OLIVE);
- m_hPen[5] = CreatePen( PS_SOLID, pen_width, LT_GREEN);
- m_hPen[6] = CreatePen( PS_SOLID, pen_width, MED_GREEN);
- m_hPen[7] = CreatePen( PS_SOLID, pen_width, GREEN);
- m_hPen[8] = CreatePen( PS_SOLID, pen_width, AQUA);
- m_hPen[9] = CreatePen( PS_SOLID, pen_width, LT_BLUE);
- m_hPen[10] = CreatePen( PS_SOLID, pen_width, ROYAL_BLUE);
- m_hPen[11] = CreatePen( PS_SOLID, pen_width, PURPLE);
- m_hPen[12] = CreatePen( PS_SOLID, pen_width, BLACK);
- m_pens = true;
-}
-//=============================================================================
-void CControlDlg::CleanUpGraphics()
-{
- int i;
- if (!m_pens)
- return;
- for (i=0; i<13; i++)
- DeleteObject( m_hPen[i] );
-// DeleteObject( m_hGrayBrush );
- m_pens = false;
-
-}
-//=============================================================================
-void CControlDlg::UpdateOdometry(CString str)
-{
- CString tmp ="Odometry:";
- int num=2;
- int pos1=0, pos2=0;
- float float3[3];
-
- tmp += CMessageParser::GetString("Name",str, pos1, pos2);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_odometry,(LPCSTR)tmp);
-
- CMessageParser::GetFloats("Pose",str,',',&num,float3);
- tmp.Format("Position(m): X=%6.2f Y=%6.2f ",float3[0],float3[1]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_od_data,(LPCSTR)tmp);
-}
-//=============================================================================
-void CControlDlg::UpdateGPS(CString str)
-{
- CString tmp ="GPS:";
- int num=3;
- int pos1=0, pos2=0;
- int lat[3], lon[3];
-
- tmp += CMessageParser::GetString("Name",str, pos1, pos2);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_gps,(LPCSTR)tmp);
-
- CMessageParser::GetInts("Latitude",str,',',&num,lat);
- CMessageParser::GetInts("Longitude",str,',',&num,lon);
- // assume North America
- lon[0] = -lon[0];
- tmp.Format("Lat=%iN%02i.%04i, Long=%iW%02i.%04i",
- lat[0], lat[1], lat[2], lon[0], lon[1], lon[2]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_gps_data,(LPCSTR)tmp);
-
-}
-//=============================================================================
-void CControlDlg::UpdateINU(CString str)
-{
- /* SEN {Type INU} {Name Compass} {Orientation 0.0000,-0.0512,0.0000}
-*/
- CString tmp ="INS:";
- int num=3;
- int pos1=0, pos2=0;
- float float3[3];
-
- tmp += CMessageParser::GetString("Name",str, pos1, pos2);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_inu,(LPCSTR)tmp);
-
- CMessageParser::GetFloats("Orientation",str,',',&num,float3);
- tmp.Format("Orientation(Rad): Roll=%6.2f Pitch=%6.2f Yaw=%6.2f",float3[0],float3[1],float3[2]);
- m_cMsgTree.SetItemText(m_dMsgTree.mt_inu_data,(LPCSTR)tmp);
-}
-//=============================================================================
void CControlDlg::OnLight()
{
- CString cmd;
- if (m_arOut==NULL) return;
CString str;
m_light.GetWindowText(str);
if (str=="Lights On") {
m_light.SetWindowText("Lights Off");
- cmd = "DRIVE {Light True}\r\n";
+ m_vehicle->Lights(true);
} else {
m_light.SetWindowText("Lights On");
- cmd = "DRIVE {Light False}\r\n";
+ m_vehicle->Lights(false);
}
- m_arOut->WriteString(cmd);
- LogData (cmd);
- m_arOut->Flush();
}
//==============================================================================
void CControlDlg::OnTrace()
{
- CString cmd;
- if (m_arOut==NULL) return;
CString str;
m_trace.GetWindowText(str);
if (str=="Trace On") {
m_trace.SetWindowText("Trace Off");
- cmd = "TRACE {On True} {Interval 0.1} {Color 2}\r\n";
+ m_vehicle->Trace(true);
} else {
m_trace.SetWindowText("Trace On");
- cmd = "TRACE {On False}\r\n";
+ m_vehicle->Trace(false);
}
- m_arOut->WriteString(cmd);
- LogData (cmd);
- m_arOut->Flush();
}
//==============================================================================
-void CControlDlg::LogData(CString str)
-{
- fprintf (pLog, "%s\n", str);
-}
-//==============================================================================
void CControlDlg::OnTvnSelchangedMsgTree(NMHDR *pNMHDR, LRESULT *pResult)
{
LPNMTREEVIEW pNMTreeView = reinterpret_cast<LPNMTREEVIEW>(pNMHDR);
// TODO: Add your control notification handler code here
*pResult = 0;
}
+void CControlDlg::ProcessMsgData()
+{
+ m_vehicle->ProcessMsgData();
+ UpdateData(false);
+}
+void CControlDlg::OnBnClickedButton1()
+{
+ m_vehicle->Planner(1);
+}
+void CControlDlg::OnBnClickedButton2()
+{
+ m_vehicle->Planner(2);
+}
+void CControlDlg::OnBnClickedButton3()
+{
+ m_vehicle->Planner(3);
+}
+
+void CControlDlg::OnBnClickedButton4()
+{
+ m_vehicle->Planner(4);
+}
+
+void CControlDlg::OnBnClickedButton5()
+{
+ m_vehicle->Planner(5);
+}
Modified: trunk/usarsim/Tools/SimpleUI/ControlDlg.h
===================================================================
--- trunk/usarsim/Tools/SimpleUI/ControlDlg.h 2007-12-04 20:37:42 UTC (rev 879)
+++ trunk/usarsim/Tools/SimpleUI/ControlDlg.h 2007-12-04 20:52:26 UTC (rev 880)
@@ -2,10 +2,9 @@
#define AFX_CONTROLDLG_H__917D5561_6084_45E1_A576_2B178677B882__INCLUDED_
#include "VideoSocket.h" // Added by ClassView
-#include "CmdSocket.h" // Added by ClassView
-#include "MessageParser.h"
#include "InfoDlg.h"
#include "FreeImage/FIIO_Mem.h"
+#include "Robot.h"
#if _MSC_VER > 1000
#pragma once
@@ -13,35 +12,8 @@
// ControlDlg.h : header file
//
-typedef struct MessageTree_t {
- HTREEITEM mt_time;
- HTREEITEM mt_state;
- HTREEITEM mt_chassis;
- HTREEITEM mt_ch_rot;
- HTREEITEM mt_ch_loc;
- HTREEITEM mt_ch_vel;
- HTREEITEM mt_camera;
- HTREEITEM mt_pan_loc;
-// HTREEITEM mt_pan_rot;
- HTREEITEM mt_tilt_loc;
-// HTREEITEM mt_tilt_rot;
- HTREEITEM mt_light;
- HTREEITEM mt_battery;
- HTREEITEM mt_odometry;
- HTREEITEM mt_od_data;
- HTREEITEM mt_gps;
- HTREEITEM mt_gps_data;
- HTREEITEM mt_inu;
- HTREEITEM mt_inu_data;
- HTREEITEM mt_other;
- HTREEITEM mt_sonar;
- UINT mt_so_count;
- HTREEITEM * mt_sonars;
- HTREEITEM mt_laser;
- UINT mt_ls_count;
- HTREEITEM * mt_ls_data;
-} MessageTree;
+
typedef struct FrameData_t {
BYTE state;
BYTE sequence;
@@ -51,84 +23,22 @@
BYTE data[640*480*3+1];
} FrameData;
-#define DRIVE_SKID 0
-#define DRIVE_ACKERMAN 1
-#define DRIVE_SUBMARINE 2
-#define DRIVE_JOINT 3
-#define RANGE_NONE 0
-#define RANGE_SCAN 1
-#define RANGE_3D 2
-#define RANGE_SONAR 3
-#define RANGE_IR 4
-#define RANGE_SCALE 5
-typedef struct Capability_t {
- BYTE drive;
- bool sonar; // or IR
- BYTE range;
- bool odometry;
- bool inu;
- bool encoder;
- bool touch;
- bool rfid;
- bool sound;
- bool motion;
- bool camera;
- bool gps;
- bool gripper;
- float startHeight;
- int index_sonar;
- int start_sonar;
- int end_sonar;
- int index_range;
- int start_range;
- int end_range;
- int start_scale;
- int end_scale;
-} Capability;
#define FRAME_PENDING 0
#define FRAME_OK 1
#define FRAME_ERROR 2
-// LIDAR ROW, COL is number of range entities, not number of pixels.
-#define LIDAR_ROW 36
-#define LIDAR_COL 342
-#define WHITE RGB(255,255,255)
-#define OFF_WHITE RGB(255,255,127)
-#define YELLOW RGB(255,255, 0)
-#define LT_TAN RGB(205,217,110)
-#define TAN RGB(210,180, 70)
-#define OLIVE RGB(155,187, 3)
-#define LT_GREEN RGB(150,255,110)
-#define MED_GREEN RGB(100,255, 75)
-#define GREEN RGB( 0,255, 0)
-#define AQUA RGB(120,200,200)
-#define LT_BLUE RGB(180,180,240)
-#define BLUE RGB(100,100,255)
-#define ROYAL_BLUE RGB( 0, 6,246)
-#define PURPLE RGB(107, 7,159)
-#define BLACK 0
-#define GRAY RGB(212,212,212)
-#define RED RGB(255, 0, 0)
-#define PINK RGB(255,200,200)
-#define QUANTIZATION 13
-
typedef FrameData * (WINAPI *pfFrameData)(void);
#ifdef VERBOSE
static FILE* log=NULL;
#endif
-enum {
- DR_STOP,
- DR_FORWARD,
- DR_BACKWARD,
- DR_LEFT,
- DR_RIGHT
-};
+
class CSimpleUIDlg;
class CCmdSocket;
class CVideoSocket;
class CMessageParser;
+class CRobot;
/////////////////////////////////////////////////////////////////////////////
// CControlDlg dialog
@@ -139,7 +49,6 @@
public:
BOOL m_show;
void UpdateVideo();
-
CControlDlg(CWnd* pParent = NULL); // standard constructor
void ProcessMsgData();
void ProcessVideoData();
@@ -148,7 +57,6 @@
enum { IDD = IDD_CONTROL_DLG };
CButton m_trace;
CButton m_light;
- CTreeCtrl m_cMsgTree;
CButton m_showUT;
UINT m_width;
UINT m_height;
@@ -170,13 +78,8 @@
clock_t m_lastClock;
int m_frameCount;
RECT m_bmpRec;
- RECT m_LidarRec;
BYTE m_videoState;
- CSocketFile * m_cmdFile;
- CArchive * m_arIn;
- CArchive * m_arOut;
CVideoSocket * m_videoSocket;
- CCmdSocket * m_cmdSocket;
CSimpleUIDlg * m_parent;
FrameData * m_pFrameData;
CWnd* m_utclient;
@@ -191,32 +94,15 @@
float m_turn;
float m_pan;
float m_tilt;
- FILE *pLog;
- MessageTree m_dMsgTree;
- Capability m_cap;
- // m_depth[row][0] is number of items in row
- // m_depth has first row for sonar or IR
- // next for range sensors
- // Remainer for 3D range sensor.
- float m_depth[LIDAR_ROW][LIDAR_COL+1];
+ MessageTree m_dMsgTree;
+ CTreeCtrl m_cMsgTree;
+ CCmdSocket * m_cmdSocket;
+// Capability m_cap;
+ CRobot *m_vehicle;
+ CSensed *m_pTree;
void SetRobot(CString name);
void CorrectPosition(CString str);
- void SetMsgTree();
- void UpdateMsgTree(CString str);
- void UpdateStatus(CString str);
- void UpdateCamera(CString str);
- void UpdateSonar(CString str);
- void UpdateRangeScan(CString str);
- void UpdateLaser(CString str, int range);
- void InitializeGraphics(int pen_width);
- void CleanUpGraphics();
- void UpdateOdometry(CString str);
- void UpdateGPS(CString str);
- void UpdateINU(CString str);
- void LogData(CString str);
- void Camera();
- void Drive();
void ShowUTC(BOOL show);
BOOL LoadUT();
void GetpfFrameData();
@@ -252,11 +138,18 @@
int m_imgWidth;
int m_nBytes;
LPBYTE m_pData;
- HBRUSH m_hGrayBrush;
- bool m_pens;
- HPEN m_hPen[QUANTIZATION];
public:
afx_msg void OnTvnSelchangedMsgTree(NMHDR *pNMHDR, LRESULT *pResult);
+public:
+ afx_msg void OnBnClickedButton1();
+public:
+ afx_msg void OnBnClickedButton2();
+public:
+ afx_msg void OnBnClickedButton3();
+public:
+ afx_msg void OnBnClickedButton4();
+public:
+ afx_msg void OnBnClickedButton5();
};
//{{AFX_INSERT_LOCATION}}
Added: trunk/usarsim/Tools/SimpleUI/Robot.cpp
===================================================================
--- trunk/usarsim/Tools/SimpleUI/Robot.cpp (rev 0)
+++ trunk/usarsim/Tools/SimpleUI/Robot.cpp 2007-12-04 20:52:26 UTC (rev 880)
@@ -0,0 +1,238 @@
+/* Robot.cpp: Main code to control the robot.
+ This is the BotController below
+
+ Design
+
+(simulation)
+BotViewer <----> BotController <---(Gamebot)---> WorldServer
+
+(or real robot)
+Observer ::: BotController <---(Gamebot)---> Robot ::: World
+
+At present, SimpleUI contains both the BotViewer and BotController.
+The intent is to split them.
+The BotController includes
+ Robot.cpp, Sense.cpp, Robot.h, MessageParser.cpp, MessageParser.h
+ as well as RNDF/MDF reader and future files for navigation and planning.
+The BotController should be independent of the operating system and designed
+to run on a microcontroller.
+All other SimpleUI files go to the BotViewer.
+*/
+
+#include "stdafx.h"
+#include "Robot.h"
+
+//=============================================================================
+CRobot::CRobot()
+{
+ Initialize();
+}
+//=============================================================================
+CRobot::CRobot(CSensed *pTree)
+{
+ Initialize();
+ m_pTree = pTree;
+ m_pTree->SetRows(&m_sonar_row, &m_range_row);
+
+}
+//=============================================================================
+CRobot::~CRobot()
+{
+ Close();
+}
+//=============================================================================
+void CRobot::Close()
+{
+ fclose(pLog);
+ if (m_arIn) {
+ delete m_arIn;
+ m_arIn=NULL;
+ }
+ if (m_arOut) {
+ delete m_arOut;
+ m_arOut=NULL;
+ }
+ if (m_cmdFile) {
+ delete m_cmdFile;
+ m_cmdFile=NULL;
+ }
+}
+//=============================================================================
+void CRobot::Initialize()
+{
+ EstimatedNorth = (float) 47.17;
+ EstimatedEast = (float) -55.7;
+ SpeedNorth = 0;
+ SpeedEast = 0;
+ AccelNorth = 0;
+ AccelEast = 0;
+ TopSpeed = 5;
+ m_arOut = NULL;
+ m_cmdFile = NULL;
+ m_arIn = NULL;
+ m_pTree = NULL;
+ for (int i = 0; i<LIDAR_ROW; i++)
+ m_depth[i][0] = 0;
+ m_sonar_row = 0;
+ m_range_row = 1;
+ FILE * oldLog = pLog;
+ int error = fopen_s (&pLog, "usar_client.log", "w");
+ if (error > 0)
+ pLog = oldLog;
+}
+
+//=============================================================================
+int CRobot::OpenFiles(CString model, CString position,
+ CCmdSocket *cmdSocket)
+{
+
+ m_cmdFile = new CSocketFile(cmdSocket);
+ m_arIn = new CArchive(m_cmdFile, CArchive::load);
+ m_arOut = new CArchive(m_cmdFile, CArchive::store);
+ if(!m_arOut)
+ return 0x10;
+ CString cmd;
+ cmd.Format("INIT {ClassName USARBot.%s} {Location %s}\r\n",
+ model, position);
+ LogData(cmd);
+ try {
+ m_arOut->WriteString(cmd);
+ m_arOut->Flush();
+ }catch (CException* e) {
+ e->Delete();
+ return 0x20;
+ }
+ m_pTree->DrawGraphics( RANGE_SCALE, &m_depth[0][0]);
+
+ return 0;
+}
+//==============================================================================
+void CRobot::LogData(CString str)
+{
+ fprintf (pLog, "%s\n", str);
+}
+//=============================================================================
+void CRobot::Lights(bool on)
+{
+ CString cmd;
+ if (m_arOut==NULL) return;
+ if (on)
+ {
+ cmd = "DRIVE {Light True}\r\n";
+ }
+ else
+ {
+ cmd = "DRIVE {Light False}\r\n";
+ }
+ m_arOut->WriteString(cmd);
+ LogData (cmd);
+ m_arOut->Flush();
+}
+//=============================================================================
+void CRobot::Trace(bool on)
+{
+ CString cmd;
+ if (m_arOut==NULL) return;
+ if (on)
+ {
+ cmd = "TRACE {On True} {Interval 0.1} {Color 2}\r\n";
+ }
+ else
+ {
+ cmd = "TRACE {On False}\r\n";
+ }
+ m_arOut->WriteString(cmd);
+ LogData (cmd);
+ m_arOut->Flush();
+}
+//=============================================================================
+void CRobot::Drive(BYTE steer, int mode, float speed, float turn)
+{
+ CString cmd;
+ if (m_arOut==NULL) return;
+ switch (steer){
+ case DRIVE_ACKERMAN:
+ switch (mode){
+ case DR_FORWARD:
+ cmd.Format("DRIVE {Speed %f} {FrontSteer %f}\r\n",speed, turn);
+ break;
+ case DR_BACKWARD:
+ cmd.Format("DRIVE {Speed -%f} {FrontSteer -%f}\r\n",speed, turn);
+ break;
+ case DR_LEFT:
+ cmd.Format("DRIVE {Speed %f} {FrontSteer %f}\r\n",speed, turn);
+ break;
+ case DR_RIGHT:
+ cmd.Format("DRIVE {Speed %f} {FrontSteer -%f}\r\n",speed,turn);
+ break;
+ case DR_STOP:
+ cmd = "DRIVE {Speed 0} \r\n";
+ break;
+ default:
+ cmd = "";
+ }
+ break;
+
+ default:
+ case DRIVE_SKID:
+ switch (mode){
+ case DR_FORWARD:
+ cmd.Format("DRIVE {Left %f} {Right %f}\r\n",speed,speed);
+ break;
+ case DR_BACKWARD:
+ cmd.Format("DRIVE {Left -%f} {Right -%f}\r\n",speed,speed);
+ break;
+ case DR_LEFT:
+ cmd.Format("DRIVE {Left -%f} {Right %f}\r\n",speed,speed);
+ break;
+ case DR_RIGHT:
+ cmd.Format("DRIVE {Left %f} {Right -%f}\r\n",speed,speed);
+ break;
+ case DR_STOP:
+ cmd = "DRIVE {Left 0} {Right 0}\r\n";
+ break;
+ default:
+ cmd = "";
+ }
+ break;
+ }
+ m_arOut->WriteString(cmd);
+ LogData(cmd);
+ m_arOut->Flush();
+}
+//=============================================================================
+void CRobot::Camera(float pan, float tilt)
+{
+ CString cmd;
+
+ if (m_arOut==NULL) return;
+
+// MISPKG {Name CameraPanTilt} {Link 1}{Value 3.1415} {Link 2][Value -0.1}
+ cmd.Format("MISPKG {Name CameraPanTilt} {Link 1} {Value %f} {Link 2} {Value %f}\r\n",
+ pan, tilt);
+ m_arOut->WriteString(cmd);
+ LogData(cmd);
+ m_arOut->Flush();
+}
+//=============================================================================
+void CRobot::Planner(int behavior)
+{
+ switch (behavior) {
+ case 1:
+ default:
+ /* drive forward and stop */
+ break;
+ case 2:
+ /* drive on a curve */
+ break;
+ case 3:
+ /* drive following the RNDF from where we are */
+ break;
+ case 4:
+ /* calibrate sensors */
+ break;
+ case 5:
+ /* drive to a GPS waypoint */
+ break;
+ }
+}
Property changes on: trunk/usarsim/Tools/SimpleUI/Robot.cpp
___________________________________________________________________
Name: svn:executable
+ *
Added: trunk/usarsim/Tools/SimpleUI/Robot.h
===================================================================
--- trunk/usarsim/Tools/SimpleUI/Robot.h (rev 0)
+++ trunk/usarsim/Tools/SimpleUI/Robot.h 2007-12-04 20:52:26 UTC (rev 880)
@@ -0,0 +1,76 @@
+#if !defined(ROBOT_H)
+#define ROBOT_H
+
+#if _MSC_VER > 1000
+#pragma once
+#endif // _MSC_VER > 1000
+// Robot.h : header file
+//
+
+#include "CmdSocket.h" // Added by ClassView
+#include "MessageParser.h"
+#include "Tree.h"
+class CRobot;
+class CSensed;
+
+/////////////////////////////////////////////////////////////////////////////
+// CRobot is the robot controller
+
+class CRobot
+{
+private:
+
+ float Path[4]; // present Hermite curve segment (m) parameterized by 0<= t <= 1
+ // [0] start point, [1] end point, [2] start tangent, [3] end tangent
+ float NextPath[4]; // NextPath[0] = Path[1]; NextPath[2] = Path[3]
+ float TopSpeed; // How fast we want to go on Path[] (m/s)
+ float EstimatedNorth; // where we think we are (m)
+ float EstimatedEast;
+ float SpeedNorth; // best guess of present velocity (m/s)
+ float SpeedEast;
+ float AccelNorth; // Estimated present acceleration
+ float AccelEast;
+ CSocketFile * m_cmdFile;
+ CArchive * m_arIn;
+ CArchive * m_arOut;
+ FILE * pLog;
+ CSensed *m_pTree;
+ // m_depth[row][0] is number of items in row
+ // m_depth has first row for sonar or IR
+ // next for range sensors
+ // Remainer for 3D range sensor.
+ float m_depth[LIDAR_ROW][LIDAR_COL+1];
+ int m_sonar_row; // the row of m_depth to start storing sonar data
+ int m_range_row; // start of range data in m_depth
+
+ void Initialize();
+
+public:
+ CRobot();
+ CRobot(CSensed *pTree);
+ void Drive(BYTE steer, int mode, float speed, float turn=0);
+ int OpenFiles(CString model, CString position,
+ CCmdSocket *cmdSocket);
+ void LogData(CString str);
+ void Lights(bool on);
+ void Trace(bool on);
+ void Camera(float pan, float tilt);
+ ~CRobot();
+ void Close();
+ void ProcessMsgData();
+ void UpdateMsgTree(CString str);
+ void UpdateStatus(CString str);
+ void UpdateCamera(CString str);
+ void UpdateSonar(CString str);
+ void UpdateRangeScan(CString str);
+ void UpdateGraphics(CString str, int range);
+ void UpdateOdometry(CString str);
+ void UpdateGPS(CString str);
+ void UpdateGroundTruth(CString str);
+ void UpdateINU(CString str);
+// void Pilot();
+ void Planner(int behavior);
+// void Navigator();
+// void Detector();
+};
+#endif // ROBOT_H
\ No newline at end of file
Property changes on: trunk/usarsim/Tools/SimpleUI/Robot.h
___________________________________________________________________
Name: svn:executable
+ *
Added: trunk/usarsim/Tools/SimpleUI/Sense.cpp
===================================================================
--- trunk/usarsim/Tools/SimpleUI/Sense.cpp (rev 0)
+++ trunk/usarsim/Tools/SimpleUI/Sense.cpp 2007-12-04 20:52:26 UTC (rev 880)
@@ -0,0 +1,318 @@
+// Sense.cpp: Process Gamebots SEN messages
+
+#include "stdafx.h"
+#include "SimpleUI.h"
+#include "Robot.h"
+#include "ControlDlg.h"
+
+void CRobot::ProcessMsgData()
+{
+ CString str;
+ int process=1;
+
+ do {
+ m_arIn->ReadString(str);
+ UpdateMsgTree(str);
+ LogData(str);
+ Sleep(1);
+ }while(!m_arIn->IsBufferEmpty());
+}
+//=============================================================================
+void CRobot::UpdateMsgTree(CString str)
+{
+ if (str.Find("STA",0)==0) {
+
+ UpdateStatus( str );
+
+ }
+ else if (str.Find("MISSTA", 0)==0) {
+ if (str.Find("{Name CameraPanTilt}",0)>=0) {
+ UpdateCamera( str );
+ }}
+ else if (str.Find("SEN",0)==0)
+ {
+ if (str.Find("{Type LIDAR}",0)>=0) {
+ UpdateRangeScan( str );
+ UpdateGraphics( str, RANGE_3D );
+ }
+ else if(str.Find("{Type RangeScanner}",0)>=0) {
+ UpdateRangeScan( str );
+ UpdateGraphics( str, RANGE_SCAN );
+ }
+ else if (str.Find("{Type Sonar}",0)>=0) {
+ UpdateSonar( str );
+ UpdateGraphics( str, RANGE_SONAR );
+ }
+ else if (str.Find("{Type IR}",0)>=0) {
+ UpdateSonar( str );
+ UpdateGraphics( str, RANGE_IR );
+ }
+ else if (str.Find("{Type Odometry}",0)>=0) {
+ UpdateOdometry( str );
+ }
+ else if (str.Find("{Type INS}",0)>=0) {
+ UpdateINU( str );
+ }
+ else if (str.Find("{Type GPS}",0)>=0) {
+ UpdateGPS( str );
+ }
+ else if (str.Find("{Type GroundTruth}",0)>=0) {
+ UpdateGroundTruth( str );
+ }
+ else
+ m_pTree->PutOther( str );
+ }
+ else {
+ CString tmp = "Other: " + str;
+ m_pTree->PutOther( tmp );
+ }
+
+}
+//=============================================================================
+void CRobot::UpdateStatus(CString str)
+{
+/* STA {Type GroundVehicle} {Time 1515.54} {FrontSteer 0.0000} {RearSteer 0.0000}
+ {LightToggle False} {LightIntensity 0} {Battery 1200} {View -1} */
+ CString time, light, fuel;
+ CString rot, loc, vel;
+ int count=3;
+
+ time = "Time:"+CMessageParser::GetString("Time",str);
+
+ light.Format("Light: On=%s Intensity=%d",
+ (LPCSTR)CMessageParser::GetString("LightToggle",str),
+ CMessageParser::GetInt("LightIntensity",str));
+
+ fuel.Format("Fuel(%%):%d",(int)CMessageParser::GetFloat("Battery",str));
+
+ m_pTree->PutStatus(time, light, fuel);
+
+}
+
+//=============================================================================
+void CRobot::UpdateCamera(CString str)
+{
+ /* MISSTA {Time 1515.76} {Name CameraPanTilt} {Link 1} {Value 0.0000} {Torque -20.00}
+ {Link 2} {Value 0.0000} {Torque -20.00}
+ Had been:
+ MISSTA {Time 4341.34} {Name CameraPanTilt} {Link1 0.00,-20.00} {Link2 0.00,-20.00}
+ */
+ CString panTorque, tiltTorque, panTilt;
+ CString pan, tilt;
+ int count=1;
+ int start=0;
+ int end =0;
+ float float2[2];
+
+ start = str.Find("Link");
+ if (start < 0)
+ return;
+ int length = str.GetLength() - start - 4;
+ CString rest = str.Right(length);
+ CMessageParser::GetFloats("Value",rest,',',&count,float2);
+ CMessageParser::GetFloats("Torque",rest,',',&count,float2+1);
+ panTorque.Format("Pan =%6.2f (Rad); torque=%6.2f",float2[0],float2[1]);
+ pan.Format(" Pan =%6.2f", float2[0]);
+
+ start = rest.Find("Link");
+ if (start < 0)
+ return;
+ length = rest.GetLength() - start - 4;
+ str = rest.Right(length);
+ CMessageParser::GetFloats("Value",str,',',&count,float2);
+ CMessageParser::GetFloats("Torque",str,',',&count,float2+1);
+ tiltTorque.Format(" Tilt =%6.2f (Rad); torque=%6.2f",float2[0],float2[1]);
+ tilt.Format("Tilt =%6.2f", float2[0]);
+
+ panTilt = "Camera: " + pan + ", " + tilt;
+ m_pTree->PutCamera(panTilt, panTorque, tiltTorque);
+}
+
+//=============================================================================
+void CRobot::UpdateSonar(CString str)
+{
+ CString tmp[32], sonar="Sonar:";
+ const char *pChar;
+ int pos1=0, pos2=0, count=0;
+ while (pos1>=0 && count<32) {
+ tmp[count++] = CMessageParser::GetString("Name",str, pos1, pos2);
+ pos1 = pos2;
+ }
+ if (pos1<0) count--;
+ m_depth[m_sonar_row][0] = (float) count;
+ m_pTree->PutSonarCount(count);
+
+ for (int i=0;i<count;i++) {
+ pos1 = tmp[i].Find("Range");
+ pos1 += 5;
+ pChar = (LPCTSTR) tmp[i];
+ pChar += pos1;
+ m_depth[m_sonar_row][i+1] = (float)atof(pChar);
+ tmp[i].Replace("Range ",":");
+ sonar += tmp[i] + " ";
+ m_pTree->PutSonarLine(tmp[i], i);
+ }
+ m_pTree->PutSonarLine(sonar, -1);
+
+}
+//=============================================================================
+void CRobot::UpdateRangeScan(CString str)
+{
+ /*
+ SEN {Time 511.94} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174}
+ {FOV 3.1415} {Range 20.0000,19.9903,20.0000,20.0000,20.0000,20.0000,
+ 19.9868,19.9946,20.0000,20.0000,19.9969,20.0000,20.0000,20.0000,20.0000,
+ 20.0000,19.9875,20.0000,19.9954,20.0000,19.9973,19.9973,20.0000,19.9809,
+ 19.9935,19.9819,20.0000,20.0000,19.9899,19.9973,19.9823,19.9940,19.9938,
+ 19.9856,20.0000,20.0000,20.0000,19.9878,20.0000,20.0000,19.9891,19.9952,
+ 19.9905,19.9976,19.9959,20.0000,19.9930,19.9943,20.0000,20.0000,20.0000,
+ 19.9811,19.9906,20.0000,20.0000,19.9937,19.9911,20.0000,19.9831,19.9973,
+ 19.9861,19.9826,19.9924,20.0000,20.0000,19.9959,20.0000,19.9993,19.9826,
+ 19.9816,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,19.9990,20.0000,
+ 20.0000,19.9828,19.9903,20.0000,20.0000,20.0000,20.0000,20.0000,19.9831,
+ 20.0000,20.0000,20.0000,20.0000,19.9989,19.9833,19.9958,19.9865,20.0000,
+ 19.9867,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,20.0000,
+ 19.9831,19.9839,19.9975,19.9827,19.9872,19.9979,20.0000,19.9877,20.0000,
+ 20.0000,19.9915,19.9887,19.9903,20.0000,20.0000,19.9950,20.0000,19.9846,
+ 19.9802,20.0000,19.9882,20.0000,20.0000,19.9873,18.9922,15.4983,13.1021,
+ 11.3225,10.0221,8.9622,8.1160,7.4080,6.8297,6.3202,5.8914,5.5086,5.1866,
+ 4.9034,4.6398,4.4134,4.2047,4.0152,3.8472,3.6852,3.5415,3.4175,3.2956,
+ 3.1792,3.0765,2.9832,2.8936,2.8057,2.7298,2.6573,2.5860,2.5266,2.4643,
+ 2.4061,2.3549,2.3038,2.2541,2.2111,2.1658,2.1286,2.0886,2.0511,2.0191,
+ 1.9859,1.9557,1.9218,1.8937,1.8702,1.8425,1.8201,1.7939,1.7732}
+ */
+
+ CString dataStr = "Ranges(";
+ dataStr += CMessageParser::GetString("Name",str) +"):" +
+ CMessageParser::GetString("Range",str);
+ m_pTree->PutLidarLine(dataStr, -1);
+
+ int count = 256;
+ CMessageParser::GetFloats("Range",str,',',&count,&m_depth[m_range_row][1]);
+ m_depth[m_range_row][0] = (float) count;
+ int lines = count/8;
+ int lastLn = count%8;
+ if (lastLn) lines+=1;
+ m_pTree->PutLidarCount(lines);
+
+ lines-=1;
+ for (int i=0;i<lines;i++)
+ {
+ dataStr.Format("Data: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f",
+ m_depth[m_range_row][i*8+1], m_depth[m_range_row][i*8+2],
+ m_depth[m_range_row][i*8+3], m_depth[m_range_row][i*8+4],
+ m_depth[m_range_row][i*8+5], m_depth[m_range_row][i*8+6],
+ m_depth[m_range_row][i*8+7], m_depth[m_range_row][i*8+8]);
+ m_pTree->PutLidarLine(dataStr, i);
+ }
+ if (lines>=0) {
+ if (lastLn==0) lastLn = 8;
+ CString data;
+ dataStr = "Data: ";
+ for (int i=0;i<lastLn;i++) {
+ data.Format("%6.2f ",m_depth[m_range_row][lines*8+i+1]);
+ dataStr += data;
+ }
+ m_pTree->PutLidarLine(dataStr, lines);
+ }
+
+}
+//=============================================================================
+void CRobot::UpdateGraphics(CString str, int range)
+// A graphical representation of a ranging instrument
+{
+ CString name;
+ unsigned int r; // temporary indices
+ unsigned int row = 0, col = 0; // number of items in lidar scan
+
+ // Grab the data
+ if( range == RANGE_3D)
+ {
+ for (r = 0; r < LIDAR_ROW; r++)
+ {
+ name.Format("Range%-i", r);
+ int count = LIDAR_COL;
+
+ // Capture the Lidar readings
+ CMessageParser::GetFloats(name,str,',',&count,&m_depth[r][1]);
+ m_depth[r][0] = (float) count;
+ if (count > 1)
+ {
+ col = (count-col)? count: col;
+ row++;
+ }
+ }
+ col--;
+ }
+ m_pTree->DrawGraphics( range, &m_depth[0][0]);
+}
+//=============================================================================
+void CRobot::UpdateOdometry(CString str)
+{
+ CString name ="Odometry:";
+ CString data;
+ int num=2;
+ int pos1=0, pos2=0;
+ float float3[3];
+
+ name += CMessageParser::GetString("Name",str, pos1, pos2);
+ CMessageParser::GetFloats("Pose",str,',',&num,float3);
+ data.Format("Position(m): X=%6.2f Y=%6.2f ",float3[0],float3[1]);
+ m_pTree->PutOdometry(name, data);
+}
+//=============================================================================
+void CRobot::UpdateGPS(CString str)
+{
+ CString name ="GPS:";
+ CString data;
+ int num=3;
+ int pos1=0, pos2=0;
+ int lat[3], lon[3];
+
+ name += CMessageParser::GetString("Name",str, pos1, pos2);
+
+ CMessageParser::GetInts("Latitude",str,',',&num,lat);
+ CMessageParser::GetInts("Longitude",str,',',&num,lon);
+ // assume North America
+ lon[0] = -lon[0];
+ data.Format("Lat=%iN%02i.%04i, Long=%iW%02i.%04i",
+ lat[0], lat[1], lat[2], lon[0], lon[1], lon[2]);
+ m_pTree->PutGPS(name, data);
+}
+//=============================================================================
+void CRobot::UpdateGroundTruth(CString str)
+{
+// SEN {Time 1515.7579} {Type GroundTruth} {Name GroundTruth}
@@ Diff output truncated at 60000 characters. @@
More information about the TeamTalk-developers
mailing list