darwin_arm_client_alg_node.cpp

Go to the documentation of this file.

#include "darwin_arm_client_alg_node.h"


DarwinArmClientAlgNode::DarwinArmClientAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DarwinArmClientAlgorithm>(),
  move_joints_client_("move_joints", 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->motion_done=true;
  sleep(10);
}

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

void DarwinArmClientAlgNode::mainNodeThread(void)
{
  static int sequence_step=0;
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
  if(this->motion_done)
  {
    this->move_joints_goal_.trajectory.joint_names.resize(5);
    this->move_joints_goal_.trajectory.joint_names[0]="j_shoulder_r";
    this->move_joints_goal_.trajectory.joint_names[1]="j_high_arm_r";
    this->move_joints_goal_.trajectory.joint_names[2]="j_low_arm_r";
    this->move_joints_goal_.trajectory.joint_names[3]="j_wrist_r";
    this->move_joints_goal_.trajectory.joint_names[4]="j_gripper_r";
    this->move_joints_goal_.trajectory.points.resize(1);
    this->move_joints_goal_.trajectory.points[0].positions.resize(5,0.0);
    this->move_joints_goal_.trajectory.points[0].velocities.resize(5,50.0);
    this->move_joints_goal_.trajectory.points[0].accelerations.resize(5,100.0);
    switch(sequence_step)
    {
      case 0: break;
      case 1: this->move_joints_goal_.trajectory.points[0].positions[2]=60.0;
              this->move_joints_goal_.trajectory.points[0].positions[0]=45.0;
              break;
      case 2: this->move_joints_goal_.trajectory.points[0].positions[3]=45.0;
              break;
      case 3: this->move_joints_goal_.trajectory.points[0].positions[2]=-45.0;
              this->move_joints_goal_.trajectory.points[0].positions[0]=-25.0;
              break;
      case 4: this->move_joints_goal_.trajectory.points[0].positions[4]=0.0;
              this->move_joints_goal_.trajectory.points[0].positions[0]=0.0;
              this->move_joints_goal_.trajectory.points[0].positions[1]=0.0;
              this->move_joints_goal_.trajectory.points[0].positions[2]=0.0;
              this->move_joints_goal_.trajectory.points[0].positions[3]=0.0;
              break;
      default: return;
    }
    move_jointsMakeActionRequest(); 
    sequence_step++;
  }

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

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

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

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

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

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

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

  this->alg_.unlock();
}

void DarwinArmClientAlgNode::addNodeDiagnostics(void)
{
}

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