convert_pose_alg_node.cpp

Go to the documentation of this file.

#include "convert_pose_alg_node.h"

ConvertPoseAlgNode::ConvertPoseAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ConvertPoseAlgorithm>(),
  tracking_head_client_("tracking_head", true),
  move_joints_client_("move_joints", true),
  move_right_arm_client_("move_right_arm", true),
  move_left_arm_client_("move_left_arm", true),
  tf_listener(ros::Duration(10.0))
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]

  // [init publishers]  
  this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 100);
  this->head_target_publisher_ = this->public_node_handle_.advertise<std_msgs::Float64MultiArray>("head_target", 100);
  this->execute_action_publisher_ = this->public_node_handle_.advertise<std_msgs::Int32>("execute_action", 100);
  
  // [init subscribers]
  this->adc_channels_subscriber_ = this->public_node_handle_.subscribe("adc_channels", 100, &ConvertPoseAlgNode::adc_channels_callback, this);
  this->joint_state_subscriber_ = this->public_node_handle_.subscribe("joint_states", 100, &ConvertPoseAlgNode::joint_state_callback, this);
  this->marker_subscriber_ = this->public_node_handle_.subscribe("marker", 100, &ConvertPoseAlgNode::marker_callback, this);
  
  // [init services]
  
  // [init clients]
  get_ik_client_ = this->public_node_handle_.serviceClient<kinematics_msgs::GetPositionIK>("get_ik");
  
  // [init action servers]
  
  // [init action clients]
  this->ready=false;
  this->joints_moving=false;
  this->right_arm_moving=false;
  this->left_arm_moving=false;  
  this->ready_for_check=false;
  this->left_ok=false;
  this->right_ok=false;  
  this->prev_left_ok=false;
  this->prev_right_ok=false;
  this->left_done=false;  
  this->left_marker_found=false;
  this->right_marker_found=false;
  this->first_goal=true;
  this->current_pan=0.0;
  this->n=0;  
  this->left_data_ready=false;
  this->right_data_ready=false;
  this->left_ready_tf=false;
  this->right_ready_tf=false;
  this->success=true;
 
  this->state=initial_pose;    

  sleep(10);
}

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

