00001 #include "obj_filter.h"
00002
00003 using namespace std;
00004
00005 ObjectFilter::ObjectFilter() {
00006
00007
00008 this->transform_grasping_point.setOrigin(tf::Vector3(0.5, 0.0, 0.5));
00009 this->transform_grasping_point.setRotation(tf::Quaternion(0, 0, 0, sqrt(2)/2));
00010 this->focused_obj = 0;
00011
00012
00013 std::string pcl2_sub_name = "/obj_filter/input/pcl2/segmented";
00014 std::string heat_map_sub_name = "/obj_filter/input/pcl2/wrinkled_map";
00015 std::string segmented_img_sub_name = "/obj_filter/input/image/segmented";
00016 std::string focused_obj_sub_name = "/obj_filter/input/focused_obj";
00017 std::string filtered_pcl_sub_name = "/obj_filter/output/pcl2/filtered";
00018 std::string compute_grasp_pose_sub_name = "/obj_filter/srv/compute_grasp_pose";
00019
00020 std::string grasping_points_list_sub_name = "/obj_filter/input/grasping_points_list";
00021
00022
00023
00024
00025 this->pcl2_sub = this->nh.subscribe(pcl2_sub_name, 1, &ObjectFilter::pcl2_sub_callback, this);
00026 this->segmented_img_sub = this->nh.subscribe(segmented_img_sub_name, 1, &ObjectFilter::segmented_img_sub_callback, this);
00027 this->heat_map_sub = this->nh.subscribe(heat_map_sub_name, 1, &ObjectFilter::heat_map_sub_callback, this);
00028 this->focused_object_sub = this->nh.subscribe(focused_obj_sub_name, 1, &ObjectFilter::focused_obj_sub_callback, this);
00029 this->grasping_points_list_sub = this->nh.subscribe(grasping_points_list_sub_name, 1, &ObjectFilter::grasping_points_list_sub_callback, this);
00030
00031
00032 this->filtered_pcl2_publisher = this->nh.advertise< pcl::PointCloud<pcl::PointXYZRGB> > (filtered_pcl_sub_name, 5);
00033
00034
00035 this->compute_grasp = this->nh.advertiseService(compute_grasp_pose_sub_name, &ObjectFilter::compute_grasp_pose_callback, this);
00036
00037
00038 this->wrinkle_client = this->nh.serviceClient<normal_descriptor_node::wrinkle>("/obj_filter/output/srv/wrinkle_map");
00039
00040
00041
00042
00043 ROS_INFO("Starting obj_filter Node");
00044 ROS_INFO("Use [ rosservice call /compute_grasp_pose ] to trigger the normal descriptor");
00045 }
00046
00047
00048 void ObjectFilter::pcl2_sub_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
00049 {
00050 ROS_DEBUG("Received segmented point cloud");
00051 pcl::fromROSMsg(*msg, this->pcl_xyzrgb);
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 }
00071
00072 void ObjectFilter::mainLoop(void)
00073 {
00074 tf_br.sendTransform(tf::StampedTransform(transform_grasping_point, ros::Time::now(), "openni_depth_optical_frame", "graspingPoint"));
00075 }
00076
00077 void ObjectFilter::heat_map_sub_callback(const normal_descriptor_node::heat_map& msg)
00078 {
00079 ROS_INFO("Received heat map");
00080 int indx = 0;
00081 int max = 0;
00082 for(int i=0; i<msg.data.size();i++){
00083 if(max < msg.data[i]){
00084 max = msg.data[i];
00085 indx = i;
00086 }
00087 }
00088 graspPose.header.stamp = ros::Time::now();
00089 graspPose.header.frame_id = "grasp_point";
00090 graspPose.pose.position.x = msg.points3d[indx].x;
00091 graspPose.pose.position.y = msg.points3d[indx].y;
00092 graspPose.pose.position.z = msg.points3d[indx].z;
00093
00094 graspPose.pose.orientation.x = 0;
00095 graspPose.pose.orientation.y = 0;
00096 graspPose.pose.orientation.z = 0;
00097 graspPose.pose.orientation.w = sqrt(2)/2;
00098
00099 ROS_INFO("Grasping point: %f %f %f ",graspPose.pose.position.x, graspPose.pose.position.y, graspPose.pose.position.z);
00100
00101 }
00102
00103 void ObjectFilter::grasping_points_list_sub_callback(const iri_clothes_grasping_point::GraspingPointList& msg)
00104 {
00105 int xpix, ypix;
00106
00107 xpix = (int) msg.pixels[0].xpixel;
00108 ypix = (int) msg.pixels[0].ypixel;
00109
00110 graspPose.header.stamp = ros::Time::now();
00111 graspPose.header.frame_id = "grasp_point";
00112 graspPose.pose.position.x = this->pcl_xyzrgb(xpix,ypix).x;
00113 graspPose.pose.position.y = this->pcl_xyzrgb(xpix,ypix).y;
00114 graspPose.pose.position.z = this->pcl_xyzrgb(xpix,ypix).z;
00115
00116 graspPose.pose.orientation.x = 0;
00117 graspPose.pose.orientation.y = 0;
00118 graspPose.pose.orientation.z = 0;
00119 graspPose.pose.orientation.w = sqrt(2)/2;
00120
00121 ROS_INFO("Grasping point: %f %f %f ",graspPose.pose.position.x, graspPose.pose.position.y, graspPose.pose.position.z);
00122
00123 }
00124
00125 void ObjectFilter::focused_obj_sub_callback(const std_msgs::Int32& msg)
00126 {
00127
00128 }
00129
00130 void ObjectFilter::segmented_img_sub_callback(const sensor_msgs::ImageConstPtr& msg)
00131 {
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 }
00147
00148
00149 bool ObjectFilter::compute_grasp_pose_callback(iri_wam_common_msgs::compute_obj_grasp_pose::Request &req, iri_wam_common_msgs::compute_obj_grasp_pose::Response &res){
00150
00151 ROS_INFO("Compute grasp pose request");
00152
00153
00154 pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = this->pcl_xyzrgb.begin();
00155 RGBValue color;
00156 switch(req.zone){
00157
00158 case ANYZONE:
00159
00160 break;
00161 case AZONE:
00162 ROS_INFO("only zone A");
00163 for (int rr = 0; rr < (int)pcl_xyzrgb.height; ++rr) {
00164 pt_iter += pcl_xyzrgb.width/2;
00165 for (int cc = 0; cc < (int)(pcl_xyzrgb.width/2); ++cc, ++pt_iter) {
00166 pt_iter->x = NAN;
00167 pt_iter->y = NAN;
00168 pt_iter->z = NAN;
00169 }
00170 }
00171 break;
00172 case BZONE:
00173 ROS_INFO("only zone B");
00174 for (int rr = 0; rr < (int)pcl_xyzrgb.height; ++rr) {
00175 for (int cc = 0; cc < (int)(pcl_xyzrgb.width/2); ++cc, ++pt_iter) {
00176 pt_iter->x = NAN;
00177 pt_iter->y = NAN;
00178 pt_iter->z = NAN;
00179 }
00180 pt_iter += pcl_xyzrgb.width/2;
00181 }
00182 break;
00183 default:
00184 ROS_ERROR("Unrecognized zone criteria");
00185 return false;
00186 break;
00187 }
00188
00189
00190
00191 this->filtered_pcl2_publisher.publish(this->pcl_xyzrgb);
00192 ROS_INFO("Calling wrinkle service");
00193 sensor_msgs::PointCloud2 cloud;
00194 pcl::toROSMsg(this->pcl_xyzrgb, cloud);
00195 wrinkle_srv.request.cloth_cloud = cloud;
00196 if(wrinkle_client.call(wrinkle_srv)) {
00197 ROS_INFO("call to wrinkle srv returned successfully");
00198 } else {
00199 ROS_ERROR("Failed to call service wrinkle_srv at %s",wrinkle_client.getService().c_str() );
00200 if(!wrinkle_client.exists())
00201 ROS_ERROR("Reason: The service does not exist! Check your connections.");
00202 return false;
00203 }
00204 ROS_INFO("Received heat map");
00205 switch(req.filterID){
00206 case ALLFILTERS:
00207 break;
00208 default:
00209 ROS_ERROR("Unrecognized filter criteria");
00210 return false;
00211 break;
00212 }
00213 graspPose.header.frame_id = "grasp_point";
00214 graspPose.pose.position.x = wrinkle_srv.response.wrinkle_map.points3d[indx].x;
00215 graspPose.pose.position.y = wrinkle_srv.response.wrinkle_map.points3d[indx].y;
00216 graspPose.pose.position.z = wrinkle_srv.response.wrinkle_map.points3d[indx].z;
00217
00218 graspPose.pose.orientation.x = 0;
00219 graspPose.pose.orientation.y = 0;
00220 graspPose.pose.orientation.z = 0;
00221 graspPose.pose.orientation.w = sqrt(2)/2;
00222
00223
00224 transform_grasping_point.setOrigin(tf::Vector3(graspPose.pose.position.x, graspPose.pose.position.y, graspPose.pose.position.z));
00225 transform_grasping_point.setRotation(tf::Quaternion( graspPose.pose.orientation.x, graspPose.pose.orientation.y,graspPose.pose.orientation.w));
00226
00227 ROS_INFO("Grasping point: %f %f %f wrinkleness %f",graspPose.pose.position.x, graspPose.pose.position.y, graspPose.pose.position.z, max);
00228
00229
00230 res.graspPose = graspPose;
00231 res.wrinkleness = max;
00232
00233
00234 return true;
00235 }
00236
00237 void maxwrinkle(){
00238 int indx = 0;
00239 double max = 0;
00240 int counter = 0;
00241 for(int i=0; i<wrinkle_srv.response.wrinkle_map.data.size();i++){
00242
00243 if(!isnan(wrinkle_srv.response.wrinkle_map.points3d[i].x) && !isnan(wrinkle_srv.response.wrinkle_map.points3d[i].y) && !isnan(wrinkle_srv.response.wrinkle_map.points3d[i].z) ){
00244 counter++;
00245 if(max < wrinkle_srv.response.wrinkle_map.data[i]){
00246 max = wrinkle_srv.response.wrinkle_map.data[i];
00247 indx = i;
00248 }
00249 }
00250 }
00251 if(max == 0){
00252 ROS_ERROR("no max wrinkle found! Candidates were all nan or no maximum diferent than 0 was found");
00253 return false;
00254 }else{
00255 ROS_INFO("valid wrinkle point found at %f %f %f with wrinkleness %f",wrinkle_srv.response.wrinkle_map.points3d[indx].x,wrinkle_srv.response.wrinkle_map.points3d[indx].y,wrinkle_srv.response.wrinkle_map.points3d[indx].z, max);
00256 }
00257 graspPose.header.stamp = ros::Time::now();
00258
00259 }
00260
00261 int main(int argc, char** argv)
00262 {
00263 ros::init(argc, argv, "obj_filter");
00264 ObjectFilter handeye_log;
00265 ros::Rate loop_rate(10);
00266 while(ros::ok()){
00267 handeye_log.mainLoop();
00268 ros::spinOnce();
00269 loop_rate.sleep();
00270 }
00271 return 0;
00272 }