tibi_dabo_kinect_arm_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_kinect_arm_alg_node.h"

TibiDaboKinectArmAlgNode::TibiDaboKinectArmAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboKinectArmAlgorithm>(),
  FollowJointTrajectory_client_("FollowJointTrajectory", true)
{
  //init class attributes if necessary
  //strcpy(desired_frames[0],"/skeleton/right_shoulder_1");
  //strcpy(desired_frames[1],"/skeleton/right_hand_1");
  this->public_node_handle_.getParam("arm_id", arm_id);
  x=250;
  y=-300;
  z=50;
  
  //Choose arm from user (note that kinect detects like a mirror, so right hand is left, and left is right)
  if(arm_id=="Left"){
    strcpy(hand_tf,"/skeleton/right_hand_1"); //Real left
  }else if(arm_id=="Right"){
    strcpy(hand_tf,"/skeleton/left_hand_1"); //Real right
  }
  
  this->loop_rate_ = 10;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  this->tf_subscriber_ = this->public_node_handle_.subscribe("tf", 20, &TibiDaboKinectArmAlgNode::tf_callback, this);
  
  // [init services]
  
  // [init clients]
  GetPositionIK_client_ = this->public_node_handle_.serviceClient<kinematics_msgs::GetPositionIK>("GetPositionIK");
  
  // [init action servers]
  
  // [init action clients]
  
  this->joint_motion_done=true;
  
  // initialize the message for all angles
  this->FollowJointTrajectory_goal_.trajectory.joint_names.resize(4);
  if(arm_id=="Left"){
  this->FollowJointTrajectory_goal_.trajectory.joint_names[0]="left_shoulder_yaw";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[1]="left_shoulder_pitch";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[2]="left_shoulder_roll";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[3]="left_elbow";   
  }else if(arm_id=="Right"){
  this->FollowJointTrajectory_goal_.trajectory.joint_names[0]="right_shoulder_yaw";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[1]="right_shoulder_pitch";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[2]="right_shoulder_roll";
  this->FollowJointTrajectory_goal_.trajectory.joint_names[3]="right_elbow"; 
  }

  this->FollowJointTrajectory_goal_.trajectory.points.resize(1);
  this->FollowJointTrajectory_goal_.trajectory.points[0].positions.resize(4);
  this->FollowJointTrajectory_goal_.trajectory.points[0].velocities.resize(4);
  this->FollowJointTrajectory_goal_.trajectory.points[0].accelerations.resize(4);
  this->FollowJointTrajectory_goal_.trajectory.points[0].time_from_start=ros::Duration(0);
  
}

TibiDaboKinectArmAlgNode::~TibiDaboKinectArmAlgNode(void)
{
  // [free dynamic memory]
}

