Go to the documentation of this file.00001 #include "gripper_points_publisher.h"
00002 #include <boost/foreach.hpp>
00003 #include "pcl_ros/transforms.h"
00004 #include <pcl/filters/passthrough.h>
00005
00006 GripperPointsPublisher::GripperPointsPublisher()
00007 { ros::NodeHandle grippernh;
00008 pubGripper = grippernh.advertise<PointCloud> ("GripperPunkte", 3);
00009 pubGripper_single = grippernh.advertise<PointCloud> ("GripperPunkte_single", 3);
00010 }
00011
00012 void GripperPointsPublisher::sendGripperPoints(PointCloud Gripper, PointCloud scene)
00013 { ROS_INFO("Sende Gripper-Punkte");
00014
00015 Gripper.header.frame_id = "/torso_lift_link";
00016 Gripper.header.stamp = ros::Time::now ();
00017
00018 this->pubGripper.publish(Gripper);
00019
00020 ROS_INFO("sending ready");
00021 }
00022
00023 void GripperPointsPublisher::callback(const PointCloud::ConstPtr& object, const PointCloud::ConstPtr& scene, tf::TransformListener *listener)
00024 { PointCloud pt;
00025 PointCloud reduzierteKpt;
00026
00027 ROS_INFO ("Empfange scene, Empfange Objekt");
00028
00029 filterPointCloud(*object, reduzierteKpt, listener);
00030
00031
00032 PointCloud single;
00033 single.points.push_back(reduzierteKpt.points[0]);
00034 single.points.push_back(reduzierteKpt.points[1]);
00035 single.header.stamp = ros::Time::now();
00036 single.header.frame_id = "/torso_lift_link";
00037 pubGripper_single.publish(single);
00038
00039 this->sendGripperPoints(reduzierteKpt, *scene);
00040 }
00041
00042 void filterPointCloud(const PointCloud& stuhl_object, PointCloud& reduzierteKpt, tf::TransformListener *listener)
00043 { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
00044 PointCloud cloud_transformed;
00045
00046 pcl_ros::transformPointCloud("/torso_lift_link", stuhl_object, cloud_transformed, *listener);
00047
00048 pcl::PointXYZ leftest_Point;
00049 leftest_Point.y = -100;
00050 pcl::PointXYZ rightest_Point;
00051 rightest_Point.y = 100;
00052
00053
00054 float z_max = -1;
00055 for (size_t i = 0; i < cloud_transformed.points.size(); ++i)
00056 { z_max = std::max(z_max,cloud_transformed.points[i].z);
00057 }
00058
00059 ROS_INFO_STREAM("MaxStuhl: " << z_max);
00060
00061
00062 pcl::PassThrough<pcl::PointXYZ> pass;
00063 pass.setInputCloud (cloud_transformed.makeShared());
00064 pass.setFilterFieldName ("z");
00065 pass.setFilterLimits (z_max - 0.1, z_max);
00066
00067 pass.filter (*cloud_filtered);
00068
00069
00070
00071
00072
00073
00074
00075
00076 BOOST_FOREACH(pcl::PointXYZ& pt, *cloud_filtered)
00077 if (pt.y > leftest_Point.y) {
00078 leftest_Point.x = pt.x;
00079 leftest_Point.y = pt.y;
00080 leftest_Point.z = pt.z;
00081
00082 }
00083
00084 for (uint i=0; i<cloud_filtered->points.size(); ++i)
00085 { pcl::PointXYZ pt = cloud_filtered->points[i];
00086 if (pt.y < rightest_Point.y)
00087 { rightest_Point.x = pt.x;
00088 rightest_Point.y = pt.y;
00089 rightest_Point.z = pt.z;
00090 }
00091 }
00092
00093 leftest_Point.z = rightest_Point.z = z_max - 0.05;
00094
00095 ROS_INFO("leftmost: %f %f %f", leftest_Point.x,leftest_Point.y,leftest_Point.z );
00096 ROS_INFO("rightmost: %f %f %f", rightest_Point.x,rightest_Point.y,rightest_Point.z );
00097
00098
00099 reduzierteKpt.push_back(leftest_Point);
00100 reduzierteKpt.push_back(rightest_Point);
00101
00102 BOOST_FOREACH(pcl::PointXYZ& pt, *cloud_filtered)
00103 reduzierteKpt.push_back(pt);
00104 }