tibi_dabo_servo_move_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_servo_move_alg_node.h"

TibiDaboServoMoveAlgNode::TibiDaboServoMoveAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboServoMoveAlgorithm>(),
  servo_angle_aserver_(public_node_handle_, "servo_angle")
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  servo_angle_aserver_.registerStartCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleStartCallback, this, _1)); 
  servo_angle_aserver_.registerStopCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleStopCallback, this)); 
  servo_angle_aserver_.registerIsFinishedCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleIsFinishedCallback, this)); 
  servo_angle_aserver_.registerHasSucceedCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleHasSucceedCallback, this)); 
  servo_angle_aserver_.registerGetResultCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleGetResultCallback, this, _1)); 
  servo_angle_aserver_.registerGetFeedbackCallback(boost::bind(&TibiDaboServoMoveAlgNode::servo_angleGetFeedbackCallback, this, _1)); 
  servo_angle_aserver_.start();
  
  // [init action clients]
  
   //Servo initialization
  event_server=CEventServer::instance();
  std::string servo_name="AX-12+_2";
  CDynamixelServer *dyn_server=CDynamixelServer::instance();
  std::vector<bool> enable(1,true);
  servo_found=false;
  
  ROS_INFO("Initializing servo...");
  try{
    if(dyn_server->get_num_buses()>0)
    {
      ROS_INFO("OK");
      //servo=new CDynamixelMotor(servo_name,0,1000000,1);
      servo=new CDynamixelMotor(servo_name,1,1000000,1);
      enable[0]=true;
      servo->enable(enable);
      #ifdef _HAVE_XSD
      servo->load_config(cont_config_file);
      events.push_back(servo->get_position_feedback_event());
      #else
      events.push_back(servo->config_position_feedback(fb_polling,100.0));
      #endif
      servo->set_torque(100.0);
      servo_found=true;
      ROS_INFO("Servo is ready");
    }
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
  
}

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

void TibiDaboServoMoveAlgNode::mainNodeThread(void)
{  
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void TibiDaboServoMoveAlgNode::servo_angleStartCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{ 
  alg_.lock(); 
    //check goal 
    //execute goal
    
    //ROS_WARN("GOAL RECEIVED");
    
    if(servo_found){
      
      std::vector<float> pos(1),vel(1),acc(1);
      pos[0]=-goal->trajectory.points[0].positions[0]+150.0;
      vel[0]=goal->trajectory.points[0].velocities[0];
      acc[0]=vel[0]*vel[0]/(0.05*pos[0]);
    
      ROS_WARN_STREAM("servo moving to " << pos[0]);
      servo->load(pos,vel,acc);
      servo->move();
    }

    
  alg_.unlock(); 
} 

void TibiDaboServoMoveAlgNode::servo_angleStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

bool TibiDaboServoMoveAlgNode::servo_angleIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool TibiDaboServoMoveAlgNode::servo_angleHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if goal was accomplished 
    //ret = true; 
  alg_.unlock(); 

  return ret; 
} 

void TibiDaboServoMoveAlgNode::servo_angleGetResultCallback(control_msgs::FollowJointTrajectoryResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void TibiDaboServoMoveAlgNode::servo_angleGetFeedbackCallback(control_msgs::FollowJointTrajectoryFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
    
    std::vector<float> pos(1);
    feedback->actual.positions.resize(1);
    
    if(servo_found) pos=servo->get_position();
    else pos[0]=150.0;
    
    //ROS_WARN_STREAM("Sending feedback with pos "<< pos[0]);
  
    feedback->actual.positions[0]=-(pos[0]-150.0);
    
  alg_.unlock(); 
}

/*  [action requests] */

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

  this->alg_.unlock();
}

void TibiDaboServoMoveAlgNode::addNodeDiagnostics(void)
{
}

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