void TibiDaboKinectArmAlgNode::mainNodeThread(void)
{  
  static bool first_time=true;
  
  // [fill msg structures]  
  // [fill srv structure and make request to the server]
  
  if(this->joint_motion_done || true)
  {
    
    if(listener.frameExists("/skeleton/neck_1") && listener.frameExists(hand_tf) && !debug){
      
      if(first_time){
        sleep(1.0);
        first_time=false;
        //Velocity[0] seed message is used as boolean to show if there is a new user (1=new, 0=not new)
        GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity.resize(1);
        GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity[0]=1;
      }
      
      
      listener.lookupTransform("/skeleton/neck_1", hand_tf, ros::Time(0), echo_transform);
      v = echo_transform.getOrigin();
      //ROS_WARN_STREAM("Hand from neck: X: " << v.getX() << " Y: " << v.getY() << " Z: " << v.getZ());
    
      //Kinect to neck1 (urdf) axes
      x=-v.getZ()*1000;
      y=-v.getX()*1000;
      z=v.getY()*1000;
    
      //Pose
      GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.x=x;
      GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.y=y;
      GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.z=z;
    
      //theta3 seed (rads)
      theta3=0;
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(1);
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(1);
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="shoulder_roll";
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=theta3;
  
      //ROS_INFO("KinematicsClientAlgNode:: Sending New Request!");
      ROS_WARN_STREAM("KinematicsClientAlgNode:: Pose sent: " << x << " " << y << " " << z);
      if (GetPositionIK_client_.call(GetPositionIK_srv_)){ 
        
        theta1=GetPositionIK_srv_.response.solution.joint_state.position[0];
        theta2=GetPositionIK_srv_.response.solution.joint_state.position[1];
        theta3=GetPositionIK_srv_.response.solution.joint_state.position[2];
        theta4=GetPositionIK_srv_.response.solution.joint_state.position[3];
      
        ROS_WARN_STREAM("KinematicsClientAlgNode:: Angles received: " << theta1*180/3.14159 << " " << theta2*180/3.14159 << " " << theta3*180/3.14159 << " " << theta4*180/3.14159);
          
        //fill all 4 angles
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[0]=theta1;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[0]=25*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[1]=theta2;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[1]=25.0*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[2]=theta3;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[2]=25.0*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[3]=theta4;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[3]=25.0*3.14159/180.0;

        this->joint_motion_done=true;
        FollowJointTrajectoryMakeActionRequest();
        
      } 
      else{
        ROS_ERROR("KinematicsClientAlgNode:: Failed to Call Server on topic GetPositionIK or IK could not be calculated");
      }
      //Velocity[1] seed message is used as boolean to show if there is a new user (1=new, 0=not new)
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity.resize(1);
      GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity[0]=0;
    }else if(debug){
        
        x=x_debug;
        y=y_debug;
        z=z_debug;
        
        if(!angle_mode_debug){
          
          
        //Debug mode (skips escalate arm in tibi_dabo_arm_kinematics)
            GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity.resize(1);
            GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.velocity[0]=2;
          
           //Pose
          GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.x=x;
          GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.y=y;
          GetPositionIK_srv_.request.ik_request.pose_stamped.pose.position.z=z;
    
          //theta3 seed (rads)
          theta3=0;
          GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(1);
          GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(1);
          GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="shoulder_roll";
          GetPositionIK_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=theta3;
  
          ROS_WARN_STREAM("KinematicsClientAlgNode:: Pose sent: " << x << " " << y << " " << z);
          
          if (GetPositionIK_client_.call(GetPositionIK_srv_)){
            theta1=GetPositionIK_srv_.response.solution.joint_state.position[0];
            theta2=GetPositionIK_srv_.response.solution.joint_state.position[1];
            theta3=GetPositionIK_srv_.response.solution.joint_state.position[2];
            theta4=GetPositionIK_srv_.response.solution.joint_state.position[3];
            
            ROS_WARN_STREAM("KinematicsClientAlgNode:: Angles received: " << theta1*180/3.14159 << " " << theta2*180/3.14159 << " " << theta3*180/3.14159 << " " << theta4*180/3.14159);
          }
        }else{
          theta1=theta1_debug*3.1416/180;
          theta2=theta2_debug*3.1416/180;
          theta3=theta3_debug*3.1416/180;
          theta4=theta4_debug*3.1416/180;
        }
      
        //fill all 4 angles (not doing IK)
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[0]=theta1;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[0]=25*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[1]=theta2;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[1]=25.0*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[2]=theta3;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[2]=25.0*3.14159/180.0;
        this->FollowJointTrajectory_goal_.trajectory.points[0].positions[3]=theta4;
        this->FollowJointTrajectory_goal_.trajectory.points[0].velocities[3]=25.0*3.14159/180.0;
        
        this->joint_motion_done=true;
        FollowJointTrajectoryMakeActionRequest();
    
    }else{
      //If user has left and comes back
      first_time=true;  
    }
  }
   
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */
void TibiDaboKinectArmAlgNode::tf_callback(const tf::tfMessage::ConstPtr& msg) 
{ 

  //ROS_INFO("TibiDaboKinectArmAlgNode::tf_callback: New Message Received");   
  
  
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->tf_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->tf_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboKinectArmAlgNode::FollowJointTrajectoryDone(const actionlib::SimpleClientGoalState& state,  const control_msgs::FollowJointTrajectoryResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryDone: Goal Achieved!"); 
  else 
    ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryDone: %s", state.toString().c_str());
  this->joint_motion_done=true;

  //copy & work with requested result 
} 

void TibiDaboKinectArmAlgNode::FollowJointTrajectoryActive() 
{ 
  //ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryActive: Goal just went active!"); 
} 

void TibiDaboKinectArmAlgNode::FollowJointTrajectoryFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var;

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    FollowJointTrajectory_client_.cancelGoal(); 
    //ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void TibiDaboKinectArmAlgNode::FollowJointTrajectoryMakeActionRequest() 
{ 
  ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  FollowJointTrajectory_client_.waitForServer();  
  ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //FollowJointTrajectory_goal_.data = my_desired_goal; 
  FollowJointTrajectory_client_.sendGoal(FollowJointTrajectory_goal_, 
              boost::bind(&TibiDaboKinectArmAlgNode::FollowJointTrajectoryDone,     this, _1, _2), 
              boost::bind(&TibiDaboKinectArmAlgNode::FollowJointTrajectoryActive,   this), 
              boost::bind(&TibiDaboKinectArmAlgNode::FollowJointTrajectoryFeedback, this, _1)); 
  ROS_INFO("TibiDaboKinectArmAlgNode::FollowJointTrajectoryMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->joint_motion_done=false;
}

void TibiDaboKinectArmAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  
  arm_id=config.arm_id;
  debug=config.debug;
  angle_mode_debug=config.angle_mode_debug;
  x_debug=config.x_debug;
  y_debug=config.y_debug;
  z_debug=config.z_debug;
  theta1_debug=config.theta1_debug;
  theta2_debug=config.theta2_debug;
  theta3_debug=config.theta3_debug;
  theta4_debug=config.theta4_debug;

  this->alg_.unlock();
}

void TibiDaboKinectArmAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<TibiDaboKinectArmAlgNode>(argc, argv, "tibi_dabo_kinect_arm_alg_node");
}