darwin_arm_simple_nav_alg_node.cpp

Go to the documentation of this file.

#include "darwin_arm_simple_nav_alg_node.h"
#include "arm_navigation_msgs/convert_messages.h"

DarwinArmSimpleNavAlgNode::DarwinArmSimpleNavAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<DarwinArmSimpleNavAlgorithm>(),
  move_arm_aserver_(public_node_handle_, "move_arm"),
  move_joints_client_("move_joints", true),
  tf_listener(ros::Duration(10.0))
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  this->robot_running=false;
  this->joints_moving=false;

  // [init publishers]
  
  // [init subscribers]
  this->joint_state_subscriber_ = this->public_node_handle_.subscribe("joint_state", 100, &DarwinArmSimpleNavAlgNode::joint_state_callback, this);
  
  // [init services]
  
  // [init clients]
  ik_info_client_ = this->public_node_handle_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("ik_info");

  get_ik_client_ = this->public_node_handle_.serviceClient<kinematics_msgs::GetPositionIK>("get_ik");
  
  // [init action servers]
  move_arm_aserver_.registerStartCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armStartCallback, this, _1)); 
  move_arm_aserver_.registerStopCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armStopCallback, this)); 
  move_arm_aserver_.registerIsFinishedCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armIsFinishedCallback, this)); 
  move_arm_aserver_.registerHasSucceedCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armHasSucceedCallback, this)); 
  move_arm_aserver_.registerGetResultCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armGetResultCallback, this, _1)); 
  move_arm_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinArmSimpleNavAlgNode::move_armGetFeedbackCallback, this, _1)); 
  move_arm_aserver_.start();
  
  // [init action clients]

  // get the default value of the arm_id parameter
  this->public_node_handle_.getParam("arm_id",this->arm_id);
  if(this->arm_id!="left" && this->arm_id!="right")
    ROS_ERROR("Invalid arm identifier (neither left nor right). The node is running but it will not execute any action.");
}

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

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

  // [publish messages]
}

/*  [subscriber callbacks] */
void DarwinArmSimpleNavAlgNode::joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg) 
{ 
  static bool first=true;

  //ROS_INFO("DarwinArmSimpleNavAlgNode::joint_state_callback: New Message Received"); 

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

  //std::cout << msg->data << std::endl; 
  this->current_state=(*msg);
  if(first)
  {
    this->robot_running=true;
    first=false;
  }
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->joint_state_mutex_.exit(); 
}

/*  [service callbacks] */

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

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

