pointcloud_cropper.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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 Willow Garage 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 #include "jsk_interactive_marker/pointcloud_cropper.h"
00037 #include "jsk_interactive_marker/interactive_marker_helpers.h"
00038 #include <pcl_conversions/pcl_conversions.h>
00039 #include <algorithm>
00040 #include <eigen_conversions/eigen_msg.h>
00041 
00042 #include <pcl_ros/pcl_nodelet.h>
00043 
00044 //using namespace jsk_interactive_marker;
00045 namespace jsk_interactive_marker {
00046   Cropper::Cropper(const unsigned int nr_parameter):
00047     nr_parameter_(nr_parameter),
00048     pose_(Eigen::Affine3f::Identity())
00049   {
00050     parameters_.resize(nr_parameter_);
00051   }
00052 
00053   Cropper::~Cropper()
00054   {
00055 
00056   }
00057 
00058   void Cropper::setPose(Eigen::Affine3f pose)
00059   {
00060     pose_ = pose;
00061   }
00062 
00063   Eigen::Affine3f Cropper::getPose()
00064   {
00065     return pose_;
00066   }
00067 
00068   void Cropper::crop(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input,
00069                       pcl::PointCloud<pcl::PointXYZ>::Ptr output)
00070   {
00071     Eigen::Vector3f transf(pose_.translation());
00072     ROS_DEBUG("%s transf: %f %f %f", __FUNCTION__, transf[0], transf[1], transf[2]);
00073     output->points.clear();
00074     for (size_t i = 0; i < input->points.size(); i++) {
00075       pcl::PointXYZ p = input->points[i];
00076       if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
00077         if (isInside(p)) {
00078           output->points.push_back(p);
00079         }
00080       }
00081     }
00082   }
00083 
00084 
00085   void Cropper::updateParameter(const double param, const unsigned int index)
00086   {
00087     if (parameters_.size() >= index + 1) {
00088       parameters_[index] = param;
00089     }
00090   }
00091   
00092   SphereCropper::SphereCropper(): Cropper(1)
00093   {
00094     fillInitialParameters();    // not so good?
00095   }
00096 
00097   SphereCropper::~SphereCropper()
00098   {
00099 
00100   }
00101 
00102   bool SphereCropper::isInside(const pcl::PointXYZ& p)
00103   {
00104     Eigen::Vector3f pos = p.getVector3fMap();
00105     Eigen::Vector3f origin(pose_.translation());
00106     double distance = (pos - origin).norm();
00107     // ROS_DEBUG("pos: [%f, %f, %f], origin: [%f, %f, %f], distance: %f, R: %f",
00108     //          pos[0], pos[1], pos[2],
00109     //          origin[0], origin[1], origin[2],
00110     //          distance, getRadius());
00111     if (distance < getRadius()) {
00112       return true;
00113     }
00114     else {
00115       return false;
00116     }
00117   }
00118 
00119   double SphereCropper::getRadius()
00120   {
00121     return parameters_[0];
00122   }
00123 
00124   void SphereCropper::fillInitialParameters()
00125   {
00126     parameters_[0] = 0.5;         // 50cm
00127   }
00128 
00129   std::string SphereCropper::getName()
00130   {
00131     return "SphereCropper";
00132   }
00133 
00134   visualization_msgs::Marker SphereCropper::getMarker()
00135   {
00136     visualization_msgs::Marker marker;
00137     marker.type = visualization_msgs::Marker::SPHERE;
00138     marker.scale.x = getRadius() * 2;
00139     marker.scale.y = getRadius() * 2;
00140     marker.scale.z = getRadius() * 2;
00141     marker.color.r = 0.5;
00142     marker.color.g = 0.5;
00143     marker.color.b = 0.5;
00144     marker.color.a = 0.5;
00145     return marker;
00146   }
00147 
00148   CubeCropper::CubeCropper(): Cropper(3)
00149   {
00150     fillInitialParameters();    // not so good?
00151   }
00152 
00153   CubeCropper::~CubeCropper()
00154   {
00155 
00156   }
00157 
00158   std::string CubeCropper::getName()
00159   {
00160     return "CubeCropper";
00161   }
00162 
00163   visualization_msgs::Marker CubeCropper::getMarker()
00164   {
00165     visualization_msgs::Marker marker;
00166     marker.type = visualization_msgs::Marker::CUBE;
00167     marker.scale.x = getWidthX() * 2;
00168     marker.scale.y = getWidthY() * 2;
00169     marker.scale.z = getWidthZ() * 2;
00170     marker.color.r = 0.5;
00171     marker.color.g = 0.5;
00172     marker.color.b = 0.5;
00173     marker.color.a = 0.5;
00174     return marker;
00175   }
00176 
00177   void CubeCropper::fillInitialParameters()
00178   {
00179     parameters_[0] = 0.5;         // 50cm
00180     parameters_[1] = 0.5;         // 50cm
00181     parameters_[2] = 0.5;         // 50cm
00182   }
00183 
00184   double CubeCropper::getWidthX()
00185   {
00186     return parameters_[0];
00187   }
00188 
00189   double CubeCropper::getWidthY()
00190   {
00191     return parameters_[1];
00192   }
00193 
00194   double CubeCropper::getWidthZ()
00195   {
00196     return parameters_[2];
00197   }
00198 
00199   bool CubeCropper::isInside(const pcl::PointXYZ& p)
00200   {
00201     Eigen::Vector3f pos = p.getVector3fMap();
00202     Eigen::Vector3f diff = pos - pose_.translation();
00203     if ((fabs(diff[0]) < getWidthX()) &&
00204         (fabs(diff[1]) < getWidthY()) &&
00205         (fabs(diff[2]) < getWidthZ())) {
00206       return true;
00207     }
00208     else {
00209       return false;
00210     }
00211   }
00212   
00213   PointCloudCropper::PointCloudCropper(ros::NodeHandle& nh, ros::NodeHandle &pnh)
00214   {
00215     tf_listener_.reset(new tf::TransformListener);
00216     // initialize cropper_candidates_
00217     cropper_candidates_.push_back(boost::make_shared<SphereCropper>());
00218     cropper_candidates_.push_back(boost::make_shared<CubeCropper>());
00219     cropper_ = cropper_candidates_[0];
00220     point_pub_ = pnh.advertise<sensor_msgs::PointCloud2>("output", 1);
00221     point_visualization_pub_ = pnh.advertise<sensor_msgs::PointCloud2>(
00222       "visualization_pointcloud", 1);
00223     server_.reset(new interactive_markers::InteractiveMarkerServer(
00224                     ros::this_node::getName()));
00225     initializeInteractiveMarker();
00226     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
00227     dynamic_reconfigure::Server<Config>::CallbackType f =
00228       boost::bind (&PointCloudCropper::configCallback, this, _1, _2);
00229     srv_->setCallback (f);
00230     point_sub_ = pnh.subscribe("input", 1, &PointCloudCropper::inputCallback, this);
00231   }
00232 
00233   PointCloudCropper::~PointCloudCropper()
00234   {
00235     
00236   }
00237 
00238   void PointCloudCropper::configCallback(Config &config, uint32_t level)
00239   {
00240     boost::mutex::scoped_lock lock(mutex_);
00241     for (size_t i = 0; i < cropper_candidates_.size(); i++) {
00242       cropper_candidates_[i]->updateParameter(config.param0, 0);
00243       cropper_candidates_[i]->updateParameter(config.param1, 1);
00244       cropper_candidates_[i]->updateParameter(config.param2, 2);
00245     }
00246     reInitializeInteractiveMarker();
00247   }
00248   
00249   void PointCloudCropper::processFeedback(
00250     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00251   {
00252     boost::mutex::scoped_lock lock(mutex_);
00253     geometry_msgs::PoseStamped input_pose_stamped, transformed_pose_stamped;
00254     input_pose_stamped.pose = feedback->pose;
00255     input_pose_stamped.header.stamp = feedback->header.stamp;
00256     input_pose_stamped.header.frame_id = feedback->header.frame_id;
00257     if (!latest_pointcloud_) {
00258       ROS_WARN("no pointcloud is available yet");
00259       return;
00260     }
00261     // 1. update cropper's pose according to the posigoin of the process feedback
00262     try {
00263       tf_listener_->transformPose(
00264         latest_pointcloud_->header.frame_id,
00265         input_pose_stamped,
00266         transformed_pose_stamped);
00267     }
00268     catch (...) {
00269       ROS_FATAL("tf exception");
00270       return;
00271     }
00272     Eigen::Affine3d pose_d;
00273     tf::poseMsgToEigen(transformed_pose_stamped.pose, pose_d);
00274     // convert Eigen::Affine3d to Eigen::Affine3f
00275     Eigen::Vector3d transd(pose_d.translation());
00276     Eigen::Quaterniond rotated(pose_d.rotation());
00277     Eigen::Vector3f transf(transd[0], transd[1], transd[2]);
00278     Eigen::Quaternionf rotatef(rotated.w(),
00279                                rotated.x(), rotated.y(), rotated.z());
00280     Eigen::Affine3f pose_f = Eigen::Translation3f(transf) * rotatef;
00281     ROS_DEBUG("transf: %f %f %f", transf[0], transf[1], transf[2]);
00282     cropper_->setPose(pose_f);
00283     // 2. crop pointcloud
00284     cropAndPublish(point_visualization_pub_);
00285   }
00286 
00287   void PointCloudCropper::cropAndPublish(ros::Publisher& pub)
00288   {
00289     pcl::PointCloud<pcl::PointXYZ>::Ptr input
00290       (new pcl::PointCloud<pcl::PointXYZ>);
00291     pcl::PointCloud<pcl::PointXYZ>::Ptr output
00292       (new pcl::PointCloud<pcl::PointXYZ>);
00293     pcl::fromROSMsg(*latest_pointcloud_, *input);
00294     cropper_->crop(input, output);
00295     ROS_DEBUG_STREAM(output->points.size() << " points to be cropped");
00296     sensor_msgs::PointCloud2 ros_output;
00297     pcl::toROSMsg(*output, ros_output);
00298     ros_output.header = latest_pointcloud_->header;
00299     pub.publish(ros_output);
00300   }
00301       
00302   void PointCloudCropper::initializeInteractiveMarker(
00303     Eigen::Affine3f pose_offset)
00304   {
00305     updateInteractiveMarker();
00306     // menu
00307     menu_handler_.insert(
00308       "Crop",
00309       boost::bind(&PointCloudCropper::menuFeedback, this, _1));
00310     // submenu to change the cropper
00311     interactive_markers::MenuHandler::EntryHandle sub_cropper_menu_handle
00312       = menu_handler_.insert("Switch");
00313     cropper_entries_.clear();
00314     for (size_t i = 0; i < cropper_candidates_.size(); i++) {
00315       Cropper::Ptr the_cropper = cropper_candidates_[i];
00316       interactive_markers::MenuHandler::EntryHandle cropper_entry
00317         = menu_handler_.insert(
00318           sub_cropper_menu_handle, the_cropper->getName(),
00319           boost::bind(&PointCloudCropper::changeCropperCallback, this, _1));
00320       if (the_cropper != cropper_) {
00321         menu_handler_.setCheckState(
00322           cropper_entry,
00323           interactive_markers::MenuHandler::UNCHECKED);
00324       }
00325       else {
00326         menu_handler_.setCheckState(
00327           cropper_entry,
00328           interactive_markers::MenuHandler::CHECKED);
00329       }
00330       cropper_entries_.push_back(cropper_entry);
00331     }
00332     menu_handler_.apply(*server_, "pointcloud cropper");
00333     server_->applyChanges();
00334   }
00335   
00336   void PointCloudCropper::reInitializeInteractiveMarker()
00337   {
00338     if (server_) {
00339       updateInteractiveMarker(cropper_->getPose());
00340       // update checkbox status of the menu
00341       updateMenuCheckboxStatus();
00342       menu_handler_.reApply(*server_);
00343       server_->applyChanges();
00344     }
00345   }
00346   
00347   void PointCloudCropper::updateInteractiveMarker(
00348     Eigen::Affine3f pose_offset)
00349   {
00350     visualization_msgs::InteractiveMarker int_marker;
00351     if (latest_pointcloud_) {
00352       int_marker.header.frame_id = latest_pointcloud_->header.frame_id;
00353     }
00354     else {
00355       int_marker.header.frame_id = "/camera_link";
00356     }
00357     int_marker.name = "pointcloud cropper";
00358     int_marker.description = cropper_->getName();
00359     visualization_msgs::InteractiveMarkerControl control;
00360     control.always_visible = true;
00361     visualization_msgs::Marker cropper_marker = cropper_->getMarker();
00362     control.interaction_mode
00363       = visualization_msgs::InteractiveMarkerControl::BUTTON;
00364     control.markers.push_back(cropper_marker);
00365     int_marker.controls.push_back(control);
00366     // set the position of the cropper_marker
00367     Eigen::Vector3f offset_pos(pose_offset.translation());
00368     Eigen::Quaternionf offset_rot(pose_offset.rotation());
00369     int_marker.pose.position.x = offset_pos[0];
00370     int_marker.pose.position.y = offset_pos[1];
00371     int_marker.pose.position.z = offset_pos[2];
00372     int_marker.pose.orientation.x = offset_rot.x();
00373     int_marker.pose.orientation.y = offset_rot.y();
00374     int_marker.pose.orientation.z = offset_rot.z();
00375     int_marker.pose.orientation.w = offset_rot.w();
00376     control.markers.push_back(cropper_marker);
00377     ROS_DEBUG("pos: %f, %f, %f", int_marker.pose.position.x,
00378               int_marker.pose.position.y,
00379               int_marker.pose.position.z);
00380     ROS_DEBUG("rot: %f.; %f, %f, %f", int_marker.pose.orientation.w,
00381               int_marker.pose.orientation.x,
00382               int_marker.pose.orientation.y,
00383               int_marker.pose.orientation.z);
00384 
00385     // add 6dof marker
00386     im_helpers::add6DofControl(int_marker, false);
00387     
00388     server_->insert(int_marker,
00389                     boost::bind(&PointCloudCropper::processFeedback, this, _1));    
00390   }
00391 
00392   void PointCloudCropper::changeCropperCallback(
00393     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00394   {
00395     unsigned int menu_entry_id = feedback->menu_entry_id;
00396     EntryHandleVector::iterator it = std::find(cropper_entries_.begin(),
00397                                                cropper_entries_.end(),
00398                                                menu_entry_id);
00399     size_t index = std::distance(cropper_entries_.begin(), it);
00400     if (index >= cropper_candidates_.size()) {
00401       ROS_ERROR("the index of the chosen cropper is out of the"
00402                 "range of candidate");
00403       return;
00404     }
00405     Cropper::Ptr next_cropper = cropper_candidates_[index];
00406     if (next_cropper == cropper_) {
00407       ROS_DEBUG("same cropper");
00408       return;
00409     }
00410     else {
00411       changeCropper(next_cropper);
00412     }
00413   }
00414 
00415   void PointCloudCropper::updateMenuCheckboxStatus()
00416   {
00417     for (size_t i = 0; i < cropper_candidates_.size(); i++) {
00418       Cropper::Ptr the_cropper = cropper_candidates_[i];
00419       if (the_cropper == cropper_) {
00420         menu_handler_.setCheckState(
00421           cropper_entries_[i],
00422           interactive_markers::MenuHandler::CHECKED);
00423       }
00424       else {
00425         menu_handler_.setCheckState(
00426           cropper_entries_[i],
00427           interactive_markers::MenuHandler::UNCHECKED);
00428       }
00429     }
00430   }
00431   
00432   void PointCloudCropper::changeCropper(Cropper::Ptr next_cropper)
00433   {
00434     next_cropper->setPose(cropper_->getPose());
00435     cropper_ = next_cropper;
00436     
00437     reInitializeInteractiveMarker();
00438   }
00439 
00440   void PointCloudCropper::menuFeedback(
00441     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00442   {
00443     unsigned int menu_entry_id = feedback->menu_entry_id;
00444     if (menu_entry_id == 1) {   // "Crop"
00445       cropAndPublish(point_pub_);
00446     }
00447   }
00448 
00449   void PointCloudCropper::inputCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00450   {
00451     boost::mutex::scoped_lock lock(mutex_);
00452     latest_pointcloud_ = msg;
00453     cropAndPublish(point_visualization_pub_);
00454   }
00455   
00456 }
00457 int main(int argc, char** argv)
00458 {
00459   ros::init(argc, argv, "pointcloud_cropper");
00460   ros::NodeHandle nh, pnh("~");
00461   jsk_interactive_marker::PointCloudCropper cropper(nh, pnh);
00462   ros::spin();
00463 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Sun Sep 13 2015 22:29:27