44 ConnectionBasedNodelet::onInit();
53 if (!frame_id_read_p) {
59 if (!polygon_read_p) {
65 NODELET_FATAL(
"the size of frame_ids(%lu) does not match the size of polygons(%lu)",
70 for (
size_t i = 0; i <
frame_ids_.size(); i++) {
77 NODELET_FATAL(
"~use_periodic, ~use_trigger nor ~use_message is not true");
84 polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(
85 *
pnh_,
"output_polygons", 1);
87 *
pnh_,
"output_coefficients", 1);
91 "output_polygons", 1);
93 "output_coefficients", 1);
109 = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
111 sync_->registerCallback(boost::bind(
129 const sensor_msgs::PointCloud2::ConstPtr& input,
130 const jsk_recognition_msgs::Int32Stamped::ConstPtr& trigger)
137 Eigen::Vector3d
A, B, C;
138 A[0] = polygon.polygon.points[0].x;
139 A[1] = polygon.polygon.points[0].y;
140 A[2] = polygon.polygon.points[0].z;
141 B[0] = polygon.polygon.points[1].x;
142 B[1] = polygon.polygon.points[1].y;
143 B[2] = polygon.polygon.points[1].z;
144 C[0] = polygon.polygon.points[2].x;
145 C[1] = polygon.polygon.points[2].y;
146 C[2] = polygon.polygon.points[2].z;
147 Eigen::Vector3d n = (B - A).
cross(C - A).normalized();
151 double d = -(a * A[0] + b * A[1] + c * A[2]);
153 coefficient.header = polygon.header;
154 coefficient.values.push_back(a);
155 coefficient.values.push_back(b);
156 coefficient.values.push_back(c);
157 coefficient.values.push_back(d);
167 if (
pnh_->hasParam(param_name)) {
169 pnh_->param(param_name, v, v);
171 for (
size_t toplevel_i = 0; toplevel_i < v.
size(); toplevel_i++) {
173 geometry_msgs::PolygonStamped polygon;
175 polygon_v.
size() >= 3) {
176 for (
size_t secondlevel_i = 0; secondlevel_i < polygon_v.
size(); secondlevel_i++) {
179 vertex_v.
size() == 3 ) {
183 geometry_msgs::Point32
point;
187 polygon.polygon.points.push_back(point);
190 NODELET_FATAL(
"%s[%lu][%lu] is not array or the length is not 3",
191 param_name.c_str(), toplevel_i, secondlevel_i);
200 NODELET_FATAL(
"%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
212 NODELET_FATAL(
"no %s is available on parameter server", param_name.c_str());
221 return (
double)((int)val);
243 for (
size_t i = 0; i <
polygons_.polygons.size(); i++) {
248 for (
size_t i = 0; i <
coefficients_.coefficients.size(); i++) {
jsk_recognition_msgs::ModelCoefficientsArray coefficients_
void publish(const boost::shared_ptr< M > &message) const
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
std::vector< std::string > frame_ids_
Type const & getType() const
virtual void timerCallback(const ros::TimerEvent &event)
virtual PCLModelCoefficientMsg polygonToModelCoefficients(const geometry_msgs::PolygonStamped &polygon)
ros::Publisher polygon_pub_
virtual bool readPolygonArray(const std::string ¶m)
virtual void triggerCallback(const sensor_msgs::PointCloud2::ConstPtr &input, const jsk_recognition_msgs::Int32Stamped::ConstPtr &trigger)
virtual void inputCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual void unsubscribe()
virtual double getXMLDoubleValue(XmlRpc::XmlRpcValue val)
virtual void publishPolygon(const ros::Time &stamp)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::StaticPolygonArrayPublisher, nodelet::Nodelet)
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)
ros::Publisher coefficients_pub_
jsk_recognition_msgs::PolygonArray polygons_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
message_filters::Subscriber< jsk_recognition_msgs::Int32Stamped > sub_trigger_
#define NODELET_FATAL(...)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
pcl::ModelCoefficients PCLModelCoefficientMsg