place_alg_node.cpp

Go to the documentation of this file.

#include "place_alg_node.h"

PlaceAlgNode::PlaceAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<PlaceAlgorithm>(),
  place_aserver_(public_node_handle_, "place")
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  place_pose_frame_id_ = "/wam_link0";
  place_action_success_ = false;
  place_action_status_ = 0;

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  joints_move_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::joints_move>("joints_move");
  get_tcp_ik_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::wamInverseKinematics>("get_tcp_ik");
  bhand_cmd_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::bhand_cmd>("bhand_cmd");
  
  // [init action servers]
  place_aserver_.registerStartCallback(boost::bind(&PlaceAlgNode::placeStartCallback, this, _1)); 
  place_aserver_.registerStopCallback(boost::bind(&PlaceAlgNode::placeStopCallback, this)); 
  place_aserver_.registerIsFinishedCallback(boost::bind(&PlaceAlgNode::placeIsFinishedCallback, this)); 
  place_aserver_.registerHasSucceedCallback(boost::bind(&PlaceAlgNode::placeHasSucceedCallback, this)); 
  place_aserver_.registerGetResultCallback(boost::bind(&PlaceAlgNode::placeGetResultCallback, this, _1)); 
  place_aserver_.registerGetFeedbackCallback(boost::bind(&PlaceAlgNode::placeGetFeedbackCallback, this, _1)); 
  place_aserver_.start();
  
  // [init action clients]
}

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

void PlaceAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  //joints_move_srv_.request.data = my_var; 
  //ROS_INFO("PlaceAlgNode:: Sending New Request!"); 
  //if (joints_move_client_.call(joints_move_srv)) 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Response: %s", joints_move_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Failed to Call Server on topic joints_move "); 
  //}
  //get_tcp_ik_srv_.request.data = my_var; 
  //ROS_INFO("PlaceAlgNode:: Sending New Request!"); 
  //if (get_tcp_ik_client_.call(get_tcp_ik_srv)) 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Response: %s", get_tcp_ik_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Failed to Call Server on topic get_tcp_ik "); 
  //}
  //bhand_cmd_srv_.request.data = my_var; 
  //ROS_INFO("PlaceAlgNode:: Sending New Request!"); 
  //if (bhand_cmd_client_.call(bhand_cmd_srv)) 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Response: %s", bhand_cmd_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Failed to Call Server on topic bhand_cmd "); 
  //}
  //pose_move_srv_.request.data = my_var; 
  //ROS_INFO("PlaceAlgNode:: Sending New Request!"); 
  //if (pose_move_client_.call(pose_move_srv)) 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Response: %s", pose_move_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("PlaceAlgNode:: Failed to Call Server on topic pose_move "); 
  //}
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */
void PlaceAlgNode::placeStartCallback(const object_manipulation_msgs::PlaceGoalConstPtr& goal)
{ 
    //check goal 
    ROS_INFO("Starting place action");
    //TODO check collision avoidance
    if(goal->place_locations[0].header.frame_id != place_pose_frame_id_){
      ROS_ERROR("frame_id mismatch");
      std::cout << goal->place_locations[0].header.frame_id << std::endl;
      std::cout << place_pose_frame_id_ << std::endl;
      alg_.lock();
      place_action_status_ = FINISH;
      alg_.unlock();
      return;
    }

    alg_.lock();
    place_action_status_ = POSE_CHECKED;
    alg_.unlock();
    //execute goal 
    get_tcp_ik_srv_.request.pose = goal->place_locations[0];
    if (get_tcp_ik_client_.call(get_tcp_ik_srv_)) { 
        ROS_INFO("PlaceAlgNode:: Response %f %f %f %f %f %f %f", get_tcp_ik_srv_.response.joints.position[0], get_tcp_ik_srv_.response.joints.position[1], get_tcp_ik_srv_.response.joints.position[2], get_tcp_ik_srv_.response.joints.position[3], get_tcp_ik_srv_.response.joints.position[4], get_tcp_ik_srv_.response.joints.position[5], get_tcp_ik_srv_.response.joints.position[6]); 
    } else { 
        ROS_ERROR("PlaceAlgNode:: Failed to Call Server on topic %s", get_tcp_ik_client_.getService().c_str()); 
        alg_.lock();
        place_action_status_ = FINISH;
        alg_.unlock();
        return;
    }

    joints_move_srv_.request.joints = get_tcp_ik_srv_.response.joints.position;
    ROS_INFO("Moving");
    if (joints_move_client_.call(joints_move_srv_)) {
      ROS_DEBUG("joints_move success %d", joints_move_srv_.response.success);
    } else {
      ROS_ERROR("Failed to call service %s", joints_move_client_.getService().c_str());
      alg_.lock();
      place_action_status_ = FINISH;
      alg_.unlock();
      return;
    }

    alg_.lock();
    place_action_status_ = POSITION_REACHED;
    alg_.unlock();

    ROS_INFO("Opening hand");
    bhand_cmd_srv_.request.bhandcmd = "GTO";
    if (bhand_cmd_client_.call(bhand_cmd_srv_))
    {
      ROS_DEBUG("bhand move success %d", bhand_cmd_srv_.response.success);
    } else {
      ROS_ERROR("Failed to call service  %s", bhand_cmd_client_.getService().c_str());
      alg_.lock();
      place_action_status_ = FINISH;
      alg_.unlock();
      return;
    }

    alg_.lock();
    place_action_status_ = HAND_OPENED; //no effect
    place_action_status_ = FINISH;
    place_action_success_ = true;
    alg_.unlock();
    ROS_INFO("Place action completed");

} 

void PlaceAlgNode::placeStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

bool PlaceAlgNode::placeIsFinishedCallback(void) 
{ 
  bool ret = false; 

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

  return ret; 
} 

bool PlaceAlgNode::placeHasSucceedCallback(void) 
{ 
  bool ret = false; 

  //TODO what happens if the action is called twice? How the correct interleaving of the action access to action_success flag is guaranteed?
  alg_.lock(); 
    //if goal was accomplished 
    //ret = true 
  last_place_action_success_ = place_action_success_;
  if(place_action_success_ == true){
      ret = true;
      //reset flag //is this concurrency safe?
      place_action_success_ = false;
  }
  alg_.unlock(); 

  return ret; 
} 

void PlaceAlgNode::placeGetResultCallback(object_manipulation_msgs::PlaceResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    if(last_place_action_success_){
      result->manipulation_result.value = object_manipulation_msgs::ManipulationResult::SUCCESS; 
    }else{
      result->manipulation_result.value = object_manipulation_msgs::ManipulationResult::FAILED; 
    }
  alg_.unlock(); 
} 

void PlaceAlgNode::placeGetFeedbackCallback(object_manipulation_msgs::PlaceFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

/*  [action requests] */

void PlaceAlgNode::node_config_update(Config &config, uint32_t level)
{

  ROS_INFO("Reconfigure called");

  this->alg_.lock();
  place_pose_frame_id_ = config.pose_frame_id; 
  this->alg_.unlock();

}

void PlaceAlgNode::addNodeDiagnostics(void)
{
}

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