void ConvertPoseAlgNode::mainNodeThread(void)
{
  int i=0;  
  
  // [fill msg structures]  
  
  // [fill srv structure and make request to the server]
     
  // [fill action structure and make request to the action server]  

  // [publish messages]  
  ROS_INFO("Main Thread");
  try{
  switch(this->state)
  {
    case initial_pose:  ROS_INFO("Current state: Initial pose");
                        this->Int32_msg_.data=15;
                        this->execute_action_publisher_.publish(this->Int32_msg_);
                        this->move_joints_goal_.trajectory.header.seq=0;
                        this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                        this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                        this->move_joints_goal_.trajectory.joint_names.resize(2);
                        this->move_joints_goal_.trajectory.joint_names[0]="j_high_arm_l";
                        this->move_joints_goal_.trajectory.joint_names[1]="j_high_arm_r";
                        this->move_joints_goal_.trajectory.points.resize(1);
                        this->move_joints_goal_.trajectory.points[0].positions.resize(2);
                        this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
                        this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
                        this->move_joints_goal_.trajectory.points[0].positions[0]=0.0;
                        this->move_joints_goal_.trajectory.points[0].positions[1]=0.0;
                        for(i=0;i<2;i++){
                          this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
                          this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
                        }
                        this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                        move_jointsMakeActionRequest();
                        this->success=true;
                        this->ready=false;
                        this->left_done=false;
                        this->left_marker_found=false;
                        this->right_marker_found=false;
                        this->state=initial_angle;                                                              
                        break;

    case initial_angle: ROS_INFO("Current state: Initial angle");
                        if(this->joints_moving)
                          this->state=initial_angle;
                        else{                                           
                          this->move_joints_goal_.trajectory.header.seq=0;
                          this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                          this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                          this->move_joints_goal_.trajectory.joint_names.resize(2);
                          this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
                          this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
                          this->move_joints_goal_.trajectory.points.resize(1);
                          this->move_joints_goal_.trajectory.points[0].positions.resize(2);
                          this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
                          this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
                          this->move_joints_goal_.trajectory.points[0].positions[0]=40.0-5.0*this->n;
                          this->move_joints_goal_.trajectory.points[0].positions[1]=0.0;
                          for(i=0;i<2;i++){
                            this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
                            this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
                          }
                          this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                          move_jointsMakeActionRequest();                                               
                          if(this->n>=3)
                            this->state=move_back;
                          else{
                            if(this->left_done)
                              this->state=search_right_marker;
                            else
                              this->state=search_left_marker;                                                                                                   
                          }
                          this->n++;
                        }
                        break;

    case move_back:     ROS_INFO("Current state: Going back");
                        this->n=0;
                        this->ready=false;
                        this->Int32_msg_.data=16;
                        this->execute_action_publisher_.publish(this->Int32_msg_);
                        sleep(2);
                        this->Twist_msg_.linear.x=-0.01;
                        this->Twist_msg_.linear.y=0.0;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.0;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        sleep(2);
                        this->state=initial_pose;                        
                        break;

    case search_left_marker:    ROS_INFO("Current state: Searching left marker");
                                if(this->left_marker_found){
                                  ROS_WARN("Left marker found");
                                  move_joints_client_.cancelGoal();
                                  this->left_ready_tf=true;
                                  this->state=transform_left_pose;
                                }else{
                                  if(this->joints_moving)
                                    this->state=search_left_marker;                                                                                             
                                  else{                                                          
                                    this->ready=true; 
                                    this->move_joints_goal_.trajectory.header.seq=0;
                                    this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                    this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                    this->move_joints_goal_.trajectory.joint_names.resize(1);
                                    this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                                    this->move_joints_goal_.trajectory.points.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                    this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                                    this->move_joints_goal_.trajectory.points[0].positions[0]=40.0;                        
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();                                                      
                                    if(this->current_state.position[18]*180.0/3.14159>=39.0)
                                      this->state=initial_angle; 
                                  }
                                }                               
                                break;

    case transform_left_pose:   ROS_INFO("Current state: Transforming left pose");                              
                                if(this->joints_moving)
                                  this->state=transform_left_pose;
                                else{                                                                                     
                                  this->move_joints_goal_.trajectory.header.seq=0;
                                  this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                  this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                  this->move_joints_goal_.trajectory.joint_names.resize(1);
                                  this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                                  this->move_joints_goal_.trajectory.points.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                  this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                                  this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;                            
                                  this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                  move_jointsMakeActionRequest();
                                }                               
                                if(this->left_data_ready){
                                  ROS_WARN("Left pose transformed");                              
                                  this->state=search_right_marker;                                
                                }else
                                  this->state=transform_left_pose;
                                break;
   
    case search_right_marker:   ROS_INFO("Current state: Searching right marker");
                                if(this->right_marker_found){
                                  ROS_WARN("Right marker found");
                                  move_joints_client_.cancelGoal();
                                  this->right_ready_tf=true;
                                  this->state=transform_right_pose;
                                }else{
                                  if(this->joints_moving)
                                    this->state=search_right_marker;                                                              
                                  else{                           
                                    this->move_joints_goal_.trajectory.header.seq=0;
                                    this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                    this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                    this->move_joints_goal_.trajectory.joint_names.resize(1);
                                    this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                                    this->move_joints_goal_.trajectory.points.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].positions[0]=-40.0;
                                    this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                    this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    if(this->current_state.position[18]*180.0/3.14159<=-39.0)
                                      this->state=initial_angle;                                          
                                  }
                                }                               
                                break;

    case transform_right_pose:  ROS_INFO("Current state: Transforming right pose");                             
                                if(this->joints_moving)
                                  this->state=transform_right_pose;
                                else{                                                               
                                  this->move_joints_goal_.trajectory.header.seq=0;
                                  this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                  this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                  this->move_joints_goal_.trajectory.joint_names.resize(1);
                                  this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                                  this->move_joints_goal_.trajectory.points.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                  this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                                  this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;
                                  this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                  move_jointsMakeActionRequest();                                                                 
                                }                               
                                if(this->right_data_ready){
                                  ROS_WARN("Right pose transformed");                                  
                                  this->ready_for_check=true;
                                  this->state=make_decision;
                                }else
                                  this->state=transform_right_pose;
                                break;

    case make_decision: ROS_INFO("Current state: Looking if the final pose have inverse kinematics solution");
                        if(this->joints_moving)
                          this->state=make_decision;
                        else{
                          //Mirar si hi ha solució o no i actuar en conseqüència.                     
                          this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_L";
                          // IK seed, get the current left arm position
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_l";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_l";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_l";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_l";                     
                          this->joint_state_mutex_.enter();
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[2];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[4];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[6];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[23];
                          this->joint_state_mutex_.exit();
                          this->get_ik_srv_.request.ik_request.pose_stamped=left_pose_;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=left_pose_.pose.position.x+0.02;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=left_pose_.pose.position.y;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=left_pose_.pose.position.z-0.16;
                          // ignore the rest of the infomation for the moment
                          if(get_ik_client_.call(get_ik_srv_)){                                              
                            if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)// success
                              this->left_ok=true;
                            else this->left_ok=false;               
                          }else
                            ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");                                       
                          
                          this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_R";
                          // IK seed, get the current right arm position
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_r";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_r";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_r";
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_r";                    
                          this->joint_state_mutex_.enter();
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[1];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[3];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[5];
                          this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[21];
                          this->joint_state_mutex_.exit();
                          this->get_ik_srv_.request.ik_request.pose_stamped=right_pose_;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=right_pose_.pose.position.x+0.03;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=right_pose_.pose.position.y+0.02;
                          this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=right_pose_.pose.position.z-0.18;
                          // ignore the rest of the infomation for the moment
                          if(get_ik_client_.call(get_ik_srv_)){
                            if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)// success
                              this->right_ok=true;

                            else this->right_ok=false;
                          }else
                            ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");
 
                          if((this->left_ok)&&(this->right_ok)){
                            ROS_WARN("Darwin can take the connectors");
                            this->state=make_decision2;
                          }else{
                            ROS_WARN("Darwin need to correct its position");
                            this->state=correct_position;       
                          }                         
                        }               
                        break;

    case make_decision2:        ROS_INFO("Current state: Looking if the previous pose have inverse kinematics solution");                                                                                 
                                // Mirar si hi ha solució o no i actuar en conseqüència.                        
                                this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_L";
                                // IK seed, get the current left arm position
                                this->left_pose_.header.stamp=ros::Time::now();
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_l";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_l";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_l";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_l";
                                this->joint_state_mutex_.enter();
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[2];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[4];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[6];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[23];
                                this->joint_state_mutex_.exit();
                                this->get_ik_srv_.request.ik_request.pose_stamped=left_pose_;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=left_pose_.pose.position.x-0.01;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=left_pose_.pose.position.y+0.04;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=left_pose_.pose.position.z-0.20;
                                // ignore the rest of the infomation for the moment
                                if(get_ik_client_.call(get_ik_srv_)){
                                  if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)// success
                                    this->prev_left_ok=true;
                                  else this->prev_left_ok=false;
                                }else
                                  ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");

                                this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_R";
                                // IK seed, get the current right arm position
                                this->right_pose_.header.stamp=ros::Time::now();
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_r";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_r";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_r";
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_r";
                                this->joint_state_mutex_.enter();
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[1];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[3];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[5];
                                this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[21];
                                this->joint_state_mutex_.exit();
                                this->get_ik_srv_.request.ik_request.pose_stamped=right_pose_;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.x=right_pose_.pose.position.x;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.y=right_pose_.pose.position.y+0.03;
                                this->get_ik_srv_.request.ik_request.pose_stamped.pose.position.z=right_pose_.pose.position.z-0.20;
                                // ignore the rest of the infomation for the moment
                                if(get_ik_client_.call(get_ik_srv_)){
                                  if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)// success
                                    this->prev_right_ok=true;
                                  else this->prev_right_ok=false;
                                }else
                                  ROS_INFO("ConvertPoseAlgNode:: Failed to Call Server on topic get_ik ");                          
                                this->state=open_gripper;                                                       
                                break;

    case open_gripper:  ROS_INFO("Current state: Open gripper");
                        this->move_joints_goal_.trajectory.header.seq=0;
                        this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                        this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                        this->move_joints_goal_.trajectory.joint_names.resize(2);
                        this->move_joints_goal_.trajectory.joint_names[0]="j_gripper_l";
                        this->move_joints_goal_.trajectory.joint_names[1]="j_gripper_r";
                        this->move_joints_goal_.trajectory.points.resize(1);
                        this->move_joints_goal_.trajectory.points[0].positions.resize(2);
                        this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
                        this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);                           
                        this->move_joints_goal_.trajectory.points[0].positions[0] = -30.0;
                        this->move_joints_goal_.trajectory.points[0].positions[1] = 30.0;                       
                        for(i=0;i<2;i++){
                          this->move_joints_goal_.trajectory.points[0].velocities[i] = 60.0;
                          this->move_joints_goal_.trajectory.points[0].accelerations[i] = 100.0;
                        }
                        this->move_joints_goal_.trajectory.points[0].time_from_start = ros::Duration(1.0);
                        move_jointsMakeActionRequest();
                        if(this->success)
                          this->state=take_connectors;
                        else{
                          this->state=correct_position;
                          sleep(1);
                        }
                        break;

    case take_connectors:       ROS_INFO("Current state: Open gripper");
                                if(this->joints_moving)
                                  this->state=take_connectors;
                                else{
                                  if(this->prev_left_ok){
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";

                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->left_pose_.pose.position.x-0.01;
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->left_pose_.pose.position.y+0.04;
                                    this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->left_pose_.pose.position.z-0.20;

                                    move_left_armMakeActionRequest();
                                  }
                                  if(this->prev_right_ok){
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";

                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->right_pose_.pose.position.x;
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->right_pose_.pose.position.y+0.03;
                                    this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->right_pose_.pose.position.z-0.20;
                                  
                                    move_right_armMakeActionRequest();
                                  }
                                  this->state=take_connectors2;
                                }
                                break;
    
    case take_connectors2:      ROS_INFO("Current state: Moving arms to a previous pose");
                                if((this->right_arm_moving)||(this->left_arm_moving))
                                  this->state=take_connectors2;
                                else{
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";

                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->left_pose_.pose.position.x+0.02;
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->left_pose_.pose.position.y;
                                  this->move_left_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->left_pose_.pose.position.z-0.16;

                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints.resize(1);
                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.seq=0;
                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.stamp=ros::Time::now();
                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id="MP_BODY";

                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.x=this->right_pose_.pose.position.x+0.03;
                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.y=this->right_pose_.pose.position.y+0.02;
                                  this->move_right_arm_goal_.motion_plan_request.goal_constraints.position_constraints[0].position.z=this->right_pose_.pose.position.z-0.18;

                                  move_left_armMakeActionRequest();
                                  move_right_armMakeActionRequest();
                                  this->state=close_gripper;
                                }
                                break;

    case close_gripper: ROS_INFO("Current state: Moving arms to the final pose");
                        if((this->left_arm_moving)||(this->right_arm_moving))
                          this->state=close_gripper;
                        else{                   
                          this->move_joints_goal_.trajectory.header.seq=0;
                          this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                          this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                          this->move_joints_goal_.trajectory.joint_names.resize(2);
                          this->move_joints_goal_.trajectory.joint_names[0]="j_gripper_l";
                          this->move_joints_goal_.trajectory.joint_names[1]="j_gripper_r";
                          this->move_joints_goal_.trajectory.points.resize(1);
                          this->move_joints_goal_.trajectory.points[0].positions.resize(2);
                          this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
                          this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
                          this->move_joints_goal_.trajectory.points[0].positions[0]=45.0;
                          this->move_joints_goal_.trajectory.points[0].positions[1]=-45.0;
                          for(i=0;i<2;i++){
                            this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
                            this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
                          }
                          this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                          move_jointsMakeActionRequest();
                          this->state=done;
                        }
                        break;

    case done:  ROS_INFO("Current state: Close gripper");
                if(this->joints_moving)
                  this->state=done;
                else{
                  ROS_WARN("Current voltage: %f", this->current_voltage);               
                  if(this->current_voltage>0.0)
                    ROS_WARN("Darwin has caught the connectors");
                  else{
                    ROS_WARN("Darwin hasn't caught the connectors");                
                    this->state=done;
                    //this->state=open_gripper;
                    this->success=false;
                  }
                }
                break;

    case correct_position: ROS_INFO("Current state: Correct position");
                           this->Int32_msg_.data=16;
                           this->execute_action_publisher_.publish(this->Int32_msg_);           
                           sleep(2);                               
                           this->ready_for_check=false;                   
                           this->left_done=false;
                           this->first_goal=true;                                                  
                           this->left_data_ready=false;
                           this->right_data_ready=false;                         
                           this->left_ready_tf=false;
                           this->right_ready_tf=false;  
                           this->state=tracking_angle;                     
                           break;

    case tracking_angle:        ROS_INFO("Current state: Tracking angle");
                                this->left_marker_found=false;
                                this->right_marker_found=false;                 
                                this->move_joints_goal_.trajectory.header.seq=0;
                                this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                this->move_joints_goal_.trajectory.joint_names.resize(2);
                                this->move_joints_goal_.trajectory.joint_names[0]="j_tilt";
                                this->move_joints_goal_.trajectory.joint_names[1]="j_pan";
                                this->move_joints_goal_.trajectory.points.resize(1);
                                this->move_joints_goal_.trajectory.points[0].positions.resize(2);
                                this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
                                this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
                                this->move_joints_goal_.trajectory.points[0].positions[0]=15.0-5.0*this->n;
                                this->move_joints_goal_.trajectory.points[0].positions[1]=0.0;
                                for(i=0;i<2;i++){
                                  this->move_joints_goal_.trajectory.points[0].velocities[i]=60.0;
                                  this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
                                }
                                this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                move_jointsMakeActionRequest();
                                if(this->n>=4)
                                  this->state=move_back;
                                if((this->right_pose_.pose.position.z>=0.12)&&(this->left_pose_.pose.position.z<0.12)){
                                  this->left_done=true;
                                  this->state=tracking_right;
                                }else
                                  this->state=tracking_left;
                                this->n++;                              
                                break;

    case tracking_left: ROS_INFO("Current state: Tracking left marker");
                        if(this->left_marker_found){
                          ROS_WARN("Left marker found");
                          move_joints_client_.cancelGoal();
                          this->state=new_goal;
                        }else{
                          if(this->joints_moving)
                            this->state=tracking_left;
                          else{                          
                            this->move_joints_goal_.trajectory.header.seq=0;
                            this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                            this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                            this->move_joints_goal_.trajectory.joint_names.resize(1);
                            this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                            this->move_joints_goal_.trajectory.points.resize(1);
                            this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                            this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                            this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                            this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                            this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                            this->move_joints_goal_.trajectory.points[0].positions[0]=40.0;
                            this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                            move_jointsMakeActionRequest();                          
                            if(this->current_state.position[18]*180.0/3.14159>=39.0)
                              this->state=tracking_angle;
                          }
                        }                        
                        break;

    case tracking_right:        ROS_INFO("Current state: Tracking right marker");
                                if(this->right_marker_found){
                                  ROS_WARN("Right marker found");
                                  move_joints_client_.cancelGoal();
                                  this->state=new_goal;
                                }else{                            
                                  if(this->joints_moving)
                                    this->state=tracking_right;
                                  else{
                                    this->move_joints_goal_.trajectory.header.seq=0;
                                    this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
                                    this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
                                    this->move_joints_goal_.trajectory.joint_names.resize(1);
                                    this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
                                    this->move_joints_goal_.trajectory.points.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].positions.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].velocities.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].accelerations.resize(1);
                                    this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                    this->move_joints_goal_.trajectory.points[0].accelerations[0]=30.0;
                                    this->move_joints_goal_.trajectory.points[0].positions[0]=-40.0;
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    if(this->current_state.position[18]*180.0/3.14159<=-39.0)
                                     this->state=tracking_angle;
                                  }
                                }                        
                                break;

    case tracking:      // The node breaks if it doesn't have a goal when a tracking action is requested.
                        ROS_INFO("Current state: Start tracking");
                        tracking_headMakeActionRequest();
                        sleep(1);
                        this->first_goal=false;
                        this->state=new_goal;                        
                        break;


    case new_goal:      ROS_INFO("Current state: New tracking head goal");
                        if(this->joints_moving)
                          this->state=new_goal;
                        else{
                          this->Float64MultiArray_msg_.data.resize(2);
                          this->Float64MultiArray_msg_.data[0]=(this->current_state.position[18]+atan(this->msg_.markers[0].pose.pose.position.x/this->msg_.markers[0].pose.pose.position.z))*180.0/3.14159;
                          this->Float64MultiArray_msg_.data[1]=(this->current_state.position[19]+atan(this->msg_.markers[0].pose.pose.position.y/this->msg_.markers[0].pose.pose.position.z))*180.0/3.14159;
                          this->tracking_head_goal_.pan_goal=this->Float64MultiArray_msg_.data[0];
                          this->tracking_head_goal_.tilt_goal=this->Float64MultiArray_msg_.data[1];
                          if(((this->Float64MultiArray_msg_.data[0]<=90.0)||(this->Float64MultiArray_msg_.data[0]>=-90.0))&&((this->Float64MultiArray_msg_.data[1]<=60.0)||(this->Float64MultiArray_msg_.data[1]>=-60.0)))
                            this->head_target_publisher_.publish(this->Float64MultiArray_msg_);
                          if(this->first_goal){
                            this->state=tracking;                            
                          }else{                                                  
                            if((this->left_pose_.pose.position.x>=0.13)&&(this->right_pose_.pose.position.x>=0.13))
                              this->state=go_forward;
                            else if((this->left_pose_.pose.position.x<0.13)&&(this->right_pose_.pose.position.x<0.13))
                              this->state=go_back;
                            else if((this->left_pose_.pose.position.x>=0.13)&&(this->right_pose_.pose.position.x<0.13))
                              this->state=go_left;
                            else if((this->right_pose_.pose.position.x>=0.13)&&(this->left_pose_.pose.position.x<0.13))
                              this->state=go_right;
                          }
                        }
                        break;

    case go_forward:    ROS_INFO("Current state: Go forward");
                        this->Twist_msg_.linear.x=0.005;
                        this->Twist_msg_.linear.y=0.0;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.0;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if((this->msg_.markers[0].pose.pose.position.z<=0.10)&&(this->msg_.markers[0].pose.pose.position.z>0.0)){
                          tracking_head_client_.cancelGoal();
                          this->state=initial_pose;
                        }else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;
                          this->Twist_msg_.linear.x=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);                          
                        }
                        break;

    case go_back:       ROS_INFO("Current state: Go back");
                        this->Twist_msg_.linear.x=-0.005;
                        this->Twist_msg_.linear.y=0.0;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.0;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if(this->msg_.markers[0].pose.pose.position.z>=0.10){
                          tracking_head_client_.cancelGoal();
                          this->state=initial_pose;
                        }else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;
                          this->Twist_msg_.linear.x=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);                          
                        }
                        break;

    case turn_right:    ROS_INFO("Current state: Turn to the right");
                        this->Twist_msg_.linear.x=0.0;
                        this->Twist_msg_.linear.y=0.0;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.005;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if((this->msg_.markers[0].pose.pose.position.z<=0.10)&&(this->msg_.markers[0].pose.pose.position.z>0.0)){
                          tracking_head_client_.cancelGoal();
                          this->state=initial_pose;
                        }else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;
                          this->Twist_msg_.angular.z=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                        }
                        break;

    case turn_left:     ROS_INFO("Current state: Turn to the left");
                        this->Twist_msg_.linear.x=0.0;
                        this->Twist_msg_.linear.y=0.0;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=-0.005;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if((this->msg_.markers[0].pose.pose.position.z<=0.10)&&(this->msg_.markers[0].pose.pose.position.z>0.0)){
                          tracking_head_client_.cancelGoal();
                          this->state=initial_pose;
                        }else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;
                          this->Twist_msg_.angular.z=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                        }
                        break;

    case go_right:      ROS_INFO("Current state: Go to the right");
                        this->Twist_msg_.linear.x=0.0;
                        this->Twist_msg_.linear.y=-0.005;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.0;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if((this->current_state.position[18]<=5.0)&&(this->current_state.position[18]>=-5.0))
                          this->state=turn_left;
                        else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;
                          this->Twist_msg_.linear.y=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);                    
                        }
                        break;

    case go_left:       ROS_INFO("Current state: Go to the left");
                        this->Twist_msg_.linear.x=0.0;
                        this->Twist_msg_.linear.y=0.005;
                        this->Twist_msg_.linear.z=0.0;
                        this->Twist_msg_.angular.x=0.0;
                        this->Twist_msg_.angular.y=0.0;
                        this->Twist_msg_.angular.z=0.0;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        if((this->current_state.position[18]<=5.0)&&(this->current_state.position[18]>=-5.0))
                          this->state=turn_right;
                        else
                          this->state=new_goal;
                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                          tracking_head_client_.cancelGoal();
                          this->state=tracking_angle;   
                          this->Twist_msg_.linear.y=0.0;
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);            
                        } 
                        break;

  }
  }catch(...){
    ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!");
  }

}

