calc_grasppoints_action_server.cpp
Go to the documentation of this file.
1 /*
2  * calc_grasppoints_action_server.cpp
3  *
4  * ActionServer for calculating grasp points with Height Accumulated Features
5  * (see Int. Journal of Robotics Research, 21st July 2015: http://ijr.sagepub.com/content/34/9/1167)
6  *
7  * Author: David Fischinger, Vienna University of Technology
8  * Date: August 2015
9  *
10  * This ActionServer calculates grasp points and approach vectors, given an input-goal
11  * (incl. point cloud and grasp search parameters such as the area where to search for grasps (center and size),
12  * goal_frame_id for point cloud transformation, maximal calculation time, approach vector, ..).
13  * In a first step for the given approach vector and different tested gripper rotations a height grid is created from the point cloud.
14  * For each 14x14 square of the height grid a feature vector is created.
15  * Using SVM with an existing model file, it is predicted if the center of the square is a good
16  * grasping point. For the best grasping point the coordinates and the direction of the approach vector and the gripper orientation
17  * is published.
18  *
19  *
20  * == Input ==
21  *
22  * An action goal including a point cloud of objects (in a local coordinates system (lcs) where z-axis is pointing upwards)
23  *
24  * == Output ==
25  *
26  * Grasp points and approach vector which are detected using Support Vector Machines with Height Accumulated Features
27  * Format: eval, gp1_x, gp1_y, gp1_z, gp2_x, gp2_y, gp2_z, appr_dir_x, appr_dir_y, appr_dir_z roll
28  * - eval: an evaluation value between 10 and 99. (99 is the top value)
29  * - gp1_x: x value for the first of the two grasp points
30  * - gp2_x: x value for the second grasp point (analog for y- and z-values)
31  * - appr_dir_x, appr_dir_y, appr_dir_z: approach direction; direction that has to be used to approach to the object with the grippers tcp
32  * - roll: the roll orientation of the gripper in degree
33  *
34  *
35  * == USAGE ==
36  *
37  * us the calc_grasppoints_action_client for convenient usage
38  *
39  * == PARAMETERS ==
40  *
41  * outputpath_full = "/tmp/features.txt";
42  * path_svm_output = "/tmp/output_calc_gp.txt";
43  *
44  */
45 
46 
47 //System includes
48 #include <stdio.h>
49 #include <stdlib.h>
50 #include <sstream>
51 #include <iostream>
52 #include <iomanip>
53 #include <unistd.h>
54 #include <math.h>
55 #include <time.h>
56 //tf
57 #include <tf/tf.h>
58 #include <tf/transform_broadcaster.h>
59 #include <tf/transform_datatypes.h>
60 #include <tf/transform_listener.h>
61 #include "tf/LinearMath/Transform.h"
62 //PCL includes
63 #include <pcl_ros/point_cloud.h>
64 #include "pcl_ros/transforms.h"
65 #include "pcl_ros/publisher.h"
66 #include "pcl/io/io.h"
67 #include "pcl/point_types.h"
68 #include "pcl/point_cloud.h"
69 #include "pcl/registration/ia_ransac.h"
70 #include <pcl/point_types.h>
71 #include <pcl/io/pcd_io.h>
72 //ROS includes
73 #include <ros/ros.h>
74 #include "ros/ros.h"
75 #include "ros/package.h"
76 #include "std_msgs/String.h"
77 #include "sensor_msgs/PointCloud2.h"
78 #include "geometry_msgs/Vector3.h"
79 #include <visualization_msgs/Marker.h>
80 #include <visualization_msgs/MarkerArray.h>
81 //Actionlib
82 #include <actionlib/server/simple_action_server.h>
83 #include <haf_grasping/CalcGraspPointsServerAction.h>
84 
85 
86 
87 //#include <opencv/cv.h>
88 
89 #include <CIntImage_to_Featurevec.h>
90 
91 //new HEIGHT and WIDTH have to be next higher even number of the up-rounded square root of (HEIGHT*HEIGHT+WIDTH*WIDTH)
92 #define HEIGHT 56
93 #define WIDTH 56
94 #define PI 3.141592653
95 #define ROLL_STEPS_DEGREE 15 //define in degree how different the tested rolls should be
96 #define TILT_STEPS_DEGREE 40 //define in degree the angle for one tilt step
97 #define TILT_STEPS 1 //define the number of tilt steps [not used anymore]
98 
99 //define maximal degree of rotation of box, should be normally 180+ROLL_STEPS_DEGREE because in OR the opposite
100 //grasp is always tested (=> overall 360 degree). Set this value to ROLL_STEPS_DEGREE if only the first rotation should be checked!
101 #define ROLL_MAX_DEGREE 190//190
102 
103 
104 using namespace std;
105 //using namespace cv;
106 
107 class CCalc_Grasppoints
108 {
109 protected:
110  // define ROS node
111  ros::NodeHandle nh_;
112  // define simple action server for grasp calculation
113  actionlib::SimpleActionServer<haf_grasping::CalcGraspPointsServerAction> as_;
114  std::string action_name_;
115  // create messages that are used to publish feedback/result
116  haf_grasping::CalcGraspPointsServerFeedback feedback_;
117  haf_grasping::CalcGraspPointsServerResult result_;
118 
119 
120 public:
121  ros::Subscriber box_position_sub; //subscriber for x and y coordinates of the box center and the rotation
122  ros::Subscriber pc_sub; //subscriber for the point cloud (point of objects without basket)
123  ros::Publisher pubInputPCROS;
124  ros::Publisher pubTransformedPCROS; //publisher for transformed point cloud for visualization purpose
125  ros::Publisher pubGraspPoints; //publisher for grasp points
126  ros::Publisher pubGraspPointsEval; //publisher for grasp points with evaluation at the first 2 position (value: 10-99)
127  ros::Publisher vis_pub; //Marker (for visualization of grasps)
128  ros::Publisher vis_pub_ma; //MarkerArray (for visualization of grasps)
129  ros::Publisher vis_pub_ma_params; //MarkerArray (for visualization of grasp parameters: search filed size, gripper closing direction,..)
130  geometry_msgs::Point graspsearchcenter; //substitute for box_center_x/y => origin of approach vector
131  float boxrot_angle_init; //angle the box is rotated in real world w.r.t. "default" position
132  int grasp_search_area_size_x_dir; // defines the size (x-direction) of the rectangle where grasps are searched
133  int grasp_search_area_size_y_dir; // defines the size (y-direction) of the rectangle where grasps are searched
134  geometry_msgs::Vector3 approach_vector; // defines approach direction for grasp
135  float max_duration_for_grasp_calc; //maximal time (in seconds) before a result is returned
136  float heightsgridroll[ROLL_MAX_DEGREE/ROLL_STEPS_DEGREE][TILT_STEPS][HEIGHT][WIDTH];
137  float integralimageroll[ROLL_MAX_DEGREE/ROLL_STEPS_DEGREE][TILT_STEPS][HEIGHT+1][WIDTH+1];
138  bool point_inside_box_grid[ROLL_MAX_DEGREE/ROLL_STEPS_DEGREE][TILT_STEPS][HEIGHT][WIDTH]; //saves for which points featurevector is calculated
139  string outputpath_full;
140  bool return_only_best_gp;
141  string base_frame_id; //base frame for calculation of grasps
142  int graspval_th; //treshold if grasp hypothesis should be returned
143  int graspval_top; //optimal grasp evaluation possible
144  int graspval_max_diff_for_pub;
145  //define x,y,and roll for top grasp for overall grasping (all tilts, all rows)
146  int id_row_top_overall;
147  int id_col_top_overall;
148  int nr_roll_top_overall;
149  int nr_tilt_top_overall;
150  int topval_gp_overall;
151  int marker_cnt;
152  haf_grasping::GraspOutput gp_result; //saves return values for ActionServer
153  tf::TransformListener tf_listener;
154  bool visualization;
155  bool print_heights_bool; //indicates if grid heights should be printed on screen
156  Eigen::Matrix4f av_trans_mat; //transformation matrix for approach vector
157  float trans_z_after_pc_transform; // additional translation in z-axis direction to guarantee that all height values are bigger zero (for haf-calculation)
158  tf::Quaternion quat_tf_to_tf_help; //saves quaternion for axis transformation
159  string feature_file_path; //path to files of Features (Features.txt)
160  string range_file_path;
161  string svmmodel_file_path;
162  int nr_features_without_shaf;
163  int gripper_opening_width;
164 
165 
166  void print_heights(int nr_roll, int nr_tilt);
167  void read_pc_cb(const haf_grasping::CalcGraspPointsServerGoalConstPtr &goal); // receive action goal incl. point cloud and grasp search parameters
168  void loop_control(pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in);
169  void generate_grid(int roll, int tilt, pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in);
170  void publish_transformed_pcl_cloud(int roll, int tilt, pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in);
171  void calc_intimage(int roll, int tilt);
172  void calc_featurevectors(int roll, int tilt);
173  void pnt_in_box(int nr_roll, int nr_tilt);
174  void predict_bestgp_withsvm(bool svm_with_probability=false);
175  void show_predicted_gps(int nr_roll, int tilt, bool svm_with_probability=false);
176  void transform_gp_in_wcs_and_publish(int id_row_top_all, int id_col_top_all,int nr_roll_top_all, int nr_tilt_top_all, int scaled_gp_eval);
177  void publish_grasp_grid(int nr_roll, int tilt, float graspsgrid[][WIDTH], int gripperwidth);
178  void gp_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, float green, bool pubmarker, int gripperwidth, int nr_roll, bool pub_grip_open_dir, bool pub_grid);
179  void grasp_area_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, int gripperwidth, int nr_roll, int param_id, bool top_grasp);
180 
181  CCalc_Grasppoints(string name) :
182  as_(nh_, name, boost::bind(&CCalc_Grasppoints::read_pc_cb, this, _1), false),
183  action_name_(name)
184  {
185  this->pubGraspPointsEval = nh_.advertise<std_msgs::String>("/haf_grasping/grasp_hypothesis_with_eval", 1);
186  this->vis_pub = nh_.advertise<visualization_msgs::Marker>( "visualization_marker", 1 ); //Marker
187  this->vis_pub_ma = nh_.advertise<visualization_msgs::MarkerArray>( "visualization_marker_array", 1 ); //MarkerArray
188  this->vis_pub_ma_params = nh_.advertise<visualization_msgs::MarkerArray>( "visualization_marker_array_grasp_params", 1 ); //MarkerArray for grasp params
189  this->pubInputPCROS = nh_.advertise<sensor_msgs::PointCloud2>( "/haf_grasping/calc_gp_as_inputpcROS", 1);
190  this->pubTransformedPCROS = nh_.advertise<sensor_msgs::PointCloud2>( "/haf_grasping/transformed_point_cloud",1);
191  this->graspsearchcenter.x = 0; //default value
192  this->graspsearchcenter.y = 0; //default value
193  this->graspsearchcenter.z = 0; //default value
194  this->grasp_search_area_size_x_dir = 32;
195  this->grasp_search_area_size_y_dir = 44;
196  this->approach_vector.x = 0; //default value
197  this->approach_vector.y = 0; //default value
198  this->approach_vector.z = 1; //default value
199  this->max_duration_for_grasp_calc = 50; //max calculation time before restult is returned (in sec)
200  outputpath_full = "/tmp/features.txt";
201  this->return_only_best_gp = false;
202  graspval_th = 70; //treshold if grasp hypothesis should be returned (in function - so program internal) (for top result of one loop run)
203  graspval_top = 119;
204  graspval_max_diff_for_pub = 80; //if the value of grasps is more than graspval_max_diff_for_pub lower than optimal value graspval_top, nothing gets published (for top result of one whole roll run)
205  id_row_top_overall = -1;
206  id_col_top_overall = -1;
207  nr_roll_top_overall = -1;
208  nr_tilt_top_overall = -1;
209  topval_gp_overall = -1000;
210  marker_cnt = 0;
211  this->visualization = true;
212  this->print_heights_bool = false;
213  this->av_trans_mat = Eigen::Matrix4f::Identity();; //transformation matrix for approach vector
214  this->trans_z_after_pc_transform = 0.15;
215  this->gripper_opening_width = 1;
216 
217  //parameter for svm classification
218  this->feature_file_path = "";
219  nh_.param("feature_file_path", this->feature_file_path, this->feature_file_path);
220  this->range_file_path = "";
221  nh_.param("range_file_path", this->range_file_path, this->range_file_path);
222  this->svmmodel_file_path = "";
223  nh_.param("svmmodel_file_path", this->svmmodel_file_path, this->svmmodel_file_path);
224  this->nr_features_without_shaf = 302; //default value
225  nh_.param("nr_features_without_shaf", this->nr_features_without_shaf, this->nr_features_without_shaf);
226 
227  as_.start();
228  }
229 };
230 
231 
232 
233 // Print heights of heightsgridroll
234 void CCalc_Grasppoints::print_heights(int nr_roll, int nr_tilt)
235 {
236  //print heights matrix
237  cout << "\n CCalc_Grasppoints::print_heights: print heights matrix for roll number : " << nr_roll << "\t and tilt nr: "<< nr_tilt << endl;
238  for (int i = 0; i < HEIGHT; i++){ //rows
239  for (int j = 0; j < WIDTH; j++){ //cols
240  cout << setw(5) << setprecision(2) << this->heightsgridroll[nr_roll][nr_tilt][HEIGHT-i-1][WIDTH-j-1];
241  }
242  cout << "\n";
243  }
244 }
245 
246 
247 
248 // Callback triggered by input goal: variable are set from the goal and the loop for grasp point calculation is started
249 // Input: action goal including point clout, approach direction, center position where to grasp (x-,y-,z-coordinates), size of area where grasps are searched
250 void CCalc_Grasppoints::read_pc_cb(const haf_grasping::CalcGraspPointsServerGoalConstPtr &goal)
251 {
252  ROS_INFO("\n ==> calc_grasppoints_action_server.cpp: read_pc_cb() --> GRASP GOAL RECEIVED (incl. point cloud)");
253  //transform point cloud to PCL
254  pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in;
255  pcl::PointCloud<pcl::PointXYZ> pc_new_cs;
256 
257  // set center of area where grasps are searched
258  this->graspsearchcenter.x = goal->graspinput.grasp_area_center.x;
259  this->graspsearchcenter.y = goal->graspinput.grasp_area_center.y;
260  this->graspsearchcenter.z = goal->graspinput.grasp_area_center.z;
261 
262  cout << "************************************************************************************************************" << endl;
263  cout << " --> SET NEW GRASP SEARCH CENTER: [x,y,z] = [" << this->graspsearchcenter.x << "," << this->graspsearchcenter.y << "," << this->graspsearchcenter.z << "]" << endl;
264 
265  // define size of area where to search for grasp points
266  this->grasp_search_area_size_x_dir = goal->graspinput.grasp_area_length_x;
267  this->grasp_search_area_size_y_dir = goal->graspinput.grasp_area_length_y;
268 
269  //define grasp approach vector (and normalize)
270  float vector_length = sqrt(goal->graspinput.approach_vector.x*goal->graspinput.approach_vector.x+goal->graspinput.approach_vector.y*goal->graspinput.approach_vector.y+goal->graspinput.approach_vector.z*goal->graspinput.approach_vector.z);
271  this->approach_vector.x = goal->graspinput.approach_vector.x/vector_length;
272  this->approach_vector.y = goal->graspinput.approach_vector.y/vector_length;
273  this->approach_vector.z = goal->graspinput.approach_vector.z/vector_length;
274  cout << " --> SET APPROACH VECTOR TO: " << " [x,y,z] = [" << this->approach_vector.x << "," << this->approach_vector.y << "," << this->approach_vector.z << "]" << endl;
275 
276  // set maximal calculation time
277  this->max_duration_for_grasp_calc = (float) goal->graspinput.max_calculation_time.toSec();
278  cout << " --> SET MAX. CALCULATION TIME TO: " << this->max_duration_for_grasp_calc << endl;
279 
280  // set pre-grasp gripper opening width (factor f for scaling pc to imitate gripper opening width of 1/f of maximum)
281  this->gripper_opening_width = (float) goal->graspinput.gripper_opening_width;
282 
283  // set if only the best grasp should be visualized
284  this->return_only_best_gp = (bool) goal->graspinput.show_only_best_grasp;
285  cout << " --> SET show_only_best_grasp TO: " << this->return_only_best_gp << endl;
286 
287  // set frame_id needed as base frame for calculation
288  string orig_tf = (string) goal->graspinput.input_pc.header.frame_id;
289  cout << " --> FRAME_ID ORIGINAL Point Cloud: " << orig_tf << endl;
290 
291  cout << "Fixed by now: this->trans_z_after_pc_transform: " << this->trans_z_after_pc_transform << endl;
292 
293  // set base_frame_id to goal_frame_id if set
294  if (!goal->graspinput.goal_frame_id.empty())
295  {
296  this->base_frame_id = goal->graspinput.goal_frame_id;
297  }
298  else
299  {
300  this->base_frame_id = "/base_link";
301  }
302  cout << " --> BASE_FRAME_ID: " << this->base_frame_id << endl;
303 
304  cout << "************************************************************************************************************" << endl;
305 
306  //search for tf transform between camera and robot coordinate system
307  bool foundTransform = tf_listener.waitForTransform(this->base_frame_id, orig_tf, goal->graspinput.input_pc.header.stamp, ros::Duration(1.0));
308  if (!foundTransform)
309  {
310  ROS_WARN(" ==> calc_grasppoints_action_server.cpp: read_pc_cb(): NO TRANSFORM FOR POINT CLOUD FOUND");
311  }
312 
313  pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in_old_cs;
314  pcl::fromROSMsg(goal->graspinput.input_pc, pcl_cloud_in_old_cs); // transform ROS msg into PCL-pointcloud
315 
316  pcl_ros::transformPointCloud(this->base_frame_id, pcl_cloud_in_old_cs, pc_new_cs, tf_listener);
317 
318  //publish input pc:
319  this->pubInputPCROS.publish(goal->graspinput.input_pc);
320 
321  //set initial values for row,col,tilt,topval for top grasp points
322  id_row_top_overall = -1;
323  id_col_top_overall = -1;
324  nr_roll_top_overall = -1;
325  nr_tilt_top_overall = -1;
326  topval_gp_overall = -1000;
327 
328  loop_control(pc_new_cs);
329 }
330 
331 
332 
333 //Main function: controls the sequence and execution of class methods
334 //loop goes through all rolls [tilts are currently not used] and executes necessary methods for calculation of gps
335 void CCalc_Grasppoints::loop_control(pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in)
336 {
337  int time_for_calc_in_secs = this->max_duration_for_grasp_calc;
338  time_t start,end;
339  time (&start);
340  double timedif;
341  bool success = true;
342 
343  for (int tilt = 0; tilt < TILT_STEPS; tilt++) // tilt loop
344  {
345  for (int roll = 0; roll < ROLL_MAX_DEGREE/ROLL_STEPS_DEGREE; roll++) // roll loop
346  {
347  // feedback: (3.12.2014)
348  feedback_.feedback.data = "this->topval_gp_overall"; //shows best gp so far
349 
350  if (as_.isPreemptRequested() || !ros::ok() || success == false)
351  {
352  ROS_INFO("%s: Preempted", action_name_.c_str());
353  // set the action state to preempted
354  as_.setPreempted();
355  success = false;
356  break;
357  }
358 
359  as_.publishFeedback(feedback_);
360 
361 
362  if (this->return_only_best_gp and (this->topval_gp_overall >= this->graspval_top)){
363  cout << "Top grasp already found\n";
364  break;
365  }
366 
367  //calculate only for defined time
368  time (&end);
369  timedif = difftime (end,start);
370  cout << "\n ===> TEST ROTATION: " << roll*ROLL_STEPS_DEGREE << "\nRuntime so far (in sec): " << timedif << endl;
371  if (timedif > time_for_calc_in_secs) {
372  cout << "\n Calculation time is over, stop calculation of grasp points!!\n";
373  break;
374  }
375 
376  generate_grid(roll, tilt, pcl_cloud_in);
377  if (this->print_heights_bool){
378  print_heights(roll,tilt);
379  }
380  calc_intimage(roll, tilt);
381  calc_featurevectors(roll, tilt);
382  //ii_to_fv->print_heights(ii_to_fv->intimagemat);
383  bool svm_with_probability = false;
384  predict_bestgp_withsvm(svm_with_probability);
385  show_predicted_gps(roll, tilt, svm_with_probability);
386  }
387  }
388  //publish original point cloud (to see where grasp was found)
389  publish_transformed_pcl_cloud(0, 0, pcl_cloud_in); //publish point cloud input (as received at beginning)
390  transform_gp_in_wcs_and_publish(this->id_row_top_overall, this->id_col_top_overall, this->nr_roll_top_overall,this->nr_tilt_top_overall, this->topval_gp_overall-20); //last entry is "scaled" (=> -16)
391 
392  time (&end);
393  timedif = difftime (end,start);
394  cout << "\n Gesamtzeit fuer Loop: " << timedif << endl;
395 
396  if(success) // ActionServer: return grasp representation (overall best grasp)
397  {
398  result_.graspOutput = this->gp_result;
399  ROS_INFO_STREAM("Succeeded:\n" << this->gp_result);
400  as_.setSucceeded(result_);
401  }
402 }
403 
404 
405 // transforms point cloud and generates height grid
406 void CCalc_Grasppoints::generate_grid(int roll, int tilt, pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in)
407 {
408  pcl::PointCloud<pcl::PointXYZ> pcl_cloud_transformed;
409  int nr_rows = HEIGHT, nr_cols = WIDTH;
410  float r_col_m = (0.5 * (float)nr_cols)/100.0; //"Matrix radius" in meter for cols
411  float r_row_m = (0.5 * (float)nr_rows)/100.0; //"Matrix radius" in meter for rows
412  float rot_about_z; //rotation needed about z-axis to align y-axis (old cs) with orthoganal projection of new z-axis on x-y-plane (rad)
413  float rot_about_x = 0; //rotation needed to align z axis old with z-axis new after rot_about_z was executed (this way old x-axis keeps direction in x-y-plane) (rad)
414  pcl::PointXYZ pnt; //point for loop
415  pcl::PointXYZ *av = new pcl::PointXYZ(0,0,-1); //approach vector (default would be (0,0,1) because we define AV in a way that it is equal to new z-axis for grasp calculation
416 
417 
418  av->x = this->approach_vector.x;
419  av->y = this->approach_vector.y;
420  av->z = this->approach_vector.z;
421  //cout << "\nApproach vector: [" << av->x << "," << av->y << "," << av->z << "]" << endl;
422 
423  Eigen::Matrix4f mat_sh_to_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from search center to origin
424  Eigen::Matrix4f mat_rot_z_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about z-axis)
425  Eigen::Matrix4f mat_rot_x_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about x-axis)
426  Eigen::Matrix4f mat_sh_from_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from origin to basket center
427  Eigen::Matrix4f mat_rot = Eigen::Matrix4f::Identity(); //matrix which rotates pc and then shifts pc back to box center
428  Eigen::Matrix4f mat_scale_x_dir = Eigen::Matrix4f::Identity();//NEW grippertest: scale point cloud
429 
430  Eigen::Matrix4f mat_transform = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about box center)
431 
432 
433  mat_scale_x_dir(0,0) = this->gripper_opening_width; //NEW grippertest: scale point cloud in x-axis direction
434 
435  mat_sh_to_orig(0,3) = -this->graspsearchcenter.x; //shift x value
436  mat_sh_to_orig(1,3) = -this->graspsearchcenter.y; //shift y-value
437  mat_sh_to_orig(2,3) = -this->graspsearchcenter.z; //shift z-value
438 
439  mat_sh_from_orig(0,3) = 0;// this->graspsearchcenter.x; //!!!! 17.8.2015
440  mat_sh_from_orig(1,3) = 0;//this->graspsearchcenter.y;
441  mat_sh_from_orig(2,3) = /*this->graspsearchcenter.z*/0 + this->trans_z_after_pc_transform; //move pc up to make calculation possible
442 
443 
444  if (av->y == 0 and av->x == 0){ //if av->y = av->x = 0
445  rot_about_z = 0;
446  if (av->z >= 0) {
447  rot_about_x = 0; //grasp from top
448  } else {
449  rot_about_x = PI; //grasp from upside down (in practice hardly relevant)
450  }
451  } else { //av->y <> 0 or = av->x <> 0
452  rot_about_z = 90*PI/180.0 - atan2(av->y, av->x); // av->y, av->x not both 0
453  rot_about_x = 90*PI/180.0 - atan2( av->z, sqrt(av->y*av->y + av->x*av->x) );
454  }
455 
456  //cout << "rot_about_z: " << rot_about_z << "\nav->y: " << av->y << "\nav->x: " << av->x << endl;
457  //cout << "rot_about_x: " << rot_about_x << "\nav->z: " << av->z << "\nav->y: " << av->y << endl;
458 
459  //define matrices s.t. transformation matrix can be calculated for point cloud (=> roll and tilt are simulated)
460 
461  //define rotation about z-axis (for roll loop):
462  float angle = roll*ROLL_STEPS_DEGREE*PI/180;//roll*ROLL_STEPS_DEGREE*PI/180 + this->boxrot_angle_init; //angle for roll
463  mat_rot(0,0) = cos(angle);
464  mat_rot(0,1) = -sin(angle);
465  mat_rot(1,0) = sin(angle);
466  mat_rot(1,1) = cos(angle);
467 
468  //define rotation about z-axis:
469  mat_rot_z_axis(0,0) = cos(rot_about_z);
470  mat_rot_z_axis(0,1) = -sin(rot_about_z);
471  mat_rot_z_axis(1,0) = sin(rot_about_z);
472  mat_rot_z_axis(1,1) = cos(rot_about_z);
473 
474 
475  //define rotation about x-axis
476  mat_rot_x_axis(1,1) = cos(rot_about_x);
477  mat_rot_x_axis(1,2) = -sin(rot_about_x);
478  mat_rot_x_axis(2,1) = sin(rot_about_x);
479  mat_rot_x_axis(2,2) = cos(rot_about_x);
480 
481  // transforms pc in a way that the old coordinate system is transformed to the new one by rotation about first: z-axis, second (new) x-axis
482  //NEW: point cloud scaling for gripper opening
483  mat_transform = mat_scale_x_dir * mat_rot* mat_sh_from_orig * mat_rot_x_axis * mat_rot_z_axis * mat_sh_to_orig; //define transformation matrix mat_transform
484  this->av_trans_mat = mat_transform; //class variable to transform visualization stuff
485 
486 
487  pcl::copyPointCloud(pcl_cloud_in, pcl_cloud_transformed);
488  pcl::transformPointCloud(pcl_cloud_in, pcl_cloud_transformed, mat_transform); //transform original point cloud
489 
490 
491  //publish transformed point cloud:
492  if (this->visualization)
493  {
494  cout << "Publish transformed point cloud! \n";
495  this->pubTransformedPCROS.publish(pcl_cloud_transformed);
496  }
497 
498  //set heightsgridroll to -1.0 at each position
499  for (int i = 0; i < nr_rows; i++)
500  for (int j = 0; j < nr_cols; j++)
501  this->heightsgridroll[roll][tilt][i][j]= -1.0; //needed because after tilting (now: general pc transform) values below 0 possible
502 
503  //make heightsgrid (find highest points for each 1x1cm rectangle); translate grid s.t. it starts with (0,0)
504  for (size_t i = 0; i < pcl_cloud_transformed.points.size(); ++i)
505  {
506  //In the current implementation the point cloud was centered about origin (at least x-/y-coordinates), before grasps are calculated w.r.t. graspsearchcenter
507  //=> no shift is needed here
508  int idx_x = -1, idx_y = -1;
509  pnt = pcl_cloud_transformed.points[i];
510  if ((pnt.x > /*this->graspsearchcenter.x*/-r_row_m) and (pnt.x < /*this->graspsearchcenter.x+*/ r_row_m) and
511  (pnt.y > /*this->graspsearchcenter.y*/-r_col_m) and (pnt.y < /*this->graspsearchcenter.y+*/r_col_m))
512  { //point is relevant for object grid
513  idx_x = (int) (floor (100*(pnt.x - (/*this->graspsearchcenter.x*/ - r_row_m))));
514  idx_y = (int) (floor (100*(pnt.y - (/*this->graspsearchcenter.y*/ - r_col_m))));;
515  if (heightsgridroll[roll][tilt][idx_x][idx_y] < pnt.z)
516  {
517  heightsgridroll[roll][tilt][idx_x][idx_y] = pnt.z;
518  }
519  }
520  }
521 
522  for (int i = 0; i < nr_rows; i++){
523  for (int j = 0; j < nr_cols; j++){
524  if (this->heightsgridroll[roll][tilt][i][j] < -0.99){
525  this->heightsgridroll[roll][tilt][i][j] = 0; //set values where no points are, to zero
526  }
527  }
528  }
529 }
530 
531 
532 //function by now only used to publish original point cloud without transformations
533 void CCalc_Grasppoints::publish_transformed_pcl_cloud(int roll, int tilt, pcl::PointCloud<pcl::PointXYZ> pcl_cloud_in)
534 {
535  pcl::PointCloud<pcl::PointXYZ> pcl_cloud_transformed;
536 
537  Eigen::Matrix4f mat_sh_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from center to origin
538  Eigen::Matrix4f mat_rot = Eigen::Matrix4f::Identity(); //matrix which rotates pc and then shifts pc back to box center
539  Eigen::Matrix4f mat_tilt = Eigen::Matrix4f::Identity(); //matrix which tilts pc NEW
540  Eigen::Matrix4f mat_sh_from_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from origin to basket center
541  Eigen::Matrix4f mat_transform = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about box center)
542 
543  mat_sh_orig(0,3) = -this->graspsearchcenter.x; //shift x value
544  mat_sh_orig(1,3) = -this->graspsearchcenter.y; //shift y-value
545  mat_sh_from_orig(0,3) = this->graspsearchcenter.x;
546  mat_sh_from_orig(1,3) = this->graspsearchcenter.y;
547 
548  //define matrices s.t. transformation matrix can be calculated for point cloud (=> roll and tilt are simulated)
549 
550  //next 5 lines could be outside tilt-loop
551  float angle = roll*ROLL_STEPS_DEGREE*PI/180 + this->boxrot_angle_init; //angle for roll
552  mat_rot(0,0) = cos(angle);
553  mat_rot(0,1) = -sin(angle);
554  mat_rot(1,0) = sin(angle);
555  mat_rot(1,1) = cos(angle);
556 
557  float beta = -tilt*TILT_STEPS_DEGREE*PI/180; //angle for tilt in Rad
558  mat_tilt(0,0) = cos(beta);
559  mat_tilt(0,2) = -sin(beta);
560  mat_tilt(2,0) = sin(beta);
561  mat_tilt(2,2) = cos(beta);
562 
563  mat_transform = mat_sh_from_orig*mat_tilt*mat_rot*mat_sh_orig; //define transformation matrix mat_transform
564 
565  pcl::copyPointCloud(pcl_cloud_in, pcl_cloud_transformed); // copy notwendig??
566  pcl::transformPointCloud(pcl_cloud_in, pcl_cloud_transformed, mat_transform); //transform original point cloud
567 
568 
569  //publish transformed point cloud:
570  this->pubTransformedPCROS.publish(pcl_cloud_transformed);
571 }
572 
573 
574 
575 
576 //calculates integral image of heights grid
577 void CCalc_Grasppoints::calc_intimage(int roll, int tilt)
578 {
579  cv::Mat heightsIntegral;
580  heightsIntegral = cv::Mat(HEIGHT+1, WIDTH+1, CV_64FC1);
581  //cout << "\n print integral height matrix on screen\n";
582 
583  double *TempHeights = (double *)malloc(HEIGHT * WIDTH * sizeof(double));
584 
585  int k = 0;
586  for(int i = 0; i < HEIGHT; i++ )
587  for(int j = 0; j < WIDTH; j++ )
588  {
589  TempHeights[k] = this->heightsgridroll[roll][tilt][i][j];
590  k++;
591  }
592 
593  cv::Mat const cvheightsmat = cv::Mat(HEIGHT, WIDTH, CV_64FC1, TempHeights);
594  //Mat const cvheightsmat = Mat(HEIGHT, WIDTH, CV_64FC1, this->heightsgridroll[roll][tilt]);
595  integral(cvheightsmat, heightsIntegral,CV_64FC1);
596 
597 
598  //copy integral image from opencv format to normal matrix format (better implementation would be fine)
599  for (int row = 0; row < 1+HEIGHT; row++) //rows
600  for (int col = 0; col < 1+WIDTH; col++) //cols
601  this->integralimageroll[roll][tilt][row][col] = ((double*)(heightsIntegral.ptr() + heightsIntegral.step*row))[col];
602 
603  //print integral matrix
604  /*cout << "\n print integral height matrix on screen (intuitiv way)\n";
605  for (int row = 0; row < 1+HEIGHT; row++){ //rows
606  cout << endl;
607  for (int col = 0; col < 1+WIDTH; col++){ //cols
608  cout << this->integralimageroll[roll][tilt][HEIGHT-row][WIDTH-col] << "\t";
609  }
610  }*/
611 
612  free(TempHeights);
613 }
614 
615 
616 void CCalc_Grasppoints::calc_featurevectors(int roll, int tilt)
617 {
618  //cout << "\n calc_featurevectors \n";
619 
620  //create object for calculating features
621  CIntImage_to_Featurevec * ii_to_fv = new CIntImage_to_Featurevec();
622 
623  string pkg_path = ros::package::getPath("haf_grasping");
624  //read all features (saved in external file)
625  //ii_to_fv->read_features(pkg_path); feature_file_path
626  if (this->feature_file_path == ""){
627  this->feature_file_path = pkg_path + "/data/Features.txt"; //path to default Features
628  }
629  ii_to_fv->read_features(this->feature_file_path);
630 
631  //silly way to delete file
632  ofstream output_fv_file(outputpath_full.c_str());
633 
634 
635  pnt_in_box(roll, tilt); //calculate if points are inside box, saved in this->point_inside_box_grid[roll][nr_tilt][][]
636 
637  for (int row = 0; row < HEIGHT - 14; row++){
638  for (int col = 0; col < WIDTH -14; col++)
639  {
640  //check if grasping point is possible (inside box) ("inside box detection code")
641  if (!this->point_inside_box_grid[roll][tilt][row+7][col+7]){ //point not inside box (not relevant)
642  continue; //do not generate feature vector for this point
643  }
644  //if (row == 10 and col == 10) cout << "\n one 15x15 intimage: " << ++cnt << "\n";
645  //write integral image (15x15!) => better implementation will come later
646  for (int i = 0; i < 15; i++){
647  for (int j = 0; j < 15; j++){
648  ii_to_fv->intimagemat[i][j] = this->integralimageroll[roll][tilt][row+i][col+j];
649  }
650  }
651 
652  //calculate featurevector and write it in file
653  ii_to_fv->write_featurevector(outputpath_full.c_str(), this->nr_features_without_shaf);
654  }
655  }
656 }
657 
658 
659 
660 
661 // (short: calculates if for a position, the feature vector should be calculated)
662 //calculates if for a given row and column and a specified number of roll,
663 //the indicated point (center of 14x14 square) is inside the real box
664 //AND (new since 6.2.2012): if the point is surrounded by 14x14 integral square!! => feature calculation is possible
665 // !!!!!!!!!!!!!! at the moment nr_tilt is not used for more exact calculation !!!!!!!!!!
666 void CCalc_Grasppoints::pnt_in_box(int nr_roll, int nr_tilt){
667 
668  bool print_pnt_in_box = false;
669  //function:
670  // ( cx ) ( cos(alpha) )
671  //g: ( ) + lambda * ( ) = r
672  // ( cy ) ( sin(alpha) )
673 
674  //function parallel to center line:
675  // ( cx ) (-sin(alpha) ( cos(alpha) )
676  //g: ( ) +/- ( )*DIMBOX/2 + lambda * ( ) = r
677  // ( cy ) ( cos(alpha) ( sin(alpha) )
678 
679  float alpha_deg = -nr_roll*ROLL_STEPS_DEGREE - this->boxrot_angle_init * 180/PI;//angle for rotation in degree
680  float alpha = alpha_deg*PI/180; //angle for rotation in rad
681  float cx = HEIGHT/2; //x-coordinate for rotation center works because !! HEIGHT and WIDTH are equal!!
682  float cy = HEIGHT/2; //y-coordinate for rotation center works because !! HEIGHT and WIDTH are equal!!
683  float cx1,cy1,cx2,cy2,cx3,cy3,cx4,cy4; //coordinates of points which are used to define points inside the box (points at box edges)
684 
685  //define points at the center of each of the 4 boarder edges of box
686  float boarder = 7.0; //ungraspable inside in box (because hand would touch box)
687  float height_r = this->grasp_search_area_size_x_dir/2 - boarder; //defines the half of the length of one side of the graspable inside of box (defines rectangle in box rectangle where grasping is possible)
688  float width_r = this->grasp_search_area_size_y_dir/2 - boarder; //width is equal to y direction!
689  cx1 = cx - sin(alpha)*height_r;
690  cy1 = cy + cos(alpha)*height_r;
691  cx2 = cx + sin(alpha)*height_r;
692  cy2 = cy - cos(alpha)*height_r;
693  cx3 = cx - sin(alpha+PI/2)*width_r;
694  cy3 = cy + cos(alpha+PI/2)*width_r;
695  cx4 = cx + sin(alpha+PI/2)*width_r;
696  cy4 = cy - cos(alpha+PI/2)*width_r;
697 
698  if (print_pnt_in_box)
699  cout << cx1 << "\t" << cy1 << "\n" << cx2 << "\t" << cy2 << "\n" << cx3 << "\t" << cy3 << "\n" << cx4 << "\t" << cy4 << "\n\n";
700  int cnt_all = 0;
701  int cnt_in = 0;
702  int cnt_out = 0;
703  //i = row, j = col
704  for (int i = 0; i < HEIGHT; i++){
705  for (int j = 0; j < HEIGHT; j++){ //(2 times HEIGHT works because HEIGHT and WIDTH are equal!!)
706  ++cnt_all;
707 
708  // parameters to check if calculation of feature vector is needed (if no object points are around a potential grasp position, no calculation necessary)
709  int th_empty_r = 4; //r..radius;if 4 => 8x8cm field is checked if integral image has at least difference of ii_th_in_r
710  float ii_th_in_r = 0.03; //threshold which must be smaller as integral-difference value if featurevec should be calculated
711 
712  // check if point (i,j) is "inside the box" AND not near the overall boarder (of 56x56cm grid)
713  if ((i>6 and i < HEIGHT-7 and j>6 and j<HEIGHT-7) and // take only points which are not in the 7cm boarder of the 56x56 grid (otherwise no featurecalc possible)
714  (this->integralimageroll[nr_roll][nr_tilt][i+th_empty_r][j+th_empty_r]- //check if there are heights (if every height about 0, no calc. of fv needed)
715  this->integralimageroll[nr_roll][nr_tilt][i-th_empty_r-1][j+th_empty_r]-
716  this->integralimageroll[nr_roll][nr_tilt][i+th_empty_r][j-th_empty_r-1]+
717  this->integralimageroll[nr_roll][nr_tilt][i-th_empty_r-1][j-th_empty_r-1] > ii_th_in_r) and
718  (-sin(alpha)*(-cx1+j) + cos(alpha)*(-cy1+i) < 0.00001) and //0.00001 and not 0 because of rounding errors otherwise
719  (-sin(alpha)*(-cx2+j) + cos(alpha)*(-cy2+i) > -0.00001) and
720  ( cos(alpha)*(-cx3+j) + sin(alpha)*(-cy3+i) > -0.00001) and
721  ( cos(alpha)*(-cx4+j) + sin(alpha)*(-cy4+i) < 0.00001)){
722  //if (print_pnt_in_box)
723  // cout << " + "; //point inside box AND (new 6.2.2012) not in boarder of 56x56 grid
724  this->point_inside_box_grid[nr_roll][nr_tilt][i][j] = true;
725  cnt_in++;
726  } else {
727  //if (print_pnt_in_box)
728  // cout << " - "; //point not inside box (or near 56x56 boarder => no 14x14 integral grid available => no calc of fv possible)
729  this->point_inside_box_grid[nr_roll][nr_tilt][i][j] = false;
730  cnt_out++;
731  }
732  }
733  }
734 
735  //print pnt_in_box if variable print_pnt_in_box is set
736  if (print_pnt_in_box){
737  cout << "points_all: " << cnt_all << " points in: " << cnt_in << " and points out: " << cnt_out << endl;
738  for (int i = HEIGHT-1; i >= 0; i--){
739  for (int j = HEIGHT-1; j >= 0; j--){ //(2 times HEIGHT works because HEIGHT and WIDTH are equal!!)
740  if (this->point_inside_box_grid[nr_roll][nr_tilt][i][j] == true){
741  cout << " + "; //point inside box AND (new 6.2.2012) not in boarder of 56x56 grid
742  } else {
743  cout << " - "; //point not inside box (or near 56x56 boarder => no 14x14 integral grid available => no calc of fv possible)
744  }
745  }
746  cout << endl;
747  }
748  }
749 }
750 
751 
752 
753 //Use trained SVM-model for grasp prediction (and scale feature values beforehand)
754 void CCalc_Grasppoints::predict_bestgp_withsvm(bool svm_with_probability){
755  //executes:
756  // ./svm-scale -r ./tools/range /tmp/features.txt > /tmp/features.txt.scale
757  // ./svm-predict /tmp/features.txt.scale ./tools/mixedmanual_features_1000_rand.txt.model output
758 
759  try{
760  string pkg_path = ros::package::getPath("haf_grasping");
761 
762  //scale: use existing scaling file to scale feature values
763  stringstream ss, ss2;
764 
765 
766 
767  if (this->range_file_path == ""){
768  this->range_file_path = pkg_path + "/data/range21062012_allfeatures"; //path to default range file
769  }
770 
771  if (this->svmmodel_file_path == ""){
772  this->svmmodel_file_path = pkg_path + "/data/all_features.txt.scale.model"; //path to default svm model file
773  }
774 
775  ss << pkg_path << "/libsvm-3.12/svm-scale -r " << this->range_file_path << " /tmp/features.txt > /tmp/features.txt.scale";
776  string command = ss.str();
777  int i = system(command.c_str());
778  if (i != 0){
779  ROS_WARN(" CCalc_Grasppoints::predict_bestgp_withsvm() SCALING OF FEATURES WAS NOT EXECUTED AS IT SHOULD (Was SVM code compiled?) ");
780  cout << "===> CCalc_Grasppoints::predict_bestgp_withsvm: System return value for scaling features is: " << i << endl;
781  }
782 
783  //predict grasping points
784  if (!svm_with_probability){
785  // trained with all examples
786  ss2 << pkg_path << "/libsvm-3.12/svm-predict /tmp/features.txt.scale " << this->svmmodel_file_path << " /tmp/output_calc_gp.txt";
787  string command2 = ss2.str();
788  i = system(command2.c_str());
789  } else {
790  // trained with all manual examples, using probability as output
791  i = system("/usr/lib/libsvm/libsvm-3.1/svm-predict -b 1 /tmp/features.txt.scale /usr/lib/libsvm/libsvm-3.1/tools/allmanualfeatures.txt.scale.p.model /tmp/output_calc_gp.txt");
792  }
793  if (i != 0){
794  ROS_WARN(" CCalc_Grasppoints::predict_bestgp_withsvm() GRASP PREDICTION FROM FEATURES WAS NOT EXECUTED AS IT SHOULD (Was SVM code compiled?) ");
795  cout << "===> CCalc_Grasppoints::predict_bestgp_withsvm: System return value for execution of grasp prediction with SVM: " << i << endl;
796  }
797  } catch (int j) {
798  cerr << "ERROR in calc_grasppoints.cpp: predict_bestgp_withsvm()" << endl;
799  }
800 }
801 
802 
803 void CCalc_Grasppoints::show_predicted_gps(int nr_roll, int tilt, bool svm_with_probability){
804  //PARAMETERS
805  string path_svm_output = "/tmp/output_calc_gp.txt";
806 
807  //open file
808  ifstream file_in(path_svm_output.c_str());
809 
810  //define x,y,and roll for top grasp
811  int id_row_top_all = -1;
812  int id_col_top_all = -1;
813  int nr_roll_top_all = -1;
814  int nr_tilt_top_all = -1;
815  int topval_gp_all = -1000;
816 
817  string line;
818  getline(file_in, line);
819 
820  bool printgraspsgrid = false;
821  bool printgraspseval = false;
822  float graspsgrid[HEIGHT][WIDTH];
823  float graspseval[HEIGHT][WIDTH];
824 
825  for (int row = 0; row < HEIGHT; row++){
826  for (int col = 0; col < WIDTH; col++){
827  //check if gp is possible => if featurevector was calculated; no calculation of fv before=> take -1
828  if (this->point_inside_box_grid[nr_roll][tilt][row][col] == false){
829  graspsgrid[row][col] = -1;
830  } else {
831  if (svm_with_probability){
832  int start=0,end=0, res;
833  res = atof(line.substr(0,2).c_str());
834  start = line.find(" ",0);
835  end = line.find(" ",start+1);
836  if (res>0){ //=> is gp => have to take second probability value in file (otherwise first)
837  start = end;
838  end = line.find(" ", start+1);
839  }
840  float prob = atof(line.substr(start,end).c_str());
841  graspsgrid[row][col] = res*prob;
842  } else {
843  graspsgrid[row][col] = atoi(line.substr(0,2).c_str());
844  }
845 
846  getline(file_in, line);
847  }
848  }
849  }
850 
851  //print graspgrid in intuitive way (if variable printgraspsgrid is set true)
852  if (printgraspsgrid){
853  cout << "\n Graspsgrid: intuitiv! \n";
854  for (int row = HEIGHT-1; row >= 0; row--){
855  cout << row << ") \t";
856  for (int col = WIDTH-1; col >= 0; col--){
857  cout << graspsgrid[row][col] << "\t";
858  }
859  cout << "\n";
860  }
861  }
862 
863 
864  if (printgraspseval) cout << "graspseval[row][col]" << "\n\n";
865  int w1=1, w2=2,w3=3,w4=4,w5=55; //weights: direct grasp neighbors have more impact
866  int topval_gp = -1000; //top rated value of all grasping points (same rating => take first gp)
867  int id_row_top = -1, id_col_top = -1;
868  for (int row = 0; row < HEIGHT; row++){
869  for (int col = 0; col < WIDTH; col++){
870  if ( graspsgrid[row][col] < 0 ){
871  graspseval[row][col] = 0;
872  } else {
873  graspseval[row][col] =
874  w1*graspsgrid[row-2][col-2]+w2*graspsgrid[row-2][col-1]+w3*graspsgrid[row-2][col]+w2*graspsgrid[row-2][col+1]+w1*graspsgrid[row-2][col+2]+ //row-2
875  w2*graspsgrid[row-1][col-2]+w3*graspsgrid[row-1][col-1]+w4*graspsgrid[row-1][col]+w3*graspsgrid[row-1][col+1]+w2*graspsgrid[row-1][col+2]+ //row -1
876  w2*graspsgrid[row][col-4] + w2*graspsgrid[row][col-3] + w3*graspsgrid[row][col-2] + w4*graspsgrid[row][col-1] + w5*graspsgrid[row][col] + w4*graspsgrid[row][col+1] + w3*graspsgrid[row][col+2] + w2*graspsgrid[row][col+3] +w2*graspsgrid[row][col+4] + //row
877  w2*graspsgrid[row+1][col-2]+w3*graspsgrid[row+1][col-1]+w4*graspsgrid[row+1][col]+w3*graspsgrid[row+1][col+1]+w2*graspsgrid[row+1][col+2]+ //row +1
878  w1*graspsgrid[row+2][col-2]+w2*graspsgrid[row+2][col-1]+w3*graspsgrid[row+2][col]+w2*graspsgrid[row+2][col+1]+w1*graspsgrid[row+2][col+2]; //row +2
879 
880  }
881  if (printgraspseval) cout << graspseval[row][col] << "\t";
882  if (graspseval[row][col] > topval_gp){
883  topval_gp = graspseval[row][col];
884  id_row_top = row;
885  id_col_top = col;
886  if (topval_gp > topval_gp_all){
887  cout << "New overall grasp evaluation with value: " << topval_gp << " in row,col: [" << id_row_top << ", " << id_col_top <<"] and rotation: " << nr_roll*ROLL_STEPS_DEGREE << endl;
888  id_row_top_all = id_row_top;
889  id_col_top_all = id_col_top;
890  nr_roll_top_all = nr_roll;
891  nr_tilt_top_all = tilt;
892  topval_gp_all = topval_gp;
893  }
894  }
895  }
896  if (printgraspseval) cout << "\n";
897  }
898 
899 
900  //publish grasp grid as ros marker message (visualization_msgs::ArrayMarker) for rviz
901  marker_cnt = 1;
902  publish_grasp_grid(nr_roll, tilt, graspseval, 1);
903 
904  //choose best gp out of top rated ones (the one in the middle)
905  int best_topval_row = -1, best_topval_col = -1;
906  int longest_topval_len = 0; //indicates the length of longest found sequence of topvals in one row
907  int cur_topval_len = 0; //indicates the length of the current run/sequence
908  bool count_topval_len = false;
909  for (int row = 0; row < HEIGHT; row++){
910  cur_topval_len = 0;
911  count_topval_len = false;
912  for (int col = 0; col < WIDTH; col++){
913  if (graspseval[row][col] == topval_gp){
914  count_topval_len = true;
915  cur_topval_len++;
916  if (cur_topval_len > longest_topval_len){ //new longest run of topvals
917  longest_topval_len = cur_topval_len;
918  best_topval_row = row;
919  best_topval_col = col - cur_topval_len/2;
920  if (topval_gp == topval_gp_all){
921  cout << "Better topval gp still with topval: " << topval_gp << " in row,col: [" << best_topval_row << ", " << best_topval_col <<"] and rotation: " << nr_roll*ROLL_STEPS_DEGREE << endl;
922  id_row_top_all = best_topval_row;
923  id_col_top_all = best_topval_col;
924  }
925  }
926  }
927  else {
928  count_topval_len = false;
929  cur_topval_len = 0;
930  }
931  }
932  }
933 
934 
935  //show grasp predicted in mirrored (more intuitive) way
936  if (printgraspseval) {
937  cout << "\n Grasps predicted more intuitively: \n";
938  for (int row = HEIGHT-1; row >= 0; row--){
939  for (int col = WIDTH-1; col >= 0; col--){
940  cout << setw(3) << setprecision(3) <<graspseval[row][col];
941  if (col > 6 and col < WIDTH-7 )
942  cout << " ";
943  }
944  if (row > 7 and row < HEIGHT - 7)
945  cout << "\n";
946  cout << endl;
947  }
948  }
949  cout << "\n Best grasp value for currently tested roll: " << topval_gp_all << endl;
950 
951  file_in.close();
952 
953  if (topval_gp_all > this->topval_gp_overall){ //new best grasp hypothesis!
954  cout << "\n New overall (overall!) grasp evaluation with value: " << topval_gp_all << " in row,col: [" << id_row_top_all << ", " << id_col_top_all <<"] and rotation: " << nr_roll*ROLL_STEPS_DEGREE << endl;
955  this->id_row_top_overall = id_row_top_all;
956  this->id_col_top_overall = id_col_top_all;
957  this->nr_roll_top_overall = nr_roll_top_all;
958  this->nr_tilt_top_overall = nr_tilt_top_all;
959  this->topval_gp_overall = topval_gp_all;
960  }
961 
962  if (this->return_only_best_gp){
963  cout << "\n Grasp detection for one roll finished. Only best grasp is published after full gp calculation! \n";
964  } else if ((topval_gp_all > this->graspval_th) ){
965  //calculate coordinates for best grasp points with roll in world
966  //transfer validation to value between 10 and 99 (if smaler 26, then value is set to 10)
967  int scaled_gp_eval = topval_gp_all-20;
968  if (scaled_gp_eval < 10) scaled_gp_eval = 10; //should not be neccessary since graspval_th is high enough
969  transform_gp_in_wcs_and_publish(id_row_top_all, id_col_top_all,nr_roll_top_all, nr_tilt_top_all, scaled_gp_eval);
970  } else { //better grasps would be published but were not found for this loop
971  cout << "\n For current roll no grasp point had an evaluation above threshold for publishing! \n";
972  }
973 }
974 
975 
976 
977 
978 
979 void CCalc_Grasppoints::publish_grasp_grid(int nr_roll, int tilt, float graspsgrid[][WIDTH], int gripperwidth=1){
980  visualization_msgs::MarkerArray ma;
981  visualization_msgs::MarkerArray ma_params;
982  visualization_msgs::Marker marker;
983  visualization_msgs::Marker marker_graps_params;
984  cout << "publish_grasp_grid: input parameter gripperwidth: " << gripperwidth << endl;
985  float x,y,z;
986  x = this->graspsearchcenter.x - 0.01*HEIGHT/2/gripperwidth;
987  y = this->graspsearchcenter.y - 0.01*WIDTH/2;
988  z = this->graspsearchcenter.z + this->trans_z_after_pc_transform;
989 
990 
991  //publish tf_frame "tf_help" and grasp position (green bars where height is indicating the quality)
992  for (int row = 0; row <= HEIGHT-1; row++){
993  for (int col = 0; col <= WIDTH-1; col++){
994  if (this->point_inside_box_grid[nr_roll][tilt][row][col] == true){
995  gp_to_marker(&marker, x+0.01/gripperwidth*row,y+0.01*col,z, graspsgrid[row][col], false, gripperwidth, nr_roll, false, true); // before 0 instead of nr_roll
996  ma.markers.push_back(marker);
997  }
998  }
999  }
1000 
1001 
1002  //show grasp params (grasp search field size, gripper closing direction)
1003  for (int i = 1; i <= 3; i++){
1004  grasp_area_to_marker(&marker_graps_params, this->graspsearchcenter.x,this->graspsearchcenter.y, z, gripperwidth, nr_roll, i /*param_id*/, false /*top_grasp*/ ); //add grasping direction to marker
1005  ma_params.markers.push_back(marker_graps_params);
1006  }
1007 
1008 
1009  // show (add) grasping direction:
1010  gp_to_marker(&marker, this->graspsearchcenter.x,this->graspsearchcenter.y,z, graspsgrid[0][0], false, gripperwidth, nr_roll, true, true); //add grasping direction to marker
1011  ma.markers.push_back(marker); // add gripper opening direction to marker array
1012  //vis_pub.publish( marker );
1013  vis_pub_ma.publish(ma);
1014  vis_pub_ma_params.publish(ma_params); //publish grasp params as marker array
1015  //ros::spinOnce();
1016 }
1017 
1018 
1019 
1020 void CCalc_Grasppoints::gp_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, float green, bool pubmarker, int gripperwidth=1, int nr_roll=0,bool pub_grip_open_dir=false, bool pub_grid=true){
1021 
1022  // broadcast helper tf-frame for correct visualization of grasping points AND green/red grasping grid
1023  string tmp_tf_help = "tf_help";
1024  static tf::TransformBroadcaster br;
1025 
1026  //calculate rotation for tf-helper coordinate system for visualization
1027  float rot_z, rot_x;
1028 
1029  if (this->approach_vector.y == 0 and this->approach_vector.x == 0){ //if av->y = av->x = 0
1030  rot_z = 0;
1031  if (this->approach_vector.z >= 0) {
1032  rot_x = 0; //grasp from top
1033  } else {
1034  rot_x = PI; //grasp from upside down (in practice hardly relevant)
1035  }
1036  } else { //av->y <> 0 or = av->x <> 0
1037  rot_z = 90*PI/180.0 - atan2(this->approach_vector.y, this->approach_vector.x); // av->y, av->x not both 0
1038  rot_x = 90*PI/180.0 - atan2( this->approach_vector.z, sqrt(this->approach_vector.y*this->approach_vector.y + this->approach_vector.x*this->approach_vector.x) );
1039  }
1040  //cout << "marker: rot_z: " << rot_z << "\nthis->approach_vector.y: " << this->approach_vector.y << "\nthis->approach_vector.x: " << this->approach_vector.x << endl;
1041  //cout << "marker: rot_x: " << rot_x << "\nthis->approach_vector.z: " << this->approach_vector.z << "\nthis->approach_vector.y: " << this->approach_vector.y << endl;
1042 
1043 
1044  //compensate roll transform
1045 
1046  Eigen::Matrix4f mat_sh_to_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from center to origin
1047  Eigen::Matrix4f mat_rot = Eigen::Matrix4f::Identity(); //matrix which rotates pc
1048  Eigen::Matrix4f mat_sh_from_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from origin to center
1049  Eigen::Matrix4f mat_transform = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about box center)
1050  Eigen::Matrix3f mat_rot_for_appr_vec = Eigen::Matrix3f::Identity(); //matrix which makes the rotation for the approximation vector
1051 
1052  Eigen::Matrix4f mat_rot_z_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about z-axis)
1053  Eigen::Matrix4f mat_rot_x_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about x-axis)
1054 
1055 
1056  mat_sh_to_orig(0,3) = -this->graspsearchcenter.x; //shift x value
1057  mat_sh_to_orig(1,3) = -this->graspsearchcenter.y; //shift y-value
1058  mat_sh_to_orig(2,3) = -this->graspsearchcenter.z; //shift z-value
1059  mat_sh_from_orig(0,3) = 0;//this->graspsearchcenter.x;
1060  mat_sh_from_orig(1,3) = 0;//this->graspsearchcenter.y;
1061  mat_sh_from_orig(2,3) = 0/*this->graspsearchcenter.z*/ + this->trans_z_after_pc_transform; //move pc up to make calculation possible
1062 
1063  //define matrices s.t. transformation matrix can be calculated for point cloud (=> roll and tilt are simulated)
1064 
1065  //define rotation about z-axis (for roll):
1066  float angle = nr_roll*ROLL_STEPS_DEGREE*PI/180;//for roll
1067  mat_rot(0,0) = cos(angle);
1068  mat_rot(0,1) = -sin(angle);
1069  mat_rot(1,0) = sin(angle);
1070  mat_rot(1,1) = cos(angle);
1071 
1072  //define rotation about z-axis:
1073  mat_rot_z_axis(0,0) = cos(rot_z);
1074  mat_rot_z_axis(0,1) = -sin(rot_z);
1075  mat_rot_z_axis(1,0) = sin(rot_z);
1076  mat_rot_z_axis(1,1) = cos(rot_z);
1077 
1078 
1079  //define rotation about x-axis
1080  mat_rot_x_axis(1,1) = cos(rot_x);
1081  mat_rot_x_axis(1,2) = -sin(rot_x);
1082  mat_rot_x_axis(2,1) = sin(rot_x);
1083  mat_rot_x_axis(2,2) = cos(rot_x);
1084 
1085  // transforms in a way that the old coordinate system is transformed to the new one by rotation about first: z-axis, second (new) x-axis
1086  mat_transform = mat_rot*mat_sh_from_orig*mat_rot_x_axis*mat_rot_z_axis*mat_sh_to_orig; //define transformation matrix mat_transform
1087 
1088 
1089  //stupid data conversion from Eigen::Matrix4f to tf::Transform
1090  Eigen::Matrix4f Tm;
1091  Tm = mat_transform.inverse();
1092  tf::Vector3 origin;
1093  origin.setValue(static_cast<double>(Tm(0,3)),static_cast<double>(Tm(1,3)),static_cast<double>(Tm(2,3)));
1094  tf::Matrix3x3 tf3d;
1095  tf3d.setValue(static_cast<double>(Tm(0,0)), static_cast<double>(Tm(0,1)), static_cast<double>(Tm(0,2)),
1096  static_cast<double>(Tm(1,0)), static_cast<double>(Tm(1,1)), static_cast<double>(Tm(1,2)),
1097  static_cast<double>(Tm(2,0)), static_cast<double>(Tm(2,1)), static_cast<double>(Tm(2,2)));
1098 
1099  tf::Quaternion tfqt;
1100  tf3d.getRotation(tfqt);
1101  this->quat_tf_to_tf_help = tfqt;
1102  tf::Transform transform;
1103  transform.setOrigin(origin);
1104  transform.setRotation(tfqt);
1105 
1106  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), this->base_frame_id, tmp_tf_help));
1107 
1108  if (pub_grid){
1109  std::stringstream ss;
1110  ss << tmp_tf_help ;
1111  (*marker).header.frame_id = ss.str(); //used: "tf_help"
1112  (*marker).header.stamp = ros::Time();
1113  (*marker).ns = "my_namespace";
1114  (*marker).id = marker_cnt++;
1115  (*marker).type = visualization_msgs::Marker::SPHERE;
1116  (*marker).action = visualization_msgs::Marker::ADD;
1117  (*marker).pose.position.x = x-this->graspsearchcenter.x;
1118  (*marker).pose.position.y = y-this->graspsearchcenter.y;
1119  (*marker).pose.position.z = z;
1120  (*marker).pose.orientation.x = 0.0;
1121  (*marker).pose.orientation.y = 0.0;
1122  (*marker).pose.orientation.z = 0.0;
1123  (*marker).pose.orientation.w = 1.0;
1124  (*marker).scale.x = 0.002;
1125  (*marker).lifetime = ros::Duration(1);
1126  (*marker).scale.y = 0.002;
1127  (*marker).scale.z = 0.002;
1128  (*marker).color.a = 1.0;
1129  if (green > 0){
1130  (*marker).color.r = 0.0;
1131  (*marker).color.g = 1.0;
1132  (*marker).scale.x = 0.005;
1133  (*marker).scale.y = 0.005;
1134  (*marker).scale.z = 0.001*green;
1135  (*marker).pose.position.z = z+(*marker).scale.z/2;
1136  } else {
1137  (*marker).color.r = 1.0;
1138  (*marker).color.g = 0.0;
1139  }
1140  (*marker).color.b = 0.0;
1141  }
1142 }
1143 
1144 //this function publishes the area where grasps are searched for specific roll for visualization AND gripper closing direction AND best approach vector
1145 void CCalc_Grasppoints::grasp_area_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, int gripperwidth=1, int nr_roll=0, int param_id=1, bool top_grasp=false){
1146 
1147  int fix_marker_id_gripper_dir = 10000;
1148  int fix_marker_id_gripper_appr_dir = 10001;
1149  int fix_marker_id_grasp_search_area = 10002;
1150  int fix_marker_id_grasp_search_area2 = 10003; //inner part where feature vectors are really calculated
1151 
1152  (*marker).header.frame_id = "tf_help";
1153  (*marker).header.stamp = ros::Time::now();//ros::Time();
1154  (*marker).ns = "haf_grasping_parameters";
1155 
1156  //select which marker part is returned
1157  switch(param_id) {
1158  case 1 : { //publish (outer) grasping search area
1159  (*marker).id = fix_marker_id_grasp_search_area;
1160  (*marker).type = visualization_msgs::Marker::CUBE;
1161  (*marker).action = visualization_msgs::Marker::ADD;
1162  (*marker).color.a = 0.05;
1163  (*marker).color.r = 0.4;
1164  (*marker).color.g = 0.4;
1165  (*marker).color.b = 0.4;
1166  (*marker).pose.position.x = 0;//-this->graspsearchcenter.x;
1167  (*marker).pose.position.y = 0;//-this->graspsearchcenter.y;
1168  (*marker).pose.position.z = z;
1169 
1170  tf::Vector3 zaxis(0, 0, 1);
1171  tf::Quaternion qt_compensate_rot(zaxis, nr_roll*ROLL_STEPS_DEGREE*PI/180); //compensate for rotation of tf "tf_help"
1172  geometry_msgs::Quaternion quat_msg_compensate_rot;
1173  tf::quaternionTFToMsg(qt_compensate_rot, quat_msg_compensate_rot);
1174  (*marker).pose.orientation = quat_msg_compensate_rot;
1175  (*marker).scale.x = 0.01*this->grasp_search_area_size_x_dir/ gripperwidth;
1176  (*marker).scale.y = 0.01*this->grasp_search_area_size_y_dir;
1177  (*marker).scale.z = 0.001;
1178  (*marker).lifetime = ros::Duration();
1179  break;
1180  }
1181 
1182  case 2 : { //pub search area, pub inner area:
1183  (*marker).id = fix_marker_id_grasp_search_area2;
1184  (*marker).type = visualization_msgs::Marker::CUBE;
1185  (*marker).action = visualization_msgs::Marker::ADD;
1186  (*marker).color.a = 0.4;
1187  (*marker).color.r = 0.2;
1188  (*marker).color.g = 0.2;
1189  (*marker).color.b = 0.2;
1190  (*marker).pose.position.x = 0;//this->graspsearchcenter.x;
1191  (*marker).pose.position.y = 0;//this->graspsearchcenter.y;
1192  (*marker).pose.position.z = z;
1193 
1194  tf::Vector3 zaxis(0, 0, 1);
1195  tf::Quaternion qt_compensate_rot(zaxis, nr_roll*ROLL_STEPS_DEGREE*PI/180); //compensate for rotation of tf "tf_help"
1196  geometry_msgs::Quaternion quat_msg_compensate_rot;
1197  tf::quaternionTFToMsg(qt_compensate_rot, quat_msg_compensate_rot);
1198  (*marker).pose.orientation = quat_msg_compensate_rot;
1199 
1200  (*marker).scale.x = 0.01*(this->grasp_search_area_size_x_dir-14)/ gripperwidth;
1201  (*marker).scale.y = 0.01*(this->grasp_search_area_size_y_dir-14);
1202  (*marker).scale.z = 0.001;
1203  (*marker).lifetime = ros::Duration();
1204  break;
1205  }
1206  case 3 : {
1207  (*marker).id = fix_marker_id_gripper_dir;
1208  (*marker).type = visualization_msgs::Marker::CUBE;
1209  (*marker).action = visualization_msgs::Marker::ADD;
1210  (*marker).color.a = 0.8;
1211  (*marker).color.r = 1.0;
1212  (*marker).color.g = 0.0;
1213  (*marker).color.b = 0.0;
1214  if (top_grasp){
1215  (*marker).pose.position.x = x;
1216  (*marker).pose.position.y = y;
1217  (*marker).pose.position.z = z;
1218  (*marker).scale.x = 0.5 * 1 / gripperwidth;
1219  (*marker).scale.y = 0.002;
1220  (*marker).scale.z = 0.002;
1221 
1222  //new transform for tf_helper
1223  visualization_msgs::Marker markerdummy;
1224  gp_to_marker(&markerdummy, this->graspsearchcenter.x,this->graspsearchcenter.y,this->graspsearchcenter.z, 1, false, 1, nr_roll, false, false/*pub_grid*/); //add grasping direction to marker
1225  } else {
1226  (*marker).pose.position.x = 0;//this->graspsearchcenter.x;
1227  (*marker).pose.position.y = 0;//this->graspsearchcenter.y;
1228  (*marker).pose.position.z = z;
1229  (*marker).scale.x = 0.5 * 1 / gripperwidth;
1230  (*marker).scale.y = 0.002;
1231  (*marker).scale.z = 0.002;
1232  (*marker).lifetime = ros::Duration(3);
1233  tf::Vector3 marker_gripper_open_dir_axis(0, 0, 1);
1234  tf::Quaternion qt_gripper_open_dir(marker_gripper_open_dir_axis, 0*(-nr_roll)*ROLL_STEPS_DEGREE*PI/180);
1235  geometry_msgs::Quaternion quat_msg_gripper_open_dir;
1236  tf::quaternionTFToMsg(qt_gripper_open_dir, quat_msg_gripper_open_dir);
1237  (*marker).pose.orientation = quat_msg_gripper_open_dir;
1238  }
1239  (*marker).lifetime = ros::Duration();
1240  break;
1241  }
1242  case 4 : { // draw grasp approach direction (black arrow)
1243  (*marker).header.frame_id = this->base_frame_id;
1244  (*marker).id = fix_marker_id_gripper_appr_dir;
1245  (*marker).type = visualization_msgs::Marker::ARROW;
1246  (*marker).action = visualization_msgs::Marker::ADD;
1247  (*marker).color.a = 0.8;
1248  (*marker).color.r = 0.0;
1249  (*marker).color.g = 0.0;
1250  (*marker).color.b = 0.0;
1251  float markerlength = 0.1;
1252  (*marker).pose.position.x = x + markerlength*this->approach_vector.x;
1253  (*marker).pose.position.y = y + markerlength*this->approach_vector.y;
1254  (*marker).pose.position.z = z + markerlength*this->approach_vector.z;
1255  (*marker).scale.x = markerlength;
1256  (*marker).scale.y = 0.003;
1257  (*marker).scale.z = 0.003;
1258  (*marker).lifetime = ros::Duration(3);
1259  tf::Vector3 marker_axis(0, 1, 0);
1260  tf::Quaternion qt(marker_axis, PI/2);
1261  geometry_msgs::Quaternion quat_msg;
1262  tf::quaternionTFToMsg( (this->quat_tf_to_tf_help)*qt, quat_msg);
1263  (*marker).pose.orientation = quat_msg;
1264  (*marker).lifetime = ros::Duration();
1265  break;
1266  }
1267  default: cout << "CCalc_Grasppoints::grasp_area_to_marker: WRONG INPUT FOR param_id " << endl;
1268  break;
1269  }
1270 }
1271 
1272 //input: best row and col in virtual rotated box and rotation angle
1273 //output: calculates grasp point in world coordinate system and publishes it
1274 void CCalc_Grasppoints::transform_gp_in_wcs_and_publish(int id_row_top_all, int id_col_top_all,int nr_roll_top_all, int nr_tilt_top_all, int scaled_gp_eval){
1275  cout << "\n ===> transform_gp_in_wcs_and_publish(): TRANSFORM FOUND GRASP" << endl;
1276  Eigen::Matrix4f mat_sh_to_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from center to origin
1277  Eigen::Matrix4f mat_rot_z_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about z-axis)
1278  Eigen::Matrix4f mat_rot_x_axis = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about x-axis)
1279  Eigen::Matrix4f mat_sh_from_orig = Eigen::Matrix4f::Identity(); //matrix which defines shift of point cloud from origin to center
1280  Eigen::Matrix4f mat_rot = Eigen::Matrix4f::Identity(); //matrix which rotates pc
1281  Eigen::Matrix4f mat_scale_x_dir = Eigen::Matrix4f::Identity();//NEW grippertest: scale point cloud
1282 
1283  Eigen::Matrix4f mat_transform = Eigen::Matrix4f::Identity(); //matrix for transformation of pointcloud (rotation about box center)
1284 
1285  Eigen::Matrix3f mat_rot_for_appr_vec = Eigen::Matrix3f::Identity(); //matrix which makes the rotation for the approximation vector
1286 
1287  float rot_about_z; //rotation needed about z-axis to align y-axis (old cs) with orthoganal projection of new z-axis on x-y-plane (rad)
1288  float rot_about_x = 0; //rotation needed to align z axis old with z-axis new after rot_about_z was executed (this way old x-axis keeps direction in x-y-plane) (rad)
1289 
1290  mat_scale_x_dir(0,0) = this->gripper_opening_width; //NEW grippertest: scale point cloud in x-axis direction
1291 
1292  mat_sh_to_orig(0,3) = -this->graspsearchcenter.x; //shift x value
1293  mat_sh_to_orig(1,3) = -this->graspsearchcenter.y; //shift y-value
1294  mat_sh_to_orig(2,3) = -this->graspsearchcenter.z; //shift z-value
1295 
1296  mat_sh_from_orig(0,3) = 0;//this->graspsearchcenter.x;
1297  mat_sh_from_orig(1,3) = 0;//this->graspsearchcenter.y;
1298  mat_sh_from_orig(2,3) = 0/*this->graspsearchcenter.z*/ + this->trans_z_after_pc_transform; //move pc up to make calculation possible
1299 
1300  if (this->approach_vector.y == 0 and this->approach_vector.x == 0){ //if this->approach_vector.y = this->approach_vector.x = 0
1301  rot_about_z = 0;
1302  if (this->approach_vector.z >= 0) {
1303  rot_about_x = 0; //grasp from top
1304  } else {
1305  rot_about_x = PI; //grasp from upside down (in practice hardly relevant)
1306  }
1307  } else { //av->y <> 0 or = av->x <> 0
1308  rot_about_z = 90*PI/180.0 - atan2(this->approach_vector.y, this->approach_vector.x); // av->y, av->x not both 0
1309  rot_about_x = 90*PI/180.0 - atan2( this->approach_vector.z, sqrt(this->approach_vector.y*this->approach_vector.y + this->approach_vector.x*this->approach_vector.x) );
1310  }
1311 
1312  //define matrices s.t. transformation matrix can be calculated for point cloud (=> roll and tilt are simulated)
1313 
1314  //define rotation about z-axis (for top roll):
1315  float angle = nr_roll_top_all*ROLL_STEPS_DEGREE*PI/180;//for top roll overall
1316  mat_rot(0,0) = cos(angle);
1317  mat_rot(0,1) = -sin(angle);
1318  mat_rot(1,0) = sin(angle);
1319  mat_rot(1,1) = cos(angle);
1320 
1321  //define rotation about z-axis:
1322  mat_rot_z_axis(0,0) = cos(rot_about_z);
1323  mat_rot_z_axis(0,1) = -sin(rot_about_z);
1324  mat_rot_z_axis(1,0) = sin(rot_about_z);
1325  mat_rot_z_axis(1,1) = cos(rot_about_z);
1326 
1327  //define rotation about x-axis
1328  mat_rot_x_axis(1,1) = cos(rot_about_x);
1329  mat_rot_x_axis(1,2) = -sin(rot_about_x);
1330  mat_rot_x_axis(2,1) = sin(rot_about_x);
1331  mat_rot_x_axis(2,2) = cos(rot_about_x);
1332 
1333  // transforms in a way that the old coordinate system is transformed to the new one by rotation about first: z-axis, second (new) x-axis
1334  mat_transform = mat_scale_x_dir * mat_rot * mat_sh_from_orig * mat_rot_x_axis * mat_rot_z_axis * mat_sh_to_orig; //define transformation matrix mat_transform
1335 
1336 
1337  cout << " ---> id_row_top_all: " << id_row_top_all << "\t" << "id_col_top_all: " << id_col_top_all << "\t and rotation: " << nr_roll_top_all*ROLL_STEPS_DEGREE << " tilt_top: " << nr_tilt_top_all <<endl;
1338  //publish best grasp point (with roll):
1339  float x_gp_roll = /*this->graspsearchcenter.x*/ - ((float)(HEIGHT/2 - id_row_top_all))/100; //x-coordinate of the graspcenterpoint if the box would be rotated by nr_roll_top_all*ROLL_STEPS_DEGREE degree
1340  float y_gp_roll = /*this->graspsearchcenter.y*/ - ((float)(WIDTH/2 - id_col_top_all))/100; //y-coordinate of the graspcenterpoint if the box would be rotated by nr_roll_top_all*ROLL_STEPS_DEGREE degree
1341  //estimate z-value for grasping (max height in area around grasp center - 0 cm
1342  float h_locmax_roll = -10;
1343  for (int row_z = -4; row_z < 5; row_z++){ //row_z defines which rows are taken into account for the calc. of max z
1344  for (int col_z = -4; col_z < 4; col_z++){
1345  //cout << "this->heightsgridroll[" << nr_roll_top_all << "][" << nr_tilt_top_all << "][" << id_row_top_all+row_z << "][" << id_col_top_all+col_z << "]: " << this->heightsgridroll[nr_roll_top_all][nr_tilt_top_all][id_row_top_all+row_z][id_col_top_all+col_z] << endl;
1346  if (id_row_top_all+row_z >= 0 and id_col_top_all+col_z>=0 and h_locmax_roll < this->heightsgridroll[nr_roll_top_all][nr_tilt_top_all][id_row_top_all+row_z][id_col_top_all+col_z]){
1347  h_locmax_roll = this->heightsgridroll[nr_roll_top_all][nr_tilt_top_all][id_row_top_all+row_z][id_col_top_all+col_z];
1348  //cout << "this->heightsgridroll[" << nr_roll_top_all <<"][" << nr_tilt_top_all<<"][" <<id_row_top_all+row_z<<"][" << id_col_top_all+col_z<<"]: " << this->heightsgridroll[nr_roll_top_all][nr_tilt_top_all][id_row_top_all+row_z][id_col_top_all+col_z] << endl;
1349  }
1350  }
1351  }
1352 
1353 
1354  h_locmax_roll -= 0.01; //reduce local maximal height by a threshold (2cm currently) => fine calculation should be done in simulation anyway
1355  float z_gp_roll = h_locmax_roll;
1356  cout << " ---> x_roll: "<< x_gp_roll << "\t y_roll: " << y_gp_roll << "\t z_gp_roll: " << z_gp_roll << endl;
1357 
1358  //make rotation
1359  //pcl::transformPointCloud(pcl_cloud_in, pcl_cloud_in_roll, mat_transform);
1360  float x_gp_dis = 0.03; //distance from grasp center to "real" grasp point
1361 
1362  Eigen::Vector4f gp1(x_gp_roll-x_gp_dis, y_gp_roll,z_gp_roll,1.0);
1363  Eigen::Vector4f gp2(x_gp_roll+x_gp_dis, y_gp_roll,z_gp_roll,1.0);
1364  Eigen::Vector4f gp1_wcs, gp2_wcs;
1365  Eigen::Vector3f appr_vec(0,0,1);
1366 
1367  gp1_wcs = mat_transform.inverse()*gp1;
1368  gp2_wcs = mat_transform.inverse()*gp2;
1369 
1370  mat_rot_for_appr_vec << this->av_trans_mat(0,0),this->av_trans_mat(1,0),this->av_trans_mat(2,0),
1371  this->av_trans_mat(0,1),this->av_trans_mat(1,1),this->av_trans_mat(2,1),
1372  this->av_trans_mat(0,2),this->av_trans_mat(1,2),this->av_trans_mat(2,2);
1373 
1374  appr_vec = mat_rot_for_appr_vec*appr_vec;
1375 
1376  cout << " ---> GP_1: x_roll_wcs: "<< gp1_wcs[0] << "\t y_roll_wcs: " << gp1_wcs[1] << "\t z_roll_wcs: " << gp1_wcs[2] << endl;
1377  cout << " ---> GP_2: x_roll_wcs: "<< gp2_wcs[0] << "\t y_roll_wcs: " << gp2_wcs[1] << "\t z_roll_wcs: " << gp2_wcs[2] << endl;
1378 
1379 
1380  //Publish grasp points
1381  std_msgs::String msgStrPoints;
1382  std::stringstream ss;
1383 
1384  ss << scaled_gp_eval << " " << gp1_wcs[0] <<" "<< gp1_wcs[1] << " "<< gp1_wcs[2] << " "<< gp2_wcs[0] << " " << gp2_wcs[1] << " " << gp2_wcs[2] << " "<< appr_vec(0) << " "<< appr_vec(1) << " "<< appr_vec(2) << " " << (gp1_wcs[0]+gp2_wcs[0])/2.0 <<" " << (gp1_wcs[1]+gp2_wcs[1])/2.0 <<" " << (gp1_wcs[2]+gp2_wcs[2])/2.0 <<" " << nr_roll_top_all*ROLL_STEPS_DEGREE;
1385  msgStrPoints.data = ss.str();
1386  this->gp_result.header.stamp = ros::Time::now();
1387  this->gp_result.header.frame_id = this->base_frame_id;
1388  this->gp_result.eval = scaled_gp_eval;
1389  this->gp_result.graspPoint1.x = gp1_wcs[0];
1390  this->gp_result.graspPoint1.y = gp1_wcs[1];
1391  this->gp_result.graspPoint1.z = gp1_wcs[2];
1392  this->gp_result.graspPoint2.x = gp2_wcs[0];
1393  this->gp_result.graspPoint2.y = gp2_wcs[1];
1394  this->gp_result.graspPoint2.z = gp2_wcs[2];
1395  this->gp_result.averagedGraspPoint.x = (gp1_wcs[0]+gp2_wcs[0])/2.0;
1396  this->gp_result.averagedGraspPoint.y = (gp1_wcs[1]+gp2_wcs[1])/2.0;
1397  this->gp_result.averagedGraspPoint.z = (gp1_wcs[2]+gp2_wcs[2])/2.0;
1398  this->gp_result.approachVector.x = appr_vec(0);
1399  this->gp_result.approachVector.y = appr_vec(1);
1400  this->gp_result.approachVector.z = appr_vec(2);
1401  this->gp_result.roll = (nr_roll_top_all * ROLL_STEPS_DEGREE * PI) / 180; // degree->radians
1402 
1403  //publish best grasp as rviz visualization_msgs::Marker
1404  visualization_msgs::Marker marker_best_params;
1405  visualization_msgs::MarkerArray ma_best_params;
1406  //show grasp params (grasp search field size, gripper closing direction)
1407  for (int i = 3; i <= 3; i++){ //closing direction (red line) for best grasp
1408  grasp_area_to_marker(&marker_best_params, (gp1[0]+gp2[0])/2,(gp1[1]+gp2[1])/2,(gp1[2]+gp2[2])/2, this->gripper_opening_width /*gripperwidth*/, nr_roll_top_all, i /*param_id*/, true /*top_grasp*/ ); //add best grasping direction to marker
1409  ma_best_params.markers.push_back(marker_best_params);
1410  }
1411  for (int i = 4; i <= 4; i++){ //approach vector (black arrow) for best grasp
1412  grasp_area_to_marker(&marker_best_params, (gp1_wcs[0]+gp2_wcs[0])/2,(gp1_wcs[1]+gp2_wcs[1])/2,(gp1_wcs[2]+gp2_wcs[2])/2, this->gripper_opening_width /*gripperwidth*/, nr_roll_top_all, i /*param_id*/, true /*top_grasp*/ ); //add best grasping direction to marker
1413  ma_best_params.markers.push_back(marker_best_params);
1414  }
1415  vis_pub_ma_params.publish(ma_best_params); //publish best grasp parameter
1416 
1417  //pubGraspPoints.publish(msgStrPoints);
1418  cout << " ---> Scaled GP Evaluation (-20-99) [-20 <=> no GP found]: " << scaled_gp_eval << endl;
1419  pubGraspPointsEval.publish(msgStrPoints);
1420 
1421 }
1422 
1423 
1424 
1425 int main (int argc, char** argv)
1426 {
1427  ROS_INFO("ROS NODE calc_grasppoints_svm_action_server (from package haf_grasping) started");
1428  ros::init(argc, argv, "calc_grasppoints_svm_action_server");
1429  CCalc_Grasppoints * calc_gps = new CCalc_Grasppoints(ros::this_node::getName());
1430  ros::spin();
1431  return (0);
1432 }
1433 
CCalc_Grasppoints::gp_to_marker
void gp_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, float green, bool pubmarker, int gripperwidth, int nr_roll, bool pub_grip_open_dir, bool pub_grid)
Definition: calc_grasppoints_action_server.cpp:1020
HEIGHT
#define HEIGHT
Definition: calc_grasppoints_action_server.cpp:92
CCalc_Grasppoints::vis_pub_ma_params
ros::Publisher vis_pub_ma_params
Definition: calc_grasppoints_action_server.cpp:129
CCalc_Grasppoints::pnt_in_box
void pnt_in_box(int nr_roll, int nr_tilt)
Definition: calc_grasppoints_action_server.cpp:666
CCalc_Grasppoints::gripper_opening_width
int gripper_opening_width
Definition: calc_grasppoints_action_server.cpp:163
CCalc_Grasppoints::generate_grid
void generate_grid(int roll, int tilt, pcl::PointCloud< pcl::PointXYZ > pcl_cloud_in)
Definition: calc_grasppoints_action_server.cpp:406
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
transform_listener.h
ros::NodeHandle
prob
struct svm_problem prob
Definition: svmtrain.c:57
ROLL_MAX_DEGREE
#define ROLL_MAX_DEGREE
Definition: calc_grasppoints_action_server.cpp:101
CCalc_Grasppoints::publish_transformed_pcl_cloud
void publish_transformed_pcl_cloud(int roll, int tilt, pcl::PointCloud< pcl::PointXYZ > pcl_cloud_in)
Definition: calc_grasppoints_action_server.cpp:533
WIDTH
#define WIDTH
Definition: calc_grasppoints_action_server.cpp:93
CIntImage_to_Featurevec::read_features
void read_features(string featurespath)
Definition: CIntImage_to_Featurevec.cpp:47
CCalc_Grasppoints::marker_cnt
int marker_cnt
Definition: calc_grasppoints_action_server.cpp:151
ROLL_STEPS_DEGREE
#define ROLL_STEPS_DEGREE
Definition: calc_grasppoints_action_server.cpp:95
CCalc_Grasppoints::vis_pub
ros::Publisher vis_pub
Definition: calc_grasppoints_action_server.cpp:127
CCalc_Grasppoints::pc_sub
ros::Subscriber pc_sub
Definition: calc_grasppoints_action_server.cpp:122
TILT_STEPS_DEGREE
#define TILT_STEPS_DEGREE
Definition: calc_grasppoints_action_server.cpp:96
CCalc_Grasppoints::pubGraspPointsEval
ros::Publisher pubGraspPointsEval
Definition: calc_grasppoints_action_server.cpp:126
CCalc_Grasppoints::id_col_top_overall
int id_col_top_overall
Definition: calc_grasppoints_action_server.cpp:147
CCalc_Grasppoints::grasp_search_area_size_x_dir
int grasp_search_area_size_x_dir
Definition: calc_grasppoints_action_server.cpp:132
PI
#define PI
Definition: calc_grasppoints_action_server.cpp:94
CCalc_Grasppoints::base_frame_id
string base_frame_id
Definition: calc_grasppoints_action_server.cpp:141
CCalc_Grasppoints::read_pc_cb
void read_pc_cb(const haf_grasping::CalcGraspPointsServerGoalConstPtr &goal)
Definition: calc_grasppoints_action_server.cpp:250
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
CCalc_Grasppoints::publish_grasp_grid
void publish_grasp_grid(int nr_roll, int tilt, float graspsgrid[][WIDTH], int gripperwidth)
Definition: calc_grasppoints_action_server.cpp:979
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
transform_broadcaster.h
CIntImage_to_Featurevec
Definition: CIntImage_to_Featurevec.h:55
tf::Matrix3x3::setValue
void setValue(const tfScalar &xx, const tfScalar &xy, const tfScalar &xz, const tfScalar &yx, const tfScalar &yy, const tfScalar &yz, const tfScalar &zx, const tfScalar &zy, const tfScalar &zz)
CCalc_Grasppoints::calc_intimage
void calc_intimage(int roll, int tilt)
Definition: calc_grasppoints_action_server.cpp:577
CCalc_Grasppoints::pubGraspPoints
ros::Publisher pubGraspPoints
Definition: calc_grasppoints_action_server.cpp:125
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
CCalc_Grasppoints::graspsearchcenter
geometry_msgs::Point graspsearchcenter
Definition: calc_grasppoints_action_server.cpp:130
ROS_WARN
#define ROS_WARN(...)
tf::Vector3::setValue
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
CCalc_Grasppoints::outputpath_full
string outputpath_full
Definition: calc_grasppoints_action_server.cpp:139
CCalc_Grasppoints::pubInputPCROS
ros::Publisher pubInputPCROS
Definition: calc_grasppoints_action_server.cpp:123
transform_datatypes.h
CCalc_Grasppoints::action_name_
std::string action_name_
Definition: calc_grasppoints_action_server.cpp:114
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
CCalc_Grasppoints::visualization
bool visualization
Definition: calc_grasppoints_action_server.cpp:154
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
CCalc_Grasppoints::nr_features_without_shaf
int nr_features_without_shaf
Definition: calc_grasppoints_action_server.cpp:162
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
main
int main(int argc, char **argv)
Definition: calc_grasppoints_action_server.cpp:1425
x
struct svm_node * x
Definition: svm-predict.c:8
CCalc_Grasppoints::topval_gp_overall
int topval_gp_overall
Definition: calc_grasppoints_action_server.cpp:150
CCalc_Grasppoints::feedback_
haf_grasping::CalcGraspPointsServerFeedback feedback_
Definition: calc_grasppoints_action_server.cpp:116
command
ROSLIB_DECL std::string command(const std::string &cmd)
CCalc_Grasppoints::nh_
ros::NodeHandle nh_
Definition: calc_grasppoints_action_server.cpp:111
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
tf::quaternionTFToMsg
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
CCalc_Grasppoints::nr_roll_top_overall
int nr_roll_top_overall
Definition: calc_grasppoints_action_server.cpp:148
ros::ok
ROSCPP_DECL bool ok()
CCalc_Grasppoints::CCalc_Grasppoints
CCalc_Grasppoints(string name)
Definition: calc_grasppoints_action_server.cpp:181
tf::TransformBroadcaster::sendTransform
void sendTransform(const StampedTransform &transform)
point_cloud.h
CCalc_Grasppoints::graspval_max_diff_for_pub
int graspval_max_diff_for_pub
Definition: calc_grasppoints_action_server.cpp:144
CCalc_Grasppoints
Definition: calc_grasppoints_action_server.cpp:107
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
CCalc_Grasppoints::graspval_top
int graspval_top
Definition: calc_grasppoints_action_server.cpp:143
CCalc_Grasppoints::vis_pub_ma
ros::Publisher vis_pub_ma
Definition: calc_grasppoints_action_server.cpp:128
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
CCalc_Grasppoints::tf_listener
tf::TransformListener tf_listener
Definition: calc_grasppoints_action_server.cpp:153
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
CIntImage_to_Featurevec::write_featurevector
void write_featurevector(string outputpath, int nr_features_without_shaf)
Definition: CIntImage_to_Featurevec.cpp:122
CCalc_Grasppoints::grasp_area_to_marker
void grasp_area_to_marker(visualization_msgs::Marker *marker, float x, float y, float z, int gripperwidth, int nr_roll, int param_id, bool top_grasp)
Definition: calc_grasppoints_action_server.cpp:1145
CCalc_Grasppoints::box_position_sub
ros::Subscriber box_position_sub
Definition: calc_grasppoints_action_server.cpp:121
CIntImage_to_Featurevec::intimagemat
float intimagemat[HEIGHTFEATURES][WIDTHFEATURES]
Definition: CIntImage_to_Featurevec.h:60
CCalc_Grasppoints::calc_featurevectors
void calc_featurevectors(int roll, int tilt)
Definition: calc_grasppoints_action_server.cpp:616
CCalc_Grasppoints::pubTransformedPCROS
ros::Publisher pubTransformedPCROS
Definition: calc_grasppoints_action_server.cpp:124
line
char * line
Definition: svm-scale.c:21
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
CCalc_Grasppoints::trans_z_after_pc_transform
float trans_z_after_pc_transform
Definition: calc_grasppoints_action_server.cpp:157
pcl_ros::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
CCalc_Grasppoints::loop_control
void loop_control(pcl::PointCloud< pcl::PointXYZ > pcl_cloud_in)
Definition: calc_grasppoints_action_server.cpp:335
CIntImage_to_Featurevec.h
CCalc_Grasppoints::av_trans_mat
Eigen::Matrix4f av_trans_mat
Definition: calc_grasppoints_action_server.cpp:156
CCalc_Grasppoints::graspval_th
int graspval_th
Definition: calc_grasppoints_action_server.cpp:142
CCalc_Grasppoints::as_
actionlib::SimpleActionServer< haf_grasping::CalcGraspPointsServerAction > as_
Definition: calc_grasppoints_action_server.cpp:113
CCalc_Grasppoints::return_only_best_gp
bool return_only_best_gp
Definition: calc_grasppoints_action_server.cpp:140
CCalc_Grasppoints::predict_bestgp_withsvm
void predict_bestgp_withsvm(bool svm_with_probability=false)
Definition: calc_grasppoints_action_server.cpp:754
CCalc_Grasppoints::nr_tilt_top_overall
int nr_tilt_top_overall
Definition: calc_grasppoints_action_server.cpp:149
ros::Time::now
static Time now()
TILT_STEPS
#define TILT_STEPS
Definition: calc_grasppoints_action_server.cpp:97
CCalc_Grasppoints::svmmodel_file_path
string svmmodel_file_path
Definition: calc_grasppoints_action_server.cpp:161
CCalc_Grasppoints::quat_tf_to_tf_help
tf::Quaternion quat_tf_to_tf_help
Definition: calc_grasppoints_action_server.cpp:158
CCalc_Grasppoints::transform_gp_in_wcs_and_publish
void transform_gp_in_wcs_and_publish(int id_row_top_all, int id_col_top_all, int nr_roll_top_all, int nr_tilt_top_all, int scaled_gp_eval)
Definition: calc_grasppoints_action_server.cpp:1274
CCalc_Grasppoints::print_heights_bool
bool print_heights_bool
Definition: calc_grasppoints_action_server.cpp:155
CCalc_Grasppoints::range_file_path
string range_file_path
Definition: calc_grasppoints_action_server.cpp:160
CCalc_Grasppoints::grasp_search_area_size_y_dir
int grasp_search_area_size_y_dir
Definition: calc_grasppoints_action_server.cpp:133
CCalc_Grasppoints::feature_file_path
string feature_file_path
Definition: calc_grasppoints_action_server.cpp:159
CCalc_Grasppoints::result_
haf_grasping::CalcGraspPointsServerResult result_
Definition: calc_grasppoints_action_server.cpp:117
CCalc_Grasppoints::id_row_top_overall
int id_row_top_overall
Definition: calc_grasppoints_action_server.cpp:146
CCalc_Grasppoints::max_duration_for_grasp_calc
float max_duration_for_grasp_calc
Definition: calc_grasppoints_action_server.cpp:135
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
simple_action_server.h
CCalc_Grasppoints::boxrot_angle_init
float boxrot_angle_init
Definition: calc_grasppoints_action_server.cpp:131
CCalc_Grasppoints::gp_result
haf_grasping::GraspOutput gp_result
Definition: calc_grasppoints_action_server.cpp:152
CCalc_Grasppoints::show_predicted_gps
void show_predicted_gps(int nr_roll, int tilt, bool svm_with_probability=false)
Definition: calc_grasppoints_action_server.cpp:803
CCalc_Grasppoints::print_heights
void print_heights(int nr_roll, int nr_tilt)
Definition: calc_grasppoints_action_server.cpp:234
CCalc_Grasppoints::approach_vector
geometry_msgs::Vector3 approach_vector
Definition: calc_grasppoints_action_server.cpp:134

haf_grasping
Author(s): David Fischinger
autogenerated on Mon Jun 10 2019 13:28:43