grasp_planning_gripper_click_node.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>

#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

#include <stereo_msgs/DisparityImage.h>

#include <object_manipulation_msgs/GraspPlanning.h>
#include <object_manipulation_msgs/PlacePlanning.h>

#include <tf/transform_listener.h>

#include <rgbd_assembler/RgbdAssembly.h>

#include "pr2_gripper_click/GripperClickPickupAction.h"
#include "pr2_gripper_click/GripperClickPlaceAction.h"


class HandDescription
{
 private:
  ros::NodeHandle root_nh_;

  inline std::string getStringParam(std::string name)
  {
    std::string value;
    if (!root_nh_.getParamCached(name, value))
    {
      ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
    }
    //ROS_INFO_STREAM("Hand description param " << name << " resolved to " << value);
    return value;
  }

  inline std::vector<std::string> getVectorParam(std::string name)
  {
    std::vector<std::string> values;
    XmlRpc::XmlRpcValue list;
    if (!root_nh_.getParamCached(name, list)) 
    {
      ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
    }
    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Hand description: bad parameter %s", name.c_str());
    }
    //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
    for (int32_t i=0; i<list.size(); i++)
    {
      if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR("Hand description: bad parameter %s", name.c_str());
      }
      values.push_back( static_cast<std::string>(list[i]) );
      //ROS_INFO_STREAM("  " << values.back());
    }
    return values;      
  }

 public:
 HandDescription() : root_nh_("~") {}
  
  inline std::string handDatabaseName(std::string arm_name)
  {
    return getStringParam("/hand_description/" + arm_name + "/hand_database_name");
  }
  
  inline std::vector<std::string> handJointNames(std::string arm_name)
  {
    return getVectorParam("/hand_description/" + arm_name + "/hand_joints");
  }
  
};

class GraspPlanningGripperClick
{
private:
  ros::NodeHandle root_nh_;

  ros::NodeHandle priv_nh_;

  ros::ServiceServer grasp_srv;
  ros::ServiceServer place_srv;

  ros::ServiceClient rgbd_assembly_srv_;
  std::string rgbd_assembly_service_name_;
  bool use_assembler_;

  tf::TransformListener listener_;

  actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPickupAction> *pickup_action_client_;
  actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPlaceAction> *place_action_client_;

  std::string grasp_planning_service_topic_;
  std::string place_planning_service_topic_;

  std::string pickup_action_topic_;
  std::string place_action_topic_;

  HandDescription hd_;

  std::string image_topic_;
  std::string disparity_image_topic_;
  std::string camera_info_topic_;

  bool callSensorDataAssembler(pr2_gripper_click::GripperClickSensorData &sensor_data)
  {
    rgbd_assembler::RgbdAssembly srv;
    if (!ros::service::waitForService(rgbd_assembly_service_name_, ros::Duration(2.0)))
    {
      ROS_ERROR("rgbd assembler at %s is not available", rgbd_assembly_service_name_.c_str());
      return false;
    }
    if (!rgbd_assembly_srv_.call(srv))
    {
      ROS_ERROR("Failed to call rgbd assembler at %s", rgbd_assembly_service_name_.c_str());
      return false;
    }
    if (srv.response.result != srv.response.SUCCESS)
    {
      ROS_ERROR("Rgbd assembler returned an error");
      return false;
    }
    sensor_data.image = srv.response.image;
    sensor_data.disparity_image = srv.response.disparity_image;
    sensor_data.camera_info = srv.response.camera_info;
    return true;
  }

