bool Hand_filter::jumped(const tf::Quaternion& q_current,const tf::Quaternion& q_previous,const float threashold) const {
tf::Vector3 axis = q_current.getAxis();
// std::cout<< "axis: " << axis.x() << " " << axis.y() << " " << axis.z() << std::endl;
return false;
}
void LeapMotionListener::leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
dataHand_=(*dataHand);
ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
ROS_INFO("DIRECTION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.direction.x,dataHand_.direction.y,dataHand_.direction.z);
ROS_INFO("NORMAL OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.normal.x,dataHand_.normal.y,dataHand_.normal.z);
//ROS_INFO("INSIDE CALLBACK");
//We create the values of reference for the first postion of our hand
if (FIRST_VALUE)
{
dataLastHand_.palmpos.x=dataHand_.palmpos.x;
dataLastHand_.palmpos.y=dataHand_.palmpos.y;
dataLastHand_.palmpos.z=dataHand_.palmpos.z;
FIRST_VALUE=0;
Updifferencex=dataLastHand_.palmpos.x+10;
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
//ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
//sleep(2);
}
else
{
//We get the distance between the finger and transform it into gripper distance
rot7=DtA+DtAx*dataHand_.finger_distance;
rot8=DtA+DtAx*dataHand_.finger_distance;
joint_msg_leap=jointstate_;
joint_msg_leap.position[7] = -rot8;
joint_msg_leap.position[6] = rot7;
if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
{
//q.setRPY(0,0,M_PI/2);//Fixed Position for testing..with this pose the robot is oriented to the ground
q.setRPY(dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z);
pose.orientation.x = q.getAxis().getX();//cambiado aposta getZ()
pose.orientation.y = q.getAxis().getY();
pose.orientation.z = q.getAxis().getZ();//cambiado aposta getX()
pose.orientation.w = q.getW();
//pose.orientation.w = ;
//pose.orientation.z=1;
//pose.orientation.y=0;
//pose.orientation.x=0;
//We need to send the correct axis to the robot. Currently they are rotated and x is z
//ROS_INFO("VALUES OF THE QUATERNION SET TO \n X: %f\n Y: %f\n Z: %f W: %f\n",pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
pose.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x) ;
pose.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y);
if(pose.position.z>Uplimitez)
pose.position.z=Uplimitez;
pose.position.x +=(dataHand_.palmpos.z-dataLastHand_.palmpos.z);
//Here we instantiate an autogenerated service class
srv.request.target = pose ;
if (pclient->call(srv))
{
//ROS_INFO("Ret: %d", (int)srv.response.ret);
dataLastHand_.palmpos.x=dataHand_.palmpos.x;
dataLastHand_.palmpos.y=dataHand_.palmpos.y;
dataLastHand_.palmpos.z=dataHand_.palmpos.z;
// Both limits for x,y,z to avoid small changes
Updifferencex=dataLastHand_.palmpos.x+10;//
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
old_pose=pose;
ROS_INFO("response %f\n",srv.response.ret);
}
else
{
ROS_ERROR("Position out of range");
pose=old_pose;
}
}
//We get the aswer of the service and publish it into the joint_msg_leap message
//joint_msg_leap.position[0] = srv.response.joint1;
//joint_msg_leap.position[1] = srv.response.joint2;
//joint_msg_leap.position[2] = srv.response.joint3;
//joint_msg_leap.position[3] = srv.response.joint4;
//joint_msg_leap.position[4] = srv.response.joint5;
//joint_msg_leap.position[5] = srv.response.joint6;
//robo_pub.publish(joint_msg_leap);
}
//ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.position.x,pose.position.y,pose.position.z);
//ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",q.getAxis().getX(),q.getAxis().getY(),q.getAxis().getZ());
}