kinect_teleop.cpp

Go to the documentation of this file.

// General Includes
#include <math.h>
#include <vector>

// ROS Includes
#include <ros/ros.h>
#include <ros/package.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_broadcaster.h>
#include <kdl/frames.hpp>

#include <std_msgs/String.h> 
#include <geometry_msgs/Twist.h> 

// Eigen includes
#include <Eigen/Core>

// Prime Sense Includes
#include <XnOpenNI.h>
#include <XnCodecIDs.h>
#include <XnCppWrapper.h>

#define my_pi 3.141592653589793

using std::string;

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> TrajClient;
typedef std::vector<std::pair<Eigen::VectorXd, Eigen::VectorXd> > Trajectory;
TrajClient *arm_client_r;
TrajClient *arm_client_l;

// Prime Sense related
xn::Context g_Context;
xn::DepthGenerator g_DepthGenerator;
xn::UserGenerator g_UserGenerator;

XnPoint3D init_usr_loc;
XnPoint3D init_usr_RS; // Initial location of the user's Right Shoulder (for rotation of the body)
XnPoint3D init_usr_LS; // Initial location of the user's Left Shoulder (for rotation of the body)

XnBool g_bNeedPose = FALSE;
XnChar g_strPose[20] = "";

// OpenNI Check Fault
#define CHECK_RC(nRetVal, what)                                         \
  if (nRetVal != XN_STATUS_OK)                                          \
    {                                                                   \
      printf("%s failed: %s\n", what, xnGetStatusString(nRetVal));      \
      return nRetVal;                                                   \
    }


struct pr2_joint
{
  float yaw;
  float pitch;
  float absolute;
};

class TeleopPR2Ni
{

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  TeleopPR2Ni();
  void
  updateTrajectory(double rate);
  void
  pubMsg(double rate);
  void
  initangles(pr2_joint &joint);
  void
  updateTrajectoryFromJoints(Trajectory &trajectory, pr2_joint &shoulderAngles,
      pr2_joint &elbowAngles, Eigen::VectorXd &curr_pos, Eigen::VectorXd &last_pos, double rate);
  void
  fillGoal(pr2_controllers_msgs::JointTrajectoryGoal &goal, Trajectory &trajectory, double rate);

  ros::NodeHandle nh_; // Node that will publish messages
  ros::NodeHandle privateNh_; // Initialized but not used in this code. Inherited from Albert-Ludwig University's implementation

  bool enabled_;
  bool teleopEnabled_; // Teleoperation enabled - Body Control (mutually exclusive with attentionEnabled)
  bool useLeftArm_;
  ros::Publisher rShoulderPub_; //           right shoulder angles
  ros::Publisher rElbowPub_; //           right elbow angles
  ros::Publisher lShoulderPub_; //           right shoulder angles
  ros::Publisher lElbowPub_; //           right elbow angles

  pr2_joint rShoulderAngles_; // Right shoulder angles
  pr2_joint rElbowAngles_; // Right elbow angles
  pr2_joint lShoulderAngles_; // Left shoulder angles
  pr2_joint lElbowAngles_; // Left elbow angles

  Eigen::VectorXd curr_pos_r_;
  Eigen::VectorXd last_pos_r_;
  Eigen::VectorXd curr_pos_l_;
  Eigen::VectorXd last_pos_l_;

  Trajectory trajectory_r_;
  Trajectory trajectory_l_;
};

// Initialization list of object teleopPR2Ni
TeleopPR2Ni::TeleopPR2Ni() :
  privateNh_("~"), teleopEnabled_(false)
{
  enabled_ = true;

  // Initialize all velocities and angles at zero.
  initangles(rShoulderAngles_);
  initangles(rElbowAngles_);
  initangles(lShoulderAngles_);
  initangles(lElbowAngles_);
  // init positions
  curr_pos_r_.setZero(4);
  last_pos_r_.setZero(4);
  curr_pos_l_.setZero(4);
  last_pos_l_.setZero(4);
}

