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