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 #include "jsk_pcl_ros/normal_direction_filter.h"
00037 #include "jsk_pcl_ros/pcl_conversion_util.h"
00038 #include <jsk_topic_tools/rosparam_utils.h>
00039 #include <eigen_conversions/eigen_msg.h>
00040
00041 namespace jsk_pcl_ros
00042 {
00043 void NormalDirectionFilter::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pnh_->param("use_imu", use_imu_, false);
00047 if (!use_imu_) {
00048 std::vector<double> direction;
00049 if (!jsk_topic_tools::readVectorParameter(*pnh_, "direction", direction)) {
00050 JSK_NODELET_ERROR("You need to specify ~direction");
00051 return;
00052 }
00053 pointFromVectorToVector<std::vector<double>, Eigen::Vector3f>(
00054 direction, static_direction_);
00055 }
00056 else {
00057 tf_listener_ = TfListenerSingleton::getInstance();
00058 }
00059
00060 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00061 dynamic_reconfigure::Server<Config>::CallbackType f =
00062 boost::bind (
00063 &NormalDirectionFilter::configCallback, this, _1, _2);
00064 srv_->setCallback (f);
00065 pnh_->param("queue_size", queue_size_, 200);
00066 pub_ = advertise<PCLIndicesMsg>(*pnh_, "output", 1);
00067 }
00068
00069
00070 void NormalDirectionFilter::configCallback(Config &config, uint32_t level)
00071 {
00072 boost::mutex::scoped_lock lock(mutex_);
00073 eps_angle_ = config.eps_angle;
00074 angle_offset_ = config.angle_offset;
00075 }
00076
00077 void NormalDirectionFilter::subscribe()
00078 {
00079 if (!use_imu_) {
00080 sub_ = pnh_->subscribe("input", 1, &NormalDirectionFilter::filter, this);
00081 }
00082 else {
00083 sub_input_.subscribe(*pnh_, "input", 1);
00084 sub_imu_.subscribe(*pnh_, "input_imu", 1);
00085 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00086 sync_->connectInput(sub_input_, sub_imu_);
00087 sync_->registerCallback(boost::bind(&NormalDirectionFilter::filter, this, _1, _2));
00088 }
00089 }
00090
00091 void NormalDirectionFilter::unsubscribe()
00092 {
00093 if (!use_imu_) {
00094 sub_.shutdown();
00095 }
00096 else {
00097 sub_input_.unsubscribe();
00098 sub_imu_.unsubscribe();
00099 }
00100 }
00101
00102 void NormalDirectionFilter::filterIndices(
00103 const pcl::PointCloud<pcl::Normal>::Ptr& normal_cloud,
00104 const Eigen::Vector3f& direction,
00105 pcl::PointIndices& indices)
00106 {
00107 for (size_t i = 0; i < normal_cloud->points.size(); i++) {
00108 Eigen::Vector3f normal = normal_cloud->points[i].getNormalVector3fMap().normalized();
00109 if (!isnan(normal[0]) &&
00110 !isnan(normal[1]) &&
00111 !isnan(normal[2])) {
00112 double dot = std::abs(normal.dot(direction));
00113 if (dot < -1.0) {
00114 dot = -1.0;
00115 }
00116 else if (dot > 1.0) {
00117 dot = 1.0;
00118 }
00119 double angle = acos(dot);
00120 double angle_diff = std::abs(angle - angle_offset_);
00121 if (angle_diff < eps_angle_) {
00122 indices.indices.push_back(i);
00123 }
00124 }
00125 }
00126 }
00127
00128 void NormalDirectionFilter::filter(
00129 const sensor_msgs::PointCloud2::ConstPtr& msg,
00130 const sensor_msgs::Imu::ConstPtr& imu_msg)
00131 {
00132 boost::mutex::scoped_lock lock(mutex_);
00133 vital_checker_->poke();
00134 pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
00135 pcl::fromROSMsg(*msg, *normal);
00136 geometry_msgs::Vector3Stamped stamped_imu, transformed_imu;
00137 stamped_imu.header = imu_msg->header;
00138 stamped_imu.vector = imu_msg->linear_acceleration;
00139 try
00140 {
00141 tf_listener_->waitForTransform(msg->header.frame_id, imu_msg->header.frame_id, imu_msg->header.stamp,
00142 ros::Duration(0.1));
00143 tf_listener_->transformVector(
00144 msg->header.frame_id, stamped_imu, transformed_imu);
00145 Eigen::Vector3d imu_vectord;
00146 Eigen::Vector3f imu_vector;
00147 tf::vectorMsgToEigen(transformed_imu.vector, imu_vectord);
00148 pointFromVectorToVector<Eigen::Vector3d, Eigen::Vector3f>(
00149 imu_vectord, imu_vector);
00150 imu_vector.normalize();
00151 pcl::PointIndices indices;
00152 filterIndices(normal, imu_vector, indices);
00153 pcl_msgs::PointIndices ros_indices;
00154 pcl_conversions::fromPCL(indices, ros_indices);
00155 ros_indices.header = msg->header;
00156 pub_.publish(ros_indices);
00157 }
00158 catch (tf2::ConnectivityException &e)
00159 {
00160 JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00161 }
00162 catch (tf2::InvalidArgumentException &e)
00163 {
00164 JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00165 }
00166 catch (tf2::ExtrapolationException &e)
00167 {
00168 JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00169 }
00170 }
00171
00172 void NormalDirectionFilter::filter(
00173 const sensor_msgs::PointCloud2::ConstPtr& msg)
00174 {
00175 boost::mutex::scoped_lock lock(mutex_);
00176 vital_checker_->poke();
00177 pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
00178 pcl::fromROSMsg(*msg, *normal);
00179
00180 pcl::PointIndices indices;
00181 filterIndices(normal, static_direction_, indices);
00182 pcl_msgs::PointIndices ros_indices;
00183 pcl_conversions::fromPCL(indices, ros_indices);
00184 ros_indices.header = msg->header;
00185 pub_.publish(ros_indices);
00186 }
00187
00188 }
00189
00190 #include <pluginlib/class_list_macros.h>
00191 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::NormalDirectionFilter, nodelet::Nodelet);