void
TeleopPR2Ni::initangles(pr2_joint &joint)
{
  // Initialize all velocities and angles at zero.
  joint.yaw = 0.0;
  joint.pitch = 0.0;
  joint.absolute = 1.0;
}

void
TeleopPR2Ni::updateTrajectoryFromJoints(Trajectory &trajectory, pr2_joint &shoulderAngles,
    pr2_joint &elbowAngles, Eigen::VectorXd &curr_pos, Eigen::VectorXd &last_pos, double rate)
{
  Eigen::VectorXd next_pos(4);
  Eigen::VectorXd next_vel(4);

  next_pos(0) = shoulderAngles.yaw;
  next_pos(1) = shoulderAngles.pitch;
  next_pos(2) = elbowAngles.yaw;
  next_pos(3) = -elbowAngles.pitch;

  next_vel = (next_pos - last_pos) / (2. * (1. / rate));

  if ((next_pos - curr_pos).norm() > 1. * my_pi / 180.)
    {
      trajectory.push_back(std::make_pair<Eigen::VectorXd, Eigen::VectorXd>(next_pos, next_vel));
      last_pos = curr_pos;
      curr_pos = next_pos;
    }
  else
    {
      last_pos = curr_pos;
      next_vel.setZero(4);
      trajectory.push_back(std::make_pair<Eigen::VectorXd, Eigen::VectorXd>(curr_pos, next_vel));
    }
}

void
TeleopPR2Ni::updateTrajectory(double rate)
{
  updateTrajectoryFromJoints(trajectory_r_, rShoulderAngles_, rElbowAngles_, curr_pos_r_,
      last_pos_r_, rate);
  updateTrajectoryFromJoints(trajectory_l_, lShoulderAngles_, lElbowAngles_, curr_pos_l_,
      last_pos_l_, rate);
}

void
TeleopPR2Ni::fillGoal(pr2_controllers_msgs::JointTrajectoryGoal &goal, Trajectory &trajectory,
    double rate)
{
  // We will have two waypoints in this goal trajectory
  goal.trajectory.points.resize(trajectory.size());
  for (size_t ind = 0; ind < trajectory.size(); ++ind)
    {
      Eigen::VectorXd &pos = trajectory[ind].first;
      Eigen::VectorXd &vel = trajectory[ind].second;
      // Positions
      goal.trajectory.points[ind].positions.resize(7);
      goal.trajectory.points[ind].positions[0] = pos(0);
      goal.trajectory.points[ind].positions[1] = pos(1);
      goal.trajectory.points[ind].positions[2] = pos(2);
      goal.trajectory.points[ind].positions[3] = pos(3);
      goal.trajectory.points[ind].positions[4] = 0.0;
      goal.trajectory.points[ind].positions[5] = 0.0;
      goal.trajectory.points[ind].positions[6] = 0.0;

      // Velocities
      vel /= 50.;
      goal.trajectory.points[ind].velocities.resize(7);
      goal.trajectory.points[ind].velocities[0] = vel(0);
      goal.trajectory.points[ind].velocities[1] = vel(1);
      goal.trajectory.points[ind].velocities[2] = vel(2);
      goal.trajectory.points[ind].velocities[3] = vel(3);
      goal.trajectory.points[ind].velocities[4] = 0.0;
      goal.trajectory.points[ind].velocities[5] = 0.0;
      goal.trajectory.points[ind].velocities[6] = 0.0;
      // To be reached 1 second after starting along the trajectory
      goal.trajectory.points[ind].time_from_start = ros::Duration((ind + 1) * (1 / rate));
    }

  //ROS_INFO("Sending goal");
}

