hand_marker_publisher_alg_node.cpp

Go to the documentation of this file.

#include "hand_marker_publisher_alg_node.h"

HandMarkerPublisherAlgNode::HandMarkerPublisherAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<HandMarkerPublisherAlgorithm>()
{
  //init class attributes if necessary
  this->public_node_handle_.getParam("arm_id", arm_id);
  first_time=true;
  pose.resize(3);
  angles.resize(4);
  arm = new CSegwayArmKinematics(arm_id);
  
    //Choose arm from user (note that kinect detects like a mirror, so right hand is left, and left is right)
  if(arm_id==0){
    strcpy(hand_tf,"/skeleton/right_hand_1"); //Real left
  }else if(arm_id==1){
    strcpy(hand_tf,"/skeleton/left_hand_1"); //Real right
  }
  
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->hand_marker_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker", 100);
  this->hand_marker_shoulder_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_shoulder", 100);
  this->hand_marker_IK_FK_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_IK_FK", 100);
  this->hand_marker_workspace_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_workspace", 100);
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  
  // Draw workspace
       // Create the vertices for the points and lines
    geometry_msgs::Point p;
       
    for(int i=1;i<=21;i++)
    {
      for(int j=1;j<=21;j++)
      {
        for(int k=1;k<=21;k++)
        {
          //if((*arm).workspace[21*21*(i-1)+21*(j-1)+k]>0)
          
          //Rviz axes
          p.x = (i*61.0-600.0)/1000.0;
          p.y = (j*61.0-600.0)/1000.0;
          p.z = (k*61.0-600.0)/1000.0;
            
          //Cinematic axes
          if(arm_id==0){
            pose[0]=p.x*1000.0;
            pose[1]=p.y*1000.0;
            pose[2]=p.z*1000.0; 
          }else if(arm_id==1){
            pose[0]=-p.y*1000.0;
            pose[1]=p.z*1000.0;
            pose[2]=-p.x*1000.0; 
          }

          if((*arm).check_workspace(pose)) this->Marker_workspace_msg_.points.push_back(p);  
        }
      }     
    }
  
}

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

