markerpublisher.cpp
Go to the documentation of this file.
1 
18 #include "VisualisationUtils/markerpublisher.h"
19 
20 
21 MarkerPublisher::MarkerPublisher(boost::shared_ptr<Calibration_Object> calibrationObject)
22 {
23  this->calibrationObject = calibrationObject;
24  this->initialize();
25  this->LINESCALE = 0.01;
26 }
27 
28 void MarkerPublisher::initialize()
29 {
30  char *argv[0];
31  int x = 0;
32  ros::init(x, argv, NAMESPACE);
33  ros::NodeHandle n;
34  pubObjectPosition = n.advertise<visualization_msgs::Marker>(NAMESPACEOBJECT, 0);
35  pubMarkerPosition = n.advertise<visualization_msgs::Marker>(NAMESPACEMARKER, 0);
36  pubCameraPosition = n.advertise<visualization_msgs::Marker>(NAMESPACECAMERA, 0);
37 
38  markerTop.header.frame_id = "calibration_center";
39  markerTop.ns = NAMESPACEOBJECT;
40  markerTop.id = 0;
41  markerTop.type = visualization_msgs::Marker::LINE_LIST;
42  markerTop.action = visualization_msgs::Marker::ADD;
43 
44  markerTop.color.a = 1.0f;
45  markerTop.color.r = 1.0f;
46  markerTop.color.g = 1.0f;
47  markerTop.color.b = 0.0f;
48  markerTop.scale.x = 2.0*LINESCALE;
49  markerTop.pose.orientation.w = 1.0;
50 
51  markerBottom.header.frame_id = "calibration_center";
52  markerBottom.ns = NAMESPACEOBJECT;
53  markerBottom.id = 1;
54  markerBottom.type = visualization_msgs::Marker::LINE_LIST;
55  markerBottom.action = visualization_msgs::Marker::ADD;
56 
57  markerBottom.color.a = 1.0f;
58  markerBottom.color.r = 0.0f;
59  markerBottom.color.g = 1.0f;
60  markerBottom.color.b = 0.0f;
61  markerBottom.scale.x = 2.0*LINESCALE;
62  markerBottom.pose.orientation.w = 1.0;
63 
64  markerTriangles.header.frame_id = "calibration_center";
65  markerTriangles.ns = NAMESPACEOBJECT;
66  markerTriangles.id = 2;
67  markerTriangles.type = visualization_msgs::Marker::LINE_LIST;
68  markerTriangles.action = visualization_msgs::Marker::ADD;
69 
70  markerTriangles.color.a = 1.0f;
71  markerTriangles.color.r = 1.0f;
72  markerTriangles.color.g = 0.0f;
73  markerTriangles.color.b = 0.0f;
74  markerTriangles.scale.x = LINESCALE;
75  markerTriangles.pose.orientation.w = 1.0;
76 
77  markerARMarker_Left.header.frame_id = "calibration_center";
78  markerARMarker_Left.ns = NAMESPACEMARKER;
79  markerARMarker_Left.id = 0;
80  markerARMarker_Left.type = visualization_msgs::Marker::LINE_LIST;
81  markerARMarker_Left.action = visualization_msgs::Marker::ADD;
82 
83  markerARMarker_Left.color.a = 1.0f;
84  markerARMarker_Left.color.r = 1.0f;
85  markerARMarker_Left.color.g = 0.0f;
86  markerARMarker_Left.color.b = 0.0f;
87  markerARMarker_Left.scale.x = LINESCALE;
88  markerARMarker_Left.pose.orientation.w = 1.0;
89 
90  markerARMarker_Right.header.frame_id = "calibration_center";
91  markerARMarker_Right.ns = NAMESPACEMARKER;
92  markerARMarker_Right.id = 1;
93  markerARMarker_Right.type = visualization_msgs::Marker::LINE_LIST;
94  markerARMarker_Right.action = visualization_msgs::Marker::ADD;
95 
96  markerARMarker_Right.color.a = 1.0f;
97  markerARMarker_Right.color.r = 1.0f;
98  markerARMarker_Right.color.g = 0.0f;
99  markerARMarker_Right.color.b = 0.0f;
100  markerARMarker_Right.scale.x = LINESCALE;
101  markerARMarker_Right.pose.orientation.w = 1.0;
102 
103  //Delete old markers
104  visualization_msgs::Marker deleteMarker;
105  deleteMarker.action = visualization_msgs::Marker::DELETE;
106  deleteMarker.ns = NAMESPACEOBJECT;
107  deleteMarker.header.frame_id = "calibration_center";
108  deleteMarker.header.stamp = ros::Time();
109  deleteMarker.id = 0;
110  pubObjectPosition.publish(deleteMarker);
111  deleteMarker.id = 1;
112  pubObjectPosition.publish(deleteMarker);
113  deleteMarker.id = 2;
114  pubObjectPosition.publish(deleteMarker);
115 
116  visualization_msgs::Marker deleteMarker2;
117  deleteMarker2.action = visualization_msgs::Marker::DELETE;
118  deleteMarker2.header.frame_id = "calibration_center";
119  deleteMarker2.ns = NAMESPACEMARKER;
120  deleteMarker2.header.stamp = ros::Time();
121  deleteMarker2.id = 0;
122  pubMarkerPosition.publish(deleteMarker2);
123  deleteMarker2.id = 1;
124  pubMarkerPosition.publish(deleteMarker2);
125 
126  visualization_msgs::Marker deleteMarker_camera;
127  deleteMarker_camera.action = visualization_msgs::Marker::DELETE;
128  deleteMarker_camera.header.frame_id = "calibration_center";
129  deleteMarker_camera.ns = NAMESPACECAMERA;
130  deleteMarker_camera.header.stamp = ros::Time();
131  deleteMarker_camera.id = 0;
132  pubCameraPosition.publish(deleteMarker_camera);
133 
134  ROS_INFO("Marker publisher initialized.");
135 }
136 void MarkerPublisher::publishARMarkers(bool activeLeft, bool activeRight, const Eigen::Matrix4d topFrame)
137 {
138  markerARMarker_Left.points.clear();
139  markerARMarker_Left.header.stamp = ros::Time();
140  markerARMarker_Right.points.clear();
141  markerARMarker_Right.header.stamp = ros::Time();
142 
143  double edgeSize = calibrationObject->marker_edge_size;
144 
145  geometry_msgs::Point A, B, C, D;
146  Eigen::Matrix4d markerBase;
147  markerBase = topFrame*calibrationObject->frame_marker_left;
148  A.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
149  A.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
150  A.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
151  B.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
152  B.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
153  B.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
154  C.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
155  C.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
156  C.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
157  D.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
158  D.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
159  D.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
160 
161  markerARMarker_Left.points.push_back(A);
162  markerARMarker_Left.points.push_back(B);
163  markerARMarker_Left.points.push_back(B);
164  markerARMarker_Left.points.push_back(C);
165  markerARMarker_Left.points.push_back(C);
166  markerARMarker_Left.points.push_back(D);
167  markerARMarker_Left.points.push_back(D);
168  markerARMarker_Left.points.push_back(A);
169 
170  markerBase = topFrame*calibrationObject->frame_marker_right;
171  geometry_msgs::Point A_, B_, C_, D_;
172  A_.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
173  A_.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
174  A_.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
175  B_.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 + markerBase(0,1)*edgeSize*0.5;
176  B_.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 + markerBase(1,1)*edgeSize*0.5;
177  B_.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 + markerBase(2,1)*edgeSize*0.5;
178  C_.x = markerBase(0,3) - markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
179  C_.y = markerBase(1,3) - markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
180  C_.z = markerBase(2,3) - markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
181  D_.x = markerBase(0,3) + markerBase(0,0)*edgeSize*0.5 - markerBase(0,1)*edgeSize*0.5;
182  D_.y = markerBase(1,3) + markerBase(1,0)*edgeSize*0.5 - markerBase(1,1)*edgeSize*0.5;
183  D_.z = markerBase(2,3) + markerBase(2,0)*edgeSize*0.5 - markerBase(2,1)*edgeSize*0.5;
184 
185  markerARMarker_Right.points.push_back(A_);
186  markerARMarker_Right.points.push_back(B_);
187  markerARMarker_Right.points.push_back(B_);
188  markerARMarker_Right.points.push_back(C_);
189  markerARMarker_Right.points.push_back(C_);
190  markerARMarker_Right.points.push_back(D_);
191  markerARMarker_Right.points.push_back(D_);
192  markerARMarker_Right.points.push_back(A_);
193 
194  if (activeLeft)
195  {
196  markerARMarker_Left.color.r = 0.0f;
197  markerARMarker_Left.color.g = 1.0f;
198  markerARMarker_Left.color.b = 0.0f;
199  }
200  else
201  {
202  markerARMarker_Left.color.r = 1.0f;
203  markerARMarker_Left.color.g = 0.0f;
204  markerARMarker_Left.color.b = 0.0f;
205  }
206 
207  if (activeRight)
208  {
209  markerARMarker_Right.color.r = 0.0f;
210  markerARMarker_Right.color.g = 1.0f;
211  markerARMarker_Right.color.b = 0.0f;
212  }
213  else
214  {
215  markerARMarker_Right.color.r = 1.0f;
216  markerARMarker_Right.color.g = 0.0f;
217  markerARMarker_Right.color.b = 0.0f;
218  }
219 
220  if (!activeLeft && !activeRight)
221  {
222  visualization_msgs::Marker deleteMarker_camera;
223  deleteMarker_camera.action = visualization_msgs::Marker::DELETE;
224  deleteMarker_camera.header.frame_id = "calibration_center";
225  deleteMarker_camera.ns = NAMESPACECAMERA;
226  deleteMarker_camera.header.stamp = ros::Time();
227  deleteMarker_camera.id = 0;
228  pubCameraPosition.publish(deleteMarker_camera);
229  }
230 
231  pubMarkerPosition.publish(markerARMarker_Left);
232  pubMarkerPosition.publish(markerARMarker_Right);
233 }
234 
235 void MarkerPublisher::publishColouredCameraFrames(std::vector<colouredCameraFrame, Eigen::aligned_allocator<colouredCameraFrame> > * camFrames)
236 {
237  ROS_INFO_STREAM("Got "<< camFrames->size() << " markers. ");
238  for (unsigned int i = 0; i < camFrames->size(); i++)
239  {
240  colouredCameraFrame cCF = camFrames->at(i);
241  Eigen::Matrix4d tempMatrix = cCF.pose;
242  visualization_msgs::Marker tempMarker;
243  tempMarker.header.frame_id = "calibration_center";
244  tempMarker.ns = NAMESPACECAMERA;
245  tempMarker.id = i+1;
246  tempMarker.type = visualization_msgs::Marker::ARROW;
247  tempMarker.action = visualization_msgs::Marker::ADD;
248 
249  tempMarker.color.r = cCF.color.r;
250  tempMarker.color.g = cCF.color.g;
251  tempMarker.color.b = cCF.color.b;
252  tempMarker.color.a = cCF.color.a;
253 
254  tempMarker.scale.x = LINESCALE / 8.0;
255  tempMarker.scale.y = LINESCALE / 4.0;
256  tempMarker.pose.orientation.w = 1.0;
257  tempMarker.header.stamp = ros::Time();
258 
259  Eigen::Vector3d tempVec(tempMatrix(0,2), tempMatrix(1,2), tempMatrix(2,2));
260  tempVec.normalize();
261  tempVec = tempVec * LINESCALE*10.0;
262 
263  geometry_msgs::Point p1, p2;
264 
265  p1.x = tempMatrix(0,3);
266  p1.y = tempMatrix(1,3);
267  p1.z = tempMatrix(2,3);
268  p2.x = tempMatrix(0,3) + tempVec[0];
269  p2.y = tempMatrix(1,3) + tempVec[1];
270  p2.z = tempMatrix(2,3) + tempVec[2];
271  tempMarker.points.push_back(p1);
272  tempMarker.points.push_back(p2);
273 
274  pubCameraPosition.publish (tempMarker);
275  }
276 }
277 
278 void MarkerPublisher::publishCameraFramePointer(const Eigen::Vector3d &startPoint, const Eigen::Vector3d &endPoint, bool isValid)
279 {
280  visualization_msgs::Marker arrowMarker;
281  arrowMarker.header.frame_id = "calibration_center";
282  arrowMarker.ns = NAMESPACECAMERA;
283  arrowMarker.id = 0;
284  arrowMarker.type = visualization_msgs::Marker::ARROW;
285  arrowMarker.action = visualization_msgs::Marker::ADD;
286  arrowMarker.color.a = 1.0f;
287  if (isValid)
288  {
289  arrowMarker.color.r = 0.0f;
290  arrowMarker.color.g = 1.0f;
291  }
292  else
293  {
294  arrowMarker.color.r = 1.0f;
295  arrowMarker.color.g = 0.0f;
296  }
297  arrowMarker.color.b = 0.0f;
298  arrowMarker.scale.x = LINESCALE;
299  arrowMarker.scale.y = 2.0*LINESCALE;
300  arrowMarker.scale.y = 5.0*LINESCALE;
301  arrowMarker.pose.orientation.w = 1.0;
302 
303  geometry_msgs::Point p1, p2;
304  p1.x = startPoint[0];
305  p1.y = startPoint[1];
306  p1.z = startPoint[2];
307  p2.x = endPoint[0];
308  p2.y = endPoint[1];
309  p2.z = endPoint[2];
310  arrowMarker.points.push_back(p1);
311  arrowMarker.points.push_back(p2);
312  pubCameraPosition.publish (arrowMarker);
313 }
314 
315 void MarkerPublisher::publishTetrahedon(const Eigen::Vector2d &baseA, const Eigen::Vector2d &baseB, const Eigen::Vector2d &baseC, const Eigen::Vector3d &top)
316 {
317  markerTop.points.clear();
318  markerTop.header.stamp = ros::Time();
319  markerBottom.points.clear();
320  markerBottom.header.stamp = ros::Time();
321 
322  Eigen::Vector3d tempVector;
323  geometry_msgs::Point A_,B_, C_, A, B, C, TOP;
324  A_.x = baseA[0];
325  A_.y = baseA[1];
326  A_.z = 0.0;
327  B_.x = baseB[0];
328  B_.y = baseB[1];
329  B_.z = 0.0;
330  C_.x = baseC[0];
331  C_.y = baseC[1];
332  C_.z = 0.0;
333  TOP.x = top[0];
334  TOP.y = top[1];
335  TOP.z = top[2];
336  tempVector = Eigen::Vector3d(baseA[0] - top[0], baseA[1] - top[1], - top[2]);
337  tempVector.normalize();
338  tempVector = tempVector* calibrationObject->side_a + top;
339  A.x = tempVector[0];
340  A.y = tempVector[1];
341  A.z = tempVector[2];
342  tempVector = Eigen::Vector3d(baseB[0] - top[0], baseB[1] - top[1], - top[2]);
343  tempVector.normalize();
344  tempVector = tempVector* calibrationObject->side_b + top;
345  B.x = tempVector[0];
346  B.y = tempVector[1];
347  B.z = tempVector[2];
348  tempVector = Eigen::Vector3d(baseC[0] - top[0], baseC[1] - top[1], - top[2]);
349  tempVector.normalize();
350  tempVector = tempVector* calibrationObject->side_c + top;
351  C.x = tempVector[0];
352  C.y = tempVector[1];
353  C.z = tempVector[2];
354 
355  markerTop.points.push_back(A_);
356  markerTop.points.push_back(TOP);
357  markerTop.points.push_back(B_);
358  markerTop.points.push_back(TOP);
359  markerTop.points.push_back(C_);
360  markerTop.points.push_back(TOP);
361 
362  markerBottom.points.push_back(A);
363  markerBottom.points.push_back(A_);
364  markerBottom.points.push_back(B);
365  markerBottom.points.push_back(B_);
366  markerBottom.points.push_back(C);
367  markerBottom.points.push_back(C_);
368  markerBottom.points.push_back(A);
369  markerBottom.points.push_back(B);
370  markerBottom.points.push_back(B);
371  markerBottom.points.push_back(C);
372 
373  pubObjectPosition.publish(markerTop);
374  pubObjectPosition.publish(markerBottom);
375 }
376 
377 
378 void MarkerPublisher::publishTriangles(std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > *triangles)
379 {
380  if (triangles->size() > 1)
381  {
382  markerTriangles.points.clear();
383  markerTriangles.header.stamp = ros::Time();
384  geometry_msgs::Point A,B;
385  A.z = 0.0;
386  B.z = 0.0;
387  for (unsigned int i = 0 ; i < triangles->size() - 1; i++)
388  {
389  if (i % 3 != 2)
390  { // Dont connect the triangles
391  A.x = (triangles->at(i))[0];
392  A.y = (triangles->at(i))[1];
393  B.x = (triangles->at(i+1))[0];
394  B.y = (triangles->at(i+1))[1];
395 
396  markerTriangles.points.push_back(A);
397  markerTriangles.points.push_back(B);
398  }
399  }
400  pubObjectPosition.publish(markerTriangles);
401  }
402 }
403 
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
MarkerPublisher::publishColouredCameraFrames
void publishColouredCameraFrames(std::vector< colouredCameraFrame, Eigen::aligned_allocator< colouredCameraFrame > > *camFrames)
Definition: markerpublisher.cpp:235
MarkerPublisher::publishTriangles
void publishTriangles(std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > *triangles)
Definition: markerpublisher.cpp:378
colouredCameraFrame::pose
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix4d pose
Definition: markerpublisher.h:40
MarkerPublisher::publishCameraFramePointer
void publishCameraFramePointer(const Eigen::Vector3d &startPoint, const Eigen::Vector3d &endPoint, bool isValid)
Definition: markerpublisher.cpp:278
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
MarkerPublisher::markerTop
visualization_msgs::Marker markerTop
Definition: markerpublisher.h:61
MarkerPublisher::publishTetrahedon
void publishTetrahedon(const Eigen::Vector2d &baseA, const Eigen::Vector2d &baseB, const Eigen::Vector2d &baseC, const Eigen::Vector3d &top)
Definition: markerpublisher.cpp:315
colouredCameraFrame::color
std_msgs::ColorRGBA color
Definition: markerpublisher.h:41
boost::shared_ptr< Calibration_Object >
MarkerPublisher::markerARMarker_Left
visualization_msgs::Marker markerARMarker_Left
Definition: markerpublisher.h:65
MarkerPublisher::markerBottom
visualization_msgs::Marker markerBottom
Definition: markerpublisher.h:62
MarkerPublisher::publishARMarkers
void publishARMarkers(bool activeLeft, bool activeRight, const Eigen::Matrix4d topFrame)
Definition: markerpublisher.cpp:136
ROS_INFO
#define ROS_INFO(...)
MarkerPublisher::calibrationObject
boost::shared_ptr< Calibration_Object > calibrationObject
Definition: markerpublisher.h:72
NAMESPACEMARKER
#define NAMESPACEMARKER
Definition: markerpublisher.h:22
NAMESPACE
#define NAMESPACE
Definition: markerpublisher.h:20
MarkerPublisher::pubObjectPosition
ros::Publisher pubObjectPosition
Definition: markerpublisher.h:58
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MarkerPublisher::pubMarkerPosition
ros::Publisher pubMarkerPosition
Definition: markerpublisher.h:59
MarkerPublisher::MarkerPublisher
MarkerPublisher(boost::shared_ptr< Calibration_Object > calibrationObject)
Definition: markerpublisher.cpp:21
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
MarkerPublisher::markerTriangles
visualization_msgs::Marker markerTriangles
Definition: markerpublisher.h:63
NAMESPACEOBJECT
#define NAMESPACEOBJECT
Definition: markerpublisher.h:21
NAMESPACECAMERA
#define NAMESPACECAMERA
Definition: markerpublisher.h:23
MarkerPublisher::markerARMarker_Right
visualization_msgs::Marker markerARMarker_Right
Definition: markerpublisher.h:66
MarkerPublisher::pubCameraPosition
ros::Publisher pubCameraPosition
Definition: markerpublisher.h:60

asr_mild_calibration_tool
Author(s): Aumann Florian, Heller Florian, Meißner Pascal
autogenerated on Mon Dec 2 2019 03:11:43