void
TeleopPR2Ni::pubMsg(double rate)
{
  if (teleopEnabled_ && arm_client_r && trajectory_r_.size() >= 3)
    { // RIGHT ARM
      pr2_controllers_msgs::JointTrajectoryGoal goal;
      goal.trajectory.header.stamp = ros::Time::now();
      // First, the joint names, which apply to all waypoints
      goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
      goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
      goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
      goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
      goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
      goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
      goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

      fillGoal(goal, trajectory_r_, rate);

      arm_client_r->sendGoal(goal);
      trajectory_r_.clear();
    }
  if (teleopEnabled_ && useLeftArm_ && arm_client_l && trajectory_l_.size() >= 3)
    { // LEFT ARM
      pr2_controllers_msgs::JointTrajectoryGoal goal;
      goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1 / rate);
      // First, the joint names, which apply to all waypoints
      goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
      goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
      goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
      goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
      goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
      goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
      goal.trajectory.joint_names.push_back("l_wrist_roll_joint");

      fillGoal(goal, trajectory_l_, rate);

      arm_client_l->sendGoal(goal);
      trajectory_l_.clear();
    }
}

void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
  {
    printf("New User %d\n", nId);
    if (g_bNeedPose)
    g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
    else
    g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
  }

void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
  {
    printf("Lost user %d\n", nId);
  }

void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie)
  {
    printf("Calibration started for user %d\n", nId);
  }

void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
  {
    if (bSuccess)
      {
        printf("Calibration complete, start tracking user %d\n", nId);
        g_UserGenerator.GetSkeletonCap().StartTracking(nId);
      }
    else
      {
        printf("Calibration failed for user %d\n", nId);
        if (g_bNeedPose)
        g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
        else
        g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
      }
  }

void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, XnChar const* strPose, XnUserID nId, void* pCookie)
  {
    printf("Pose %s detected for user %d\n", strPose, nId);
    g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
    g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
  }

XnPoint3D
getBodyLoc(XnUserID const& user, XnSkeletonJoint const& eJoint1)
{
  XnPoint3D pt;

  // Fault Check
  if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
    {
      printf("not tracked!\n");
      pt.X = 0.0;
      pt.Y = 0.0;
      pt.Z = 0.0;
      return pt;
    }

  // Get joint position (this is supposed to be "the Torso point" of the skeleton)
  XnSkeletonJointPosition joint1;
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint1, joint1);

  // Make sure that all joints are found with enough confidence...
  if (joint1.fConfidence < 0.7)
    {
      pt.X = 0.0;
      pt.Y = 0.0;
      pt.Z = 0.0;
      return pt;
    }

  pt = joint1.position; // Return the position
  return pt;
}

