Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "DrawInterface.h"
00030
00031
00032 #include "ros/ros.h"
00033
00034 #include <Eigen/Geometry>
00035 #include <Eigen/Dense>
00036
00037
00038 #include <visualization_msgs/MarkerArray.h>
00039
00040 class HectorDrawings : public DrawInterface
00041 {
00042 public:
00043
00044 HectorDrawings()
00045 {
00046 idCounter = 0;
00047 maxId = 0;
00048
00049 ros::NodeHandle nh_;
00050
00051 markerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1, true);
00052 markerArrayPublisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1, true);
00053
00054 tempMarker.header.frame_id = "map";
00055 tempMarker.ns = "marker";
00056
00057 this->setScale(1.0);
00058 this->setColor(1.0, 1.0, 1.0);
00059
00060 tempMarker.action = visualization_msgs::Marker::ADD;
00061 };
00062
00063 virtual void setNamespace(const std::string& ns)
00064 {
00065 tempMarker.ns = ns;
00066 }
00067
00068 virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame)
00069 {
00070 tempMarker.id = idCounter++;
00071
00072 tempMarker.pose.position.x = pointWorldFrame.x();
00073 tempMarker.pose.position.y = pointWorldFrame.y();
00074
00075 tempMarker.pose.orientation.w = 0.0;
00076 tempMarker.pose.orientation.z = 0.0;
00077 tempMarker.type = visualization_msgs::Marker::CUBE;
00078
00079
00080
00081 markerArray.markers.push_back(tempMarker);
00082 }
00083
00084 virtual void drawArrow(const Eigen::Vector3f& poseWorld)
00085 {
00086 tempMarker.id = idCounter++;
00087
00088 tempMarker.pose.position.x = poseWorld.x();
00089 tempMarker.pose.position.y = poseWorld.y();
00090
00091 tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f);
00092 tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f);
00093
00094 tempMarker.type = visualization_msgs::Marker::ARROW;
00095
00096
00097
00098 markerArray.markers.push_back(tempMarker);
00099
00100 }
00101
00102 virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix)
00103 {
00104
00105 tempMarker.pose.position.x = mean[0];
00106 tempMarker.pose.position.y = mean[1];
00107
00108 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covMatrix);
00109
00110 const Eigen::Vector2f& eigValues (eig.eigenvalues());
00111 const Eigen::Matrix2f& eigVectors (eig.eigenvectors());
00112
00113 float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));
00114
00115 tempMarker.type = visualization_msgs::Marker::CYLINDER;
00116
00117 double lengthMajor = sqrt(eigValues[0]);
00118 double lengthMinor = sqrt(eigValues[1]);
00119
00120 tempMarker.scale.x = lengthMajor;
00121 tempMarker.scale.y = lengthMinor;
00122 tempMarker.scale.z = 0.001;
00123
00124
00125 tempMarker.pose.orientation.w = cos(angle*0.5);
00126 tempMarker.pose.orientation.z = sin(angle*0.5);
00127
00128 tempMarker.id = idCounter++;
00129 markerArray.markers.push_back(tempMarker);
00130 }
00131
00132 virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix)
00133 {
00134 tempMarker.type = visualization_msgs::Marker::SPHERE;
00135
00136 tempMarker.color.r = 0.0;
00137 tempMarker.color.a = 0.5;
00138
00139 tempMarker.pose.position.x = mean[0];
00140 tempMarker.pose.position.y = mean[1];
00141 tempMarker.pose.position.z = mean[2];
00142
00143 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(covMatrix);
00144
00145 const Eigen::Vector3f& eigValues (eig.eigenvalues());
00146 const Eigen::Matrix3f& eigVectors (eig.eigenvectors());
00147
00148
00149 Eigen::Matrix3f eigVectorsFlipped;
00150 eigVectorsFlipped.col(0) = eigVectors.col(2);
00151 eigVectorsFlipped.col(1) = eigVectors.col(1);
00152 eigVectorsFlipped.col(2) = eigVectors.col(0);
00153
00154 if (eigVectorsFlipped.determinant() < 0){
00155 eigVectorsFlipped.col(2) = -eigVectorsFlipped.col(2);
00156 }
00157
00158 Eigen::Quaternionf quaternion (eigVectorsFlipped);
00159
00160
00161
00162
00163
00164
00165
00166 tempMarker.pose.orientation.w = quaternion.w();
00167 tempMarker.pose.orientation.x = quaternion.x();
00168 tempMarker.pose.orientation.y = quaternion.y();
00169 tempMarker.pose.orientation.z = quaternion.z();
00170
00171 tempMarker.scale.x = sqrt(eigValues[2]);
00172 tempMarker.scale.y = sqrt(eigValues[1]);
00173 tempMarker.scale.z = sqrt(eigValues[0]);
00174
00175 tempMarker.id = idCounter++;
00176 markerArray.markers.push_back(tempMarker);
00177
00178
00179 }
00180
00181 virtual void setScale(double scale)
00182 {
00183 tempMarker.scale.x = scale;
00184 tempMarker.scale.y = scale;
00185 tempMarker.scale.z = scale;
00186 }
00187
00188 virtual void setColor(double r, double g, double b, double a = 1.0)
00189 {
00190 tempMarker.color.r = r;
00191 tempMarker.color.g = g;
00192 tempMarker.color.b = b;
00193 tempMarker.color.a = a;
00194 }
00195
00196 virtual void addMarker(visualization_msgs::Marker marker) {
00197 if (marker.id == 0) marker.id = idCounter++;
00198 if (marker.ns.empty()) marker.ns = tempMarker.ns;
00199 markerArray.markers.push_back(marker);
00200 }
00201
00202 virtual void addMarkers(visualization_msgs::MarkerArray markers) {
00203 for(visualization_msgs::MarkerArray::_markers_type::iterator it = markers.markers.begin(); it != markers.markers.end(); ++it) {
00204 visualization_msgs::Marker &marker = *it;
00205 addMarker(marker);
00206 }
00207 }
00208
00209 virtual void sendAndResetData()
00210 {
00211 allMarkers.markers.insert(allMarkers.markers.end(), markerArray.markers.begin(), markerArray.markers.end());
00212 markerArrayPublisher_.publish(markerArray);
00213 markerArray.markers.clear();
00214 if (idCounter > maxId) maxId = idCounter;
00215 idCounter = 0;
00216 }
00217
00218 void setTime(const ros::Time& time)
00219 {
00220 tempMarker.header.stamp = time;
00221 }
00222
00223 void reset()
00224 {
00225 for(visualization_msgs::MarkerArray::_markers_type::iterator it = allMarkers.markers.begin(); it != allMarkers.markers.end(); ++it)
00226 {
00227 it->action = visualization_msgs::Marker::DELETE;
00228 }
00229 markerArrayPublisher_.publish(allMarkers);
00230 allMarkers.markers.clear();
00231 }
00232
00233 ros::Publisher markerPublisher_;
00234 ros::Publisher markerArrayPublisher_;
00235
00236 visualization_msgs::Marker tempMarker;
00237 visualization_msgs::MarkerArray markerArray;
00238 visualization_msgs::MarkerArray allMarkers;
00239
00240 int idCounter;
00241 int maxId;
00242 };