00001 /* 00002 * Multi Marker Pose Estimation using ARToolkit 00003 * Copyright (C) 2010, CCNY Robotics Lab 00004 * Ivan Dryanovski <ivan.dryanovski@gmail.com> 00005 * William Morris <morris@ee.ccny.cuny.edu> 00006 * Gautier Dumonteil <gautier.dumonteil@gmail.com> 00007 * http://robotics.ccny.cuny.edu 00008 * 00009 * This program is free software: you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation, either version 3 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU General Public License for more details. 00018 * 00019 * You should have received a copy of the GNU General Public License 00020 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00021 */ 00022 00023 #ifndef AR_POSE_AR_MULTI_H 00024 #define AR_POSE_AR_MULTI_H 00025 00026 #include <string.h> 00027 #include <stdarg.h> 00028 00029 #include <AR/gsub.h> 00030 #include <AR/video.h> 00031 #include <AR/param.h> 00032 #include <AR/ar.h> 00033 #include <AR/arMulti.h> 00034 00035 #include <ros/ros.h> 00036 #include <ros/package.h> 00037 #include <ros/console.h> 00038 #include <geometry_msgs/TransformStamped.h> 00039 #include <tf/transform_broadcaster.h> 00040 #include <image_transport/image_transport.h> 00041 #include <sensor_msgs/CameraInfo.h> 00042 #include <visualization_msgs/Marker.h> 00043 #include <resource_retriever/retriever.h> 00044 00045 #include <opencv/cv.h> 00046 #include <cv_bridge/CvBridge.h> 00047 00048 #include <ar_pose/ARMarkers.h> 00049 #include <ar_pose/ARMarker.h> 00050 #include <ar_pose/object.h> 00051 00052 const std::string cameraImageTopic_ = "/usb_cam/image_raw"; 00053 const std::string cameraInfoTopic_ = "/usb_cam/camera_info"; 00054 00055 const double AR_TO_ROS = 0.001; 00056 00057 namespace ar_pose 00058 { 00059 class ARSinglePublisher 00060 { 00061 public: 00062 ARSinglePublisher (ros::NodeHandle & n); 00063 ~ARSinglePublisher (void); 00064 00065 private: 00066 void arInit (); 00067 void getTransformationCallback (const sensor_msgs::ImageConstPtr &); 00068 void camInfoCallback (const sensor_msgs::CameraInfoConstPtr &); 00069 00070 ros::NodeHandle n_; 00071 tf::TransformBroadcaster broadcaster_; 00072 ros::Subscriber sub_; 00073 image_transport::Subscriber cam_sub_; 00074 ros::Publisher arMarkerPub_; 00075 00076 image_transport::ImageTransport it_; 00077 sensor_msgs::CvBridge bridge_; 00078 sensor_msgs::CameraInfo cam_info_; 00079 00080 // **** for visualisation in rviz 00081 ros::Publisher rvizMarkerPub_; 00082 visualization_msgs::Marker rvizMarker_; 00083 00084 // **** parameters 00085 ARParam cam_param_; // Camera Calibration Parameters 00086 ARMultiMarkerInfoT *config; // AR Marker Info 00087 ar_object::ObjectData_T * object; 00088 int objectnum; 00089 char pattern_filename_[FILENAME_MAX]; 00090 00091 ar_pose::ARMarkers arPoseMarkers_; 00092 int threshold_; 00093 bool getCamInfo_; 00094 bool publishTf_; 00095 bool publishVisualMarkers_; 00096 CvSize sz_; 00097 IplImage *capture_; 00098 00099 }; // end class ARSinglePublisher 00100 } //end namespace ar_pose 00101 00102 #endif