// Get and return the angle between two NOT JOINT limbs of the skeleton, e.g. angle between (hip to hip vector) ^ ( upper arm )
// !!!!! NOTE : This method originally comes from  the nao teleop stack 
float
getAngleBetweenLimbs(XnUserID const& user, XnSkeletonJoint const& eJoint1,
    XnSkeletonJoint const& eJoint2, XnSkeletonJoint const& eJoint3, XnSkeletonJoint const& eJoint4)
{

  // This function is different than getLimbAngle, in the sense that, it finds the angle of two disconnected
  // limbs (each limb considered as a vector).

  // In order to find the shoulder roll angleo, we need to find the angle between
  // [Right Hip --> Left Hip] vector and [Right Shoulder --> Right Elbow] vector

  if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
    {
      printf("not tracked!\n");
      return -998.0; // Error code (though, it's not used yet)
    }

  // Get joint positions
  XnSkeletonJointPosition joint1, joint2, joint3, joint4;
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint1, joint1);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint2, joint2);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint3, joint3);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint4, joint4);

  // Make sure that all joints are found with enough confidence...
  if (joint1.fConfidence < 0.6 && joint2.fConfidence < 0.6 && joint3.fConfidence < 0.6
      && joint4.fConfidence < 0.6)
    {
      return -999.0; // Error code (though, it's not used yet)
    }

  XnPoint3D pt[4]; // Create four points
  pt[0] = joint1.position; // Vector 0, Point 0
  pt[1] = joint2.position; // Vector 0, Point 1
  pt[2] = joint3.position; // Vector 1, Point 0
  pt[3] = joint4.position; // Vector 1, Point 1

  XnVector3D v[2]; // Create two vectors

  // First vector
  // V_{RHIP,LHIP} = V_{LHIP} - V_{RHIP}
  v[0].X = pt[2].X - pt[3].X;
  v[0].Y = pt[2].Y - pt[3].Y;
  v[0].Z = pt[2].Z - pt[3].Z;

  // Second vector
  // V_{S,E} = V_{E} - V_{S}
  v[1].X = pt[0].X - pt[1].X;
  v[1].Y = pt[0].Y - pt[1].Y;
  v[1].Z = pt[0].Z - pt[1].Z;

  // Calculate the magnitude of the vectors (limbs)
  float v0_magnitude = sqrt(v[0].X * v[0].X + v[0].Y * v[0].Y + v[0].Z * v[0].Z);
  float v1_magnitude = sqrt(v[1].X * v[1].X + v[1].Y * v[1].Y + v[1].Z * v[1].Z);

  //printf("V0 MAG: %f, V1 MAG: %f\n",v0_magnitude, v1_magnitude);

  // If neither of vectors != 0.0 then find the angle between them
  if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
    {
      v[0].X = v[0].X * (1.0 / v0_magnitude);
      v[0].Y = v[0].Y * (1.0 / v0_magnitude);
      v[0].Z = v[0].Z * (1.0 / v0_magnitude);

      v[1].X = v[1].X * (1.0 / v1_magnitude);
      v[1].Y = v[1].Y * (1.0 / v1_magnitude);
      v[1].Z = v[1].Z * (1.0 / v1_magnitude);

      // Find and convert the angle between upper and lower arms
      float theta = acos(v[0].X * v[1].X + v[0].Y * v[1].Y + v[0].Z * v[1].Z);
      float angle_in_degrees = theta * 180 / my_pi;

      return angle_in_degrees;
    }
  else
    return -997.0; // Error code (though, it's not used yet)
}

// Get and return the angle between JOINT limbs (e.g. upper arm ^ lower arm; upper leg ^ lower leg; hip_to_hip ^ upper_leg etc.)
// !!!!! NOTE : This method originally comes from  the nao teleop stack 
float
getLimbAngle(XnUserID const& user, XnSkeletonJoint const& eJoint1, XnSkeletonJoint const& eJoint2,
    XnSkeletonJoint const& eJoint3)
{
  if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
    {
      printf("not tracked!\n");
      return -998.0; // Error code (though, it's not used yet)
    }

  // Get joint positions
  XnSkeletonJointPosition joint1, joint2, joint3;
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint1, joint1);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint2, joint2);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint3, joint3);

  // Make sure that all joints are found with enough confidence...
  if (joint1.fConfidence < 0.6 && joint2.fConfidence < 0.6 && joint3.fConfidence < 0.6)
    {
      return -999.0; // Error code (though, it's not used yet)
    }

  XnPoint3D pt[3]; // Create 3 points
  pt[0] = joint1.position; // E.g. Shoulder
  pt[1] = joint2.position; // E.g. Elbow
  pt[2] = joint3.position; // E.g. Hand

  XnVector3D v[2]; // Create two vectors

  // S: Shoulder, E: Elbow, H: Hand
  // S  *
  //    |
  // E  *---*
  //        H

  // Calculate the first vector
  // V_{H,E} = V_{E} - V_{H}
  v[0].X = pt[1].X - pt[2].X;
  v[0].Y = pt[1].Y - pt[2].Y;
  v[0].Z = pt[1].Z - pt[2].Z;

  // Calculate the second vector
  // V_{E,S} = V_{S} - V_{E}
  v[1].X = pt[0].X - pt[1].X;
  v[1].Y = pt[0].Y - pt[1].Y;
  v[1].Z = pt[0].Z - pt[1].Z;

  // Calculate the magnitudes of the vectors
  float v0_magnitude = sqrt(v[0].X * v[0].X + v[0].Y * v[0].Y + v[0].Z * v[0].Z);
  float v1_magnitude = sqrt(v[1].X * v[1].X + v[1].Y * v[1].Y + v[1].Z * v[1].Z);


  if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
    {
      v[0].X = v[0].X * (1.0 / v0_magnitude);
      v[0].Y = v[0].Y * (1.0 / v0_magnitude);
      v[0].Z = v[0].Z * (1.0 / v0_magnitude);

      v[1].X = v[1].X * (1.0 / v1_magnitude);
      v[1].Y = v[1].Y * (1.0 / v1_magnitude);
      v[1].Z = v[1].Z * (1.0 / v1_magnitude);

      // Find and convert the angle between JOINT limbs
      float theta = acos(v[0].X * v[1].X + v[0].Y * v[1].Y + v[0].Z * v[1].Z);
      float angle_in_degrees = theta * 180 / my_pi;

      return angle_in_degrees;
    }
  else
    return -997.0; // Error code (though, it's not used yet)
}