  bool assembleSensorData(pr2_gripper_click::GripperClickSensorData &sensor_data,
                          ros::Duration time_out)
  {
    sensor_msgs::Image::ConstPtr recent_image;
    stereo_msgs::DisparityImage::ConstPtr recent_disparity_image;
    sensor_msgs::CameraInfo::ConstPtr recent_camera_info;
    
    ROS_INFO("Planning with gripper click: waiting for messages...");
    ros::Time start_time = ros::Time::now();
    while ((!recent_image || !recent_disparity_image || !recent_camera_info) && priv_nh_.ok())
    {
      if (!recent_image)
      {
        ROS_INFO_STREAM("  Waiting for image message on topic " << image_topic_);
        recent_image = ros::topic::waitForMessage<sensor_msgs::Image>(image_topic_, root_nh_, ros::Duration(0.5));
      }
      if (!recent_disparity_image)
      {
        ROS_INFO_STREAM("  Waiting for disparity image message on topic " << disparity_image_topic_);
        recent_disparity_image = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
          (disparity_image_topic_, root_nh_, ros::Duration(0.5));
      }
      if (!recent_camera_info)
      {
        ROS_INFO_STREAM("  Waiting for camera info message on topic " << camera_info_topic_);
        recent_camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
          (camera_info_topic_, root_nh_, ros::Duration(0.5));
      }
      ros::Time current_time = ros::Time::now();
      if (time_out >= ros::Duration(0) && current_time - start_time >= time_out)
      {
        ROS_INFO("Timed out");
        return false;
      }
    }
    if (!priv_nh_.ok()) return false;
    ROS_INFO("All required messages received");
    
    sensor_data.image = *recent_image;
    sensor_data.disparity_image = *recent_disparity_image;
    sensor_data.camera_info = *recent_camera_info;
    return true;
  }

