line_segment_detector.h
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 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 
00037 #ifndef JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_
00038 #define JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_
00039 
00040 #include <pcl/segmentation/sac_segmentation.h>
00041 
00042 #include <jsk_topic_tools/diagnostic_nodelet.h>
00043 #include <message_filters/subscriber.h>
00044 #include <message_filters/time_synchronizer.h>
00045 #include <message_filters/synchronizer.h>
00046 #include <message_filters/sync_policies/exact_time.h>
00047 #include <message_filters/sync_policies/approximate_time.h>
00048 
00049 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00050 #include <jsk_recognition_msgs/ModelCoefficientsArray.h>
00051 #include <jsk_pcl_ros/LineSegmentDetectorConfig.h>
00052 #include <dynamic_reconfigure/server.h>
00053 #include <visualization_msgs/Marker.h>
00054 #include "jsk_recognition_utils/geo_util.h"
00055 
00056 namespace jsk_pcl_ros
00057 {
00058 
00059   class LineSegment
00060   {
00061   public:
00062     typedef boost::shared_ptr<LineSegment> Ptr;
00063     LineSegment(const std_msgs::Header& input_header,
00064                 pcl::PointIndices::Ptr indices,
00065                 pcl::ModelCoefficients::Ptr coefficients,
00066                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
00067     LineSegment(pcl::PointIndices::Ptr indices,
00068                 pcl::ModelCoefficients::Ptr coefficients);
00069     virtual ~LineSegment();
00070     virtual bool addMarkerLine(
00071       visualization_msgs::Marker& marker,
00072       const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
00073       double minimum_line_length);
00074     //virtual Segment::Ptr toSegment();
00075     virtual jsk_recognition_utils::Line::Ptr toSegment();
00076     pcl::PointIndices::Ptr getIndices() { return indices_; }
00077     pcl::ModelCoefficients::Ptr getCoefficients() { return coefficients_; }
00078     pcl::PointCloud<pcl::PointXYZ>::Ptr getPoints() { return points_; }
00079     pcl::PointCloud<pcl::PointXYZ>::Ptr getRawPoints() { return raw_points_; }
00080     std_msgs::Header header;
00081   protected:
00082     pcl::PointIndices::Ptr indices_;
00083     pcl::ModelCoefficients::Ptr coefficients_;
00084     pcl::PointCloud<pcl::PointXYZ>::Ptr points_;
00085     pcl::PointCloud<pcl::PointXYZ>::Ptr raw_points_;
00086   private:
00087     
00088   };
00089   
00090   class LineSegmentDetector: public jsk_topic_tools::DiagnosticNodelet
00091   {
00092   public:
00093     LineSegmentDetector(): DiagnosticNodelet("LineSegmentDetector")
00094     {
00095     }
00096     ~LineSegmentDetector()
00097     {
00098       sync_.reset();
00099       srv_.reset();
00100     }
00101     typedef message_filters::sync_policies::ExactTime<
00102       sensor_msgs::PointCloud2,
00103       jsk_recognition_msgs::ClusterPointIndices> SyncPolicy;
00104     typedef message_filters::sync_policies::ApproximateTime<
00105       sensor_msgs::PointCloud2,
00106       jsk_recognition_msgs::ClusterPointIndices> ApproximateSyncPolicy;
00107     typedef pcl::PointXYZ PointT;
00108     typedef jsk_pcl_ros::LineSegmentDetectorConfig Config;
00109   protected:
00111     // methods
00113     virtual void subscribe();
00114     virtual void unsubscribe();
00115     virtual void updateDiagnostic(
00116       diagnostic_updater::DiagnosticStatusWrapper &stat);
00117     virtual void onInit();
00118     virtual void segment(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00119                          const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_msg);
00120     virtual void configCallback(Config &config, uint32_t level);
00121     virtual void segmentLines(
00122       const pcl::PointCloud<PointT>::Ptr& cloud,
00123       const pcl::PointIndices::Ptr& indices,
00124       std::vector<pcl::PointIndices::Ptr>& line_indices,
00125       std::vector<pcl::ModelCoefficients::Ptr>& line_coefficients);
00126     virtual void publishResult(
00127       const std_msgs::Header& header,
00128       const pcl::PointCloud<PointT>::Ptr& cloud,
00129       const std::vector<LineSegment::Ptr>& segments);
00131     // ROS variables
00133     ros::Publisher pub_line_marker_;
00134     ros::Publisher pub_indices_;
00135     ros::Publisher pub_coefficients_;
00136     boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
00137     boost::shared_ptr<message_filters::Synchronizer<ApproximateSyncPolicy> > async_;
00138     message_filters::Subscriber<sensor_msgs::PointCloud2> sub_input_;
00139     message_filters::Subscriber<jsk_recognition_msgs::ClusterPointIndices> sub_indices_;
00140     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00141     boost::mutex mutex_;
00142     boost::recursive_mutex config_mutex_;
00143 
00145     // parameters
00147     bool approximate_sync_;
00148     double outlier_threshold_;
00149     int max_iterations_;
00150     int min_indices_;
00151     double min_length_;
00152     double line_width_;
00153 
00154     pcl::SACSegmentation<PointT> seg_;
00155 
00156   private:
00157     
00158   };
00159 }
00160 
00161 #endif


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