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