47 DiagnosticNodelet::onInit();
53 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
54 dynamic_reconfigure::Server<Config>::CallbackType
f =
57 srv_->setCallback (f);
63 = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output/vertical/inliers", 1);
65 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*
pnh_,
"output/vertical/coefficients", 1);
67 = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output/vertical/polygons", 1);
69 = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output/horizontal/inliers", 1);
71 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*
pnh_,
"output/horizontal/coefficients", 1);
73 = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output/horizontal/polygons", 1);
87 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
91 this, _1, _2, _3, _4));
112 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
113 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& inliers_msg,
114 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
115 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons_msg)
119 if ((inliers_msg->cluster_indices.size()
120 != coefficients_msg->coefficients.size()) ||
121 (inliers_msg->cluster_indices.size()
122 != polygons_msg->polygons.size())) {
123 NODELET_FATAL(
"the size of inliers, coefficients and polygons are not same");
127 pcl::PointCloud<PointT>::Ptr input_cloud (
new pcl::PointCloud<PointT>);
131 std::vector<pcl::PointIndices::Ptr> inliers
135 coefficients_msg->coefficients);
137 std::vector<geometry_msgs::PolygonStamped>
polygons = polygons_msg->polygons;
138 std::vector<PlaneInfoContainer> plane_infos
139 =
packInfo(inliers, coefficients, planes, polygons);
140 std::vector<PlaneInfoContainer> horizontal_planes
142 std::vector<PlaneInfoContainer> vertical_planes
159 std::vector<PlaneInfoContainer>& containers,
160 const std_msgs::Header& header,
161 pcl::PointCloud<PointT>::Ptr
cloud,
166 std::vector<pcl::PointIndices::Ptr> inliers;
168 std::vector<geometry_msgs::PolygonStamped>
polygons;
169 for (
size_t i = 0; i < containers.size(); i++) {
170 inliers.push_back(containers[i].get<0>());
171 coefficients.push_back(containers[i].get<1>());
172 polygons.push_back(containers[i].get<3>());
174 jsk_recognition_msgs::ClusterPointIndices ros_indices;
175 jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
176 jsk_recognition_msgs::PolygonArray ros_polygons;
177 ros_indices.header = header;
178 ros_coefficients.header = header;
179 ros_polygons.header = header;
182 ros_coefficients.coefficients
184 coefficients, header);
185 ros_polygons.polygons = polygons;
186 pub_inlier.
publish(ros_indices);
187 pub_coefficients.
publish(ros_coefficients);
188 pub_polygons.
publish(ros_polygons);
191 std::vector<PlaneInfoContainer>
193 double reference_angle,
195 std::vector<PlaneInfoContainer>& infos)
198 std::vector<PlaneInfoContainer> ret;
199 for (
size_t i = 0; i < infos.size(); i++) {
202 plane_info.get<3>().header.frame_id,
203 plane_info.get<3>().header.stamp)) {
207 plane_info.get<3>().header.stamp,
209 Eigen::Affine3d eigen_transform;
211 Eigen::Affine3f eigen_transform_3f;
212 Eigen::Vector3d up_d = (eigen_transform.rotation() * Eigen::Vector3d(0, 0, 1));
214 jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector3d, Eigen::Vector3f>(up_d, up);
216 double angle = plane->angle(up);
220 if (fabs(angle - reference_angle) < thrshold) {
221 ret.push_back(plane_info);
228 std::vector<PlaneInfoContainer>
230 std::vector<PlaneInfoContainer>& infos)
238 std::vector<PlaneInfoContainer>
240 std::vector<PlaneInfoContainer>& infos)
248 std::vector<PlaneInfoContainer>
250 std::vector<pcl::PointIndices::Ptr>& inliers,
251 std::vector<pcl::ModelCoefficients::Ptr>& coefficients,
252 std::vector<jsk_recognition_utils::Plane::Ptr>& planes,
253 std::vector<geometry_msgs::PolygonStamped>& polygons)
255 std::vector<PlaneInfoContainer> ret;
256 for (
size_t i = 0; i < inliers.size(); i++) {
257 ret.push_back(boost::make_tuple<pcl::PointIndices::Ptr,
258 pcl::ModelCoefficients::Ptr,
260 planes[i], polygons[i]));
std::vector< Plane::Ptr > convertToPlanes(std::vector< pcl::ModelCoefficients::Ptr >)
double horizontal_angular_threshold_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PlaneReasoner, nodelet::Nodelet)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_
virtual void unsubscribe()
std::vector< PCLModelCoefficientMsg > convertToROSModelCoefficients(const std::vector< pcl::ModelCoefficients::Ptr > &coefficients, const std_msgs::Header &header)
std::vector< pcl::ModelCoefficients::Ptr > convertToPCLModelCoefficients(const std::vector< PCLModelCoefficientMsg > &coefficients)
virtual std::vector< PlaneInfoContainer > packInfo(std::vector< pcl::PointIndices::Ptr > &inliers, std::vector< pcl::ModelCoefficients::Ptr > &coefficients, std::vector< jsk_recognition_utils::Plane::Ptr > &planes, std::vector< geometry_msgs::PolygonStamped > &polygons)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
std::vector< pcl::PointIndices::Ptr > convertToPCLPointIndices(const std::vector< PCLIndicesMsg > &cluster_indices)
ros::Publisher pub_vertical_coefficients_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
virtual void configCallback(Config &config, uint32_t level)
ros::Publisher pub_horizontal_inliers_
boost::tuple< pcl::PointIndices::Ptr, pcl::ModelCoefficients::Ptr, jsk_recognition_utils::Plane::Ptr, geometry_msgs::PolygonStamped > PlaneInfoContainer
tf::TransformListener * tf_listener_
jsk_pcl_ros_utils::PlaneReasonerConfig Config
virtual void reason(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &inliers_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons_msg)
double vertical_angular_threshold_
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices::Ptr > cluster_indices, const std_msgs::Header &header)
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_inliers_
virtual std::vector< PlaneInfoContainer > filterHorizontalPlanes(std::vector< PlaneInfoContainer > &infos)
ros::Publisher pub_vertical_inliers_
ros::Publisher pub_horizontal_coefficients_
virtual std::vector< PlaneInfoContainer > filterPlanesAroundAngle(double reference_angle, double thrshold, std::vector< PlaneInfoContainer > &infos)
ros::Publisher pub_vertical_polygons_
static tf::TransformListener * getInstance()
#define NODELET_FATAL(...)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void publishPlaneInfo(std::vector< PlaneInfoContainer > &containers, const std_msgs::Header &header, pcl::PointCloud< PointT >::Ptr cloud, ros::Publisher &pub_inlier, ros::Publisher &pub_coefficients, ros::Publisher &pub_polygons)
std::string global_frame_id_
ros::Publisher pub_horizontal_polygons_
virtual std::vector< PlaneInfoContainer > filterVerticalPlanes(std::vector< PlaneInfoContainer > &infos)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_