void DarwinArmSimpleNavAlgNode::move_jointsFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("DarwinArmSimpleNavAlgNode::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("DarwinArmSimpleNavAlgNode::move_jointsFeedback: Cancelling Action!"); 
  } 
}
void DarwinArmSimpleNavAlgNode::move_armStartCallback(const arm_navigation_msgs::MoveArmGoalConstPtr& goal)
{
  geometry_msgs::PoseStamped local_goal,target_goal;
  double time=0.0,max_angle=0.0;
  bool tf_exists;
  int i=0;

  if(this->robot_running)
  {
    alg_.lock(); 
    // create the target_goal from the input message
    target_goal.header=goal->motion_plan_request.goal_constraints.position_constraints[0].header;
    target_goal.pose.position=goal->motion_plan_request.goal_constraints.position_constraints[0].position;   
    target_goal.pose.orientation=tf::createQuaternionMsgFromYaw(0);    
    try{
      // transform the target_goal to the local goal in the MP_BODY reference frame
      tf_exists = tf_listener.waitForTransform(target_goal.header.frame_id,std::string("MP_BODY"), target_goal.header.stamp,
                                               ros::Duration(10),ros::Duration(0.01));
      tf_listener.transformPose(std::string("MP_BODY"),target_goal,local_goal);
      // chose the arm to move depending on the target postion
      if(this->arm_id=="left")
      {
        this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_L";
        // IK seed, get the current arm position
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_l";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_l";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_l";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_l";
        this->joint_state_mutex_.enter();
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[2];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[4];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[6];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[23];
        this->joint_state_mutex_.exit();
      } 
      else if(this->arm_id=="right")
      { 
        this->get_ik_srv_.request.ik_request.ik_link_name="MP_ARM_GRIPPER_FIX_R";
        // IK seed, get the current arm position
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name.resize(4);
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[0]="j_shoulder_r";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[1]="j_high_arm_r";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[2]="j_low_arm_r";
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name[3]="j_wrist_r";
        this->joint_state_mutex_.enter();
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position.resize(4);
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[0]=this->current_state.position[1];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[1]=this->current_state.position[3];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[2]=this->current_state.position[5];
        this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[3]=this->current_state.position[21];
        this->joint_state_mutex_.exit();
      }
      // desired pose
      this->get_ik_srv_.request.ik_request.pose_stamped=local_goal;
      // ignore the rest of the infomation for the moment
      if (get_ik_client_.call(get_ik_srv_))
      {
        ROS_INFO("DarwinArmSimpleNavAlgNode:: Response: %s", arm_navigation_msgs::armNavigationErrorCodeToString(get_ik_srv_.response.error_code).c_str());
        if(this->get_ik_srv_.response.error_code.val==this->get_ik_srv_.response.error_code.SUCCESS)// success
        {
          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=this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.name;
          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);
          for(i=0;i<4;i++)
            if(fabs(this->get_ik_srv_.response.solution.joint_state.position[i]-this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[i])>max_angle)
              max_angle=fabs(this->get_ik_srv_.response.solution.joint_state.position[i]-this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[i]);
          time=max_angle/50.0;
          for(i=0;i<4;i++)
          {
            this->move_joints_goal_.trajectory.points[0].positions[i]=this->get_ik_srv_.response.solution.joint_state.position[i]*180.0/3.141592;
            this->move_joints_goal_.trajectory.points[0].velocities[i]=fabs(this->get_ik_srv_.response.solution.joint_state.position[i]-this->get_ik_srv_.request.ik_request.ik_seed_state.joint_state.position[i])/time;
            if(this->move_joints_goal_.trajectory.points[0].velocities[i]<5.0)
              this->move_joints_goal_.trajectory.points[0].velocities[i]=5.0;
            this->move_joints_goal_.trajectory.points[0].accelerations[i]=100.0;
          }
          this->move_joints_goal_.trajectory.points[0].time_from_start=ros::Duration(time);
          // make the request
          this->move_jointsMakeActionRequest();
        }
        else
        {
          ROS_INFO("DarwinArmSimpleNavAlgNode:: No solution to the inverse kinematics");
          this->move_arm_aserver_.setAborted();
        }
      }
      else
      {
        ROS_INFO("DarwinArmSimpleNavAlgNode:: Failed to Call Server on topic get_ik ");
        // cancel the action
        this->move_arm_aserver_.setAborted();
      }
      //execute goal 
      alg_.unlock();
    }catch(tf::TransformException ex){
      ROS_ERROR("NoCollisionAlgNode:: %s",ex.what());
      this->move_arm_aserver_.setAborted();
      alg_.unlock();
    }
  }
  else
  {
    ROS_INFO("DarwinArmSimpleNavAlgNode:: Robot is not running");
    this->move_arm_aserver_.setAborted();
  }
} 

void DarwinArmSimpleNavAlgNode::move_armStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
    // cancel the joints motion
    this->move_joints_client_.cancelGoal();
    this->joints_moving=false;
  alg_.unlock(); 
} 

bool DarwinArmSimpleNavAlgNode::move_armIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    //if action has finish for any reason 
    ret = !this->joints_moving; 
  alg_.unlock(); 

  return ret; 
} 

bool DarwinArmSimpleNavAlgNode::move_armHasSucceedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

void DarwinArmSimpleNavAlgNode::move_armGetResultCallback(arm_navigation_msgs::MoveArmResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void DarwinArmSimpleNavAlgNode::move_armGetFeedbackCallback(arm_navigation_msgs::MoveArmFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

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

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

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

  this->arm_id=config.arm_id;
  if(this->arm_id!="left" && this->arm_id!="right")
    ROS_ERROR("Invalid arm identifier (neither left nor right). The node is running but it will not execute any action.");

  this->alg_.unlock();
}

void DarwinArmSimpleNavAlgNode::addNodeDiagnostics(void)
{
}

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