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 #include <jsk_topic_tools/log_utils.h>
00036 #include "jsk_pcl_ros/pointcloud_moveit_filter.h"
00037
00038 namespace jsk_pcl_ros
00039 {
00040 PointCloudMoveitFilter::PointCloudMoveitFilter():
00041 OccupancyMapUpdater("PointCloudMoveitFilter"),
00042 private_nh_("~"),
00043 scale_(1.0),
00044 padding_(0.0),
00045 max_range_(std::numeric_limits<double>::infinity()),
00046 point_subsample_(1),
00047 point_cloud_subscriber_(NULL),
00048 point_cloud_filter_(NULL),
00049 use_color_(false),
00050 keep_organized_(false)
00051 {
00052 }
00053
00054 PointCloudMoveitFilter::~PointCloudMoveitFilter()
00055 {
00056 }
00057
00058 bool PointCloudMoveitFilter::setParams(XmlRpc::XmlRpcValue ¶ms)
00059 {
00060 try
00061 {
00062 if (!params.hasMember("point_cloud_topic"))
00063 return false;
00064 point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
00065
00066 readXmlParam(params, "max_range", &max_range_);
00067 readXmlParam(params, "padding_offset", &padding_);
00068 readXmlParam(params, "padding_scale", &scale_);
00069 readXmlParam(params, "point_subsample", &point_subsample_);
00070 if (!params.hasMember("filtered_cloud_topic")) {
00071 ROS_ERROR("filtered_cloud_topic is required");
00072 return false;
00073 }
00074 else {
00075 filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
00076 }
00077 if (params.hasMember("filtered_cloud_use_color")) {
00078 use_color_ = (bool)params["filtered_cloud_use_color"];
00079 }
00080 if (params.hasMember("filtered_cloud_keep_organized")) {
00081 keep_organized_ = (bool)params["filtered_cloud_keep_organized"];
00082 }
00083 }
00084 catch (XmlRpc::XmlRpcException& ex)
00085 {
00086 ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
00087 return false;
00088 }
00089
00090 return true;
00091 }
00092
00093
00094
00095 bool PointCloudMoveitFilter::initialize()
00096 {
00097 tf_ = monitor_->getTFClient();
00098 shape_mask_.reset(new point_containment_filter::ShapeMask());
00099 shape_mask_->setTransformCallback(
00100 boost::bind(&PointCloudMoveitFilter::getShapeTransform, this, _1, _2));
00101 filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(
00102 filtered_cloud_topic_, 10, false);
00103 return true;
00104 }
00105
00106 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00107 bool PointCloudMoveitFilter::getShapeTransform(ShapeHandle h, Eigen::Isometry3d &transform) const
00108 #else
00109 bool PointCloudMoveitFilter::getShapeTransform(ShapeHandle h, Eigen::Affine3d &transform) const
00110 #endif
00111 {
00112 ShapeTransformCache::const_iterator it = transform_cache_.find(h);
00113 if (it == transform_cache_.end())
00114 {
00115 ROS_ERROR("Internal error. Shape filter handle %u not found", h);
00116 return false;
00117 }
00118 transform = it->second;
00119 return true;
00120 }
00121
00122 ShapeHandle PointCloudMoveitFilter::excludeShape(const shapes::ShapeConstPtr &shape)
00123 {
00124 ShapeHandle h = 0;
00125 if (shape_mask_)
00126 h = shape_mask_->addShape(shape, scale_, padding_);
00127 else
00128 ROS_ERROR("Shape filter not yet initialized!");
00129 return h;
00130 }
00131
00132
00133 void PointCloudMoveitFilter::forgetShape(ShapeHandle handle)
00134 {
00135 if (shape_mask_)
00136 shape_mask_->removeShape(handle);
00137 }
00138
00139
00140 void PointCloudMoveitFilter::start()
00141 {
00142 if (point_cloud_subscriber_)
00143 return;
00144
00145 point_cloud_subscriber_
00146 = new message_filters::Subscriber<sensor_msgs::PointCloud2>(
00147 root_nh_, point_cloud_topic_, 5);
00148 if (tf_ && !monitor_->getMapFrame().empty())
00149 {
00150 point_cloud_filter_
00151 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
00152 = new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(
00153 *point_cloud_subscriber_, *tf_, monitor_->getMapFrame(), 5, nullptr);
00154 #else
00155 = new tf::MessageFilter<sensor_msgs::PointCloud2>(
00156 *point_cloud_subscriber_, *tf_, monitor_->getMapFrame(), 5);
00157 #endif
00158 if (use_color_) {
00159 point_cloud_filter_->registerCallback(
00160 boost::bind(
00161 &PointCloudMoveitFilter::cloudMsgCallback<pcl::PointXYZRGB>,
00162 this, _1));
00163 }
00164 else {
00165 point_cloud_filter_->registerCallback(
00166 boost::bind(
00167 &PointCloudMoveitFilter::cloudMsgCallback<pcl::PointXYZ>,
00168 this, _1));
00169 }
00170 ROS_INFO("Listening to '%s' using message filter with target frame '%s'",
00171 point_cloud_topic_.c_str(),
00172 point_cloud_filter_->getTargetFramesString().c_str());
00173 }
00174 else
00175 {
00176 if (use_color_) {
00177 point_cloud_subscriber_->registerCallback(
00178 boost::bind(
00179 &PointCloudMoveitFilter::cloudMsgCallback<pcl::PointXYZRGB>,
00180 this, _1));
00181 }
00182 else {
00183 point_cloud_subscriber_->registerCallback(
00184 boost::bind(&PointCloudMoveitFilter::cloudMsgCallback<pcl::PointXYZ>,
00185 this, _1));
00186 }
00187 ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
00188 }
00189 }
00190
00191 void PointCloudMoveitFilter::stopHelper()
00192 {
00193 delete point_cloud_filter_;
00194 delete point_cloud_subscriber_;
00195 }
00196
00197 void PointCloudMoveitFilter::stop()
00198 {
00199 stopHelper();
00200 point_cloud_filter_ = NULL;
00201 point_cloud_subscriber_ = NULL;
00202 }
00203
00204 }
00205
00206 #include <class_loader/class_loader.h>
00207 CLASS_LOADER_REGISTER_CLASS(jsk_pcl_ros::PointCloudMoveitFilter, occupancy_map_monitor::OccupancyMapUpdater)