Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef JSK_PCL_ROS_POINTCLOUD_MOVEIT_FILTER_H_
00038 #define JSK_PCL_ROS_POINTCLOUD_MOVEIT_FILTER_H_
00039
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00043 #include <tf2_ros/message_filter.h>
00044 #else
00045 #include <tf/message_filter.h>
00046 #endif
00047 #include <message_filters/subscriber.h>
00048 #include <sensor_msgs/PointCloud2.h>
00049 #include <moveit/version.h>
00050 #include <moveit/occupancy_map_monitor/occupancy_map_updater.h>
00051 #include <moveit/point_containment_filter/shape_mask.h>
00052 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00053 #include <XmlRpcException.h>
00054 #include <pcl/point_types.h>
00055 #include <pcl/point_cloud.h>
00056 #include <pcl_conversions/pcl_conversions.h>
00057 #include <pcl/filters/extract_indices.h>
00058
00059 namespace jsk_pcl_ros
00060 {
00061 typedef occupancy_map_monitor::ShapeHandle ShapeHandle;
00062 typedef occupancy_map_monitor::ShapeTransformCache ShapeTransformCache;
00063 class PointCloudMoveitFilter:
00064 public occupancy_map_monitor::OccupancyMapUpdater
00065 {
00066 public:
00067 PointCloudMoveitFilter();
00068 virtual ~PointCloudMoveitFilter();
00069
00070 virtual bool setParams(XmlRpc::XmlRpcValue ¶ms);
00071 virtual bool initialize();
00072 virtual void start();
00073 virtual void stop();
00074 virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape);
00075 virtual void forgetShape(ShapeHandle handle);
00076
00077 protected:
00078 virtual void stopHelper();
00079 virtual bool getShapeTransform(ShapeHandle h,
00080 #if ROS_VERSION_MINIMUM(1,14,0)
00081 Eigen::Isometry3d &transform) const;
00082 #else
00083 Eigen::Affine3d &transform) const;
00084 #endif
00085 template <typename PointT>
00086 void cloudMsgCallback(
00087 const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
00088 {
00089 if (monitor_->getMapFrame().empty())
00090 monitor_->setMapFrame(cloud_msg->header.frame_id);
00091 tf::StampedTransform map_H_sensor;
00092 if (monitor_->getMapFrame() == cloud_msg->header.frame_id) {
00093 map_H_sensor.setIdentity();
00094 }
00095 else {
00096 if (tf_) {
00097 try
00098 {
00099 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00100 tf::transformStampedMsgToTF(tf_->lookupTransform(monitor_->getMapFrame(),
00101 cloud_msg->header.frame_id,
00102 cloud_msg->header.stamp),
00103 map_H_sensor);
00104 #else
00105 tf_->lookupTransform(monitor_->getMapFrame(),
00106 cloud_msg->header.frame_id,
00107 cloud_msg->header.stamp,
00108 map_H_sensor);
00109 #endif
00110 }
00111 catch (tf::TransformException& ex)
00112 {
00113 ROS_ERROR_STREAM("Transform error of sensor data: "
00114 << ex.what() << "; quitting callback");
00115 return;
00116 }
00117 }
00118 else {
00119 return;
00120 }
00121 }
00122
00123
00124 typename pcl::PointCloud<PointT>::Ptr cloud;
00125 cloud.reset(new pcl::PointCloud<PointT>());
00126 pcl::PointCloud<pcl::PointXYZ> xyz_cloud;
00127 pcl::fromROSMsg(*cloud_msg, *cloud);
00128 pcl::fromROSMsg(*cloud_msg, xyz_cloud);
00129 const tf::Vector3 &sensor_origin_tf = map_H_sensor.getOrigin();
00130 octomap::point3d sensor_origin(
00131 sensor_origin_tf.getX(),
00132 sensor_origin_tf.getY(),
00133 sensor_origin_tf.getZ());
00134 Eigen::Vector3d sensor_origin_eigen(
00135 sensor_origin_tf.getX(),
00136 sensor_origin_tf.getY(),
00137 sensor_origin_tf.getZ());
00138 if (!updateTransformCache(cloud_msg->header.frame_id,
00139 cloud_msg->header.stamp))
00140 {
00141 ROS_ERROR_THROTTLE(
00142 1, "Transform cache was not updated. Self-filtering may fail.");
00143 return;
00144 }
00145
00146 #if (MOVEIT_VERSION_MAJOR == 0 and MOVEIT_VERSION_MINOR < 6)
00147 shape_mask_->maskContainment(xyz_cloud, sensor_origin_eigen, 0.0,
00148 max_range_, mask_);
00149 #else // from moveit 0.6 (indigo), PCL dependency is removed
00150 shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0,
00151 max_range_, mask_);
00152 #endif
00153 typename pcl::PointCloud<PointT>::Ptr
00154 filtered_cloud (new pcl::PointCloud<PointT>);
00155 pcl::PointIndices::Ptr indices (new pcl::PointIndices);
00156
00157 for (size_t i = 0; i < mask_.size(); i++) {
00158 if (mask_[i] == point_containment_filter::ShapeMask::OUTSIDE) {
00159 indices->indices.push_back(i);
00160 }
00161 }
00162 pcl::ExtractIndices<PointT> ex;
00163 ex.setInputCloud(cloud);
00164 ex.setIndices(indices);
00165 ex.setKeepOrganized(keep_organized_);
00166 ex.filter(*filtered_cloud);
00167 sensor_msgs::PointCloud2 ros_cloud;
00168 pcl::toROSMsg(*filtered_cloud, ros_cloud);
00169 ros_cloud.header = cloud_msg->header;
00170 filtered_cloud_publisher_.publish(ros_cloud);
00171 }
00172
00174
00176 ros::NodeHandle root_nh_;
00177 ros::NodeHandle private_nh_;
00178 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00179 std::shared_ptr<tf2_ros::Buffer> tf_;
00180 #else
00181 boost::shared_ptr<tf::Transformer> tf_;
00182 #endif
00183 std::string point_cloud_topic_;
00184 double scale_;
00185 double padding_;
00186 double max_range_;
00187 unsigned int point_subsample_;
00188 std::string filtered_cloud_topic_;
00189 ros::Publisher filtered_cloud_publisher_;
00190
00191 message_filters::Subscriber<sensor_msgs::PointCloud2> *point_cloud_subscriber_;
00192 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00193 tf2_ros::MessageFilter<sensor_msgs::PointCloud2> *point_cloud_filter_;
00194 #else
00195 tf::MessageFilter<sensor_msgs::PointCloud2> *point_cloud_filter_;
00196 #endif
00197
00198 boost::scoped_ptr<point_containment_filter::ShapeMask> shape_mask_;
00199 std::vector<int> mask_;
00200
00201 bool use_color_;
00202 bool keep_organized_;
00203
00204 private:
00205
00206 };
00207 }
00208
00209 #endif