/*  [subscriber callbacks] */
void ConvertPoseAlgNode::adc_channels_callback(const std_msgs::Float64MultiArray::ConstPtr& msg) 
{ 
  //ROS_INFO("ConvertPoseAlgNode::adc_channels_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->adc_channels_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 
  this->current_voltage=msg->data[2];

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->adc_channels_mutex_.exit(); 
}
void ConvertPoseAlgNode::joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg) 
{ 
  //ROS_INFO("ConvertPoseAlgNode::joint_state_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->joint_state_mutex_.enter(); 
  //std::cout << msg->data << std::endl; 
  this->current_state=(*msg);
  
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->joint_state_mutex_.exit(); 
}
void ConvertPoseAlgNode::marker_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 
  bool tf_exists;
  int x,y,z;

  //ROS_INFO("ConvertPoseAlgNode::marker_callback: New Message Received");
  if((this->ready)&&(!this->ready_for_check)){       
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->marker_mutex_.enter(); 
  this->msg_=(*msg);
  this->msg_.markers.resize(1);
  ROS_INFO("marker dist x, y, z: %f, %f, %f", this->msg_.markers[0].pose.pose.position.x, this->msg_.markers[0].pose.pose.position.y, this->msg_.markers[0].pose.pose.position.z);
  //ROS_INFO("id: %d", this->msg_.markers[0].id);  
  if(this->msg_.markers[0].pose.pose.position.z>0.0){  
    this->conv_msg_.header=this->msg_.markers[0].header;
    this->conv_msg_.pose.position=this->msg_.markers[0].pose.pose.position;
    //std::cout << this->conv_msg_.pose.position.x << "," << this->conv_msg_.pose.position.y << "," << this->conv_msg_.pose.position.z << std::endl;
    this->conv_msg_.pose.orientation=this->msg_.markers[0].pose.pose.orientation;               
    try{
      this->conv_msg_.header.stamp=ros::Time::now();
      // transform the usb_cam frame to the MP_HEAD frame
      tf_exists=tf_listener.waitForTransform(this->conv_msg_.header.frame_id,std::string("/MP_HEAD"),this->conv_msg_.header.stamp, ros::Duration(10), ros::Duration(0.01));
      tf_listener.transformPose(std::string("/MP_HEAD"),this->conv_msg_,this->marker_);                
    }catch(tf::TransformException ex){
      ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());    
    }catch(...){
      ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
    }    
    if((this->msg_.markers[0].id==0)&&(!this->left_done)){
      this->left_marker_found=true;
      this->n=0;
      this->current_pan=this->current_state.position[18]*180.0/3.14159;      
      if(this->left_ready_tf){      
      try{
        // transform the MP_HEAD frame to the MP_BODY frame to move the left arm
        this->marker_.header.stamp=ros::Time::now();    
        tf_exists=tf_listener.waitForTransform(this->marker_.header.frame_id,std::string("/MP_BODY"),this->marker_.header.stamp, ros::Duration(10), ros::Duration(0.01));
        tf_listener.transformPose(std::string("/MP_BODY"),this->marker_,this->left_pose_);

        x=1000*this->left_pose_.pose.position.x;
        y=1000*this->left_pose_.pose.position.y;
        z=1000*this->left_pose_.pose.position.z;
        this->left_pose_.pose.position.x=x/1000.0;
        this->left_pose_.pose.position.y=y/1000.0;
        this->left_pose_.pose.position.z=z/1000.0;

        ROS_WARN_STREAM(this->left_pose_);
                        
        this->left_done=true;   
        this->left_data_ready=true;
        this->left_marker_found=false;  
      }catch(tf::TransformException ex){
        ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
      }catch(...){
        ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
      }           
      }
    }
    if((this->msg_.markers[0].id==1)&&(this->left_done)){               
      this->right_marker_found=true;
      this->n=0;
      this->current_pan=this->current_state.position[18]*180.0/3.14159;
      if(this->right_ready_tf){      
      try{
        // transform the MP_HEAD frame to the MP_BODY frame to move the right arm
        this->marker_.header.stamp=ros::Time::now();    
        tf_exists=tf_listener.waitForTransform(this->marker_.header.frame_id,std::string("/MP_BODY"),this->marker_.header.stamp, ros::Duration(10), ros::Duration(0.01));
        tf_listener.transformPose(std::string("/MP_BODY"),this->marker_,this->right_pose_);

        x=1000*this->right_pose_.pose.position.x;
        y=1000*this->right_pose_.pose.position.y;
        z=1000*this->right_pose_.pose.position.z;
        this->right_pose_.pose.position.x=x/1000.0;
        this->right_pose_.pose.position.y=y/1000.0;
        this->right_pose_.pose.position.z=z/1000.0;
        
        ROS_WARN_STREAM(this->right_pose_);
        
        this->left_done=false;  
        this->right_data_ready=true;
        this->right_marker_found=false;
      }catch(tf::TransformException ex){
        ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
      }catch(...){
        ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
      }
      }     
    }    
  }  
    //unlock previously blocked shared variables 
    //this->alg_.unlock();     
    this->marker_mutex_.exit();  
  }
}
/*  [service callbacks] */

