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)); }