tibi_dabo_arm_client_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_arm_client_alg_node.h"

TibiDaboArmClientAlgNode::TibiDaboArmClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboArmClientAlgorithm>(),
  joint_motion_client_("joint_motion", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  this->public_node_handle_.getParam("robot",this->robot);
  this->public_node_handle_.getParam("side",this->side);

  this->joint_motion_done=true;
  // initialize the message by default
  this->change_joint_names(this->robot,this->side);
}

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

void TibiDaboArmClientAlgNode::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]
}

void TibiDaboArmClientAlgNode::change_joint_names(int robot_id, int side_id)
{
  std::string side_prefix;
  
  if(side==0)//left
    side_prefix="left_";    
  else//right
    side_prefix="right_";

  this->joint_motion_goal_.trajectory.points.resize(1);
  if(robot_id==0)//dabo
  {
    this->joint_motion_goal_.trajectory.joint_names.resize(4);
    this->joint_motion_goal_.trajectory.joint_names[0]=side_prefix + "shoulder_yaw";
    this->joint_motion_goal_.trajectory.joint_names[1]=side_prefix + "shoulder_pitch";
    this->joint_motion_goal_.trajectory.joint_names[2]=side_prefix + "shoulder_roll";
    this->joint_motion_goal_.trajectory.joint_names[3]=side_prefix + "elbow";
    this->joint_motion_goal_.trajectory.points[0].positions.resize(4);
    this->joint_motion_goal_.trajectory.points[0].velocities.resize(4);
    this->joint_motion_goal_.trajectory.points[0].accelerations.resize(4);
    this->joint_motion_goal_.trajectory.points[0].time_from_start=ros::Duration(0);
  }
  else
  {
    this->joint_motion_goal_.trajectory.joint_names.resize(2);
    this->joint_motion_goal_.trajectory.joint_names[0]=side_prefix + "shoulder_yaw";
    this->joint_motion_goal_.trajectory.joint_names[1]=side_prefix + "shoulder_roll";
    this->joint_motion_goal_.trajectory.points[0].positions.resize(2);
    this->joint_motion_goal_.trajectory.points[0].velocities.resize(2);
    this->joint_motion_goal_.trajectory.points[0].accelerations.resize(2);
    this->joint_motion_goal_.trajectory.points[0].time_from_start=ros::Duration(0);
  }
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

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

void TibiDaboArmClientAlgNode::joint_motionActive() 
{ 
  //ROS_INFO("TibiDaboArmClientAlgNode::joint_motionActive: Goal just went active!"); 
} 

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

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

  //wait for the action server to start 
  //will wait for infinite time 
  joint_motion_client_.waitForServer();  
  ROS_INFO("TibiDaboArmClientAlgNode::joint_motionMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //joint_motion_goal_.data = my_desired_goal; 
  joint_motion_client_.sendGoal(joint_motion_goal_, 
              boost::bind(&TibiDaboArmClientAlgNode::joint_motionDone,     this, _1, _2), 
              boost::bind(&TibiDaboArmClientAlgNode::joint_motionActive,   this), 
              boost::bind(&TibiDaboArmClientAlgNode::joint_motionFeedback, this, _1)); 
  ROS_INFO("TibiDaboArmClientAlgNode::joint_motionMakeActionRequest: Goal Sent. Wait for Result!"); 
  this->joint_motion_done=false;
}

void TibiDaboArmClientAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  if(this->joint_motion_done)
  {
    if(config.shoulder_yaw)
    {
      ROS_INFO("TibiDaboArmClientAlgNode::shouler yaw loaded!");
      this->joint_motion_goal_.trajectory.points[0].positions[0]  = config.shoulder_yaw_angle*3.14159/180.0;
      this->joint_motion_goal_.trajectory.points[0].velocities[0] = config.speed*3.14159/180.0;
    }
    if(config.shoulder_pitch && this->robot==0)
    {
      ROS_INFO("TibiDaboArmClientAlgNode::shoudler pitch loaded!");
      this->joint_motion_goal_.trajectory.points[0].positions[1]  = config.shoulder_pitch_angle*3.14159/180.0;
      this->joint_motion_goal_.trajectory.points[0].velocities[1] = config.speed*3.14159/180.0;
    }
    else
    {
      config.shoulder_pitch=false;
      config.shoulder_pitch_angle=0.0;
    }
    if(config.shoulder_roll)
    {
      ROS_INFO("TibiDaboArmClientAlgNode::shoulder roll loaded!");
      if(this->robot==0)
      {
        this->joint_motion_goal_.trajectory.points[0].positions[2]  = config.shoulder_roll_angle*3.14159/180.0;
        this->joint_motion_goal_.trajectory.points[0].velocities[2] = config.speed*3.14159/180.0;
      }
      else
      {
        this->joint_motion_goal_.trajectory.points[0].positions[1]  = config.shoulder_roll_angle*3.14159/180.0;
        this->joint_motion_goal_.trajectory.points[0].velocities[1] = config.speed*3.14159/180.0;
      }
    }
    if(config.elbow && this->robot==0)
    {
      ROS_INFO("TibiDaboArmClientAlgNode::elbow loaded!");
      this->joint_motion_goal_.trajectory.points[0].positions[3]  = config.elbow_angle*3.14159/180.0;
      this->joint_motion_goal_.trajectory.points[0].velocities[3] = config.speed*3.14159/180.0;
    }
    else
    {
      config.elbow=false;
      config.elbow_angle=0.0;
    }
    if(config.move)
    {
      ROS_INFO("TibiDaboArmClientAlgNode:: requesting head move!");
      joint_motionMakeActionRequest();
      config.move=false;
    }
  }

  this->alg_.unlock();
}

void TibiDaboArmClientAlgNode::addNodeDiagnostics(void)
{
}

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