00001 00018 #ifndef TRANSFORMATION_PUBLISHER_H 00019 #define TRANSFORMATION_PUBLISHER_H 00020 #include <QThread> 00021 00022 #include <calibration_object.h> 00023 00024 #ifndef Q_MOC_RUN 00025 #include <sensor_msgs/JointState.h> 00026 #include <tf/transform_broadcaster.h> 00027 #include <Eigen/Dense> 00028 #include <ros/ros.h> 00029 #include <ros/init.h> 00030 #include <boost/shared_ptr.hpp> 00031 #endif 00032 00033 enum Markertype 00034 { 00035 LEFT, 00036 RIGHT 00037 }; 00038 00039 class Transformation_Publisher : public QThread 00040 { 00041 Q_OBJECT 00042 public: 00043 explicit Transformation_Publisher(boost::shared_ptr<Calibration_Object> object); 00044 00045 void stop(); 00046 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00047 protected: 00048 00049 ros::NodeHandle nh; 00050 ros::Publisher joint_pub; 00051 tf::TransformBroadcaster broadcaster; 00052 00053 std::vector<geometry_msgs::TransformStamped> staticTransforms; 00054 geometry_msgs::TransformStamped mapToTop; 00055 geometry_msgs::TransformStamped topToScanFrame; 00056 geometry_msgs::TransformStamped markerLeftToCamera; 00057 geometry_msgs::TransformStamped markerRightToCamera; 00058 00059 Markertype currentMarkertype; 00060 00061 void run(); 00062 geometry_msgs::Transform matrixToTransform(const Eigen::Matrix4d& matrix); 00063 00064 public slots: 00065 void newTransform_Laserscan(const Eigen::Matrix4d& transform, double distanceToTop); 00066 void newTransform_Camera(const Eigen::Matrix4d& transform, Markertype markertype); 00067 }; 00068 00069 00070 #endif // TRANSFORMATION_PUBLISHER_H