line_segment_detector.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
37 #ifndef JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_
38 #define JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_
39 
40 #include <pcl/segmentation/sac_segmentation.h>
41 
48 
49 #include <jsk_recognition_msgs/ClusterPointIndices.h>
50 #include <jsk_recognition_msgs/ModelCoefficientsArray.h>
51 #include <jsk_pcl_ros/LineSegmentDetectorConfig.h>
52 #include <dynamic_reconfigure/server.h>
53 #include <visualization_msgs/Marker.h>
55 
56 namespace jsk_pcl_ros
57 {
58 
60  {
61  public:
63  LineSegment(const std_msgs::Header& input_header,
64  pcl::PointIndices::Ptr indices,
65  pcl::ModelCoefficients::Ptr coefficients,
66  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
67  LineSegment(pcl::PointIndices::Ptr indices,
68  pcl::ModelCoefficients::Ptr coefficients);
69  virtual ~LineSegment();
70  virtual bool addMarkerLine(
71  visualization_msgs::Marker& marker,
72  const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
73  double minimum_line_length);
74  //virtual Segment::Ptr toSegment();
76  pcl::PointIndices::Ptr getIndices() { return indices_; }
77  pcl::ModelCoefficients::Ptr getCoefficients() { return coefficients_; }
78  pcl::PointCloud<pcl::PointXYZ>::Ptr getPoints() { return points_; }
79  pcl::PointCloud<pcl::PointXYZ>::Ptr getRawPoints() { return raw_points_; }
80  std_msgs::Header header;
81  protected:
82  pcl::PointIndices::Ptr indices_;
83  pcl::ModelCoefficients::Ptr coefficients_;
84  pcl::PointCloud<pcl::PointXYZ>::Ptr points_;
85  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_points_;
86  private:
87 
88  };
89 
91  {
92  public:
93  LineSegmentDetector(): DiagnosticNodelet("LineSegmentDetector")
94  {
95  }
97  {
98  sync_.reset();
99  srv_.reset();
100  }
102  sensor_msgs::PointCloud2,
103  jsk_recognition_msgs::ClusterPointIndices> SyncPolicy;
105  sensor_msgs::PointCloud2,
106  jsk_recognition_msgs::ClusterPointIndices> ApproximateSyncPolicy;
107  typedef pcl::PointXYZ PointT;
108  typedef jsk_pcl_ros::LineSegmentDetectorConfig Config;
109  protected:
111  // methods
113  virtual void subscribe();
114  virtual void unsubscribe();
115  virtual void onInit();
116  virtual void segment(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
117  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_msg);
118  virtual void configCallback(Config &config, uint32_t level);
119  virtual void segmentLines(
120  const pcl::PointCloud<PointT>::Ptr& cloud,
121  const pcl::PointIndices::Ptr& indices,
122  std::vector<pcl::PointIndices::Ptr>& line_indices,
123  std::vector<pcl::ModelCoefficients::Ptr>& line_coefficients);
124  virtual void publishResult(
125  const std_msgs::Header& header,
126  const pcl::PointCloud<PointT>::Ptr& cloud,
127  const std::vector<LineSegment::Ptr>& segments);
129  // ROS variables
140  boost::recursive_mutex config_mutex_;
141 
143  // parameters
149  double min_length_;
150  double line_width_;
151 
152  pcl::SACSegmentation<PointT> seg_;
153 
154  private:
155 
156  };
157 }
158 
159 #endif
pcl::PointCloud< pcl::PointXYZ >::Ptr points_
pcl::PointCloud< pcl::PointXYZ >::Ptr getRawPoints()
jsk_pcl_ros::LineSegmentDetectorConfig Config
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > SyncPolicy
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
void subscribe()
pcl::ModelCoefficients::Ptr coefficients_
virtual jsk_recognition_utils::Line::Ptr toSegment()
pcl::PointCloud< pcl::PointXYZ >::Ptr raw_points_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
pcl::ModelCoefficients::Ptr getCoefficients()
pcl::PointCloud< pcl::PointXYZ >::Ptr getPoints()
boost::mutex mutex
global mutex.
pcl::PointIndices::Ptr getIndices()
pcl::PointIndices::Ptr indices_
virtual bool addMarkerLine(visualization_msgs::Marker &marker, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double minimum_line_length)
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy
pcl::SACSegmentation< PointT > seg_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
LineSegment(const std_msgs::Header &input_header, pcl::PointIndices::Ptr indices, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
boost::shared_ptr< LineSegment > Ptr
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47