/*  [action callbacks] */
void ConvertPoseAlgNode::tracking_headDone(const actionlib::SimpleClientGoalState& state,  const iri_darwin_robot::tracking_headResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ConvertPoseAlgNode::tracking_headDone: Goal Achieved!"); 
  else 
    ROS_INFO("ConvertPoseAlgNode::tracking_headDone: %s", state.toString().c_str()); 

  //copy & work with requested result   
} 

void ConvertPoseAlgNode::tracking_headActive() 
{ 
  //ROS_INFO("ConvertPoseAlgNode::tracking_headActive: Goal just went active!"); 
} 

void ConvertPoseAlgNode::tracking_headFeedback(const iri_darwin_robot::tracking_headFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ConvertPoseAlgNode::tracking_headFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //feedback_is_ok=false; 

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    tracking_head_client_.cancelGoal(); 
    //ROS_INFO("ConvertPoseAlgNode::tracking_headFeedback: Cancelling Action!"); 
  } 
}
void ConvertPoseAlgNode::move_jointsDone(const actionlib::SimpleClientGoalState& state,  const control_msgs::FollowJointTrajectoryResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ConvertPoseAlgNode::move_jointsDone: Goal Achieved!"); 
  else 
    ROS_INFO("ConvertPoseAlgNode::move_jointsDone: %s", state.toString().c_str()); 

  //copy & work with requested result
  this->joints_moving=false;  
} 