void HandMarkerPublisherAlgNode::mainNodeThread(void)
{  
    static int calibration_countdown=10, arm_length=0, automatic_position_x=1,automatic_position_y=1,automatic_position_z=1;
  
    
   
    
  if(listener.frameExists("/skeleton/neck_1") && listener.frameExists(hand_tf)){
      
      if(first_time){
        sleep(1.0);
        first_time=false;
      }
      
      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());
    
      //Transformacion de xyz kinect a xyz neck1
        x=-v.getZ();
        y=-v.getX();
        z=v.getY();

      
      
      //ROS_INFO_STREAM("X: " << x << " Y: " << y << " Z: " << z);
      
      //From neck
      
      this->Marker_msg_.pose.position.x = x;
      this->Marker_msg_.pose.position.y = y;
      this->Marker_msg_.pose.position.z = z;
      
      
      //From shoulder
      
      pose[0]=x*1000;
      pose[1]=y*1000;
      pose[2]=z*1000;
      
      pose=(*arm).neck_to_shoulder_transform(pose);
      
      //Corregir el cambio de base
      if(arm_id==0){
        
      }else if(arm_id==1){
        float aux=pose[0];
        pose[0]=pose[1];
        pose[1]=-aux;
        pose[2]=pose[2];
      }
  
      //Try to get the arm length (get the maximum length) (only at the beginning)
      if(sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2])>arm_length && calibration_countdown >= 0)
      {
        if(sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2])>arm_length)
        {
          arm_length=sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2]);
        }
        calibration_countdown--;
        ROS_WARN_STREAM("CALIBRATING: PLEASE EXTEND ARM  " << calibration_countdown);
        sleep(1);
      }
      
  }else first_time=true;  
   
  
   
      
   
   /*
   
   
      // Debug automatic hand movement over workspace
      
      arm_length=((*arm).D+(*arm).A);
      
      bool ok=false;
      while(!ok)
      {
      
      pose[0]=(automatic_position_x*61.0-600.0);
      pose[1]=(automatic_position_y*61.0-600.0);
      pose[2]=(automatic_position_z*61.0-600.0);
      
      
      ROS_INFO_STREAM(" POSE: " << pose[0] << " " << pose[1] << " " << pose[2]);
      

      
      automatic_position_z++;
      if(automatic_position_z>21){
        automatic_position_y++;
        automatic_position_z=1;
      }
      if(automatic_position_y>21){
        automatic_position_x++;
        automatic_position_y=1;
      }
      if(automatic_position_x>21){
        automatic_position_x=1;
      }
      
      //Cinematic axes
      float aux=pose[0];
      pose[0]=-pose[1];
      pose[1]=pose[2];
      pose[2]=-aux;
      
      if((*arm).check_workspace(pose)) ok=true;
      
      //Rviz axes
      aux=pose[2];
      pose[2]=pose[1];
      pose[1]=-pose[0];
      pose[0]=-aux;
      
      }
      sleep(0.1);
      
      */
      
      
      this->Marker_shoulder_msg_.pose.position.x = pose[0]/1000;
      this->Marker_shoulder_msg_.pose.position.y = pose[1]/1000;
      this->Marker_shoulder_msg_.pose.position.z = pose[2]/1000;
      
      
      //IK-FK
      //Cinematic axes
      if(arm_id==0){
                
      }else if(arm_id==1){
        float aux=pose[0];
        pose[0]=-pose[1];
        pose[1]=pose[2];
        pose[2]=-aux;
      } 

  
      //Escalate to robot's arm 
      pose[0]=pose[0]/arm_length*((*arm).D+(*arm).A);
      pose[1]=pose[1]/arm_length*((*arm).D+(*arm).A);
      pose[2]=pose[2]/arm_length*((*arm).D+(*arm).A);
    
      if(!(*arm).check_workspace(pose)){
        this->Marker_IK_FK_msg_.pose.position.x = 0.0;
        this->Marker_IK_FK_msg_.pose.position.y = 0.0;
        this->Marker_IK_FK_msg_.pose.position.z = 0.0;
      }else{
        if(!(*arm).best_ikine(pose,0.0,angles)){
          this->Marker_IK_FK_msg_.pose.position.x = 1.0;
          this->Marker_IK_FK_msg_.pose.position.y = 1.0;
          this->Marker_IK_FK_msg_.pose.position.z = 1.0;
        }else{
          pose=(*arm).fkine(angles);
          //Rviz axes
          if(arm_id==0){
                
          }else if(arm_id==1){
            float aux=pose[2];
            pose[2]=pose[1];
            pose[1]=-pose[0];
            pose[0]=-aux;
          }

          this->Marker_IK_FK_msg_.pose.position.x = pose[0]/1000;
          this->Marker_IK_FK_msg_.pose.position.y = pose[1]/1000;
          this->Marker_IK_FK_msg_.pose.position.z = pose[2]/1000;
        }       
      }   
      
      

  
      //Fullfill /tf hand
      
      //From neck
      this->Marker_msg_.header.frame_id="/dabo/neck1";
      this->Marker_msg_.header.stamp = ros::Time::now();
      
      this->Marker_msg_.ns = "basic_shapes";
      this->Marker_msg_.id = 0;
      
      this->Marker_msg_.type = visualization_msgs::Marker::SPHERE;
      this->Marker_msg_.action = visualization_msgs::Marker::ADD;
      
      this->Marker_msg_.pose.orientation.x = 0.0;
      this->Marker_msg_.pose.orientation.y = 0.0;
      this->Marker_msg_.pose.orientation.z = 0.0;
      this->Marker_msg_.pose.orientation.w = 1.0;
      
      this->Marker_msg_.scale.x = 0.1;
      this->Marker_msg_.scale.y = 0.1;
      this->Marker_msg_.scale.z = 0.1;

      this->Marker_msg_.color.r = 0.0f;
      this->Marker_msg_.color.g = 1.0f;
      this->Marker_msg_.color.b = 0.0f;
      this->Marker_msg_.color.a = 1.0;

      this->Marker_msg_.lifetime = ros::Duration();
      
      this->hand_marker_publisher_.publish(this->Marker_msg_);
      
      
      //From shoulder
      
      if(arm_id==0){
      this->Marker_shoulder_msg_.header.frame_id="/dabo/shoulder_left";
      }else if(arm_id==1){
      this->Marker_shoulder_msg_.header.frame_id="/dabo/shoulder_right";        
      }
      this->Marker_shoulder_msg_.header.stamp = ros::Time::now();
      
      this->Marker_shoulder_msg_.ns = "basic_shapes";
      this->Marker_shoulder_msg_.id = 1;
      
      this->Marker_shoulder_msg_.type = visualization_msgs::Marker::SPHERE;
      this->Marker_shoulder_msg_.action = visualization_msgs::Marker::ADD;
      
      this->Marker_shoulder_msg_.pose.orientation.x = 0.0;
      this->Marker_shoulder_msg_.pose.orientation.y = 0.0;
      this->Marker_shoulder_msg_.pose.orientation.z = 0.0;
      this->Marker_shoulder_msg_.pose.orientation.w = 1.0;
      
      this->Marker_shoulder_msg_.scale.x = 0.1;
      this->Marker_shoulder_msg_.scale.y = 0.1;
      this->Marker_shoulder_msg_.scale.z = 0.1;

      this->Marker_shoulder_msg_.color.r = 1.0f;
      this->Marker_shoulder_msg_.color.g = 0.0f;
      this->Marker_shoulder_msg_.color.b = 0.0f;
      this->Marker_shoulder_msg_.color.a = 1.0;

      this->Marker_shoulder_msg_.lifetime = ros::Duration();
      
      this->hand_marker_shoulder_publisher_.publish(this->Marker_shoulder_msg_);
      
      
      //IK-FK from neck
      if(arm_id==0){
      this->Marker_IK_FK_msg_.header.frame_id="/dabo/shoulder_left";
      }else if(arm_id==1){
      this->Marker_IK_FK_msg_.header.frame_id="/dabo/shoulder_right";   
      }
      
      this->Marker_IK_FK_msg_.header.stamp = ros::Time::now();
      
      this->Marker_IK_FK_msg_.ns = "basic_shapes";
      this->Marker_IK_FK_msg_.id = 1;
      
      this->Marker_IK_FK_msg_.type = visualization_msgs::Marker::SPHERE;
      this->Marker_IK_FK_msg_.action = visualization_msgs::Marker::ADD;
      
      this->Marker_IK_FK_msg_.pose.orientation.x = 0.0;
      this->Marker_IK_FK_msg_.pose.orientation.y = 0.0;
      this->Marker_IK_FK_msg_.pose.orientation.z = 0.0;
      this->Marker_IK_FK_msg_.pose.orientation.w = 1.0;
      
      this->Marker_IK_FK_msg_.scale.x = 0.1;
      this->Marker_IK_FK_msg_.scale.y = 0.1;
      this->Marker_IK_FK_msg_.scale.z = 0.1;

      this->Marker_IK_FK_msg_.color.r = 0.0f;
      this->Marker_IK_FK_msg_.color.g = 0.5f;
      this->Marker_IK_FK_msg_.color.b = 0.8f;
      this->Marker_IK_FK_msg_.color.a = 1.0;

      this->Marker_IK_FK_msg_.lifetime = ros::Duration();
      
      this->hand_marker_IK_FK_publisher_.publish(this->Marker_IK_FK_msg_);
      
      
      //Draw workspace (got from rviz tutorial)
    if(arm_id==0){
      this->Marker_workspace_msg_.header.frame_id = "/dabo/shoulder_left";
    }else if(arm_id==1){
      this->Marker_workspace_msg_.header.frame_id = "/dabo/shoulder_right";     
    }
    
    this->Marker_workspace_msg_.header.stamp = ros::Time::now();
    this->Marker_workspace_msg_.ns = "points_and_lines";
    this->Marker_workspace_msg_.action = visualization_msgs::Marker::ADD;
    this->Marker_workspace_msg_.pose.orientation.w = 1.0;
    
    this->Marker_workspace_msg_.id = 0;

    this->Marker_workspace_msg_.type = visualization_msgs::Marker::POINTS;

    // POINTS markers use x and y scale for width/height respectively
    this->Marker_workspace_msg_.scale.x = 0.005;
    this->Marker_workspace_msg_.scale.y = 0.005;

    // Points are green
    this->Marker_workspace_msg_.color.b = 1.0f;
    this->Marker_workspace_msg_.color.a = 1.0;

    this->hand_marker_workspace_publisher_.publish(this->Marker_workspace_msg_);
      
   
  
  // [fill msg structures]
  //this->Marker_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]

}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void HandMarkerPublisherAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void HandMarkerPublisherAlgNode::addNodeDiagnostics(void)
{
}

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