darwin_autocharge_alg_node.cpp

Go to the documentation of this file.

/*
This node is an algorithm that allows Darwin-OP robot to charge itself. To work properly, the robot has to be in front of the station terminals with any orientation.
The node needs the stack ccny_vision that contains the package ar_pose that is used to detect the markers located on the charge station.  
*/
#include "darwin_autocharge_alg_node.h"

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

  // [init publishers]
  this->execute_action_publisher_ = this->public_node_handle_.advertise<std_msgs::Int32>("execute_action", 100);
  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);
    
 
  // [init subscribers]
  this->adc_channels_subscriber_ = this->public_node_handle_.subscribe("adc_channels", 100, &DarwinAutochargeAlgNode::adc_channels_callback, this);
  this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 100, &DarwinAutochargeAlgNode::joint_states_callback, this);
  this->marker_subscriber_ = this->public_node_handle_.subscribe("marker", 100, &DarwinAutochargeAlgNode::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]
  sleep(10);
  this->n=0; // Counter used to modify the head tilt.
  this->k=-1; // Counter used to modify the head pan.   
  this->m=0; // Counter used to filter markers wrong detections.  
  this->p=0; // Counter used to know if the maximum time to transform the position of a marker has been elapsed. 
  this->current_pan=0.0;
  this->current_tilt=0.0;
  this->marker_found=false;  
  this->marker_really_founded=false;
  this->ready=false; // False if the robot is not ready to receive information of the percepcion node yet.  
  this->close_to_station=false; // True if robot is close to the charge station.
  this->joints_moving=false;
  this->right_arm_moving=false;
  this->left_arm_moving=false;
  this->head_is_tracking=false;
  this->ready_for_check=false;  // False if the robot is not ready to check if the points have inverse kinematics solution yet.
  this->left_ok=false; // False if the final left position doesn't have inverse kinematics solution.
  this->right_ok=false; // False if the final right position doesn't have inverse kinematics solution.
  this->prev_left_ok=false; // False if the previous left position doesn't have inverse kinematics solution.
  this->prev_right_ok=false; // False if the previous right position doesn't have inverse kinematics solution.
  this->left_done=false; // True if the left marker has been detected and its position has been saved, therefore the robot can search the right marker. 
  this->left_marker_found=false;
  this->right_marker_found=false;
  this->first_goal=true; // True if the position of the marker received for tracking it is the first, and the tracking action service needs to be activated. 
  this->left_data_ready=false; // True if the position of the left marker has been saved.
  this->right_data_ready=false; // True if the position of the right marker has been saved.
  this->left_ready_tf=false; // True if the position of the left marker can be transformed respect to the frame /MP_BODY.
  this->right_ready_tf=false; // True if the position of the right marker can be transformed respect to the frame /MP_BODY.
  this->success=true; // True if electric contact exist when the grippers have been closed. 

  this->state=searching;
  //this->state=initial_pose;
}

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