// Gives the angle between:
// the initial AND current position of the user's shoulders, to understand how much he/she rotated his/her shoulders.
float
getTorsoRotation(XnUserID const& user, XnSkeletonJoint const& eJoint1,
    XnSkeletonJoint const& eJoint2)
{

  if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
    {
      printf("not tracked!\n");
      return -998.0; // Error code (though, it's not used yet)
    }

  // Get joint positions
  XnSkeletonJointPosition joint1, joint2;
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint1, joint1);
  g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, eJoint2, joint2);

  // Make sure that all joints are found with enough confidence...
  if (joint1.fConfidence < 0.6 && joint2.fConfidence < 0.6)
    {
      return -999.0; // Error code (though, it's not used yet)
    }

  XnPoint3D pt[4]; // Create four points
  pt[0] = init_usr_LS; // Vector 0, Point 0 - Initial location of the left shoulder
  pt[1] = init_usr_RS; // Vector 0, Point 1 - Initial location of the right shoulder
  pt[2] = joint1.position; // Vector 1, Point 0 - Current location of the left shoulder
  pt[3] = joint2.position; // Vector 1, Point 1 - Current location of the right shoulder

  XnVector3D v[2]; // Create two vectors

  // Assign the vectors
  v[0].X = pt[2].X - pt[3].X;
  v[0].Y = pt[2].Y - pt[3].Y;
  v[0].Z = pt[2].Z - pt[3].Z;

  v[1].X = pt[0].X - pt[1].X;
  v[1].Y = pt[0].Y - pt[1].Y;
  v[1].Z = pt[0].Z - pt[1].Z;

  // Calculate the magnitude of the vectors
  float v0_magnitude = sqrt(v[0].X * v[0].X + v[0].Y * v[0].Y + v[0].Z * v[0].Z);
  float v1_magnitude = sqrt(v[1].X * v[1].X + v[1].Y * v[1].Y + v[1].Z * v[1].Z);

  //printf("V0 MAG: %f, V1 MAG: %f\n",v0_magnitude, v1_magnitude);

  // If neither of the vectors are != 0.0 then calculate the angle between them
  if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
    {
      v[0].X = v[0].X * (1.0 / v0_magnitude);
      v[0].Y = v[0].Y * (1.0 / v0_magnitude);
      v[0].Z = v[0].Z * (1.0 / v0_magnitude);

      v[1].X = v[1].X * (1.0 / v1_magnitude);
      v[1].Y = v[1].Y * (1.0 / v1_magnitude);
      v[1].Z = v[1].Z * (1.0 / v1_magnitude);

      // Calculate and convert the angle between the initial and current locations of the shoulders
      float theta = acos(v[0].X * v[1].X + v[0].Y * v[1].Y + v[0].Z * v[1].Z);
      float angle_in_degrees = theta * 180 / my_pi;

      return angle_in_degrees;
    }
  else
    return -997.0; // Error code (though, it's not used yet)
}

