pcl_ros_util.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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 
41 #ifndef JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_
42 #define JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_
43 
44 #include <ros/ros.h>
45 
46 #include <std_msgs/Header.h>
47 #include <pcl_msgs/PointIndices.h>
48 
49 #include <jsk_recognition_msgs/BoundingBox.h>
50 #include <pcl/PointIndices.h>
51 #include <pcl/filters/crop_box.h>
52 #include <sensor_msgs/PointCloud2.h>
53 
55 
56 namespace jsk_recognition_utils
57 {
64  const pcl::PointIndices& indices,
65  const std_msgs::Header& header);
66 
71  bool isSameFrameId(const std::string& a, const std::string& b);
72 
77  bool isSameFrameId(const std_msgs::Header& a, const std_msgs::Header& b);
78 
83  template<class T1, class T2>
84  bool isSameFrameId(const T1& a, const T2& b)
85  {
86  return isSameFrameId(a.header, b.header);
87  }
88 
93  bool hasField(const std::string& field_name, const sensor_msgs::PointCloud2& msg);
94 
99  template<typename PointT>
100  void
101  cropPointCloud(const typename pcl::PointCloud<PointT>::Ptr& cloud,
102  const jsk_recognition_msgs::BoundingBox& bbox_msg,
103  std::vector<int>* indices,
104  bool extract_removed_indices=false)
105  {
106  if (cloud->header.frame_id != bbox_msg.header.frame_id)
107  {
108  fprintf(stderr, "Frame id of input cloud and bounding box must be same. Cloud: %s, BoundingBox: %s.",
109  cloud->header.frame_id.c_str(), bbox_msg.header.frame_id.c_str());
110  return;
111  }
112  pcl::CropBox<PointT> crop_box(/*extract_removed_indices=*/extract_removed_indices);
113 
114  crop_box.setInputCloud(cloud);
115 
116  Eigen::Affine3f box_pose;
117  tf::poseMsgToEigen(bbox_msg.pose, box_pose);
118  crop_box.setTranslation(box_pose.translation());
119  float roll, pitch, yaw;
120  pcl::getEulerAngles(box_pose, roll, pitch, yaw);
121  crop_box.setRotation(Eigen::Vector3f(roll, pitch, yaw));
122 
123  Eigen::Vector4f max_point(bbox_msg.dimensions.x / 2,
124  bbox_msg.dimensions.y / 2,
125  bbox_msg.dimensions.z / 2,
126  0);
127  Eigen::Vector4f min_point(-bbox_msg.dimensions.x / 2,
128  -bbox_msg.dimensions.y / 2,
129  -bbox_msg.dimensions.z / 2,
130  0);
131  crop_box.setMax(max_point);
132  crop_box.setMin(min_point);
133 
134  crop_box.filter(*indices);
135  }
136 
137 } // namespace jsk_recognition_utils
138 
139 #endif // JSK_RECOGNITION_UTILS_PCL_ROS_UTIL_H_
msg
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
pitch
void publishPointIndices(ros::Publisher &pub, const pcl::PointIndices &indices, const std_msgs::Header &header)
Convert pcl::PointIndices to pcl_msgs::PointIndices and publish it with overriding header...
bool hasField(const std::string &field_name, const sensor_msgs::PointCloud2 &msg)
check if sensor_msgs/PointCloud2 message has the specified field.
yaw
void cropPointCloud(const typename pcl::PointCloud< PointT >::Ptr &cloud, const jsk_recognition_msgs::BoundingBox &bbox_msg, std::vector< int > *indices, bool extract_removed_indices=false)
Crop point cloud with jsk_recognition_msgs/BoundingBox.
Definition: pcl_ros_util.h:101
bool isSameFrameId(const std::string &a, const std::string &b)
Return true if a and b are the same frame_id.
roll


jsk_recognition_utils
Author(s):
autogenerated on Mon May 3 2021 03:03:03