  bool graspServiceCallback(object_manipulation_msgs::GraspPlanning::Request &request,
                            object_manipulation_msgs::GraspPlanning::Response &response)
  {
    // reinitializing clients on each call to get around potential actionlib bug
    delete pickup_action_client_;
    pickup_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPickupAction>
      (pickup_action_topic_, true);
    while(!pickup_action_client_->waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
    {
      ROS_INFO("Waiting for action client on topic %s", pickup_action_topic_.c_str());
    }

    pr2_gripper_click::GripperClickPickupGoal click_goal;
    if (use_assembler_)
    {
      if (!callSensorDataAssembler(click_goal.sensor_data)) return false;
    }
    else
    {
      if (!assembleSensorData(click_goal.sensor_data, ros::Duration(5.0))) return false;
    }

    pickup_action_client_->sendGoal(click_goal);
    while (!pickup_action_client_->waitForResult(ros::Duration(0.5)) && priv_nh_.ok())
    {
    }
    if (!priv_nh_.ok()) 
    {
      pickup_action_client_->cancelGoal();
      return false;
    }
    
    if (pickup_action_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      ROS_INFO("The gripper click grasp action has not succeeded;");
      response.error_code.value = response.error_code.OTHER_ERROR;
    }
    else
    {    
      ROS_INFO("Gripper click grasp has succeeded;");
      object_manipulation_msgs::Grasp grasp;
    
      std::vector<std::string> joint_names = hd_.handJointNames(request.arm_name);
      grasp.pre_grasp_posture.name = joint_names;
      grasp.grasp_posture.name = joint_names;

      pr2_gripper_click::GripperClickPickupResultConstPtr result = pickup_action_client_->getResult();
      grasp.pre_grasp_posture.position.resize( joint_names.size(), result->gripper_opening / 0.0857 * 0.5);
      ROS_INFO("Using gripper angles of %.3f", result->gripper_opening / 0.0857 * 0.5);
      grasp.grasp_posture.position.resize( joint_names.size(), 0.0);     
      //for now the effort is not in the database so we hard-code it in here
      //this will change at some point
      grasp.grasp_posture.effort.resize(joint_names.size(), 10000);
      grasp.pre_grasp_posture.effort.resize(joint_names.size(), 10000);
      geometry_msgs::PoseStamped pose_stamped_in = result->grasp_pose;
      pose_stamped_in.header.stamp = ros::Time(0);
      geometry_msgs::PoseStamped pose_stamped;
      try
      {
        listener_.transformPose(request.target.reference_frame_id, pose_stamped_in, pose_stamped);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("Failed to transform gripper click grasp to %s frame; exception: %s", 
                  pickup_action_client_->getResult()->grasp_pose.header.frame_id.c_str(), ex.what());
        response.error_code.value = response.error_code.TF_ERROR;
        return true;
      }
      grasp.grasp_pose = pose_stamped.pose;
      grasp.success_probability = 1.0;
      response.grasps.push_back(grasp);
      response.error_code.value = response.error_code.SUCCESS;
    }
    return true;
  }

  bool placeServiceCallback(object_manipulation_msgs::PlacePlanning::Request &request,
                            object_manipulation_msgs::PlacePlanning::Response &response)
  {
    // reinitializing clients on each call to get around potential actionlib bug
    delete place_action_client_;
    place_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPlaceAction>
      (place_action_topic_, true);
    while(!place_action_client_->waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
    {
      ROS_INFO("Waiting for action client on topic %s", place_action_topic_.c_str());
    }

    pr2_gripper_click::GripperClickPlaceGoal click_goal;
    if (use_assembler_)
    {
      if (!callSensorDataAssembler(click_goal.sensor_data)) return false;
    }
    else
    {
      if (!assembleSensorData(click_goal.sensor_data, ros::Duration(5.0))) return false;
    }

    click_goal.object = request.target;
    click_goal.grasp_pose = request.grasp_pose;
    click_goal.default_orientation = request.default_orientation;
    
    place_action_client_->sendGoal(click_goal);
    while (!place_action_client_->waitForResult(ros::Duration(0.5)) && priv_nh_.ok())
    {
    }
    if (!priv_nh_.ok()) 
    {
      place_action_client_->cancelGoal();
      return false;
    }
    
    if (place_action_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      ROS_INFO("The gripper click place action has not succeeded;");
      response.error_code.value = response.error_code.OTHER_ERROR;
    }
    else
    {    
      ROS_INFO("The gripper click place action has succeeded;");
      //convert to the base link frame, hard-coded in for now
      geometry_msgs::PoseStamped pose_stamped_in = place_action_client_->getResult()->place_pose;
      pose_stamped_in.header.stamp = ros::Time(0);
      geometry_msgs::PoseStamped pose_stamped;
      try
      {
        listener_.transformPose("base_link",pose_stamped_in, pose_stamped);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("Failed to transform gripper click place to base_link frame; exception: %s", ex.what());
        response.error_code.value = response.error_code.TF_ERROR;
        return true;
      }
      response.place_locations.push_back(pose_stamped);
      response.error_code.value = response.error_code.SUCCESS;
    }
    return true;
  }

public:
  GraspPlanningGripperClick() : 
    root_nh_(""), priv_nh_("~")
  {
    priv_nh_.param<std::string>("grasp_planning_service_topic", grasp_planning_service_topic_, 
                                "/grasp_planning_gripper_click");
    grasp_srv = root_nh_.advertiseService(grasp_planning_service_topic_, 
                                          &GraspPlanningGripperClick::graspServiceCallback, this);
    
    priv_nh_.param<std::string>("pickup_action_topic", pickup_action_topic_, "/gripper_click_pickup_ui");
    pickup_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPickupAction>
      (pickup_action_topic_, true);
    
    priv_nh_.param<std::string>("place_planning_service_topic", place_planning_service_topic_, 
                                "/place_planning_gripper_click");
    place_srv = root_nh_.advertiseService(place_planning_service_topic_, 
                                          &GraspPlanningGripperClick::placeServiceCallback, this);
    
    priv_nh_.param<std::string>("place_action_topic", place_action_topic_, "/gripper_click_place_ui");    
    place_action_client_ = new actionlib::SimpleActionClient<pr2_gripper_click::GripperClickPlaceAction>
      (place_action_topic_, true);

    priv_nh_.param<std::string>("image_topic", image_topic_, "undefined");
    priv_nh_.param<std::string>("disparity_image_topic", disparity_image_topic_, "undefined");
    priv_nh_.param<std::string>("camera_info_topic", camera_info_topic_, "undefined");

    priv_nh_.param<bool>("use_assembler", use_assembler_, true);
    priv_nh_.param<std::string>("rgbd_assembly_service_name", rgbd_assembly_service_name_, "/rgbd_assembly");

    if (use_assembler_)
    {
      while(!ros::service::waitForService(rgbd_assembly_service_name_, ros::Duration(2.0)) && priv_nh_.ok())
      {
        ROS_WARN("waiting for rgbd assembler at %s", rgbd_assembly_service_name_.c_str());
      }
    }
    rgbd_assembly_srv_ = root_nh_.serviceClient<rgbd_assembler::RgbdAssembly>(rgbd_assembly_service_name_, false);

    ROS_INFO("Grasp planning gripper click node started");
  }
  ~GraspPlanningGripperClick()
  {
    delete pickup_action_client_;
    delete place_action_client_;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "grasp_planning_gripper_click_node");
  GraspPlanningGripperClick node;
  ros::spin();
  return 0;
}