void GQuiver::poseStampedCallback(const geometry_msgs::PoseStamped msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
Ogre::SceneNode * node;
// If none exists, create a blank rendered frame
if (xCones.size() == 0)
{
// Create scene node
node = graphicNode->createChildSceneNode();
sceneNodes.push_back(node);
addFrameToNode(node);
}
else
{
node = sceneNodes[0];
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
Ogre::Quaternion Qe(msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z);
Ogre::Vector3 Xe(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
// Set pose indicator position
node->setPosition(Xf + Qf * Xe);
node->setOrientation(Qf * Qe);
node->setScale(scale, scale, scale);
if (drawXAxis)
{
xCones[0]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
xCylinders[0]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
}
if (drawYAxis)
{
yCones[0]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
yCylinders[0]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
}
if (drawZAxis)
{
zCones[0]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
zCylinders[0]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
}
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
void GQuiver::poseArrayCallback(const geometry_msgs::PoseArray msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
// Cull axes to keep their number within the limit
while (sceneNodes.size() > msg.poses.size())
{
removeFirstIn();
}
while (sceneNodes.size() < msg.poses.size())
{
// Create scene node at odo position
Ogre::SceneNode * newNode = graphicNode->createChildSceneNode();
sceneNodes.push_back(newNode);
addFrameToNode(newNode);
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
for (int i = 0; i < msg.poses.size(); i++)
{
Ogre::Quaternion Qe(msg.poses[i].orientation.w,
msg.poses[i].orientation.x,
msg.poses[i].orientation.y,
msg.poses[i].orientation.z);
Ogre::Vector3 Xe(msg.poses[i].position.x,
msg.poses[i].position.y,
msg.poses[i].position.z);
Ogre::Vector3 X1 = Xf + Qf * Xe;
Ogre::Quaternion Q1 = Qf * Qe;
Ogre::SceneNode * node = sceneNodes[i];
node->setPosition(X1);
node->setOrientation(Q1);
node->setScale(scale, scale, scale);
if (drawXAxis)
{
xCones[i]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
xCylinders[i]->setColor(Ogre::ColourValue(xColour[0] / 255.f, xColour[1] / 255.f, xColour[2] / 255.f, 1));
}
if (drawYAxis)
{
yCones[i]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
yCylinders[i]->setColor(Ogre::ColourValue(yColour[0] / 255.f, yColour[1] / 255.f, yColour[2] / 255.f, 1));
}
if (drawZAxis)
{
zCones[i]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
zCylinders[i]->setColor(Ogre::ColourValue(zColour[0] / 255.f, zColour[1] / 255.f, zColour[2] / 255.f, 1));
}
}
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
void GQuiver::odometryCallback(const nav_msgs::Odometry msg)
{
sourceFrame = Frame::findFrame(msg.header.frame_id);
if (sourceFrame < 0)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
if (!availableFrames[sourceFrame].valid)
{
publishStatus(Gadget::ERROR, "No transform from fixed frame to message frame");
return;
}
// Cull axes to keep their number within the limit
while (sceneNodes.size() > countLimit)
{
removeFirstIn();
}
tf::StampedTransform & tran = availableFrames[sourceFrame].extrapTransform;
tf::Vector3 & pos = tran.getOrigin();
tf::Quaternion rot = tran.getRotation();
Ogre::Quaternion Qf(rot.w(), rot.x(), rot.y(), rot.z());
Ogre::Vector3 Xf(pos.x(), pos.y(), pos.z());
Ogre::Quaternion Qe(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
Ogre::Vector3 Xe(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Ogre::Vector3 X1 = Xf + Qf * Xe;
Ogre::Quaternion Q1 = Qf * Qe;
if (sceneNodes.size() > 0)
{
Ogre::Vector3 X2 = sceneNodes.back()->getPosition();
Ogre::Quaternion Q2 = sceneNodes.back()->getOrientation();
if ((X1 - X2).normalise() < positionTolerance)
{
return;
}
if (acos((Q2.Inverse() * Q1).w) * 2 < angleTolerance)
{
return;
}
}
// Create scene node at odo position
Ogre::SceneNode * newNode = graphicNode->createChildSceneNode();
newNode->setPosition(X1);
newNode->setOrientation(Q1);
newNode->setScale(scale, scale, scale);
sceneNodes.push_back(newNode);
addFrameToNode(newNode);
publishStatus(Gadget::OKAY, "Okay");
msgsRecieved++;
}
//рисуем куб, связанный с подвижной системой координат
void DrawBox(HWND hwnd, HDC hdc, ANGLS an)
{
sf=sin(M_PI*an.fi/180);
cf=cos(M_PI*an.fi/180);
st=sin(M_PI*an.teta/180);
ct=cos(M_PI*an.teta/180);
double xe, ye;
int x1,y1,x2,y2;
double xt1,yt1,zt1,xt2,yt2,zt2;
int j;
for(int i=0; i<4; i++)
{
j = i + 1;
if(j==4)
j = 0;
xt1 = Point[i].x; yt1 = Point[i].y; zt1 = Point[i].z;
xt2 = Point[j].x; yt2 = Point[j].y; zt2 = Point[j].z;
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xe = Xe(xt2, yt2, zt2);
ye=Ye(xt2,yt2,zt2);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
}
for(int i=4; i<8; i++)
{
j = i + 1;
if(j==8)
j = 4;
xt1 = Point[i].x; yt1 = Point[i].y; zt1 = Point[i].z;
xt2 = Point[j].x; yt2 = Point[j].y; zt2 = Point[j].z;
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xe = Xe(xt2, yt2, zt2);
ye=Ye(xt2,yt2,zt2);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
}
for(int i=0; i<4; i++)
{
xt1 = Point[i].x; yt1 = Point[i].y; zt1 = Point[i].z;
xt2 = Point[i+4].x; yt2 = Point[i+4].y; zt2 = Point[i+4].z;
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xe = Xe(xt2, yt2, zt2);
ye=Ye(xt2,yt2,zt2);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
}
for(int i=0; i<2; i++)
{
xt1 = Point[i].x; yt1 = Point[i].y; zt1 = Point[i].z;
xt2 = Point[i+2].x; yt2 = Point[i+2].y; zt2 = Point[i+2].z;
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xe = Xe(xt2, yt2, zt2);
ye=Ye(xt2,yt2,zt2);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
}
}
//рисуем картину приложения
void LinePicture(HWND hwnd, int Context)
{
//выбираем нужный контектс устройства для экрана
//---------------------------------------------
HDC hdcWin;
PAINTSTRUCT ps;
//получаем контест устройства для экрана
if(Context == 1)
hdcWin = BeginPaint(hwnd, &ps);
else
hdcWin = GetDC(hwnd);
//-----------------------------------------------
//связываем размеры поля вывода с размерами клиентской области окна
//--------------------------------------------------------------------
RECT rct;
GetClientRect(hwnd,&rct);
ne1 = rct.left+50; ne2 = rct.right -50;
me1 = rct.bottom -50; me2 = rct.top + 50;
//------------------------------------------------------------------
//создаем контекст экрана
//------------------------------------------------------------
HDC hdc = CreateCompatibleDC(hdcWin); //создаем контекст
//памяти связаный с контекстом экрана
//памяти надо придать вид экрана - подходт битовая карта с форматом
// как у экрана. В памяти будем рисовать на битовой карте
HBITMAP hBitmap, hBitmapOld;
hBitmap = CreateCompatibleBitmap(hdcWin, ne2, me1); //создаем
//битовую карту совместмую с контекстом экрана
hBitmapOld = (HBITMAP)SelectObject(hdc, hBitmap); //помещаем
// битовую карту в контекст памяти
//--------------------------------------------------------------
//выводи значения углов в верхней части поля вывода
//--------------------------------------------------------
//создание прямоугольной области для вывода углов поворота
HRGN hrgn2 = CreateRectRgn(ne1,me2-30,ne2,me1);
//заливаем выделенную область серым цветом
HBRUSH hBrush2 = CreateSolidBrush(RGB(0x80,0x80,0x80));
HBRUSH hBrushOld = (HBRUSH)SelectObject(hdc,hBrush2);
FillRgn(hdc,hrgn2,hBrush2);
SelectObject(hdc,hBrushOld);
DeleteObject(hBrush2);
DeleteObject(hrgn2);
//вычисление угловых коэффициентов поворота системы координат
sf=sin(M_PI*angl.fi/180);
cf=cos(M_PI*angl.fi/180);
st=sin(M_PI*angl.teta/180);
ct=cos(M_PI*angl.teta/180);
//информация об углах поворота системы координат
TCHAR ss[20];
SetBkColor(hdc,RGB(0xC0,0xC0,0xC0));
SetTextColor(hdc,RGB(0,0,0x80));
swprintf_s(ss,20,L"fi = %4.0lf",angl.fi);
TextOut(hdc,(ne1+ne2)/2-80,me2-25,ss,9);
swprintf_s(ss,20,L"teta = %4.0lf",angl.teta);
TextOut(hdc,(ne1+ne2)/2+20,me2-25,ss,11);
//------------------------------------------------
//выделение памяти под Z-буфер и начальное его заполнение
//-------------------------------------------------------------
//вычисляем число пикселей в поле вывода
Np = ne2-ne1 + 1, Mp = me1-me2 +1, NM = Np*Mp;
//выделяем память под Z-буфер для каждого пикселя
zb = new ZbuffS [NM];
//начальное заполнение z-буфера для каждого пикселя
for ( long unsigned p=0; p<NM; p++)
{
zb[p].z = -1000;
zb[p].c.R = 0xC0; zb[p].c.G = 0xC0; zb[p].c.B = 0xC0;
}
//-----------------------------------------------------------
//"рисуем" магнитную пластинку заданным цветом заполняя Z-буфер
//-----------------------------------------------------------------------
//мировые координаты проецируемой точки
double xt1,yt1,zt1;
//видовые координаты проецируемой точки
double xe,ye,ze1;
//пиксельные координаты проецируемой точки
int x1,y1;
//пиксельные координаты 4-х углов пластинки
int xp[4], yp[4];
//видовые z-координаты 4-х углов пластинки
double ze[4];
for(int n=0; n<4; n++)
{
xt1 = Px[n]; yt1 = Py[n]; zt1 = Pz[n];
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
ze1=Ze(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xp[n] = x1; yp[n] = y1; ze[n] = ze1;
}
//ZbufParallelogram(hdc,xp[0],yp[0],ze[0],xp[1],yp[1],ze[1],
//xp[2],yp[2],ze[2],xp[3],yp[3],ze[3],RGB(255,255,0));
//------------------------------------------------------------------
//"рисуем" линии поля заполняя Z-буфер
//----------------------------------------------------------------
for (int i = 0; i < 20; i++)
{
LineField(hdc,PointB[i],RGB(255,0,0),1);
}
for(int i=20; i<40; i++)
{
LineField(hdc,PointB[i],RGB(0,0,255), 1);
}
for (int i = 40; i<60; i++)
{
LineField(hdc, PointB[i], RGB(0, 255,0), 1);
}
//-----------------------------------------------------------------------
//выводим содержимое Z-буфера в контекст памяти
//--------------------------------------------------------------------
//двигаемся по всем пикселям окна вывода
for (unsigned long ij=0; ij<NM; ij++)
{
x1 = ne1 + ij%Np;
y1 = me2 + ij/Np;
SetPixel(hdc,x1,y1,RGB(zb[ij].c.R,zb[ij].c.G,zb[ij].c.B));
}
delete [] zb; //очищаем память под Z-буфером
//-------------------------------------------------------------
//рисуем координтные оси
//------------------------------------------------------------------------
HPEN hPen = CreatePen(PS_SOLID,1,RGB(0,255,255));
HPEN hPenOld = (HPEN)SelectObject(hdc,hPen);
int x2,y2;
//ось Ox
xe=Xe(-xmax/3,0,0);
ye=Ye(-xmax/3,0,0);
x1=xn(xe);
y1=ym(ye);
xe=Xe(xmax,0,0);
ye=Ye(xmax,0,0);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
SetBkColor(hdc,RGB(0xC0,0xC0,0xC0));
SetTextColor(hdc,RGB(120,120,120));
TextOut(hdc,x2, y2, _T("X"),1);
//Ось Oy
xe=Xe(0,-ymax/3, 0);
ye=Ye(0,-ymax/3,0);
x1=xn(xe);
y1=ym(ye);
xe=Xe(0,ymax, 0);
ye=Ye(0,ymax,0);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
SetBkColor(hdc,RGB(0xC0,0xC0,0xC0));
SetTextColor(hdc,RGB(120,120,120));
TextOut(hdc,x2, y2, _T("Y"),1);
//Ось Oz
xe=Xe(0,0, 0);
ye=Ye(0,0,-zmax/3);
x1=xn(xe);
y1=ym(ye);
xe=Xe(0,0, 0);
ye=Ye(0,0,zmax);
x2=xn(xe);
y2=ym(ye);
MoveToEx(hdc,x1,y1,NULL);
LineTo(hdc,x2,y2);
SetBkColor(hdc,RGB(0xC0,0xC0,0xC0));
SetTextColor(hdc,RGB(120,120,120));
TextOut(hdc,x2, y2, _T("Z"),1);
SelectObject(hdc,hPenOld);
DeleteObject(hPen);
//-----------------------------------------------------------------------------
//рисуем куб
//----------------------------------------------------------------------
hPen = CreatePen(PS_SOLID,1,RGB(160,160,160));
hPenOld = (HPEN)SelectObject(hdc,hPen);
DrawBox(hwnd, hdc, angl);
SelectObject(hdc,hPenOld);
DeleteObject(hPen);
//------------------------------------------------------------------------
//копируем контекст памяти в контекст экрана
//-----------------------------------------------------------------------
BitBlt(hdcWin,ne1,me2-30,ne2,me1,hdc,ne1,me2-30,SRCCOPY);
//----------------------------------------------------------------------
//завершаем работу с контекстами и памятью
//-------------------------------------------------------------------
SelectObject(hdc, hBitmapOld); //востанавливаем контекст памяти
DeleteObject(hBitmap); //убираем битовую карту
DeleteDC(hdc); // освобождаем контекст памяти
//освобождаем контекст экрана
if(Context == 1)
EndPaint(hwnd, &ps);
else
ReleaseDC(hwnd, hdcWin);
}
//"рисует" одну линию поля из начальной точки PointB
//с помощью функций работающих с Z-буфером
void LineField(HDC hdc,POINT3 PointB,COLORREF rgb, double force)
{
VECTORS vect;
vect.x = PointB.x;
vect.y = PointB.y;
vect.z = PointB.z;
//видовые координаты проецируемой точки
double xe, ye, ze1, ze2;
//координаты пикселов
int x1,y1,x2,y2;
double dt = force > 0 ? 0.1 : -0.1; //длина шага на линии поля
double x, y, z, Hx, Hy, Hz, Ha;
int k = 0;
VECMAG mag;
int step = 0;
double xt1,yt1,zt1,xt2,yt2,zt2;
do
{
step++;
x = vect.x;
y = vect.y;
z = vect.z;
mag = magn(x,y,z);
Hx = mag.hx;
Hy = mag.hy;
Hz = mag.hz;
Ha = sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
vect.dx = Hx/Ha;
vect.dy = Hy/Ha;
vect.dz = Hz/Ha;
xt1 = vect.x; yt1 = vect.y; zt1 = vect.z;
xt2 = xt1 + vect.dx*dt;
yt2 = yt1 + vect.dy*dt;
zt2 = zt1 + vect.dz*dt;
xe = Xe(xt1, yt1, zt1);
ye=Ye(xt1,yt1,zt1);
ze1=Ze(xt1,yt1,zt1);
x1=xn(xe);
y1=ym(ye);
xe = Xe(xt2, yt2, zt1);
ye=Ye(xt2,yt2,zt2);
ze2=Ze(xt2,yt2,zt2);
x2=xn(xe);
y2=ym(ye);
//"рисуем" отрезок линии поля
ZbufLineWidth(hdc,x1,y1,x2,y2,ze1,ze2,3,rgb);
vect.x = xt2;
vect.y = yt2;
vect.z = zt2;
//после 10-и шагов на лини поля "рисуем" стрелку
k++;
if(k == 10)
{
arrowVector(hdc,x1,y1,x2,y2,ze2,RGB(0,0,255));
k = 0;
}
//прекращаем рисовать линию поля на границе куба
} while (step < 1000 && (x>-xmax) && (x<xmax) && (y>-ymax) && (y<ymax) && (z>-zmax) && (z<zmax));
}