void DarwinAutochargeAlgNode::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 searching:   ROS_INFO("Current state: Searching marker");
                                                                                        if(this->marker_found){
                                                                                          move_joints_client_.cancelGoal();
                                                                                          ROS_WARN("Marker founded");                                                                                     
                              this->state=new_goal;
                        }else{
                                                                                          if((this->joints_moving)||(this->head_is_tracking))
                                                                                            this->state=searching;
                                                                                          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_pan";
                          this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
                          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);
                                                                                            if(this->n==0)
                                  this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;         
                                                                                            else
                                                                                              this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_state.position[18]*180.0/3.14159;
                          this->move_joints_goal_.trajectory.points[0].positions[1]=60.0-15.0*this->n;
                          for(i=0;i<2;i++)
                          {
                            this->move_joints_goal_.trajectory.points[0].velocities[i]=30.0;
                            this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
                          }
                          this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                                                    move_jointsMakeActionRequest();
                                                                                        if(this->n>3){  // Tilt limit exceeded
                                                                                                                // If the last distance obtained is less than 0.2m the robot goes back, if not the robot rotates on itself. 
                                if((this->conv_msg_.pose.position.z>0.0)&&(this->conv_msg_.pose.position.z<=0.2)) 
                                                                                                                        this->state=move_back;
                                                                                              else  
                                                                                                        this->state=rotate;
                                                                                            }else
                                                                                              this->state=searching2;                                               
                                                                                          }       
                                                                                        }      
                      break;

      case searching2:  ROS_INFO("Current state: Searching marker");
                                                                                                if(this->marker_found){
                                                                                                  move_joints_client_.cancelGoal();
                                                                                                  ROS_WARN("Marker founded");                                                                                             
                          this->state=new_goal;
                        }else{
                                                                                                  if(this->joints_moving)
                            this->state=searching2;
                          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]=30.0;
                            this->move_joints_goal_.trajectory.points[0].accelerations[0]=60.0;
                                                                                                    if(this->k==-1){  
                              this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
                                                                                                      this->k=1;
                                                                                                    }else{
                              this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
                              this->k=-1;
                            }
                                                                                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                            move_jointsMakeActionRequest();
                                                                                                    this->n++;
                                                                                                    this->state=searching;                                                  
                                                                                                  }                                      
                                                                                                }                       
                        break;

      case rotate:      ROS_INFO("Current state: Rotating");                    
                                                                                if(this->joints_moving)
                      this->state=rotate;
                    else{
                                                                                  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=1.05;
                      this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                  sleep(5);
                                                                                  this->Twist_msg_.angular.z=0.0;
                      this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                        // Init search's counters.
                                                                                  this->n=0;
                                                                                  this->k=-1;
                                                                                  this->state=searching;
                                                                                }
                                                                                break;

      case new_goal:    ROS_INFO("Current state: New goal");
                                                                                        if(this->joints_moving)
                        this->state=new_goal;
                      else{
                                                                                                // Send head tilt and head pan goals to the tracking action server.
                                                                                          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;         if((fabs(this->Float64MultiArray_msg_.data[0])<=60.0)&&(fabs(this->Float64MultiArray_msg_.data[1])<=40.0))
                                                                                            ROS_WARN("target pan and tilt: %f, %f", Float64MultiArray_msg_.data[0], Float64MultiArray_msg_.data[1]);
                                                                                          else{
                                                                                            this->Float64MultiArray_msg_.data[0]=this->current_state.position[18]*180.0/3.14159;
                          this->Float64MultiArray_msg_.data[1]=this->current_state.position[19]*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];
                                                                                                this->head_target_publisher_.publish(this->Float64MultiArray_msg_);
                                                                                                // If first goal start tracking
                        if(this->first_goal)
                          this->state=tracking;                   
                        else{
                                                                                                        // If the robot is close to the station corrects its position, if not the robot approaches to the station. 
                                                                                                  if(this->close_to_station){           
                                                                                                                // If current pan is greater than 35 degrees the robot needs to go to the left. If the current pan is smaller than -35 degrees the robot needs to go to the right.           
                                                                                                    if(fabs(this->current_pan)>=35.0){                                                                                                                  
                                                                                                                if(this->current_pan>0.0)
                                                                                                                          this->state=go_left;
                                                                                                                        else
                                                                                                                          this->state=go_right;
                                                                                                    }else{
                                                                                                      if((this->left_pose_.pose.position.x>=0.11)&&(this->right_pose_.pose.position.x>=0.11))
                                                                                                                          this->state=go_forward;
                              else if((this->left_pose_.pose.position.x<0.11)&&(this->right_pose_.pose.position.x<0.11))
                                this->state=go_back;
                              else if((this->left_pose_.pose.position.x>=0.11)&&(this->right_pose_.pose.position.x<0.11))
                                this->state=go_left;
                              else if((this->right_pose_.pose.position.x>=0.11)&&(this->left_pose_.pose.position.x<0.11))
                                this->state=go_right;                       
                                                                                                    }
                                                                                        }else
                                                                                                    this->state=walking;                        
                                                                                          }
                                                                                        }
                                                                                        break;

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

      case walking:                     ROS_INFO("Current state: Walking");
                                                                                                // The robot sits down if it is inside the solution zone, if not it keeps walking.                      
                                                                                                if((this->conv_msg_.pose.position.z<=0.15)&&(this->conv_msg_.pose.position.z>0.0)){
                          tracking_head_client_.cancelGoal();
                                                                                                  this->first_goal=true;
                                                                                                        this->state=initial_pose;
                        }else{
                          this->state=new_goal;
                                                                                                  this->Twist_msg_.linear.x=0.03;
                          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.8*this->current_state.position[18];
                          this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                                        // If robot loses the marker the tracking and walking actions are canceled and the robot starts the search algorithm.           
                          if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                     
                            this->marker_found=false;
                                                                                                    this->marker_really_founded=false;
                                                                                                    this->first_goal=true;
                                                                                                                // Init search's counters.
                            this->n=0;
                                                                                                    this->k=-1;
                            this->Twist_msg_.linear.x=0.0;
                            this->Twist_msg_.angular.z=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=searching;                          
                                                                                                  }
                        }
                                                                                                break;

      case initial_pose:        ROS_INFO("Current state: Initial pose");
                                                                                                        if((this->joints_moving)||(this->head_is_tracking))
                                                                                                        this->state=initial_pose;
                                                                                                        else{
                                                                                                                // Init parameters
                                                                                                this->success=true;
                            this->ready=false;
                            this->left_done=false;
                            this->left_marker_found=false;
                            this->right_marker_found=false;
                                                                                                        this->close_to_station=true;
                                                                                                        this->n=0;
                                  this->k=-1;
                                                                                                                // The robot sits down.                         
                                  this->Int32_msg_.data=15;
                                  this->execute_action_publisher_.publish(this->Int32_msg_);
                                                                                                                // The robot moves its arms to a initial position.
                                                                                                        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(4);
                                  this->move_joints_goal_.trajectory.joint_names[0]="j_shoulder_r";
                                  this->move_joints_goal_.trajectory.joint_names[1]="j_shoulder_l";
                                  this->move_joints_goal_.trajectory.joint_names[2]="j_low_arm_r";
                                  this->move_joints_goal_.trajectory.joint_names[3]="j_low_arm_l";
                                  this->move_joints_goal_.trajectory.points.resize(1);
                                  this->move_joints_goal_.trajectory.points[0].positions.resize(4);
                                  this->move_joints_goal_.trajectory.points[0].velocities.resize(4);
                                  this->move_joints_goal_.trajectory.points[0].accelerations.resize(4);
                                  this->move_joints_goal_.trajectory.points[0].positions[0]=-30.0;
                                  this->move_joints_goal_.trajectory.points[0].positions[1]=35.0;
                                  this->move_joints_goal_.trajectory.points[0].positions[2]=20.0;
                                  this->move_joints_goal_.trajectory.points[0].positions[3]=-20.0;
                                  for(i=0;i<4;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=search_left_marker;
                                                                                                        }
                                break;

      case search_left_marker:  ROS_INFO("Current state: Searching left marker");
                                if(this->left_marker_found){
                                  ROS_WARN("Left marker founded");
                                  move_joints_client_.cancelGoal();
                                  this->state=transform_left_pose;
                                }else{
                                  if(this->joints_moving)
                                    this->state=search_left_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(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;                           
                                    if(this->n==0)
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=60.0;
                                    else
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
                                    for(i=0;i<2;i++){
                                      this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
                                      this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();                              
                                    if(this->n>3){ // Tilt limit exceeded
                                      move_joints_client_.cancelGoal();
                                      this->state=move_back;
                                    }else
                                      this->state=search_left_marker2;
                                  }
                                }
                                break;

      case search_left_marker2: ROS_INFO("Current state: Searching left marker");                
                                if(this->left_marker_found){
                                  ROS_WARN("Left marker founded");
                                  move_joints_client_.cancelGoal();
                                  this->state=transform_left_pose;
                                }else{
                                  if(this->joints_moving)
                                    this->state=search_left_marker2;
                                  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]=40.0;
                                    if(this->k==-1){
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
                                      this->k=1;
                                    }else{
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
                                      this->k=-1;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    this->n++;
                                    this->state=search_left_marker;                              
                                  }
                                }
                                break;

      case search_right_marker: ROS_INFO("Current state: Searching right marker");
                                if(this->right_marker_found){
                                  ROS_WARN("Right marker founded");
                                  move_joints_client_.cancelGoal();
                                  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(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;
                                    if(this->n==0)
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=-60.0;
                                    else
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
                                    for(i=0;i<2;i++){
                                      this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
                                      this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    if(this->n>3){ // Tilt limit exceeded
                                      move_joints_client_.cancelGoal();
                                      this->state=move_back;
                                    }else
                                      this->state=search_right_marker2;
                                  }
                                }
                                break;

      case search_right_marker2:        ROS_INFO("Current state: Searching right marker");
                                        if(this->right_marker_found){
                                          ROS_WARN("Right marker founded");
                                          move_joints_client_.cancelGoal();
                                          this->state=transform_right_pose;
                                        }else{
                                          if(this->joints_moving)
                                            this->state=search_right_marker2;
                                          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);
                                            if(this->k==-1){
                                              this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
                                              this->k=1;
                                            }else{
                                              this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
                                              this->k=-1;
                                            }
                                            this->move_joints_goal_.trajectory.points[0].velocities[0]=20.0;
                                            this->move_joints_goal_.trajectory.points[0].accelerations[0]=40.0;
                                            this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                            move_jointsMakeActionRequest();
                                            this->n++;                                      
                                            this->state=search_right_marker;                             
                                          }
                                        }
                                        break;
   
      case move_back:           ROS_INFO("Current state: Going back");
                                                                                                // The robot stands up.
                        this->Int32_msg_.data=9;
                        this->execute_action_publisher_.publish(this->Int32_msg_);
                        sleep(2);
                                                                                                // The robot goes back during a time of 500ms.
                        this->Twist_msg_.linear.x=-0.03;
                        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_);
                        usleep(500000);
                                                                                                this->Twist_msg_.linear.x=0.0;
                                                                                                this->cmd_vel_publisher_.publish(this->Twist_msg_);
                        this->state=initial_pose;
                        break;

      case transform_left_pose: ROS_INFO("Current state: Transforming left pose");
                                if(this->joints_moving)
                                  this->state=transform_left_pose;
                                else{
                                                                                                                                        // The robot moves its head to the position where the marker was detected.
                                  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_pan";
                                  this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
                                  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);
                                  for(i=0;i<2;i++){
                                    this->move_joints_goal_.trajectory.points[0].velocities[i]=40.0;
                                    this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
                                  }
                                  this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;
                                  this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_tilt;
                                  this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                  move_jointsMakeActionRequest();
                                                                                                                                        // When the left pose is transformed the robot can search the right marker. If maximum waiting time is exceeded the robot searches the left marker again.                                 
                                                                                                                                if(this->left_data_ready){
                                    ROS_WARN("Left pose transformed");
                                    this->n=0;
                                    this->k=1;                            
                                    this->state=search_right_marker;
                                  }else{
                                                                                                                                if(this->p>2){ // Maximum waiting time exceeded.                                                                                                                                                                                                                                                        
                                                                                                                                                        this->p=0;
                                                                                                                                                        // Init search's counters for left marker detection.
                                                                                                                                this->n=0;
                                      this->k=-1;
                                                                                                                                this->state=search_left_marker;
                                                                                                                                }else
                                      this->state=transform_left_pose;
                                                                                                                                }
                                                                                                                                this->left_ready_tf=true;      
                                                                                                                                this->p++;                    
                                }
                                break;

      case transform_right_pose:  ROS_INFO("Current state: Transforming right pose");              
                                        if(this->joints_moving)
                                          this->state=transform_right_pose;
                                        else{
                                                                                                                                                // The robot moves its head to the position where the marker was detected.
                                          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_pan";
                                          this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
                                          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);
                                          for(i=0;i<2;i++){
                                            this->move_joints_goal_.trajectory.points[0].velocities[i]=40.0;
                                            this->move_joints_goal_.trajectory.points[0].accelerations[i]=60.0;
                                          }
                                          this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_pan;
                                          this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_tilt;
                                          this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                          move_jointsMakeActionRequest();
                                                                                                                                                // When the left pose is transformed the robot can search the right marker. If maximum waiting time is exceeded the robot searches the left marker again.
                                                                                                                                        if(this->right_data_ready){
                                            ROS_WARN("Right pose transformed");
                                            this->ready_for_check=true;                                   
                                            this->state=make_decision;
                                          }else{
                                                                                                                                        if(this->p>2){ // Maximum waiting time exceeded.
                                                                                                                                        this->p=0;
                                                                                                                                                                // Init search's counters for right marker detection.
                                              this->n=0;
                                        this->k=1;
                                        this->state=search_right_marker;
                                                                                                                                        }else
                                              this->state=transform_right_pose;
                                                                                                                                        }
                                          this->right_ready_tf=true;
                                                                                                                                        this->p++;
                                        }
                                        break;
   
      case make_decision:       ROS_INFO("Current state: Looking if the final pose has inverse kinematics solution");
                                                                                                        if(this->joints_moving)
                                                                                                        this->state=make_decision;
                                                                                                        else{
                                                                                                        // Look if the final pose has inverse kinematics solution and act in consequence.         
                                  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_states_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_states_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.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->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->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_states_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_states_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 has inverse kinematics solution");
                                // Look if the previous pose has inverse kinematics solution and act in consequence.                        
                                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_states_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_states_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_states_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_states_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();
                                                                                                        // Open grippers because the robot needs to correct its position or it can move its arms for take the station's terminals. 
                                if(this->success)
                                  this->state=open_gripper2;
                                else
                                  this->state=correct_position;
                                break;

      case open_gripper2:                               ROS_INFO("Current state: Open gripper");
                                if(this->joints_moving)
                                  this->state=open_gripper2;
                                else{
                                                                                                                                        // If the previous left pose has inverse kinematics solution, send the position to the arm navigation node.
                                  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 the previous right pose has inverse kinematics solution, send the position to the arm navigation node.
                                  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_connectors;
                                }
                                break;

      case take_connectors:     ROS_INFO("Current state: Moving arms to a previous pose");
                                if((this->right_arm_moving)||(this->left_arm_moving))
                                  this->state=take_connectors;
                                else{
                                                                                                                                        // Send the positions of the final poses to the arm navigation node.
                                  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.18;

                                  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=take_connectors2;
                                }
                                break;

      case take_connectors2:    ROS_INFO("Current state: Moving arms to the final pose");
                                                if((this->left_arm_moving)||(this->right_arm_moving))
                                                this->state=take_connectors2;
                                                else{
                                                                                                                                // When arms will reach the final pose start to close the grippers.
                                                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=close_gripper;
                                                        }
                                                        break;

      case close_gripper: ROS_INFO("Current state: Close gripper");
                                                                    if(this->joints_moving)
                                                                    this->state=close_gripper;
                                                                  else{
                                                                      ROS_WARN("Current voltage: %f", this->current_voltage);
                                                                                                                sleep(2);
                                                                                                                // If voltage is positive the robot has caught the connectors, if not the robot needs to correct its position.
                                                                    if(this->current_voltage>0.0)
                                                                      ROS_WARN("Darwin has caught the connectors");
                                                                      else{
                                                                      ROS_WARN("Darwin hasn't caught the connectors");
                                                                      //this->state=close_gripper;
                                                                        this->state=open_gripper;
                                                                      this->success=false;
                                                                    }
                                                                    }
                                                                  break;

      case correct_position:    ROS_INFO("Current state: Correct position");
                                                                                                                        if((this->joints_moving)||(this->head_is_tracking))
                                                                                                                                this->state=correct_position;
                                                                                                                        else{
                                                                                                                                // Stand up for correct the position.
                                                                                                                                this->Int32_msg_.data=9;
                                                              this->execute_action_publisher_.publish(this->Int32_msg_);
                                                          sleep(1);
                                                                                                                                // Init parameters
                                                          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->left_marker_found=false;
                                                        this->right_marker_found=false;
                                                                                                                                this->n=0;
                                                                                                                                // If the right terminal is farther than the left terminal the robot track the right marker, otherwise the robot track the left marker.
                                                        if(this->right_pose_.pose.position.x>this->left_pose_.pose.position.x){
                                                          this->left_done=true;
                                                          this->k=1;
                                                          this->state=tracking_right;
                                                          }else{
                                                          this->k=-1;
                                                            this->state=tracking_left;
                                                              }
                                                                                                                        }
                                                        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(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-10.0*this->n;
                                    if(this->n==0)
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=60.0;
                                    else
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
                                    for(i=0;i<2;i++){
                                      this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
                                      this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    if(this->n>2) // Tilt limit exceeded.
                                      this->state=move_back;
                                    else
                                      this->state=tracking_left2;                                        
                                  }
                                }
                                break;

      case tracking_left2:      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_left2;
                                  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]=40.0;
                                    if(this->k==-1){
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
                                      this->k=1;
                                    }else{
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
                                      this->k=-1;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    this->n++;
                                    this->state=tracking_left;
                                  }
                                }
                                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(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-10.0*this->n;
                                    if(this->n==0)
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=-60.0;
                                    else
                                      this->move_joints_goal_.trajectory.points[0].positions[1]=this->current_state.position[18]*180.0/3.14159;
                                    for(i=0;i<2;i++){
                                      this->move_joints_goal_.trajectory.points[0].velocities[i]=20.0;
                                      this->move_joints_goal_.trajectory.points[0].accelerations[i]=40.0;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    if(this->n>2) // Tilt limit exceeded.
                                      this->state=move_back;
                                    else
                                      this->state=tracking_right2;                               
                                  }
                                }
                                break;

      case tracking_right2:     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_right2;
                                  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]=40.0;
                                    if(this->k==-1){
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=-60.0;
                                      this->k=1;
                                    }else{
                                      this->move_joints_goal_.trajectory.points[0].positions[0]=60.0;
                                      this->k=-1;
                                    }
                                    this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(1.0);
                                    move_jointsMakeActionRequest();
                                    this->n++;
                                    this->state=tracking_right;
                                  }
                                }
                                break;

      case go_forward:  ROS_INFO("Current state: Go forward");
                                                                                                this->Twist_msg_.linear.x=0.03;
                        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.4*this->current_state.position[18];
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                                                                                                usleep(100000);
                                                                                                // Sit down if marker position is smaller than 12cm.
                                                                                                if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.12)){
                          tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                    
                          this->state=initial_pose;
                        }else{                          
                                                                                                        // If the robot lost the marker it cancel the tracking process and start the search process again.
                          if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                     
                            this->Twist_msg_.linear.x=0.0;
                                                                                                this->Twist_msg_.angular.z=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                                                                                                                this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }                        
                        break;

      case go_back:     ROS_INFO("Current state: Go back");
                                                                                                this->Twist_msg_.linear.x=-0.03;
                        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.4*this->current_state.position[18];
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                                usleep(100000);
                                                                                                // Sit down if marker position is greater than 9cm.
                                                                                                if(this->msg_.markers[0].pose.pose.position.z>0.09){
                          tracking_head_client_.cancelGoal();
                                                                                                this->first_goal=true;                    
                          this->state=initial_pose;
                        }else{  
                                                                                                        // If the robot lost the marker, cancel the tracking process and start the search process again.                        
                                                                                        if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();
                                                                                                this->first_goal=true;                     
                            this->Twist_msg_.linear.x=0.0;
                                                                                                this->Twist_msg_.angular.z=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }
                        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.65;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                                usleep(100000);
                                                                                                // Sit down if marker position is smaller than 10cm.
                                                                                                if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.10)){
                          tracking_head_client_.cancelGoal();                                             
                                                                                                this->first_goal=true;                    
                          this->state=initial_pose;
                        }else{        
                                                                                                        // If the robot lost the marker, cancel the tracking process and start the search process again.                  
                          if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                   
                                                                                                this->first_goal=true;                             
                            this->Twist_msg_.angular.z=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }
                        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.65;
                        this->cmd_vel_publisher_.publish(this->Twist_msg_);
                                                                                                usleep(100000);
                                                                                                // Sit down if marker position is smaller than 10cm.
                                                                                                if((this->msg_.markers[0].pose.pose.position.z>0.0)&&(this->msg_.markers[0].pose.pose.position.z<=0.10)){
                          tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                    
                          this->state=initial_pose;
                        }else{     
                                                                                                        // If the robot lost the marker, cancel the tracking process and start the search process again.                     
                          if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                     
                            this->Twist_msg_.angular.z=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }
                        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.04;
                        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_);
                                                                                                usleep(400000); 
                                                                                                // Stop when head pan will be between -30 and 30 degrees.       
                                                                                                if(fabs(this->current_state.position[18])<=0.52)
                          this->state=turn_left;
                        else{    
                                                                                                        // If the robot lost the marker, cancel the tracking process and start the search process again.                      
                                                                                                if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                     
                            this->Twist_msg_.linear.y=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }
                        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.04;
                        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_);
                                                                                                usleep(400000);
                                                                                                //Stop when head pan will be between -30 and 30 degrees.                
                                                                                                if(fabs(this->current_state.position[18])<=0.52)
                          this->state=turn_right;
                        else{  
                                                                                                        // If the robot lost the marker, cancel the tracking process and start the search process again.                        
                          if(this->msg_.markers[0].pose.pose.position.z==0.0){
                            tracking_head_client_.cancelGoal();                     
                                                                                                this->first_goal=true;                      
                            this->Twist_msg_.linear.y=0.0;
                            this->cmd_vel_publisher_.publish(this->Twist_msg_);                     
                            this->state=correct_position;
                                                                                                        }else
                                                                                                                this->state=new_goal;
                        }
                        break;
    }
  }catch(...){
    ROS_INFO("exception!!!!!!!!!!!!!!!!!!!!!1");
  }
}

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

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->adc_channels_mutex_.enter(); 
  this->current_voltage=msg->data[2];
  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->adc_channels_mutex_.exit(); 
}
void DarwinAutochargeAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg) 
{ 
  //ROS_INFO("DarwinAutochargeAlgNode::joint_states_callback: Current Angles"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->joint_states_mutex_.enter(); 
  this->current_state=(*msg); 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->joint_states_mutex_.exit(); 
}
void DarwinAutochargeAlgNode::marker_callback(const ar_pose::ARMarkers::ConstPtr& msg) 
{ 
  bool tf_exists;
  int x,y,z;

  //ROS_INFO("DarwinAutochargeAlgNode::marker_callback: New Message Received"); 
 
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock();   
  if((this->ready)&&(!this->ready_for_check)){
    this->marker_mutex_.enter();
    this->msg_ = *msg;
    this->msg_.markers.resize(1);
                // If the distance on z is negative rotate the axes to get the correct information.
    if(this->msg_.markers[0].pose.pose.position.z<0.0){
      this->msg_.markers[0].pose.pose.position.x=-this->msg_.markers[0].pose.pose.position.x;
      this->msg_.markers[0].pose.pose.position.y=-this->msg_.markers[0].pose.pose.position.y;
      this->msg_.markers[0].pose.pose.position.z=-this->msg_.markers[0].pose.pose.position.z;
    }  
    ROS_INFO("marker dist x, y, z; id: %f, %f, %f, %d", this->msg_.markers[0].pose.pose.position.x, this->msg_.markers[0].pose.pose.position.y, this->msg_.markers[0].pose.pose.position.z, this->msg_.markers[0].id);
                // If a marker position on z greater than 0cm is received, the robot has found a marker.
    if(this->msg_.markers[0].pose.pose.position.z>0.0){
      this->marker_found=true;
                        // Filtering the information to know if the object that the robot is detecting is really a marker.
      if(this->m>2){      
        this->marker_really_founded=true;
        this->conv_msg_.header=this->msg_.markers[0].header;
        this->conv_msg_.pose.position=this->msg_.markers[0].pose.pose.position;
        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 the id of the marker is 0 the robot has found the left marker.      
        if((this->msg_.markers[0].id==0)&&(!this->left_done)){
          this->left_marker_found=true;                
          this->current_pan=this->current_state.position[18]*180.0/3.14159;
          this->current_tilt=this->current_state.position[19]*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 the id of the marker is 1 the robot has found the right marker.
        if((this->msg_.markers[0].id==1)&&(this->left_done)){
          this->right_marker_found=true;
          this->current_pan=this->current_state.position[18]*180.0/3.14159;
                                this->current_tilt=this->current_state.position[19]*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!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
            }
          }
        }
        this->m=0;
      }
      this->m++;
    }
    //unlock previously blocked shared variables 
    //this->alg_.unlock(); 
    this->marker_mutex_.exit(); 
  }
}

