35 #define BOOST_PARAMETER_MAX_ARITY 7 38 #include <geometry_msgs/PoseStamped.h> 39 #include <geometry_msgs/PoseArray.h> 40 #include <geometry_msgs/PointStamped.h> 53 pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
55 Eigen::Vector4f center;
56 bool is_valid = pcl::compute3DCentroid(cloud_xyz, center);
62 input->header.frame_id,
frame_));
64 geometry_msgs::PoseStamped
pose;
65 pose.pose.orientation.w = 1.0;
66 pose.pose.position.x = center[0];
67 pose.pose.position.y = center[1];
68 pose.pose.position.z = center[2];
69 pose.header = input->header;
71 geometry_msgs::PointStamped
point;
72 point.point.
x = center[0];
73 point.point.y = center[1];
74 point.point.z = center[2];
75 point.header = input->header;
83 int size = std::min(100, (
int)input->polygons.size());
85 geometry_msgs::PoseArray pose_array;
86 pose_array.header = input->header;
87 pose_array.poses.resize(size);
89 for (
size_t i = 0; i < size; ++i) {
92 Eigen::Vector3f
c = polygon->centroid(), n = polygon->getNormal();
94 ez << 0.0f, 0.0f, 1.0f;
96 q.setFromTwoVectors(ez, n);
100 ss <<
frame_ << std::setfill(
'0') << std::setw(2) << std::right << i;
105 input->header.frame_id, ss.str()));
108 pose_array.poses[i].position.x = c.x();
109 pose_array.poses[i].position.y = c.y();
110 pose_array.poses[i].position.z = c.z();
111 pose_array.poses[i].orientation.x = q.x();
112 pose_array.poses[i].orientation.y = q.y();
113 pose_array.poses[i].orientation.z = q.z();
114 pose_array.poses[i].orientation.w = q.w();
134 DiagnosticNodelet::onInit();
143 pub_pose_ =
pnh_->advertise<geometry_msgs::PoseStamped>(
"output/pose", 1);
150 pub_pose_ = advertise<geometry_msgs::PoseStamped>(*
pnh_,
"output/pose", 1);
151 pub_point_ = advertise<geometry_msgs::PointStamped>(
152 *
pnh_,
"output/point", 1);
virtual void extractPolygons(const jsk_recognition_msgs::PolygonArray::ConstPtr &input)
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_point_
virtual void extract(const sensor_msgs::PointCloud2ConstPtr &input)
const std::string & getName() const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ros::Publisher pub_pose_array_
static tf::Quaternion createIdentityQuaternion()
virtual void unsubscribe()
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::CentroidPublisher, nodelet::Nodelet)
ros::Subscriber sub_cloud_
tf::TransformBroadcaster br_
ros::Subscriber sub_polygons_