void
checkPose(TeleopPR2Ni &obj)
{
  XnUserID users[2];
  XnUInt16 users_count = 2;
  g_UserGenerator.GetUsers(users, users_count);
  obj.teleopEnabled_ = false;

  for (int i = 0; i < users_count; ++i)
    {
      XnUserID user = users[i];
      if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
        continue;

      obj.teleopEnabled_ = true;
      /*  If right and left shoulder - elbow - hand make 90 (+/-5) degrees
       *  with a certain confidence switch to teleoperation mode
       */

      // Get the current body (torso) location
      XnPoint3D current_bl = getBodyLoc(user, XN_SKEL_TORSO);

      // Get the current right shoulder locations
      XnPoint3D current_rs = getBodyLoc(user, XN_SKEL_RIGHT_SHOULDER);
      // Get the current left shoulder locations
      XnPoint3D current_ls = getBodyLoc(user, XN_SKEL_LEFT_SHOULDER);

      // if positive rotate right, else rotate left
      //      float current_angle = getTorsoRotation(user, XN_SKEL_LEFT_SHOULDER, XN_SKEL_RIGHT_SHOULDER)
      //          * my_pi / 180.0;

      // Right shoulder angles pitch
      float rsap = getLimbAngle(user, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_SHOULDER,
          XN_SKEL_RIGHT_ELBOW) * my_pi / 180.0;
      rsap = rsap - 1.6;
      if (rsap >= -0.5 && rsap <= 1.4)
        {
          obj.rShoulderAngles_.pitch = rsap;
        }
      //      printf("Right Shoulder Pitch: %f\n", rsap);

      // Right elbow angle roll
      float rear = getLimbAngle(user, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW,
          XN_SKEL_RIGHT_HAND) * my_pi / 180.0;
      if (2.4 >= rear && rear >= 0.0)
        {
          obj.rElbowAngles_.pitch = rear;
        }
      //printf("Right elbow Pitch: %f\n", rear);

      // Right elbow angle yaw
      float reay = getAngleBetweenLimbs(user, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_HIP,
          XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND) * my_pi / 180.0;
      reay = reay - 2.8; //1.57;
      if (0.8 >= reay && reay >= -3.9)
        {
          obj.rElbowAngles_.yaw = reay;
        }
      //printf("Right elbow yaw: %f\n", reay);

      // Right shoulder angle roll
      float rsar = getAngleBetweenLimbs(user, XN_SKEL_RIGHT_HIP, XN_SKEL_LEFT_HIP,
          XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW) * my_pi / 180.0;
      // In front: 90 (1.57) --> -0.5
      // Wide open: 180 (3.14) --> -94.5

      rsar = 1.63 - rsar;
      if ( 0.7 >= rsar && rsar >= -2.3)
        {
          obj.rShoulderAngles_.yaw = rsar;
        }

      // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
      // NOW the same stuff as above for the left shoulder!
      // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

      // Left shoulder angles pitch
      float lsap = getLimbAngle(user, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW)
          * my_pi / 180.0;
      lsap = lsap - 1.6;
      if (lsap >= -0.5 && lsap <= 1.4)
        {
          obj.lShoulderAngles_.pitch = lsap;
        }
      //      printf("Left Shoulder Pitch: %f\n", rsap);

      // Left elbow angle roll
      float lear = getLimbAngle(user, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND)
          * my_pi / 180.0;
      if (2.4 >= lear && lear >= 0.0)
        {
          obj.lElbowAngles_.pitch = lear;
        }
      //printf("Left elbow Pitch: %f\n", rear);

      // Left elbow angle yaw
      float leay = getAngleBetweenLimbs(user, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_HIP,
          XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND) * my_pi / 180.0;
      leay = 2.8 - leay; //1.57;
      if (3.9 >= leay && leay >= -0.8)
        {
          obj.lElbowAngles_.yaw = leay;
        }
      //printf("Left elbow yaw: %f\n", reay);

      // Left shoulder angle roll
      float lsar = getAngleBetweenLimbs(user, XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP,
          XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW) * my_pi / 180.0;
      // In front: 90 (1.57) --> -0.5
      // Wide open: 180 (3.14) --> -94.5

      lsar = lsar - 1.63;
      if (2.3 >= lsar && lsar >= -0.7)
        {
          //lsar = -1. *lsar;
          obj.lShoulderAngles_.yaw = lsar;
        }
      //ROS_INFO("lsar %f rsar %f", lsar, rsar );
    }
}

