pointcloud_cropper.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/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage 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_INTERACTIVE_MARKER_POINTCLOUD_CLOPPER_H_
38 #define JSK_INTERACTIVE_MARKER_POINTCLOUD_CLOPPER_H_
39 
40 #define BOOST_PARAMETER_MAX_ARITY 7 // its a hack
41 
42 #include <jsk_topic_tools/time_accumulator.h>
43 
46 
47 #include <sensor_msgs/PointCloud2.h>
48 
49 #include <pcl/point_types.h>
50 #include <pcl/point_cloud.h>
51 #include <sensor_msgs/PointCloud2.h>
52 
53 #include <tf/transform_listener.h>
54 
55 #include <dynamic_reconfigure/server.h>
56 #include <jsk_interactive_marker/PointCloudCropperConfig.h>
57 
58 namespace jsk_interactive_marker
59 {
60 
61  class Cropper
62  {
63  public:
64  typedef std::shared_ptr<Cropper> Ptr;
65  Cropper(const unsigned int nr_parameter);
66  virtual ~Cropper();
67 
68  virtual void crop(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input,
69  pcl::PointCloud<pcl::PointXYZ>::Ptr output);
70  virtual std::string getName() = 0;
71  virtual visualization_msgs::Marker getMarker() = 0;
72  virtual void updateParameter(const double val, const unsigned int index);
73  // return true if the point p is inside of the cropper
74  virtual bool isInside(const pcl::PointXYZ& p) = 0;
75  virtual void fillInitialParameters() = 0;
76  virtual void setPose(Eigen::Affine3f pose);
77  virtual Eigen::Affine3f getPose();
78  protected:
79  unsigned int nr_parameter_;
80  std::vector<double> parameters_;
81  // pose_ of the Cropper should be respected to the frame_id of pointcloud
82  Eigen::Affine3f pose_;
83  private:
84 
85  };
86 
87  class SphereCropper: public Cropper
88  {
89  public:
90  typedef std::shared_ptr<SphereCropper> Ptr;
91 
92  SphereCropper();
93  virtual ~SphereCropper();
94  virtual std::string getName();
95  virtual visualization_msgs::Marker getMarker();
96  virtual bool isInside(const pcl::PointXYZ& p);
97  virtual void fillInitialParameters();
98  virtual double getRadius();
99  protected:
100  private:
101 
102  };
103 
104  class CubeCropper: public Cropper
105  {
106  public:
107  typedef std::shared_ptr<CubeCropper> Ptr;
108 
109  CubeCropper();
110  virtual ~CubeCropper();
111  virtual std::string getName();
112  virtual visualization_msgs::Marker getMarker();
113  virtual bool isInside(const pcl::PointXYZ& p);
114  virtual void fillInitialParameters();
115  virtual double getWidthX();
116  virtual double getWidthY();
117  virtual double getWidthZ();
118  protected:
119  private:
120 
121  };
122 
123 
125  {
126  public:
128  virtual ~PointCloudCropper();
129  protected:
130  typedef PointCloudCropperConfig Config;
131 
132  typedef std::vector<interactive_markers::MenuHandler::EntryHandle>
134  virtual void changeCropper(Cropper::Ptr next_cropper);
135  virtual void inputCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
136  virtual void reInitializeInteractiveMarker();
137  virtual void updateInteractiveMarker(
138  Eigen::Affine3f pose_offset = Eigen::Affine3f::Identity());
139  virtual void initializeInteractiveMarker(
140  Eigen::Affine3f pose_offset = Eigen::Affine3f::Identity());
141  virtual void processFeedback(
142  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
143  virtual void menuFeedback(
144  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
145  virtual void changeCropperCallback(
146  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
147  virtual void updateMenuCheckboxStatus();
148  virtual void cropAndPublish(ros::Publisher& pub);
149  virtual void configCallback(Config &config, uint32_t level);
154  std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
155  //pcl::PointCloud<pcl::PointXYZ>::Ptr latest_pointcloud_;
156  sensor_msgs::PointCloud2::ConstPtr latest_pointcloud_;
157  std::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
160  std::vector<Cropper::Ptr> cropper_candidates_;
162  std::shared_ptr<tf::TransformListener> tf_listener_;
163 
164  private:
165  };
166 }
167 
168 #endif
jsk_interactive_marker::CubeCropper::getWidthZ
virtual double getWidthZ()
Definition: pointcloud_cropper.cpp:226
jsk_interactive_marker::Cropper::setPose
virtual void setPose(Eigen::Affine3f pose)
Definition: pointcloud_cropper.cpp:90
jsk_interactive_marker::PointCloudCropper::server_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: pointcloud_cropper.h:154
jsk_interactive_marker::CubeCropper::getWidthY
virtual double getWidthY()
Definition: pointcloud_cropper.cpp:221
jsk_interactive_marker::SphereCropper::getName
virtual std::string getName()
Definition: pointcloud_cropper.cpp:161
jsk_interactive_marker::Cropper::Cropper
Cropper(const unsigned int nr_parameter)
Definition: pointcloud_cropper.cpp:78
ros::Publisher
jsk_interactive_marker::SphereCropper::isInside
virtual bool isInside(const pcl::PointXYZ &p)
Definition: pointcloud_cropper.cpp:134
jsk_interactive_marker::PointCloudCropper::menu_handler_
interactive_markers::MenuHandler menu_handler_
Definition: pointcloud_cropper.h:158
jsk_interactive_marker::Cropper::updateParameter
virtual void updateParameter(const double val, const unsigned int index)
Definition: pointcloud_cropper.cpp:117
jsk_interactive_marker::PointCloudCropper::cropper_candidates_
std::vector< Cropper::Ptr > cropper_candidates_
Definition: pointcloud_cropper.h:160
jsk_interactive_marker::Cropper::nr_parameter_
unsigned int nr_parameter_
Definition: pointcloud_cropper.h:79
jsk_interactive_marker::PointCloudCropper::mutex_
boost::mutex mutex_
Definition: pointcloud_cropper.h:150
jsk_interactive_marker::PointCloudCropper::Config
PointCloudCropperConfig Config
Definition: pointcloud_cropper.h:130
jsk_interactive_marker::Cropper::isInside
virtual bool isInside(const pcl::PointXYZ &p)=0
jsk_interactive_marker::Cropper::crop
virtual void crop(const pcl::PointCloud< pcl::PointXYZ >::Ptr &input, pcl::PointCloud< pcl::PointXYZ >::Ptr output)
Definition: pointcloud_cropper.cpp:100
jsk_interactive_marker::PointCloudCropper::point_pub_
ros::Publisher point_pub_
Definition: pointcloud_cropper.h:152
jsk_interactive_marker::SphereCropper::fillInitialParameters
virtual void fillInitialParameters()
Definition: pointcloud_cropper.cpp:156
jsk_interactive_marker::SphereCropper::~SphereCropper
virtual ~SphereCropper()
Definition: pointcloud_cropper.cpp:129
jsk_interactive_marker::PointCloudCropper::initializeInteractiveMarker
virtual void initializeInteractiveMarker(Eigen::Affine3f pose_offset=Eigen::Affine3f::Identity())
Definition: pointcloud_cropper.cpp:334
jsk_interactive_marker::Cropper::pose_
Eigen::Affine3f pose_
Definition: pointcloud_cropper.h:82
menu_handler.h
jsk_interactive_marker::SphereCropper::Ptr
std::shared_ptr< SphereCropper > Ptr
Definition: pointcloud_cropper.h:90
jsk_interactive_marker::Cropper::getName
virtual std::string getName()=0
jsk_interactive_marker
Definition: camera_info_publisher.h:48
jsk_interactive_marker::Cropper
Definition: pointcloud_cropper.h:61
jsk_interactive_marker::CubeCropper::isInside
virtual bool isInside(const pcl::PointXYZ &p)
Definition: pointcloud_cropper.cpp:231
dummy_camera.pub
pub
Definition: dummy_camera.py:8
jsk_interactive_marker::PointCloudCropper::reInitializeInteractiveMarker
virtual void reInitializeInteractiveMarker()
Definition: pointcloud_cropper.cpp:368
jsk_interactive_marker::Cropper::getMarker
virtual visualization_msgs::Marker getMarker()=0
jsk_interactive_marker::SphereCropper::SphereCropper
SphereCropper()
Definition: pointcloud_cropper.cpp:124
jsk_interactive_marker::Cropper::getPose
virtual Eigen::Affine3f getPose()
Definition: pointcloud_cropper.cpp:95
jsk_interactive_marker::PointCloudCropper
Definition: pointcloud_cropper.h:124
jsk_interactive_marker::CubeCropper::CubeCropper
CubeCropper()
Definition: pointcloud_cropper.cpp:180
jsk_interactive_marker::PointCloudCropper::srv_
std::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: pointcloud_cropper.h:157
jsk_interactive_marker::SphereCropper::getRadius
virtual double getRadius()
Definition: pointcloud_cropper.cpp:151
interactive_markers::MenuHandler
mutex
boost::mutex mutex
Definition: bounding_box_marker.cpp:44
jsk_interactive_marker::CubeCropper::getName
virtual std::string getName()
Definition: pointcloud_cropper.cpp:190
jsk_interactive_marker::PointCloudCropper::PointCloudCropper
PointCloudCropper(ros::NodeHandle &nh, ros::NodeHandle &pnh)
Definition: pointcloud_cropper.cpp:245
jsk_interactive_marker::Cropper::~Cropper
virtual ~Cropper()
Definition: pointcloud_cropper.cpp:85
jsk_interactive_marker::PointCloudCropper::configCallback
virtual void configCallback(Config &config, uint32_t level)
Definition: pointcloud_cropper.cpp:270
jsk_interactive_marker::CubeCropper::getWidthX
virtual double getWidthX()
Definition: pointcloud_cropper.cpp:216
jsk_interactive_marker::PointCloudCropper::changeCropper
virtual void changeCropper(Cropper::Ptr next_cropper)
Definition: pointcloud_cropper.cpp:464
jsk_interactive_marker::CubeCropper::~CubeCropper
virtual ~CubeCropper()
Definition: pointcloud_cropper.cpp:185
jsk_interactive_marker::PointCloudCropper::cropper_
Cropper::Ptr cropper_
Definition: pointcloud_cropper.h:159
jsk_interactive_marker::PointCloudCropper::EntryHandleVector
std::vector< interactive_markers::MenuHandler::EntryHandle > EntryHandleVector
Definition: pointcloud_cropper.h:133
jsk_interactive_marker::SphereCropper::getMarker
virtual visualization_msgs::Marker getMarker()
Definition: pointcloud_cropper.cpp:166
jsk_interactive_marker::PointCloudCropper::~PointCloudCropper
virtual ~PointCloudCropper()
Definition: pointcloud_cropper.cpp:265
transform_listener.h
interactive_marker_server.h
jsk_interactive_marker::CubeCropper
Definition: pointcloud_cropper.h:104
jsk_interactive_marker::PointCloudCropper::menuFeedback
virtual void menuFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: pointcloud_cropper.cpp:472
jsk_interactive_marker::PointCloudCropper::updateInteractiveMarker
virtual void updateInteractiveMarker(Eigen::Affine3f pose_offset=Eigen::Affine3f::Identity())
Definition: pointcloud_cropper.cpp:379
jsk_interactive_marker::PointCloudCropper::changeCropperCallback
virtual void changeCropperCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: pointcloud_cropper.cpp:424
jsk_interactive_marker::Cropper::parameters_
std::vector< double > parameters_
Definition: pointcloud_cropper.h:80
jsk_interactive_marker::PointCloudCropper::latest_pointcloud_
sensor_msgs::PointCloud2::ConstPtr latest_pointcloud_
Definition: pointcloud_cropper.h:156
jsk_interactive_marker::SphereCropper
Definition: pointcloud_cropper.h:87
jsk_interactive_marker::PointCloudCropper::inputCallback
virtual void inputCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: pointcloud_cropper.cpp:481
jsk_interactive_marker::PointCloudCropper::point_visualization_pub_
ros::Publisher point_visualization_pub_
Definition: pointcloud_cropper.h:153
jsk_interactive_marker::PointCloudCropper::point_sub_
ros::Subscriber point_sub_
Definition: pointcloud_cropper.h:151
jsk_interactive_marker::PointCloudCropper::cropper_entries_
EntryHandleVector cropper_entries_
Definition: pointcloud_cropper.h:161
jsk_interactive_marker::CubeCropper::fillInitialParameters
virtual void fillInitialParameters()
Definition: pointcloud_cropper.cpp:209
jsk_interactive_marker::Cropper::fillInitialParameters
virtual void fillInitialParameters()=0
jsk_interactive_marker::PointCloudCropper::updateMenuCheckboxStatus
virtual void updateMenuCheckboxStatus()
Definition: pointcloud_cropper.cpp:447
jsk_interactive_marker::PointCloudCropper::cropAndPublish
virtual void cropAndPublish(ros::Publisher &pub)
Definition: pointcloud_cropper.cpp:319
jsk_interactive_marker::CubeCropper::Ptr
std::shared_ptr< CubeCropper > Ptr
Definition: pointcloud_cropper.h:107
ros::NodeHandle
ros::Subscriber
jsk_interactive_marker::PointCloudCropper::tf_listener_
std::shared_ptr< tf::TransformListener > tf_listener_
Definition: pointcloud_cropper.h:162
jsk_interactive_marker::PointCloudCropper::processFeedback
virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: pointcloud_cropper.cpp:281
jsk_interactive_marker::Cropper::Ptr
std::shared_ptr< Cropper > Ptr
Definition: pointcloud_cropper.h:64
jsk_interactive_marker::CubeCropper::getMarker
virtual visualization_msgs::Marker getMarker()
Definition: pointcloud_cropper.cpp:195


jsk_interactive_marker
Author(s): furuta
autogenerated on Fri Aug 2 2024 08:50:24