36 #define BOOST_PARAMETER_MAX_ARITY 7 47 DiagnosticNodelet::onInit();
48 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
51 srv_->setCallback (f);
52 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output", 1);
68 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
72 jsk_recognition_msgs::PolygonArray new_msg(*msg);
74 double min_area = DBL_MAX;
75 double max_area = - DBL_MAX;
76 std::vector<double> areas;
77 for (
size_t i = 0; i < msg->polygons.size(); i++) {
80 double area = polygon->area();
81 min_area = std::min(area, min_area);
82 max_area = std::max(area, max_area);
83 areas.push_back(area);
87 for (
size_t i = 0; i < areas.size(); i++) {
90 if (msg->likelihood.size() == 0) {
91 new_msg.likelihood.push_back(likelihood);
94 new_msg.likelihood[i] = new_msg.likelihood[i] *
likelihood;
101 Config& config, uint32_t level)
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void unsubscribe()
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
virtual void likelihood(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
virtual void configCallback(Config &config, uint32_t level)
PolygonArrayAreaLikelihoodConfig Config
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PolygonArrayAreaLikelihood, nodelet::Nodelet)