/*  [service callbacks] */

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

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

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

void DarwinAutochargeAlgNode::move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DarwinAutochargeAlgNode::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("DarwinAutochargeAlgNode::move_left_armFeedback: Cancelling Action!"); 
  } 
}
void DarwinAutochargeAlgNode::move_right_armDone(const actionlib::SimpleClientGoalState& state,  const arm_navigation_msgs::MoveArmResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DarwinAutochargeAlgNode::move_right_armDone: Goal Achieved!"); 
  else 
    ROS_INFO("DarwinAutochargeAlgNode::move_right_armDone: %s", state.toString().c_str()); 

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

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

void DarwinAutochargeAlgNode::move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DarwinAutochargeAlgNode::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("DarwinAutochargeAlgNode::move_right_armFeedback: Cancelling Action!"); 
  } 
}
void DarwinAutochargeAlgNode::tracking_headDone(const actionlib::SimpleClientGoalState& state,  const iri_darwin_robot::tracking_headResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("DarwinAutochargeAlgNode::tracking_headDone: Goal Achieved!"); 
  else 
    ROS_INFO("DarwinAutochargeAlgNode::tracking_headDone: %s", state.toString().c_str()); 
  this->head_is_tracking=false;
  //copy & work with requested result 
} 

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

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

  bool feedback_is_ok = true; 

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

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

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

