void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv)
{
double att_unit_coef= 0.0139882;
double phi, theta, yaw;
sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw);
phi*=att_unit_coef*DEG2RAD;
theta*=-att_unit_coef*DEG2RAD;
yaw*=-att_unit_coef*DEG2RAD;
q.setRPY(phi,theta,yaw);
//ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw);
//ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w());
imu_data.header.stamp = ros::Time::now();
imu_data.orientation.x=q.x();
imu_data.orientation.y=q.y();
imu_data.orientation.z=q.z();
imu_data.orientation.w=q.w();
imu_message.publish(imu_data);
//Only temporary until the rates are equal
att_data.header.stamp = ros::Time::now();
att_data.orientation.x=q.x();
att_data.orientation.y=q.y();
att_data.orientation.z=q.z();
att_data.orientation.w=q.w();
att_message.publish(att_data);
}
/**
* Packs current state in a odom message. Needs a quaternion for conversion.
*/
void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom)
{
q.setRPY(0, 0, _theta);
odom.header.stamp = ros::Time::now();
odom.pose.pose.position.x = _x;
odom.pose.pose.position.y = _y;
odom.pose.pose.orientation.x = q.x();
odom.pose.pose.orientation.y = q.y();
odom.pose.pose.orientation.z = q.z();
odom.pose.pose.orientation.w = q.w();
}
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
double r, p ,y;
lastQuat = tf::Quaternion(msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
btMatrix3x3(lastQuat).getRPY(r, p, y);
lastQuat.setRPY(r, p, y + M_PI / 2.0);
lastOdom = *msg;
}
void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
{
tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
tfScalar yaw, pitch, roll;
tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
tmp_.setRPY(roll, pitch, 0.0);
transform_.setRotation(tmp_);
transform_.stamp_ = imu_msg.header.stamp;
tfB_->sendTransform(transform_);
}
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());
}