==========================================================
==               PACKAGE: haf_grasping              ==
==========================================================

Author:  David Fischinger, Vienna University of Technology
Version: 1.0
Date:    15.5.2015

HAF_GRASPING
is calculating grasp points for unknown and known objects represented by the surface point cloud data.
For scientific foundation see:
D. Fischinger, M. Vincze: "Learning Grasps for Unknown Objects in Cluttered Scenes", IEEE International Conference on Robotics and Automation (ICRA), 2013.
<a href="files/ICRA2013.pdf">[pdf]</a>
D. Fischinger, A. Weiss, M. Vincze: "Learning Grasps with Topographic Features", The International Journal of Robotics Research.


In a first step the point cloud is read from a ROS topic and a heightsgrid is created.
For each 14x14 square of the hightsgrid a featurevector is created. 
Using SVM with an existing model file, it is predicted if the center of the square is a good 
grasping point. For good grasping points the coordinates and the direction of the approach vectors
are published.


DOWNLOAD CODE

>> git 


HOW TO USE HAF_GRASPING - GET STARTED

Start calculation server (does the work), haf_client (small programming incl. class that shows how to use haf_grasping) and a visualization in rviz:

>> roslaunch haf_grasping haf_grasping_all.launch

Publish the path of a point cloud to calculate grasp points on this object with the gripper approaching direction along the z-axis:

>> rostopic pub /haf_grasping/input_pcd_rcs_path std_msgs/String "$(rospack find haf_grasping)/data/pcd2.pcd" -1

(Alternatively, publish a point cloud at the ros topic:  /haf_grasping/depth_registered/single_cloud/points_in_lcs)


EXPLANATION FOR THE RVIZ VISUALIZATION

RVIZ will now visualize the point cloud with corresponding frame (blue indicates the z-axis). 
Bigger rectangle: indicates the area where heights can be used for grasp calculation
Inner rectangle:  defines the area where grasps (grasp centers) are searched.
Long red line:    indicates the closing direction (for a two finger gripper)
Red/green spots:  indicate the positions where grasps are really tested for the current gripper roll (ignoring points where no calculation is needed, e.g. no data there)
Green bars:       indicate where possible grasps were found. The height of the bars indicate an grasp evaluation score (the higher the better)
Black arrow:      indicates the best grasp position found and the approching direction (for a parallel two finger gripper)



HAF-GRASPING CLIENT - CODE EXPLAINDED

In calc_grasppoints_action_client.cpp we subscribe to a point_cloud topic and start the following callback when a point cloud comes in:

== code start ==

//get goal (input point cloud) for grasp calculation, send it to grasp action server and receive result
void CCalcGrasppointsClient::get_grasp_cb(const sensor_msgs::PointCloud2ConstPtr& pc_in)
{
    ROS_INFO("\nFrom calc_grasppoints_action_client: point cloud received");

    // create the action client
    // true causes the client to spin its own thread
    actionlib::SimpleActionClient<haf_grasping::CalcGraspPointsServerAction> ac("calc_grasppoints_svm_action_server", true);

    ROS_INFO("Waiting for action server to start.");
    // wait for the action server to start
    ac.waitForServer(); //will wait for infinite time

    ROS_INFO("Action server started, sending goal.");
    // send a goal to the action
    haf_grasping::CalcGraspPointsServerGoal goal;
    goal.graspinput.input_pc = *pc_in;

    goal.graspinput.grasp_area_center = this->graspsearchcenter;

    // set size of grasp search area
    goal.graspinput.grasp_area_length_x = this->grasp_search_size_x+14;
    goal.graspinput.grasp_area_length_y = this->grasp_search_size_y+14;

    // set max grasp calculation time
    goal.graspinput.max_calculation_time = this->grasp_calculation_time_max;

    //send goal
    ac.sendGoal(goal);

    //wait for the action to return
    bool finished_before_timeout = ac.waitForResult(ros::Duration(50.0));

    if (finished_before_timeout)
    {
        actionlib::SimpleClientGoalState state = ac.getState();
        boost::shared_ptr<const haf_grasping::CalcGraspPointsServerResult_<std::allocator<void> > > result = ac.getResult();
        ROS_INFO("Result: %s", (*(result)).result.data.c_str());
        ROS_INFO("Action finished: %s",state.toString().c_str());
    }
    else
        ROS_INFO("Action did not finish before the time out.");
}

== code end ==


PARAMETER SETTING

There are a number of parameters that can be set (directly or via a service call). Set grasp search center (in m) to (x=0.1,y=0):

Grasp_center: the x-,y-position, that is the center of the area where grasps are searched.
Service call to change it:

>> rosservice call /haf_grasping/set_grasp_center "graspsearchcenter:
  x: 0.10
  y: 0.0
  z: 0.0" 

Grasp_area_size: the size of the area were grasps should be detected. Set rectangle to 16x10 centimeter:

>> rosservice call /haf_grasping/set_grasp_search_area_size "grasp_search_size_x: 16
grasp_search_size_y: 10"

Grasp_calculation_time_max: maximal time in seconds until a grasp has to be returned. Set max timt to 3 sec:

>> rosservice call /haf_grasping/set_grasp_calculation_time_max "max_calculation_time:
  secs: 3
  nsecs: 0" 




== Input == 

A point cloud from objects 


== Output ==

Grasp points and approach vectors which are detected using Support Vector Machines
(at the beginning the approach vectors are parallel to the z-axis) 



== LIBSVM ==

We have included LIBSVM to work as our classifier (go to folder libsvm-3.12 and type "make" after checking out):

Chih-Chung Chang and Chih-Jen Lin, LIBSVM : a library for support
vector machines. ACM Transactions on Intelligent Systems and
Technology, 2:27:1--27:27, 2011. Software available at
http://www.csie.ntu.edu.tw/~cjlin/libsvm