grasp_actions_alg_node_base.cpp

Go to the documentation of this file.

#include "grasp_actions_alg_node.h"

GraspActionsAlgNode::GraspActionsAlgNode(void) :
    algorithm_base::IriBaseAlgorithm<GraspActionsAlgorithm>(),
    pickup_aserver_(public_node_handle_, "pickup"),
    open_gripper_client_("open_gripper", true),
    close_gripper_client_("close_gripper", true)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  // [init subscribers]
  // [init services]
  // [init clients]
  bhand_cmd_client_    = public_node_handle_.serviceClient<iri_wam_common_msgs::bhand_cmd>("bhand_cmd");
  joints_move_client_  = public_node_handle_.serviceClient<iri_wam_common_msgs::joints_move>("joints_move");
  wam_bhand_ik_client_ = public_node_handle_.serviceClient<iri_wam_common_msgs::wamInverseKinematics>("get_wam_ik");

  // [init action servers]
  pickup_aserver_.registerStartCallback(boost::bind(&GraspActionsAlgNode::pickupStartCallback, this, _1)); 
  pickup_aserver_.registerStopCallback(boost::bind(&GraspActionsAlgNode::pickupStopCallback, this)); 
  pickup_aserver_.registerIsFinishedCallback(boost::bind(&GraspActionsAlgNode::pickupIsFinishedCallback, this)); 
  pickup_aserver_.registerHasSucceedCallback(boost::bind(&GraspActionsAlgNode::pickupHasSucceedCallback, this)); 
  pickup_aserver_.registerGetResultCallback(boost::bind(&GraspActionsAlgNode::pickupGetResultCallback, this, _1)); 
  pickup_aserver_.registerGetFeedbackCallback(boost::bind(&GraspActionsAlgNode::pickupGetFeedbackCallback, this, _1)); 
  pickup_aserver_.start();
  // [init action clients]
  
//   ros::NodeHandle private_node_handle("~");
  private_node_handle_.param<bool>("using_bhand", using_bhand_, true);
}

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

void GraspActionsAlgNode::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
GraspActionsAlgNode::moveArm(const geometry_msgs::PoseStamped pose)
{
    iri_wam_common_msgs::wamInverseKinematics wam_bhand_ik_srv;

    // Need to transform from cartesian pose to joint state. Call to wam_ik
    wam_bhand_ik_srv.request.pose = pose;

    if (! wam_bhand_ik_client_.call(wam_bhand_ik_srv))
    {
        ROS_ERROR("Failed to call service %s", wam_bhand_ik_client_.getService().c_str());
        ROS_ERROR("No hand movement done");
        return;
    }

    iri_wam_common_msgs::joints_move joints_move_srv;

    // Convert JointState to a float64[] message
    for (std::vector<double>::iterator it = wam_bhand_ik_srv.response.joints.position.begin();
                                       it < wam_bhand_ik_srv.response.joints.position.end();
                                       it++)
        joints_move_srv.request.joints.push_back(*it);

    if (! joints_move_client_.call(joints_move_srv))
    {
      ROS_ERROR("Failed to move to grasp position: service call  %s",
                joints_move_client_.getService().c_str());

      ROS_ERROR("No hand movement done");
      return;
    }

    ROS_DEBUG("Grasp position move success %d", joints_move_srv.response.success);
    return;
}

void
GraspActionsAlgNode::moveFingers(const FingersConfiguration & fingers_config)
{
    if (using_bhand_) // using Barret Hand
        moveFingersBHand(fingers_config);
    else // using Dynamixel Gripper
        moveFingersGripper(fingers_config);
    return;
}

void
GraspActionsAlgNode::moveFingersGripper(const FingersConfiguration & fingers_config)
{
    for (std::vector<std::string>::const_iterator it = fingers_config.begin();
                                                  it < fingers_config.end();
                                                  it++)
    {
        bool finished_within_time = false;
        if (it->compare("open") == 0) {
            open_gripperMakeActionRequest();
            finished_within_time = open_gripper_client_.waitForResult(ros::Duration(6.0));
        }
        else if (it->compare("close") == 0) {
            close_gripperMakeActionRequest();
            finished_within_time = close_gripper_client_.waitForResult(ros::Duration(6.0));
        }
        else{
            ROS_WARN_STREAM("Unknown gripper command " << *it << ". Only \"open\" and \"close\" commands are accepted.");
        }
        
        if (!finished_within_time)
            ROS_ERROR_STREAM("Failed call to gripper with action: " << * it);
    }
    return;
}

void
GraspActionsAlgNode::moveFingersBHand(const FingersConfiguration & fingers_config)
{
    iri_wam_common_msgs::bhand_cmd bhand_cmd_srv;

    for (std::vector<std::string>::const_iterator it = fingers_config.begin();
                                                  it < fingers_config.end();
                                                  it++)
    {
        bhand_cmd_srv.request.bhandcmd = * it;

        if (! bhand_cmd_client_.call(bhand_cmd_srv))
        {
          ROS_ERROR("Failed to call service %s", bhand_cmd_client_.getService().c_str());
          return;
        }

        ROS_DEBUG("bhand move success %d", bhand_cmd_srv.response.success);
    }
    return;
}

void
GraspActionsAlgNode::run_grasp_actions(const GraspPhaseSequence & actions)
{
    for (GraspPhaseSequence::const_iterator it = actions.begin(); it < actions.end(); it++)
    {
        // TODO: replace the swith by a nice std::map
        // TODO: not very happy with the cast system. Should think something
        // smarter
        switch((*it)->id_)
        {
            case SET_FINGER_POSITION:
                moveFingers(static_cast<GraspPhaseSetFingers *>((* it).get())->finger_config_);
                break;
            case MOVE_ARM:
                moveArm(static_cast<GraspPhaseMoveArm *>((* it).get())->pose_);
                break;
            default:
                ROS_ERROR("Recieved an unknown GraspPhaseType. No action if performed");
        }
    }
}

