00001
00018 #include "VisualisationUtils/markerpublisher.h"
00019
00020
00021 MarkerPublisher::MarkerPublisher(boost::shared_ptr<Calibration_Object> calibrationObject)
00022 {
00023 this->calibrationObject = calibrationObject;
00024 this->initialize();
00025 this->LINESCALE = 0.01;
00026 }
00027
00028 void MarkerPublisher::initialize()
00029 {
00030 char *argv[0];
00031 int x = 0;
00032 ros::init(x, argv, NAMESPACE);
00033 ros::NodeHandle n;
00034 pubObjectPosition = n.advertise<visualization_msgs::Marker>(NAMESPACEOBJECT, 0);
00035 pubMarkerPosition = n.advertise<visualization_msgs::Marker>(NAMESPACEMARKER, 0);
00036 pubCameraPosition = n.advertise<visualization_msgs::Marker>(NAMESPACECAMERA, 0);
00037
00038 markerTop.header.frame_id = "calibration_center";
00039 markerTop.ns = NAMESPACEOBJECT;
00040 markerTop.id = 0;
00041 markerTop.type = visualization_msgs::Marker::LINE_LIST;
00042 markerTop.action = visualization_msgs::Marker::ADD;
00043
00044 markerTop.color.a = 1.0f;
00045 markerTop.color.r = 1.0f;
00046 markerTop.color.g = 1.0f;
00047 markerTop.color.b = 0.0f;
00048 markerTop.scale.x = 2.0*LINESCALE;
00049 markerTop.pose.orientation.w = 1.0;
00050
00051 markerBottom.header.frame_id = "calibration_center";
00052 markerBottom.ns = NAMESPACEOBJECT;
00053 markerBottom.id = 1;
00054 markerBottom.type = visualization_msgs::Marker::LINE_LIST;
00055 markerBottom.action = visualization_msgs::Marker::ADD;
00056
00057 markerBottom.color.a = 1.0f;
00058 markerBottom.color.r = 0.0f;
00059 markerBottom.color.g = 1.0f;
00060 markerBottom.color.b = 0.0f;
00061 markerBottom.scale.x = 2.0*LINESCALE;
00062 markerBottom.pose.orientation.w = 1.0;
00063
00064 markerTriangles.header.frame_id = "calibration_center";
00065 markerTriangles.ns = NAMESPACEOBJECT;
00066 markerTriangles.id = 2;
00067 markerTriangles.type = visualization_msgs::Marker::LINE_LIST;
00068 markerTriangles.action = visualization_msgs::Marker::ADD;
00069
00070 markerTriangles.color.a = 1.0f;
00071 markerTriangles.color.r = 1.0f;
00072 markerTriangles.color.g = 0.0f;
00073 markerTriangles.color.b = 0.0f;
00074 markerTriangles.scale.x = LINESCALE;
00075 markerTriangles.pose.orientation.w = 1.0;
00076
00077 markerARMarker_Left.header.frame_id = "calibration_center";
00078 markerARMarker_Left.ns = NAMESPACEMARKER;
00079 markerARMarker_Left.id = 0;
00080 markerARMarker_Left.type = visualization_msgs::Marker::LINE_LIST;
00081 markerARMarker_Left.action = visualization_msgs::Marker::ADD;
00082
00083 markerARMarker_Left.color.a = 1.0f;
00084 markerARMarker_Left.color.r = 1.0f;
00085 markerARMarker_Left.color.g = 0.0f;
00086 markerARMarker_Left.color.b = 0.0f;
00087 markerARMarker_Left.scale.x = LINESCALE;
00088 markerARMarker_Left.pose.orientation.w = 1.0;
00089
00090 markerARMarker_Right.header.frame_id = "calibration_center";
00091 markerARMarker_Right.ns = NAMESPACEMARKER;
00092 markerARMarker_Right.id = 1;
00093 markerARMarker_Right.type = visualization_msgs::Marker::LINE_LIST;
00094 markerARMarker_Right.action = visualization_msgs::Marker::ADD;
00095
00096 markerARMarker_Right.color.a = 1.0f;
00097 markerARMarker_Right.color.r = 1.0f;
00098 markerARMarker_Right.color.g = 0.0f;
00099 markerARMarker_Right.color.b = 0.0f;
00100 markerARMarker_Right.scale.x = LINESCALE;
00101 markerARMarker_Right.pose.orientation.w = 1.0;
00102
00103
00104 visualization_msgs::Marker deleteMarker;
00105 deleteMarker.action = visualization_msgs::Marker::DELETE;
00106 deleteMarker.ns = NAMESPACEOBJECT;
00107 deleteMarker.header.frame_id = "calibration_center";
00108 deleteMarker.header.stamp = ros::Time();
00109 deleteMarker.id = 0;
00110 pubObjectPosition.publish(deleteMarker);
00111 deleteMarker.id = 1;
00112 pubObjectPosition.publish(deleteMarker);
00113 deleteMarker.id = 2;
00114 pubObjectPosition.publish(deleteMarker);
00115
00116 visualization_msgs::Marker deleteMarker2;
00117 deleteMarker2.action = visualization_msgs::Marker::DELETE;
00118 deleteMarker2.header.frame_id = "calibration_center";
00119 deleteMarker2.ns = NAMESPACEMARKER;
00120 deleteMarker2.header.stamp = ros::Time();
00121 deleteMarker2.id = 0;
00122 pubMarkerPosition.publish(deleteMarker2);
00123 deleteMarker2.id = 1;
00124 pubMarkerPosition.publish(deleteMarker2);
00125
00126 visualization_msgs::Marker deleteMarker_camera;
00127 deleteMarker_camera.action = visualization_msgs::Marker::DELETE;
00128 deleteMarker_camera.header.frame_id = "calibration_center";
00129 deleteMarker_camera.ns = NAMESPACECAMERA;
00130 deleteMarker_camera.header.stamp = ros::Time();
00131 deleteMarker_camera.id = 0;
00132 pubCameraPosition.publish(deleteMarker_camera);
00133
00134 ROS_INFO("Marker publisher initialized.");
00135 }
00136 void MarkerPublisher::publishARMarkers(bool activeLeft, bool activeRight, const Eigen::Matrix4d topFrame)
00137 {
00138 markerARMarker_Left.points.clear();
00139 markerARMarker_Left.header.stamp = ros::Time();
00140 markerARMarker_Right.points.clear();
00141 markerARMarker_Right.header.stamp = ros::Time();
00142
00143 double edgeSize = calibrationObject->marker_edge_size;
00144
00145 geometry_msgs::Point A, B, C, D;
00146 Eigen::Matrix4d markerBase;
00147 markerBase = topFrame*calibrationObject->frame_marker_left;
00148 A.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
00149 A.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
00150 A.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
00151 B.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
00152 B.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
00153 B.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
00154 C.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
00155 C.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
00156 C.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
00157 D.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
00158 D.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
00159 D.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
00160
00161 markerARMarker_Left.points.push_back(A);
00162 markerARMarker_Left.points.push_back(B);
00163 markerARMarker_Left.points.push_back(B);
00164 markerARMarker_Left.points.push_back(C);
00165 markerARMarker_Left.points.push_back(C);
00166 markerARMarker_Left.points.push_back(D);
00167 markerARMarker_Left.points.push_back(D);
00168 markerARMarker_Left.points.push_back(A);
00169
00170 markerBase = topFrame*calibrationObject->frame_marker_right;
00171 geometry_msgs::Point A_, B_, C_, D_;
00172 A_.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
00173 A_.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
00174 A_.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
00175 B_.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
00176 B_.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
00177 B_.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
00178 C_.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
00179 C_.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
00180 C_.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
00181 D_.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
00182 D_.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
00183 D_.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
00184
00185 markerARMarker_Right.points.push_back(A_);
00186 markerARMarker_Right.points.push_back(B_);
00187 markerARMarker_Right.points.push_back(B_);
00188 markerARMarker_Right.points.push_back(C_);
00189 markerARMarker_Right.points.push_back(C_);
00190 markerARMarker_Right.points.push_back(D_);
00191 markerARMarker_Right.points.push_back(D_);
00192 markerARMarker_Right.points.push_back(A_);
00193
00194 if (activeLeft)
00195 {
00196 markerARMarker_Left.color.r = 0.0f;
00197 markerARMarker_Left.color.g = 1.0f;
00198 markerARMarker_Left.color.b = 0.0f;
00199 }
00200 else
00201 {
00202 markerARMarker_Left.color.r = 1.0f;
00203 markerARMarker_Left.color.g = 0.0f;
00204 markerARMarker_Left.color.b = 0.0f;
00205 }
00206
00207 if (activeRight)
00208 {
00209 markerARMarker_Right.color.r = 0.0f;
00210 markerARMarker_Right.color.g = 1.0f;
00211 markerARMarker_Right.color.b = 0.0f;
00212 }
00213 else
00214 {
00215 markerARMarker_Right.color.r = 1.0f;
00216 markerARMarker_Right.color.g = 0.0f;
00217 markerARMarker_Right.color.b = 0.0f;
00218 }
00219
00220 if (!activeLeft && !activeRight)
00221 {
00222 visualization_msgs::Marker deleteMarker_camera;
00223 deleteMarker_camera.action = visualization_msgs::Marker::DELETE;
00224 deleteMarker_camera.header.frame_id = "calibration_center";
00225 deleteMarker_camera.ns = NAMESPACECAMERA;
00226 deleteMarker_camera.header.stamp = ros::Time();
00227 deleteMarker_camera.id = 0;
00228 pubCameraPosition.publish(deleteMarker_camera);
00229 }
00230
00231 pubMarkerPosition.publish(markerARMarker_Left);
00232 pubMarkerPosition.publish(markerARMarker_Right);
00233 }
00234
00235 void MarkerPublisher::publishColouredCameraFrames(std::vector<colouredCameraFrame, Eigen::aligned_allocator<colouredCameraFrame> > * camFrames)
00236 {
00237 ROS_INFO_STREAM("Got "<< camFrames->size() << " markers. ");
00238 for (unsigned int i = 0; i < camFrames->size(); i++)
00239 {
00240 colouredCameraFrame cCF = camFrames->at(i);
00241 Eigen::Matrix4d tempMatrix = cCF.pose;
00242 visualization_msgs::Marker tempMarker;
00243 tempMarker.header.frame_id = "calibration_center";
00244 tempMarker.ns = NAMESPACECAMERA;
00245 tempMarker.id = i+1;
00246 tempMarker.type = visualization_msgs::Marker::ARROW;
00247 tempMarker.action = visualization_msgs::Marker::ADD;
00248
00249 tempMarker.color.r = cCF.color.r;
00250 tempMarker.color.g = cCF.color.g;
00251 tempMarker.color.b = cCF.color.b;
00252 tempMarker.color.a = cCF.color.a;
00253
00254 tempMarker.scale.x = LINESCALE / 8.0;
00255 tempMarker.scale.y = LINESCALE / 4.0;
00256 tempMarker.pose.orientation.w = 1.0;
00257 tempMarker.header.stamp = ros::Time();
00258
00259 Eigen::Vector3d tempVec(tempMatrix(0,2), tempMatrix(1,2), tempMatrix(2,2));
00260 tempVec.normalize();
00261 tempVec = tempVec * LINESCALE*10.0;
00262
00263 geometry_msgs::Point p1, p2;
00264
00265 p1.x = tempMatrix(0,3);
00266 p1.y = tempMatrix(1,3);
00267 p1.z = tempMatrix(2,3);
00268 p2.x = tempMatrix(0,3) + tempVec[0];
00269 p2.y = tempMatrix(1,3) + tempVec[1];
00270 p2.z = tempMatrix(2,3) + tempVec[2];
00271 tempMarker.points.push_back(p1);
00272 tempMarker.points.push_back(p2);
00273
00274 pubCameraPosition.publish (tempMarker);
00275 }
00276 }
00277
00278 void MarkerPublisher::publishCameraFramePointer(const Eigen::Vector3d &startPoint, const Eigen::Vector3d &endPoint, bool isValid)
00279 {
00280 visualization_msgs::Marker arrowMarker;
00281 arrowMarker.header.frame_id = "calibration_center";
00282 arrowMarker.ns = NAMESPACECAMERA;
00283 arrowMarker.id = 0;
00284 arrowMarker.type = visualization_msgs::Marker::ARROW;
00285 arrowMarker.action = visualization_msgs::Marker::ADD;
00286 arrowMarker.color.a = 1.0f;
00287 if (isValid)
00288 {
00289 arrowMarker.color.r = 0.0f;
00290 arrowMarker.color.g = 1.0f;
00291 }
00292 else
00293 {
00294 arrowMarker.color.r = 1.0f;
00295 arrowMarker.color.g = 0.0f;
00296 }
00297 arrowMarker.color.b = 0.0f;
00298 arrowMarker.scale.x = LINESCALE;
00299 arrowMarker.scale.y = 2.0*LINESCALE;
00300 arrowMarker.scale.y = 5.0*LINESCALE;
00301 arrowMarker.pose.orientation.w = 1.0;
00302
00303 geometry_msgs::Point p1, p2;
00304 p1.x = startPoint[0];
00305 p1.y = startPoint[1];
00306 p1.z = startPoint[2];
00307 p2.x = endPoint[0];
00308 p2.y = endPoint[1];
00309 p2.z = endPoint[2];
00310 arrowMarker.points.push_back(p1);
00311 arrowMarker.points.push_back(p2);
00312 pubCameraPosition.publish (arrowMarker);
00313 }
00314
00315 void MarkerPublisher::publishTetrahedon(const Eigen::Vector2d &baseA, const Eigen::Vector2d &baseB, const Eigen::Vector2d &baseC, const Eigen::Vector3d &top)
00316 {
00317 markerTop.points.clear();
00318 markerTop.header.stamp = ros::Time();
00319 markerBottom.points.clear();
00320 markerBottom.header.stamp = ros::Time();
00321
00322 Eigen::Vector3d tempVector;
00323 geometry_msgs::Point A_,B_, C_, A, B, C, TOP;
00324 A_.x = baseA[0];
00325 A_.y = baseA[1];
00326 A_.z = 0.0;
00327 B_.x = baseB[0];
00328 B_.y = baseB[1];
00329 B_.z = 0.0;
00330 C_.x = baseC[0];
00331 C_.y = baseC[1];
00332 C_.z = 0.0;
00333 TOP.x = top[0];
00334 TOP.y = top[1];
00335 TOP.z = top[2];
00336 tempVector = Eigen::Vector3d(baseA[0] - top[0], baseA[1] - top[1], - top[2]);
00337 tempVector.normalize();
00338 tempVector = tempVector* calibrationObject->side_a + top;
00339 A.x = tempVector[0];
00340 A.y = tempVector[1];
00341 A.z = tempVector[2];
00342 tempVector = Eigen::Vector3d(baseB[0] - top[0], baseB[1] - top[1], - top[2]);
00343 tempVector.normalize();
00344 tempVector = tempVector* calibrationObject->side_b + top;
00345 B.x = tempVector[0];
00346 B.y = tempVector[1];
00347 B.z = tempVector[2];
00348 tempVector = Eigen::Vector3d(baseC[0] - top[0], baseC[1] - top[1], - top[2]);
00349 tempVector.normalize();
00350 tempVector = tempVector* calibrationObject->side_c + top;
00351 C.x = tempVector[0];
00352 C.y = tempVector[1];
00353 C.z = tempVector[2];
00354
00355 markerTop.points.push_back(A_);
00356 markerTop.points.push_back(TOP);
00357 markerTop.points.push_back(B_);
00358 markerTop.points.push_back(TOP);
00359 markerTop.points.push_back(C_);
00360 markerTop.points.push_back(TOP);
00361
00362 markerBottom.points.push_back(A);
00363 markerBottom.points.push_back(A_);
00364 markerBottom.points.push_back(B);
00365 markerBottom.points.push_back(B_);
00366 markerBottom.points.push_back(C);
00367 markerBottom.points.push_back(C_);
00368 markerBottom.points.push_back(A);
00369 markerBottom.points.push_back(B);
00370 markerBottom.points.push_back(B);
00371 markerBottom.points.push_back(C);
00372
00373 pubObjectPosition.publish(markerTop);
00374 pubObjectPosition.publish(markerBottom);
00375 }
00376
00377
00378 void MarkerPublisher::publishTriangles(std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > *triangles)
00379 {
00380 if (triangles->size() > 1)
00381 {
00382 markerTriangles.points.clear();
00383 markerTriangles.header.stamp = ros::Time();
00384 geometry_msgs::Point A,B;
00385 A.z = 0.0;
00386 B.z = 0.0;
00387 for (unsigned int i = 0 ; i < triangles->size() - 1; i++)
00388 {
00389 if (i % 3 != 2)
00390 {
00391 A.x = (triangles->at(i))[0];
00392 A.y = (triangles->at(i))[1];
00393 B.x = (triangles->at(i+1))[0];
00394 B.y = (triangles->at(i+1))[1];
00395
00396 markerTriangles.points.push_back(A);
00397 markerTriangles.points.push_back(B);
00398 }
00399 }
00400 pubObjectPosition.publish(markerTriangles);
00401 }
00402 }
00403