int
main(int argc, char** argv)
{

  ros::init(argc, argv, "teleop_pr2_ni");// Initialize ROS

  bool useLeftArm = true;
  arm_client_r = new TrajClient("r_arm_controller/joint_trajectory_action", true);
  arm_client_l = new TrajClient("l_arm_controller/joint_trajectory_action", true);

  ros::NodeHandle n("/kinect_teleop");

  // check if left arm should be used as well
  if (!n.getParam("use_left_arm", useLeftArm))
    {
      ROS_INFO("Not using left arm!");
      useLeftArm = false;
    }

  //wait for the action server to come up
  while (!arm_client_r->waitForServer(ros::Duration(5.0)))
    {
      ROS_INFO("Waiting for the right arm action server to come up");
    }
  while (useLeftArm && !arm_client_l->waitForServer(ros::Duration(5.0)))
    {
      ROS_INFO("Waiting for the left arm action server to come up");
    }

  TeleopPR2Ni teleopPR2;// Declare and initialize the variables and object(s)
  teleopPR2.useLeftArm_ = useLeftArm;
  string configFilename = ros::package::getPath("kinect_teleop") + "/openni_tracker.xml";

  int count = 0;
  double publishRate = 50; 
  teleopPR2.privateNh_.param("motion_publish_rate", publishRate, publishRate);
  ros::Rate pubRate(publishRate); // Set the publishing rate

  // OpenNI functions begin -------------------------------------------------
  // Refer to OpenNI Manual and Discussion Group to get help about these functions and variables
  XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
  CHECK_RC(nRetVal, "InitFromXml");

  nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
  CHECK_RC(nRetVal, "Find depth generator");

  nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
  if (nRetVal != XN_STATUS_OK)
    {
      nRetVal = g_UserGenerator.Create(g_Context);
      CHECK_RC(nRetVal, "Find user generator");
    }

  if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
    {
      printf("Supplied user generator doesn't support skeleton\n");
      return 1;
    }

  XnCallbackHandle hUserCallbacks;
  g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);

  XnCallbackHandle hCalibrationCallbacks;
  g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart,
      UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

  if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
    {
      g_bNeedPose = TRUE;
      if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
        {
          printf("Pose required, but not supported\n");
          return 1;
        }

      XnCallbackHandle hPoseCallbacks;
      g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL,
          NULL, hPoseCallbacks);

      g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
    }

  g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

  nRetVal = g_Context.StartGeneratingAll();
  CHECK_RC(nRetVal, "StartGenerating");
  // OpenNI functions end-------------------------------------------------

  // Endless loop to publish messages
  while (teleopPR2.nh_.ok())
    {

      g_Context.WaitAndUpdateAll(); // Update skeleton recognition (OpenNI's callbacks)

      checkPose(teleopPR2); // Check user's pose and assign according values to the messages that will be published

      teleopPR2.updateTrajectory(publishRate); // Update Trajectory

      if (count % 3 == 0)
        {
          teleopPR2.pubMsg(publishRate / 50); // Publish messages to PR2
          count = 0;
        }

      ++count;
      pubRate.sleep(); // Sleep to get to the rate of publishing
    }

  g_Context.Shutdown(); // Close OpenNI
  return 0;

}