00001 00009 #ifndef CALIBRATION_FROM_CARL_H_ 00010 #define CALIBRATION_FROM_CARL_H_ 00011 00012 #include <ros/ros.h> 00013 #include <ar_track_alvar_msgs/AlvarMarkers.h> 00014 #include <geometry_msgs/Pose.h> 00015 #include <tf/transform_broadcaster.h> 00016 #include <tf/transform_listener.h> 00017 #include <std_msgs/Int16.h> 00018 #include <fstream> 00019 00024 #define CAMERA_LINK_NAME "calibration_ceiling_camera_" 00025 00026 #define REQUIRED_SAMPLES 25 00027 00028 class CalibrationFromCarl 00029 { 00030 public: 00034 CalibrationFromCarl(); 00035 00036 void publishTransforms(); 00037 00038 private: 00039 ros::NodeHandle n; 00040 00041 ros::Subscriber startCalibrationSubscriber; 00042 std::vector<ros::Subscriber> markerSubscribers; 00043 00044 tf::TransformListener tfListener; 00045 tf::TransformBroadcaster br; 00047 int markerID; 00048 bool calibrationComplete; 00049 std::vector< std::vector<tf::StampedTransform> > transformSamples; 00050 std::vector<tf::StampedTransform> finalTransforms; 00051 std::vector<bool> calibrated; 00052 std::vector<bool> calibrationEnabled; 00053 00059 void startCalibrationCallback(const std_msgs::Int16::ConstPtr& msg); 00060 00066 void markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg); 00067 }; 00068 00076 int main(int argc, char **argv); 00077 00078 #endif