void ConvertPoseAlgNode::move_jointsActive() 
{ 
  //ROS_INFO("ConvertPoseAlgNode::move_jointsActive: Goal just went active!"); 
} 

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

  bool feedback_is_ok = true; 

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

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  {
    move_joints_client_.cancelGoal(); 
    //ROS_INFO("ConvertPoseAlgNode::move_jointsFeedback: Cancelling Action!"); 
  } 
}
void ConvertPoseAlgNode::move_right_armDone(const actionlib::SimpleClientGoalState& state,  const arm_navigation_msgs::MoveArmResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 )
    ROS_INFO("ConvertPoseAlgNode::move_right_armDone: Goal Achieved!"); 
  else 
    ROS_INFO("ConvertPoseAlgNode::move_right_armDone: %s", state.toString().c_str()); 

  //copy & work with requested result
  this->right_arm_moving=false;  
} 

void ConvertPoseAlgNode::move_right_armActive() 
{ 
  //ROS_INFO("ConvertPoseAlgNode::move_right_armActive: Goal just went active!"); 
} 

void ConvertPoseAlgNode::move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ConvertPoseAlgNode::move_right_armFeedback: 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 ) 
  { 
    move_right_arm_client_.cancelGoal(); 
    //ROS_INFO("ConvertPoseAlgNode::move_right_armFeedback: Cancelling Action!"); 
  } 
}

