00001 00009 #ifndef CAMERA_EXTERNAL_CALIBRATION_H_ 00010 #define CAMERA_EXTERNAL_CALIBRATION_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/Float64.h> 00018 00019 #define REQUIRED_SAMPLES 100 00020 00021 class CameraExternalCalibration 00022 { 00023 public: 00027 CameraExternalCalibration(); 00028 00029 void publishTransforms(); 00030 00031 private: 00032 ros::NodeHandle n; 00033 00034 ros::Subscriber markerSubscriber; 00035 ros::Publisher asusCommandPublisher; 00036 00037 tf::TransformListener tfListener; 00038 tf::TransformBroadcaster br; 00040 int markerID; 00041 std::vector<tf::StampedTransform> transformSamples; 00042 tf::StampedTransform finalTransform; 00043 bool calibrated; 00044 bool calibrationEnabled; 00045 bool calibrationWritten; 00046 00052 void markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg); 00053 00057 void setCameraPos(); 00058 }; 00059 00067 int main(int argc, char **argv); 00068 00069 #endif