void DarwinAutochargeAlgNode::move_jointsFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DarwinAutochargeAlgNode::move_jointsFeedback: 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_joints_client_.cancelGoal(); 
    //ROS_INFO("DarwinAutochargeAlgNode::move_jointsFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void DarwinAutochargeAlgNode::move_left_armMakeActionRequest() 
{ 
  ROS_INFO("DarwinAutochargeAlgNode::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("DarwinAutochargeAlgNode::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(&DarwinAutochargeAlgNode::move_left_armDone,     this, _1, _2), 
              boost::bind(&DarwinAutochargeAlgNode::move_left_armActive,   this), 
              boost::bind(&DarwinAutochargeAlgNode::move_left_armFeedback, this, _1)); 
  ROS_INFO("DarwinAutochargeAlgNode::move_left_armMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->left_arm_moving=true;
}
void DarwinAutochargeAlgNode::move_right_armMakeActionRequest() 
{ 
  ROS_INFO("DarwinAutochargeAlgNode::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("DarwinAutochargeAlgNode::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(&DarwinAutochargeAlgNode::move_right_armDone,     this, _1, _2), 
              boost::bind(&DarwinAutochargeAlgNode::move_right_armActive,   this), 
              boost::bind(&DarwinAutochargeAlgNode::move_right_armFeedback, this, _1)); 
  ROS_INFO("DarwinAutochargeAlgNode::move_right_armMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->right_arm_moving=true;
}
void DarwinAutochargeAlgNode::tracking_headMakeActionRequest() 
{ 
  ROS_INFO("DarwinAutochargeAlgNode::tracking_headMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  tracking_head_client_.waitForServer();  
  ROS_INFO("DarwinAutochargeAlgNode::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(&DarwinAutochargeAlgNode::tracking_headDone,     this, _1, _2), 
              boost::bind(&DarwinAutochargeAlgNode::tracking_headActive,   this), 
              boost::bind(&DarwinAutochargeAlgNode::tracking_headFeedback, this, _1)); 
  ROS_INFO("DarwinAutochargeAlgNode::tracking_headMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->head_is_tracking=true;
}
void DarwinAutochargeAlgNode::move_jointsMakeActionRequest() 
{ 
  ROS_INFO("DarwinAutochargeAlgNode::move_jointsMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  move_joints_client_.waitForServer();  
  ROS_INFO("DarwinAutochargeAlgNode::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(&DarwinAutochargeAlgNode::move_jointsDone,     this, _1, _2), 
              boost::bind(&DarwinAutochargeAlgNode::move_jointsActive,   this), 
              boost::bind(&DarwinAutochargeAlgNode::move_jointsFeedback, this, _1)); 
  ROS_INFO("DarwinAutochargeAlgNode::move_jointsMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->joints_moving=true;
}

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

  this->alg_.unlock();
}

void DarwinAutochargeAlgNode::addNodeDiagnostics(void)
{
}

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