void ConvertPoseAlgNode::move_left_armDone(const actionlib::SimpleClientGoalState& state,  const arm_navigation_msgs::MoveArmResultConstPtr& result)
{
  if( state.toString().compare("SUCCEEDED") == 0 )
    ROS_INFO("ConvertPoseAlgNode::move_left_armDone: Goal Achieved!");
  else
    ROS_INFO("ConvertPoseAlgNode::move_left_armDone: %s", state.toString().c_str());

  //copy & work with requested result
  this->left_arm_moving=false;  
}

void ConvertPoseAlgNode::move_left_armActive()
{
  //ROS_INFO("ConvertPoseAlgNode::move_left_armActive: Goal just went active!"); 
}

void ConvertPoseAlgNode::move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback)
{
  //ROS_INFO("ConvertPoseAlgNode::move_left_armFeedback: 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 )
  {
    move_left_arm_client_.cancelGoal();
    //ROS_INFO("ConvertPoseAlgNode::move_left_armFeedback: Cancelling Action!"); 
  }
}

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

  //wait for the action server to start 
  //will wait for infinite time 
  tracking_head_client_.waitForServer();  
  ROS_INFO("ConvertPoseAlgNode::tracking_headMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //tracking_head_goal_.data = my_desired_goal; 
  tracking_head_client_.sendGoal(tracking_head_goal_, 
              boost::bind(&ConvertPoseAlgNode::tracking_headDone,     this, _1, _2), 
              boost::bind(&ConvertPoseAlgNode::tracking_headActive,   this), 
              boost::bind(&ConvertPoseAlgNode::tracking_headFeedback, this, _1)); 
  ROS_INFO("ConvertPoseAlgNode::tracking_headMakeActionRequest: Goal Sent. Wait for Result!");     
}
void ConvertPoseAlgNode::move_jointsMakeActionRequest() 
{ 
  ROS_INFO("ConvertPoseAlgNode::move_jointsMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  move_joints_client_.waitForServer();  
  ROS_INFO("ConvertPoseAlgNode::move_jointsMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //move_joints_goal_.data = my_desired_goal; 
  move_joints_client_.sendGoal(move_joints_goal_, 
              boost::bind(&ConvertPoseAlgNode::move_jointsDone,     this, _1, _2), 
              boost::bind(&ConvertPoseAlgNode::move_jointsActive,   this), 
              boost::bind(&ConvertPoseAlgNode::move_jointsFeedback, this, _1)); 
  ROS_INFO("ConvertPoseAlgNode::move_jointsMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->joints_moving=true;  
}
void ConvertPoseAlgNode::move_right_armMakeActionRequest() 
{
  ROS_INFO("ConvertPoseAlgNode::move_right_armMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  move_right_arm_client_.waitForServer();  
  ROS_INFO("ConvertPoseAlgNode::move_right_armMakeActionRequest: Server is Available!"); 

  //send a goal to the action      
  //move_right_arm_goal_.data = my_desired_goal; 
  move_right_arm_client_.sendGoal(move_right_arm_goal_, 
              boost::bind(&ConvertPoseAlgNode::move_right_armDone,     this, _1, _2), 
              boost::bind(&ConvertPoseAlgNode::move_right_armActive,   this), 
              boost::bind(&ConvertPoseAlgNode::move_right_armFeedback, this, _1)); 
  ROS_INFO("ConvertPoseAlgNode::move_right_armMakeActionRequest: Goal Sent. Wait for Result!");   
  this->right_arm_moving=true;  
}
void ConvertPoseAlgNode::move_left_armMakeActionRequest()
{
  ROS_INFO("ConvertPoseAlgNode::move_left_armMakeActionRequest: Starting New Request!");

  //wait for the action server to start 
  //will wait for infinite time 
  move_left_arm_client_.waitForServer();
  ROS_INFO("ConvertPoseAlgNode::move_left_armMakeActionRequest: Server is Available!");

  //send a goal to the action      
  //move_left_arm_goal_.data = my_desired_goal; 
  move_left_arm_client_.sendGoal(move_left_arm_goal_,
              boost::bind(&ConvertPoseAlgNode::move_left_armDone,     this, _1, _2),
              boost::bind(&ConvertPoseAlgNode::move_left_armActive,   this),
              boost::bind(&ConvertPoseAlgNode::move_left_armFeedback, this, _1));
  ROS_INFO("ConvertPoseAlgNode::move_left_armMakeActionRequest: Goal Sent. Wait for Result!");    
  this->left_arm_moving=true;  
}

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

  this->alg_.unlock();
}

void ConvertPoseAlgNode::addNodeDiagnostics(void)
{
}

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