00001 00018 #ifndef MARKERPUBLISHER_H 00019 #define MARKERPUBLISHER_H 00020 #define NAMESPACE "calibrationObject" 00021 #define NAMESPACEOBJECT "calibrationObject/Object" //The namespace to which the calibration Object is published 00022 #define NAMESPACEMARKER "calibrationObject/Marker" //The namespace to which the marker positions are published 00023 #define NAMESPACECAMERA "calibrationObject/Camera" //The namespace to which thecamera poses are published 00024 00025 00026 00027 #include "calibration_object.h" 00028 #ifndef Q_MOC_RUN 00029 #include <visualization_msgs/Marker.h> 00030 #include <geometry_msgs/Point.h> 00031 #include <Eigen/Dense> 00032 #include <ros/ros.h> 00033 #include <ros/init.h> 00034 #include <boost/shared_ptr.hpp> 00035 #endif 00036 00037 struct colouredCameraFrame 00038 { 00039 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00040 Eigen::Matrix4d pose; 00041 std_msgs::ColorRGBA color; 00042 }; 00043 00044 //Class used to publish a tetrahedon using markers 00045 class MarkerPublisher 00046 { 00047 public: 00048 MarkerPublisher(boost::shared_ptr<Calibration_Object> calibrationObject); 00049 00050 void publishTriangles(std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > *triangles); 00051 void publishTetrahedon(const Eigen::Vector2d &baseA, const Eigen::Vector2d &baseB, const Eigen::Vector2d &baseC, const Eigen::Vector3d &top); 00052 void publishARMarkers(bool activeLeft, bool activeRight, const Eigen::Matrix4d topFrame); 00053 void publishColouredCameraFrames(std::vector<colouredCameraFrame, Eigen::aligned_allocator<colouredCameraFrame> > * camFrames); 00054 void publishCameraFramePointer(const Eigen::Vector3d &startPoint, const Eigen::Vector3d &endPoint, bool isValid); 00055 protected: 00056 void initialize(); 00057 //Used to publish the active tetrahedon 00058 ros::Publisher pubObjectPosition; 00059 ros::Publisher pubMarkerPosition; 00060 ros::Publisher pubCameraPosition; 00061 visualization_msgs::Marker markerTop; 00062 visualization_msgs::Marker markerBottom; 00063 visualization_msgs::Marker markerTriangles; 00064 00065 visualization_msgs::Marker markerARMarker_Left; 00066 visualization_msgs::Marker markerARMarker_Right; 00067 00068 visualization_msgs::Marker markerCameraFrame; 00069 00070 std::vector<visualization_msgs::Marker> markerCameraFrames; 00071 00072 boost::shared_ptr<Calibration_Object> calibrationObject; 00073 00074 double LINESCALE; 00075 }; 00076 00077 00078 00079 #endif // MARKERPUBLISHER_H