36 #define BOOST_PARAMETER_MAX_ARITY 7 46 DiagnosticNodelet::onInit();
51 pub_ = advertise<jsk_recognition_msgs::RectArray>(*
pnh_,
"output", 1);
52 pub_internal_ = pnh_->advertise<jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo>(
"internal", 1);
63 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
67 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
74 async_box_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicyBox> >(
queue_size_);
93 const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg)
95 jsk_recognition_msgs::BoundingBoxArray::Ptr boxes_msg(
96 new jsk_recognition_msgs::BoundingBoxArray());
97 boxes_msg->header = box_msg->header;
98 boxes_msg->boxes.push_back(*box_msg);
103 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& boxes_msg)
116 jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo internal_msg;
117 internal_msg.header = boxes_msg->header;
118 internal_msg.boxes = *boxes_msg;
119 internal_msg.camera_info = *info_msg;
124 const jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo::ConstPtr& msg)
129 msg->boxes.header.frame_id,
130 msg->camera_info.header.frame_id,
131 msg->boxes.header.stamp,
133 Eigen::Affine3f box_to_info;
137 jsk_recognition_msgs::RectArray rect_array;
138 rect_array.header = msg->camera_info.header;
139 for (
size_t i = 0; i < msg->boxes.boxes.size(); i++) {
140 jsk_recognition_msgs::BoundingBox box = msg->boxes.boxes[i];
144 cv::Rect rect = cv::boundingRect(points);
145 jsk_recognition_msgs::Rect ros_rect;
148 ros_rect.width = rect.width;
149 ros_rect.height = rect.height;
150 rect_array.rects.push_back(ros_rect);
virtual void inputBoxCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg, const jsk_recognition_msgs::BoundingBox::ConstPtr &box_msg)
virtual void inputCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg, const jsk_recognition_msgs::BoundingBoxArray::ConstPtr &boxes_msg)
ros::Publisher pub_internal_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicyBox > > sync_box_
Vertices transformVertices(const Eigen::Affine3f &pose_offset)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
message_filters::Subscriber< jsk_recognition_msgs::BoundingBoxArray > sub_boxes_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicyBox > > async_box_
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
virtual void unsubscribe()
message_filters::Subscriber< jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo > sub_box_with_info_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
boost::shared_ptr< tf::MessageFilter< jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo > > tf_filter_
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
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)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
static tf::TransformListener * getInstance()
message_filters::Subscriber< jsk_recognition_msgs::BoundingBox > sub_box_
std::vector< cv::Point > project3DPointstoPixel(const image_geometry::PinholeCameraModel &model, const Vertices &vertices)
PLUGINLIB_EXPORT_CLASS(jsk_perception::BoundingBoxToRect, nodelet::Nodelet)
tf::TransformListener * tf_listener_
virtual void internalCallback(const jsk_recognition_msgs::BoundingBoxArrayWithCameraInfo::ConstPtr &msg)