void GraspActionsAlgNode::pickupStartCallback(const iri_wam_common_msgs::SimpleBhandPickUpGoalConstPtr& goal)
{
    // Generate grasp actions from PickUpGoal and run them
    run_grasp_actions(grasp_generator_.generate(* goal));

    alg_.lock();
    status_ = 100;
    alg_.unlock();

    ROS_DEBUG("goal finished successfully, returning");
}

void GraspActionsAlgNode::pickupStopCallback(void) 
{ 
  alg_.lock(); 
    //stop action 
  alg_.unlock(); 
} 

bool GraspActionsAlgNode::pickupIsFinishedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    if(status_ == 100)
        ret = true; 
  alg_.unlock(); 

  return ret; 
} 

bool GraspActionsAlgNode::pickupHasSucceedCallback(void) 
{ 
  bool ret = false; 

  alg_.lock(); 
    if(status_ == 100)
        ret = true; 
  alg_.unlock(); 

  return ret; 
} 

void GraspActionsAlgNode::pickupGetResultCallback(iri_wam_common_msgs::SimpleBhandPickUpResultPtr& result) 
{ 
  alg_.lock(); 
    //update result data to be sent to client 
    //result->data = data; 
  alg_.unlock(); 
} 

void GraspActionsAlgNode::pickupGetFeedbackCallback(iri_wam_common_msgs::SimpleBhandPickUpFeedbackPtr& feedback) 
{ 
  alg_.lock(); 
    //keep track of feedback 
    //ROS_INFO("feedback: %s", feedback->data.c_str()); 
  alg_.unlock(); 
}

/*  [action requests] */
void GraspActionsAlgNode::node_config_update(Config &config, uint32_t level)
{
    this->alg_.lock();

    ROS_INFO("Fingers configuration change. Pre-grasp: %s with name %s",
          config.pre_grasp_fingers_configuration.c_str(),
          config.grasp_fingers_configuration.c_str());

    pre_grasp_fingers_configuration_ = config.pre_grasp_fingers_configuration;
    grasp_fingers_configuration_     = config.grasp_fingers_configuration;

    this->alg_.unlock();
}

void GraspActionsAlgNode::addNodeDiagnostics(void)
{ }

void GraspActionsAlgNode::open_gripperMakeActionRequest() 
{ 
    ROS_INFO("GraspActionsAlgNode::open_gripperMakeActionRequest: Starting New Request!"); 

    //wait for the action server to start 
    //will wait for infinite time 
    open_gripper_client_.waitForServer();  
    ROS_INFO("GraspActionsAlgNode::open_gripperMakeActionRequest: Server is Available!"); 

    //send a goal to the action 
    //open_gripper_goal_.data = my_desired_goal; 
    open_gripper_client_.sendGoal(open_gripper_goal_, 
                boost::bind(&GraspActionsAlgNode::open_gripperDone,     this, _1, _2), 
                boost::bind(&GraspActionsAlgNode::open_gripperActive,   this), 
                boost::bind(&GraspActionsAlgNode::open_gripperFeedback, this, _1)); 
    ROS_INFO("GraspActionsAlgNode::open_gripperMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void GraspActionsAlgNode::close_gripperMakeActionRequest() 
{ 
  ROS_DEBUG("GraspActionsAlgNode::close_gripperMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  close_gripper_client_.waitForServer();  
  ROS_DEBUG("GraspActionsAlgNode::close_gripperMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //close_gripper_goal_.data = my_desired_goal; 
  close_gripper_client_.sendGoal(close_gripper_goal_, 
              boost::bind(&GraspActionsAlgNode::close_gripperDone,     this, _1, _2), 
              boost::bind(&GraspActionsAlgNode::close_gripperActive,   this), 
              boost::bind(&GraspActionsAlgNode::close_gripperFeedback, this, _1)); 
  ROS_DEBUG("GraspActionsAlgNode::close_gripperMakeActionRequest: Goal Sent. Wait for Result!"); 
}

/* [action callbacks] */
void GraspActionsAlgNode::open_gripperDone(const actionlib::SimpleClientGoalState& state,  const iri_dynamixel_gripper::openResultConstPtr& result) 
{ 
    if( state.toString().compare("SUCCEEDED") == 0 ) 
        ROS_INFO("GraspActionsAlgNode::open_gripperDone: Goal Achieved!"); 
    else 
        ROS_WARN("GraspActionsAlgNode::open_gripperDone: %s", state.toString().c_str()); 
} 
void GraspActionsAlgNode::open_gripperActive() {}
void GraspActionsAlgNode::open_gripperFeedback(const iri_dynamixel_gripper::openFeedbackConstPtr& feedback) {}

void GraspActionsAlgNode::close_gripperDone(const actionlib::SimpleClientGoalState& state,  const iri_dynamixel_gripper::closeResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_DEBUG("GraspActionsAlgNode::close_gripperDone: Goal Achieved!"); 
  else 
    ROS_WARN("GraspActionsAlgNode::close_gripperDone: %s", state.toString().c_str()); 
} 
void GraspActionsAlgNode::close_gripperActive() {}
void GraspActionsAlgNode::close_gripperFeedback(const iri_dynamixel_gripper::closeFeedbackConstPtr& feedback) {}