gripper_points_publisher.cpp

Go to the documentation of this file.

#include "gripper_points_publisher.h"
#include <boost/foreach.hpp>
#include "pcl_ros/transforms.h"
#include <pcl/filters/passthrough.h>

GripperPointsPublisher::GripperPointsPublisher()
        {       ros::NodeHandle grippernh;
                pubGripper = grippernh.advertise<PointCloud> ("GripperPunkte", 3);
                pubGripper_single  = grippernh.advertise<PointCloud> ("GripperPunkte_single", 3);
        }

        void GripperPointsPublisher::sendGripperPoints(PointCloud Gripper, PointCloud scene)
        {       ROS_INFO("Sende Gripper-Punkte");

                Gripper.header.frame_id = "/torso_lift_link";
                Gripper.header.stamp = ros::Time::now ();

                this->pubGripper.publish(Gripper);

                ROS_INFO("sending ready");
        }

        void GripperPointsPublisher::callback(const PointCloud::ConstPtr& object, const PointCloud::ConstPtr& scene, tf::TransformListener *listener)
        {       PointCloud pt;
                PointCloud reduzierteKpt;

                ROS_INFO ("Empfange scene, Empfange Objekt");

                filterPointCloud(*object, reduzierteKpt, listener);

                //---publish only the gripper_Points for visualization in rviz---
                PointCloud single;
                single.points.push_back(reduzierteKpt.points[0]);
                single.points.push_back(reduzierteKpt.points[1]);
                single.header.stamp = ros::Time::now();
                single.header.frame_id = "/torso_lift_link";
                pubGripper_single.publish(single);

                this->sendGripperPoints(reduzierteKpt, *scene);
        }

        void filterPointCloud(const PointCloud& stuhl_object, PointCloud& reduzierteKpt, tf::TransformListener *listener)
        {       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
                PointCloud cloud_transformed;

                pcl_ros::transformPointCloud("/torso_lift_link", stuhl_object, cloud_transformed, *listener);

                pcl::PointXYZ leftest_Point;
                leftest_Point.y = -100;
                pcl::PointXYZ rightest_Point;
                rightest_Point.y = 100;

                //-------get highest point of the object-----------------
                float z_max = -1;
                for (size_t i = 0; i < cloud_transformed.points.size(); ++i)
                {               z_max = std::max(z_max,cloud_transformed.points[i].z);
                }

                ROS_INFO_STREAM("MaxStuhl: " << z_max);

                //----Create the filtering object----
                pcl::PassThrough<pcl::PointXYZ> pass;
                pass.setInputCloud (cloud_transformed.makeShared());
                pass.setFilterFieldName ("z");
                pass.setFilterLimits (z_max - 0.1, z_max);
                //pass.setFilterLimitsNegative (true);
                pass.filter (*cloud_filtered);

//              pcl::PointXYZ a,b;
//
//              pcl::getMinMax3D(*cloud_filtered,a,b);
//
//              ROS_INFO("min: %f %f %f, max %f %f %f", a.x,a.y,a.z, b.x,b.y,b.z);

                //----get the leftest_Point and rightest_Point of the filtered cloud as gripper_Points----
                BOOST_FOREACH(pcl::PointXYZ& pt, *cloud_filtered)
                if (pt.y > leftest_Point.y) {
                        leftest_Point.x = pt.x;
                        leftest_Point.y = pt.y;
                        leftest_Point.z = pt.z;

                }

                for (uint i=0; i<cloud_filtered->points.size(); ++i)
                {       pcl::PointXYZ pt = cloud_filtered->points[i];
                        if (pt.y < rightest_Point.y)
                        {       rightest_Point.x = pt.x;
                                rightest_Point.y = pt.y;
                                rightest_Point.z = pt.z;
                        }
                }

                leftest_Point.z = rightest_Point.z = z_max - 0.05;

                ROS_INFO("leftmost: %f %f %f", leftest_Point.x,leftest_Point.y,leftest_Point.z );
                ROS_INFO("rightmost: %f %f %f", rightest_Point.x,rightest_Point.y,rightest_Point.z );

                //---push the gripper_Points and the filtered cloud (for visualization) into a new one---
                reduzierteKpt.push_back(leftest_Point);
                reduzierteKpt.push_back(rightest_Point);

                BOOST_FOREACH(pcl::PointXYZ& pt, *cloud_filtered)
                reduzierteKpt.push_back(pt);
        }