void Communication::publish_objectToGrasp_moveitFormat(){ ros::Rate r(1); ros::NodeHandle node; static tf::TransformBroadcaster br; Eigen::Matrix4f camToWorldMatrix; camToWorldMatrix.setZero();camToWorldMatrix(0,2) = 1; camToWorldMatrix(1,0) = -1; camToWorldMatrix(2,1) = -1; camToWorldMatrix(3,3) = 1; while(node.ok()){ pcl::PointCloud<PointT>::Ptr object_pc = m_object_ex_ptr->getObjectToGrasp(); pcl::PointCloud<PointT>::Ptr object_pc_transformed(new pcl::PointCloud<PointT>); // Eigen::Vector4f c = m_object_ex_ptr->getGraspCentroid(); c(3)=1; // tf::Transform simpleTF; tf::StampedTransform camToJacoTf; tf::TransformListener listener; bool tf_ready = listener.waitForTransform("camera_rgb_frame","root",ros::Time(0),ros::Duration(5.0)); if(object_pc->size() > 0 && tf_ready){ listener.lookupTransform("camera_rgb_frame","root",ros::Time(0),camToJacoTf); Eigen::Matrix4f camToJacoMatrix; pcl_ros::transformAsMatrix(camToJacoTf,camToJacoMatrix); Eigen::Matrix4f combinedMatrix = camToJacoMatrix.inverse() * camToWorldMatrix; // Eigen::Vector4f result = combinedMatrix * c; // Eigen::Matrix4f res; res(0,3) = result(0); res(1,3) = result(1); res(2,3) = result(2); res(3,3) = 1; // simpleTF = tfFromEigen(res); // br.sendTransform(tf::StampedTransform(simpleTF, ros::Time::now(), "root", "object_centroid_test")); pcl::transformPointCloud(*object_pc, *object_pc_transformed, combinedMatrix); // std::cout << "old : " << object_pc->at(100) << std::endl; // std::cout << "new : " << object_pc_transformed->at(100) << std::endl; ObjectToGrasp_publisher_.publish(object_pc_transformed); } r.sleep(); } }
Eigen::Matrix4f ForwardKinematicsLiego::getDash(Eigen::Vector3f vec) { Eigen::Matrix4f res; res.setZero(); res(0, 0) = 0; res(0, 1) = -vec[2]; res(0, 2) = vec[1]; res(1, 0) = vec[2]; res(1, 1) = 0; res(1, 2) = -vec[0]; res(2, 0) = -vec[1]; res(2, 1) = vec[0]; res(2, 2) = 0; return res; }
void Scene::DrawScene(ParticleSystem* m, double strainSize, bool drawPoints) { int pSize; int cSize; //float* points = m->GetPositions3d(&pSize); //float* colors = m->GetColors(&cSize, strainSize); float *points, *colors; switch(drawMode) { case 0: points = m->GetSurfaceTriangles3d(&pSize); colors = m->GetTriColors(&cSize, strainSize); break; case 1: points = m->GetPositions3d(&pSize, prevMode); colors = m->GetColors(&cSize, strainSize, xpos, ypos, zpos); break; case 2: points = m->GetSurfaceTriangles3d(&pSize); colors = m->GetStrainSurfaceTriColors(&cSize, strainSize); break; case 3: points = m->GetTetCenter(&pSize); colors = m->GetCenterColors(&cSize, strainSize); break; case 4: points = m->GetAllTriangles3d(&pSize); colors = m->GetStrainAllTriColors(&cSize, strainSize); break; } Eigen::Matrix4f rotationMatrix; Eigen::Matrix4f projectionMatrix; rotationMatrix.setZero(); projectionMatrix.setZero(); Eigen::Vector3f pos, target, up; pos << xpos, ypos, zpos; m->GetCameraPosAndSize(&xtarg, &ytarg, &ztarg); target << xtarg, ytarg, ztarg; up << 0, -1, 0; RotationMatrix(pos, target, up, rotationMatrix); PerspectiveMatrix((65*PI)/180.0, ((float)DDWIDTH)/DDHEIGHT, .5, 100, projectionMatrix); g_viewMatrix = projectionMatrix * rotationMatrix; DrawDelegate::BeginFrame(); DrawDelegate::SetViewMatrix(g_viewMatrix.data()); Scene::DrawGrid(1, m->groundLevel); if (drawPoints) { DrawDelegate::SetLineSize(3); switch(drawMode) { case 0: DrawDelegate::DrawTriangles(points, pSize, colors, cSize); break; case 1: DrawDelegate::DrawLines(points, pSize, colors, cSize); break; case 2: DrawDelegate::DrawTriangles(points, pSize, colors, cSize); break; case 3: DrawDelegate::DrawPoints(points, pSize, colors, cSize); break; case 4: DrawDelegate::DrawTriangles(points, pSize, colors, cSize); break; } } }