pointcloud_localization_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/pointcloud_localization.h"
00038 #include <jsk_recognition_msgs/ICPAlign.h>
00039 #include "jsk_recognition_utils/pcl_conversion_util.h"
00040 #include <pcl_ros/transforms.h>
00041 #include <pcl/filters/voxel_grid.h>
00042 #include <pcl/filters/passthrough.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void PointCloudLocalization::onInit()
00047   {
00048     pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00049     DiagnosticNodelet::onInit();
00050     tf_listener_ = TfListenerSingleton::getInstance();
00051     // initialize localize_transform_ as identity
00052     localize_transform_.setIdentity();
00053     pnh_->param("global_frame", global_frame_, std::string("map"));
00054     pnh_->param("odom_frame", odom_frame_, std::string("odom"));
00055     pnh_->param("leaf_size", leaf_size_, 0.01);
00056     pnh_->param("initialize_from_tf", initialize_from_tf_, false);
00057     if (initialize_from_tf_) {
00058       pnh_->param("initialize_tf", initialize_tf_, std::string("odom_on_ground"));
00059     }
00060     pnh_->param("clip_unseen_pointcloud", clip_unseen_pointcloud_, false);
00061     if (clip_unseen_pointcloud_) {
00062       pnh_->param("sensor_frame", sensor_frame_, std::string("BODY"));
00063     }
00064     pnh_->param("use_normal", use_normal_, false);
00065     double cloud_rate;
00066     pnh_->param("cloud_rate", cloud_rate, 10.0);
00067     double tf_rate;
00068     pnh_->param("tf_rate", tf_rate, 20.0);
00069     pub_cloud_ = pnh_->advertise<sensor_msgs::PointCloud2>("output", 1);
00070     // always subscribe pointcloud...?
00071     sub_ = pnh_->subscribe("input", 1, &PointCloudLocalization::cloudCallback, this);
00072     localization_srv_ = pnh_->advertiseService(
00073       "localize", &PointCloudLocalization::localizationRequest, this);
00074     update_offset_srv_ = pnh_->advertiseService(
00075       "update_offset", &PointCloudLocalization::updateOffsetCallback, this);
00076 
00077     // timer to publish cloud
00078     cloud_timer_ = pnh_->createTimer(
00079       ros::Duration(1.0 / cloud_rate),
00080       boost::bind(&PointCloudLocalization::cloudTimerCallback, this, _1));
00081     // timer to publish tf
00082     tf_timer_ = pnh_->createTimer(
00083       ros::Duration(1.0 / tf_rate),
00084       boost::bind(&PointCloudLocalization::tfTimerCallback, this, _1));
00085 
00086     onInitPostProcess();
00087   }
00088 
00089   void PointCloudLocalization::subscribe()
00090   {
00091     // dummy
00092   }
00093   
00094   void PointCloudLocalization::unsubscribe()
00095   {
00096     // dummy
00097   }
00098 
00099   void PointCloudLocalization::applyDownsampling(
00100     pcl::PointCloud<pcl::PointNormal>::Ptr in_cloud,
00101     pcl::PointCloud<pcl::PointNormal>& out_cloud)
00102   {
00103     pcl::VoxelGrid<pcl::PointNormal> vg;
00104     vg.setInputCloud(in_cloud);
00105     vg.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
00106     vg.filter(out_cloud);
00107   }
00108   
00109   void PointCloudLocalization::cloudTimerCallback(
00110     const ros::TimerEvent& event)
00111   {
00112     boost::mutex::scoped_lock lock(mutex_);
00113     ros::Time stamp = event.current_real;
00114     if (all_cloud_) {
00115       sensor_msgs::PointCloud2 ros_cloud;
00116       pcl::toROSMsg(*all_cloud_, ros_cloud);
00117       ros_cloud.header.stamp = stamp;
00118       ros_cloud.header.frame_id = global_frame_;
00119       pub_cloud_.publish(ros_cloud);
00120     }
00121   }
00122 
00123   void PointCloudLocalization::tfTimerCallback(
00124     const ros::TimerEvent& event)
00125   {
00126     boost::mutex::scoped_lock lock(tf_mutex_);
00127     try {
00128     ros::Time stamp = event.current_real;
00129     if (initialize_from_tf_ && first_time_) {
00130       // Update localize_transform_ to points initialize_tf
00131       tf::StampedTransform transform = lookupTransformWithDuration(
00132         tf_listener_,
00133         initialize_tf_, odom_frame_, stamp, ros::Duration(1.0));
00134       localize_transform_ = transform;
00135 
00136     }
00137     tf_broadcast_.sendTransform(tf::StampedTransform(localize_transform_,
00138                                                      stamp,
00139                                                      global_frame_,
00140                                                      odom_frame_));
00141     }
00142     catch (tf2::TransformException& e) {
00143       NODELET_FATAL("Failed to lookup transformation: %s", e.what());
00144     }
00145   }
00146 
00147   void PointCloudLocalization::cloudCallback(
00148     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00149   {
00150     vital_checker_->poke();
00151     boost::mutex::scoped_lock lock(mutex_);
00152     //NODELET_INFO("cloudCallback");
00153     latest_cloud_ = cloud_msg;
00154     if (localize_requested_){
00155       NODELET_INFO("localization is requested");
00156       try {
00157         pcl::PointCloud<pcl::PointNormal>::Ptr
00158           local_cloud (new pcl::PointCloud<pcl::PointNormal>);
00159         pcl::fromROSMsg(*latest_cloud_, *local_cloud);
00160         NODELET_INFO("waiting for tf transformation from %s tp %s",
00161                      latest_cloud_->header.frame_id.c_str(),
00162                      global_frame_.c_str());
00163         if (tf_listener_->waitForTransform(
00164               latest_cloud_->header.frame_id,
00165               global_frame_,
00166               latest_cloud_->header.stamp,
00167               ros::Duration(1.0))) {
00168           pcl::PointCloud<pcl::PointNormal>::Ptr
00169             input_cloud (new pcl::PointCloud<pcl::PointNormal>);
00170           if (use_normal_) {
00171             pcl_ros::transformPointCloudWithNormals(global_frame_,
00172                                                     *local_cloud,
00173                                                     *input_cloud,
00174                                                     *tf_listener_);
00175           }
00176           else {
00177             pcl_ros::transformPointCloud(global_frame_,
00178                                          *local_cloud,
00179                                          *input_cloud,
00180                                          *tf_listener_);
00181           }
00182           pcl::PointCloud<pcl::PointNormal>::Ptr
00183             input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
00184           applyDownsampling(input_cloud, *input_downsampled_cloud);
00185           if (isFirstTime()) {
00186             all_cloud_ = input_downsampled_cloud;
00187             first_time_ = false;
00188           }
00189           else {
00190             // run ICP
00191             ros::ServiceClient client
00192               = pnh_->serviceClient<jsk_recognition_msgs::ICPAlign>("icp_align");
00193             jsk_recognition_msgs::ICPAlign icp_srv;
00194 
00195             if (clip_unseen_pointcloud_) {
00196               // Before running ICP, remove pointcloud where we cannot see
00197               // First, transform reference pointcloud, that is all_cloud_, into
00198               // sensor frame.
00199               // And after that, remove points which are x < 0.
00200               tf::StampedTransform global_sensor_tf_transform
00201                 = lookupTransformWithDuration(
00202                   tf_listener_,
00203                   global_frame_,
00204                   sensor_frame_,
00205                   cloud_msg->header.stamp,
00206                   ros::Duration(1.0));
00207               Eigen::Affine3f global_sensor_transform;
00208               tf::transformTFToEigen(global_sensor_tf_transform,
00209                                      global_sensor_transform);
00210               pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
00211                 (new pcl::PointCloud<pcl::PointNormal>);
00212               pcl::transformPointCloudWithNormals(
00213                 *all_cloud_,
00214                 *sensor_cloud,
00215                 global_sensor_transform.inverse());
00216               // Remove negative-x points
00217               pcl::PassThrough<pcl::PointNormal> pass;
00218               pass.setInputCloud(sensor_cloud);
00219               pass.setFilterFieldName("x");
00220               pass.setFilterLimits(0.0, 100.0);
00221               pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
00222                 (new pcl::PointCloud<pcl::PointNormal>);
00223               pass.filter(*filtered_cloud);
00224               NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
00225               // Convert the pointcloud to global frame again
00226               pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
00227                 (new pcl::PointCloud<pcl::PointNormal>);
00228               pcl::transformPointCloudWithNormals(
00229                 *filtered_cloud,
00230                 *global_filtered_cloud,
00231                 global_sensor_transform);
00232               pcl::toROSMsg(*global_filtered_cloud,
00233                             icp_srv.request.target_cloud);
00234             }
00235             else {
00236               pcl::toROSMsg(*all_cloud_,
00237                             icp_srv.request.target_cloud);
00238             }
00239             pcl::toROSMsg(*input_downsampled_cloud,
00240                           icp_srv.request.reference_cloud);
00241             
00242             if (client.call(icp_srv)) {
00243               Eigen::Affine3f transform;
00244               tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
00245               Eigen::Vector3f transform_pos(transform.translation());
00246               float roll, pitch, yaw;
00247               pcl::getEulerAngles(transform, roll, pitch, yaw);
00248               NODELET_INFO("aligned parameter --");
00249               NODELET_INFO("  - pos: [%f, %f, %f]",
00250                            transform_pos[0],
00251                            transform_pos[1],
00252                            transform_pos[2]);
00253               NODELET_INFO("  - rot: [%f, %f, %f]", roll, pitch, yaw);
00254               pcl::PointCloud<pcl::PointNormal>::Ptr
00255                 transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>);
00256               if (use_normal_) {
00257                 pcl::transformPointCloudWithNormals(*input_cloud,
00258                                                     *transformed_input_cloud,
00259                                                     transform);
00260               }
00261               else {
00262                 pcl::transformPointCloud(*input_cloud,
00263                                          *transformed_input_cloud,
00264                                          transform);
00265               }
00266               pcl::PointCloud<pcl::PointNormal>::Ptr
00267                 concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>);
00268               *concatenated_cloud = *all_cloud_ + *transformed_input_cloud;
00269               // update *all_cloud
00270               applyDownsampling(concatenated_cloud, *all_cloud_);
00271               // update localize_transform_
00272               tf::Transform icp_transform;
00273               tf::transformEigenToTF(transform, icp_transform);
00274               {
00275                 boost::mutex::scoped_lock tf_lock(tf_mutex_);
00276                 localize_transform_ = localize_transform_ * icp_transform;
00277               }
00278             }
00279             else {
00280               NODELET_ERROR("Failed to call ~icp_align");
00281               return;
00282             }
00283           }
00284           localize_requested_ = false;
00285         }
00286         else {
00287           NODELET_WARN("No tf transformation is available");
00288         }
00289       }
00290       catch (tf2::ConnectivityException &e)
00291       {
00292         NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00293         return;
00294       }
00295       catch (tf2::InvalidArgumentException &e)
00296       {
00297         NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00298         return;
00299       }
00300     }
00301   }
00302 
00303   bool PointCloudLocalization::isFirstTime()
00304   {
00305     return first_time_;
00306   }
00307 
00308   bool PointCloudLocalization::localizationRequest(
00309     std_srvs::Empty::Request& req,
00310     std_srvs::Empty::Response& res)
00311   {
00312     NODELET_INFO("localize!");
00313     boost::mutex::scoped_lock lock(mutex_);
00314     localize_requested_ = true;
00315     return true;
00316   }
00317 
00318   bool PointCloudLocalization::updateOffsetCallback(
00319     jsk_recognition_msgs::UpdateOffset::Request& req,
00320     jsk_recognition_msgs::UpdateOffset::Response& res)
00321   {
00322     boost::mutex::scoped_lock lock(mutex_);
00323     geometry_msgs::TransformStamped next_pose = req.transformation;
00324     // convert geometry_msgs::TransformStamped into tf::Transform
00325     tf::StampedTransform tf_transform;
00326     tf::transformStampedMsgToTF(next_pose, tf_transform);
00327     // TODO: resolve tf
00328     localize_transform_ = tf_transform;
00329     return true;
00330   }
00331 }
00332 
00333 #include <pluginlib/class_list_macros.h>
00334 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PointCloudLocalization, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50