ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
figurestubzmq.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include "zmqobjectprovider.h"
17 #include "plcommands.h"
18 #include "afigurestub.h"
19 #include <algorithm>
20 #include <string>
21 #include <stdio.h>
22 #include <iostream>
23 #include <cmath>
24 
25 namespace DLR_TS
26 {
27  namespace PlotLab
28  {
30  {
31  private:
32  int m_id;
36 
37 
38  protected:
39  void send_paint(PLComPaint::PLComPaintType comtype,std::string hashtag,std::string options, int target,int size,double* X,double* Y,double* Z=0)
40  {
41  PLComPaint p;
43  p.target = target;
44  p.comtype = comtype;
45  for(int i=0;i<p.size;i++)p.X[i] = (double)X[i];
46  for(int i=0;i<p.size;i++)p.Y[i] = (double)Y[i];
47  if(Z==0)
48  {
49  for(int i=0;i<p.size;i++)p.Z[i] = 0.01f;
50  }
51  else
52  {
53  for(int i=0;i<p.size;i++)p.Z[i] = (double)Z[i];
54  }
55  hashtag.copy(&p.hashtag[0],(std::min)(sizeof(char)*PLCom::max_size_hashtag,hashtag.size()));
56  p.hashtag[(std::min)(sizeof(char)*PLCom::max_size_hashtag-1,hashtag.size())] = '\0';
57  options.copy(&p.options[0],(std::min)(sizeof(char)*PLCom::max_size_options,options.size()));
58  p.options[(std::min)(sizeof(char)*PLCom::max_size_options-1,options.size())] = '\0';
59  //_snprintf_s(&p.hashtag[0],sizeof(char)*PLCom::max_size_hashtag,hashtag.size(),"%s",hashtag.c_str());
60  //_snprintf_s(&p.options[0],sizeof(char)*PLCom::max_size_options,options.size(),"%s",options.c_str());
61  m_paint->send(&p);
62  }
63  virtual void plotTexture(std::string hashtag, std::string url, double x,double y,double z,double psi,double w,double l) override
64  {
65  PLComPaint p;
66  p.size = 2;
67  p.target = m_id;
69  p.X[0] = x;
70  p.Y[0] = y;
71  p.Z[0] = z;
72  p.X[1] = psi;
73  p.Y[1] = w;
74  p.Z[1] = l;
75  hashtag.copy(&p.hashtag[0],(std::min)(sizeof(char) * PLCom::max_size_hashtag,hashtag.size()));
76  p.hashtag[(std::min)(sizeof(char)*PLCom::max_size_hashtag-1,hashtag.size())] = '\0';
77  url.copy(&p.options[0],(std::min)(sizeof(char)*PLCom::max_size_options,url.size()));
78  p.options[(std::min)(sizeof(char)*PLCom::max_size_options-1,url.size())] = '\0';
79  m_paint->send(&p);
80  }
81  void send_paint(PLComPaint::PLComPaintType comtype,std::string hashtag,std::string options, int target,int size,double* X,double* Y,double z)
82  {
83  PLComPaint p;
85  p.target = target;
86  p.comtype = comtype;
87  for(int i=0;i<p.size;i++)p.X[i] = (double)X[i];
88  for(int i=0;i<p.size;i++)p.Y[i] = (double)Y[i];
89  for(int i=0;i<p.size;i++)p.Z[i] = (double)z;
90  hashtag.copy(&p.hashtag[0],(std::min)(sizeof(char)*PLCom::max_size_hashtag,hashtag.size()));
91  p.hashtag[(std::min)(sizeof(char)*PLCom::max_size_hashtag-1,hashtag.size())] = '\0';
92  options.copy(&p.options[0],(std::min)(sizeof(char)*PLCom::max_size_options,options.size()));
93  p.options[(std::min)(sizeof(char)*PLCom::max_size_options-1,options.size())] = '\0';
94  //_snprintf_s(&p.hashtag[0],sizeof(char)*PLCom::max_size_hashtag,hashtag.size(),"%s",hashtag.c_str());
95  //_snprintf_s(&p.options[0],sizeof(char)*PLCom::max_size_options,options.size(),"%s",options.c_str());
96  m_paint->send(&p);
97  }
98  void send_other(PLComOther::PLComOtherType comtype,std::string hashtag,std::string options,int target)
99  {
100  PLComOther p;
101  p.target = target;
102  p.comtype = comtype;
103  hashtag.copy(&p.hashtag[0],(std::min)(sizeof(char)*PLCom::max_size_hashtag,hashtag.size()));
104  p.hashtag[(std::min)(sizeof(char)*PLCom::max_size_hashtag-1,hashtag.size())] = '\0';
105  options.copy(&p.options[0],(std::min)(sizeof(char)*PLCom::max_size_options,options.size()));
106  p.options[(std::min)(sizeof(char)*PLCom::max_size_options-1,options.size())] = '\0';
107  //_snprintf_s(&p.hashtag[0],sizeof(char)*PLCom::max_size_hashtag,hashtag.size(),"%s",hashtag.c_str());
108  //_snprintf_s(&p.options[0],sizeof(char)*PLCom::max_size_options,options.size(),"%s",options.c_str());
109  m_other->send(&p);
110 
111  }
112  void send_view(PLComView::PLComViewType viewtype, int target, double targetX, double targetY, double orientDeg, double zoom)
113  {
114  PLComView v;
115  v.targetX = targetX;
116  v.targetY = targetY;
117  v.orientDeg = orientDeg;
118  v.zoom = zoom;
119  v.viewType = viewtype;
120  v.target = target;
121  m_view->send(&v);
122  }
123  public:
124  FigureStubZMQ(zmq::context_t& context,int id)
125  {
126  m_id = id;
127  m_paint = new ZMQObjectProvider<PLComPaint>(context,"tcp://localhost:12345");
128  m_other = new ZMQObjectProvider<PLComOther>(context,"tcp://localhost:12346");
129  m_view = new ZMQObjectProvider<PLComView> (context,"tcp://localhost:12347");
130  }
131  FigureStubZMQ(zmq::context_t& context,std::string url,int id)
132  {
133  m_id = id;
134  std::cout<<"connecting to PlotLabServer "<<url<<"...\n";
135  m_paint = new ZMQObjectProvider<PLComPaint>(context,("tcp://" + url + ":12345").c_str());
136  m_other = new ZMQObjectProvider<PLComOther>(context,("tcp://" + url + ":12346").c_str());
137  m_view = new ZMQObjectProvider<PLComView> (context,("tcp://" + url + ":12347").c_str());
138  }
139  virtual ~FigureStubZMQ()
140  {
141  delete m_paint;
142  delete m_other;
143  delete m_view;
144  }
145 
146  void circlestrip(std::string hashtag, double* X,double* Y,double* R,int size,std::string options) override
147  {
148  send_paint(PLComPaint::circlestrip,hashtag,options,m_id,size,X,Y,R);
149  }
150  virtual void tristrip(std::string hashtag, double* X,double* Y,double* Z,int size,std::string options)override
151  {
152  send_paint(PLComPaint::tristrip,hashtag,options,m_id,size,X,Y,Z);
153  }
154  virtual void tristrip(std::string hashtag, double* X,double* Y,int size,std::string options)override
155  {
156  send_paint(PLComPaint::tristrip,hashtag,options,m_id,size,X,Y);
157  }
158  virtual void tristrip(std::string hashtag, double* X,double* Y,double z,int size,std::string options)override
159  {
160  send_paint(PLComPaint::tristrip,hashtag,options,m_id,size,X,Y,z);
161  }
162  virtual void patch(std::string hashtag, double* X,double* Y,double* Z,int size,std::string options) override
163  {
164  send_paint(PLComPaint::patch,hashtag,options,m_id,size,X,Y,Z);
165  }
166  virtual void patch(std::string hashtag, double* X,double* Y,int size,std::string options) override
167  {
168  send_paint(PLComPaint::patch,hashtag,options,m_id,size,X,Y);
169  }
170  virtual void patch(std::string hashtag, double* X,double* Y,double z,int size,std::string options)override
171  {
172  send_paint(PLComPaint::patch,hashtag,options,m_id,size,X,Y,z);
173  }
174  virtual void plot(std::string hashtag, double* X,double* Y,double* Z,int size,std::string options) override
175  {
176  send_paint(PLComPaint::line,hashtag,options,m_id,size,X,Y,Z);
177  }
178  virtual void plot(std::string hashtag, double* X,double* Y,double z,int size,std::string options) override
179  {
180  send_paint(PLComPaint::line,hashtag,options,m_id,size,X,Y,z);
181  }
182  virtual void plot(std::string hashtag, double* X,double* Y,int size,std::string options) override
183  {
184  send_paint(PLComPaint::line,hashtag,options,m_id,size,X,Y);
185  }
186  virtual void append(std::string hashtag, double* X,double* Y,int size,std::string options) override
187  {
188  send_paint(PLComPaint::append,hashtag,options,m_id,size,X,Y);
189  }
190  virtual void append(std::string hashtag, double* X,double* Y,double* Z,int size,std::string options) override
191  {
192  send_paint(PLComPaint::append,hashtag,options,m_id,size,X,Y,Z);
193  }
197  virtual void plotVehicle(std::string hashtag,double X,double Y,double Z,double PSI,double delta,double a,double b,double width,double forward,double backward,std::string options) override
198  {
199  static const int size = 12;
200  double x[size];
201  double y[size];
202  double tx[size];
203  double ty[size];
204  double tz[size];
205  double w2 = width/2.0f;
206  //lo and behold, das haus vom nikolaus
207  x[0] = -backward;//rear bumper
208  x[1] = 0;//rear axle
209  x[2] = 0;
210  x[3] = 0;
211  x[4] = a+b;//front axle
212  x[5] = a+b;
213  x[6] = a+b+forward;//tip of arrow
214  x[7] = a+b;//front axle
215  x[8] = a+b+forward;//front bumper
216  x[9] = a+b+forward;//front bumper
217  x[10] = -backward;//rear bumper
218  x[11] = -backward;
219 
220  y[0] = -w2;
221  y[1] = -w2;
222  y[2] = w2;//rear axle
223  y[3] = -w2;
224  y[4] = -w2;
225  y[5] = w2;//front axle
226  y[6] = 0;//tip of arrrow
227  y[7] = -w2;
228  y[8] = -w2;
229  y[9] = w2;
230  y[10] = w2;
231  y[11] = -w2;
232 
233  for(int i=0;i<size;i++)
234  {
235  tx[i] = std::cos(PSI)*x[i] -std::sin(PSI)*y[i] + X;
236  ty[i] = std::sin(PSI)*x[i] +std::cos(PSI)*y[i] + Y;
237  tz[i] = Z;
238  }
239  if(options.find("FillColor")!=std::string::npos) // for obstacle instead of vehicle
240  {
241  send_paint(PLComPaint::patch,hashtag,options,m_id,size,tx,ty,tz);
242  return;
243  }
244  send_paint(PLComPaint::line,hashtag,options,m_id,size,tx,ty,tz);
245  //double target[2];
246  //double orientZoom[2];
247  //target[0] = X;
248  //target[1] = Y;
249  //orientZoom[0] = (360.0 * PSI) / (2.0 * 3.14159);
250  //orientZoom[1] = 10.0;
251  //send_paint(PLComPaint::view,hashtag,options,m_id,2,target,orientZoom);
252  //send_view(DLR_TS::PlotLab::PLComView::PLComViewType::ViewPosOrientation,m_id,X,Y,360.0*(PSI / (2.0 *3.14159))-90.0,100.0);
253  }
254  virtual void plotText(std::string hashtag,double x,double y,std::string text) override
255  {
256  send_paint(PLComPaint::text,hashtag,text,m_id,1,&x,&y,1.0f);
257  }
258  virtual void plotVehicle(std::string hashtag,double X,double Y) override
259  {
260  plotVehicle(hashtag,X,Y,0,0,0,0,0,0,0,0,"");
261  }
262  virtual void clear() override
263  {
265  }
266  virtual void show() override
267  {
269  }
270  virtual void hide() override
271  {
273  }
274  virtual void showAxis() override
275  {
277  }
278  virtual void hideAxis() override
279  {
281  }
282  virtual void showGrid() override
283  {
285  }
286  virtual void hideGrid() override
287  {
289  }
290  virtual void setFixAxis(double x0,double y0,double z0,double x1,double y1,double z1)override
291  {
292  PLComView v;
293  v.targetX = 0.5*(x0+x1);
294  v.targetY = 0.5*(y0+y1);
295  v.targetZ = 0.5*(z0+z1);
296  v.fix_xwidth = std::abs(x1-x0);
297  v.fix_ywidth = std::abs(y1-y0);
298  v.fix_zwidth = std::abs(z1-z0);
300  v.target = m_id;
301  m_view->send(&v);
302  }
303  virtual void setFixAxisOff()override
304  {
305  PLComView v;
307  v.target = m_id;
308  m_view->send(&v);
309  }
310  virtual void erase(std::string hashtag) override
311  {
312  send_other(PLComOther::erase,hashtag,"",m_id);
313  }
314  virtual void erase_similar(std::string hashtag) override
315  {
317  }
318  virtual void setXLabel(std::string value) override
319  {
321  }
322  virtual void setYLabel(std::string value) override
323  {
325  }
326  virtual void setZLabel(std::string value) override
327  {
329  }
330  virtual void setName(std::string value) override
331  {
332  send_other(PLComOther::name,"",value,m_id);
333  }
334  virtual void setTitle(std::string value) override
335  {
337  }
338  virtual void setViewPortOffsets(double targetX, double targetY, double orientDeg, double zoom) override
339  {
340  send_view(DLR_TS::PlotLab::PLComView::ViewPosOrientZoom,m_id,targetX,targetY,orientDeg,zoom);
341  }
342  virtual void setViewPortOffsets(double targetX, double targetY, double orientDeg) override
343  {
344  send_view(DLR_TS::PlotLab::PLComView::ViewPosOrientation,m_id,targetX,targetY,orientDeg,0.0);
345  }
346  virtual void setViewPortOffsets(double targetX, double targetY) override
347  {
348  send_view(DLR_TS::PlotLab::PLComView::ViewPosOnly,m_id,targetX,targetY,0.0,0.0);
349  }
350  virtual void setViewPortOffsets(bool disable) override
351  {
352  if (disable)
353  {
355  }
356  }
357 
358  };
359  }
360 }
Definition: afigurestub.h:24
Definition: figurestubzmq.h:30
virtual void patch(std::string hashtag, double *X, double *Y, double z, int size, std::string options) override
Definition: figurestubzmq.h:170
virtual void hideGrid() override
Definition: figurestubzmq.h:286
virtual void plotVehicle(std::string hashtag, double X, double Y) override
Definition: figurestubzmq.h:258
virtual void plot(std::string hashtag, double *X, double *Y, int size, std::string options) override
Definition: figurestubzmq.h:182
virtual void plot(std::string hashtag, double *X, double *Y, double z, int size, std::string options) override
Definition: figurestubzmq.h:178
virtual void showAxis() override
Definition: figurestubzmq.h:274
virtual void setViewPortOffsets(bool disable) override
Definition: figurestubzmq.h:350
ZMQObjectProvider< PLComOther > * m_other
Definition: figurestubzmq.h:34
virtual void show() override
Definition: figurestubzmq.h:266
virtual void setViewPortOffsets(double targetX, double targetY) override
Definition: figurestubzmq.h:346
virtual void tristrip(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options) override
Definition: figurestubzmq.h:150
virtual ~FigureStubZMQ()
Definition: figurestubzmq.h:139
virtual void plotVehicle(std::string hashtag, double X, double Y, double Z, double PSI, double delta, double a, double b, double width, double forward, double backward, std::string options) override
Definition: figurestubzmq.h:197
virtual void setFixAxisOff() override
Definition: figurestubzmq.h:303
virtual void plotTexture(std::string hashtag, std::string url, double x, double y, double z, double psi, double w, double l) override
Definition: figurestubzmq.h:63
virtual void append(std::string hashtag, double *X, double *Y, int size, std::string options) override
Definition: figurestubzmq.h:186
virtual void plot(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options) override
Definition: figurestubzmq.h:174
virtual void hide() override
Definition: figurestubzmq.h:270
virtual void setViewPortOffsets(double targetX, double targetY, double orientDeg, double zoom) override
Definition: figurestubzmq.h:338
virtual void append(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options) override
Definition: figurestubzmq.h:190
ZMQObjectProvider< PLComView > * m_view
Definition: figurestubzmq.h:35
virtual void setYLabel(std::string value) override
Definition: figurestubzmq.h:322
virtual void setTitle(std::string value) override
Definition: figurestubzmq.h:334
void circlestrip(std::string hashtag, double *X, double *Y, double *R, int size, std::string options) override
Definition: figurestubzmq.h:146
ZMQObjectProvider< PLComPaint > * m_paint
Definition: figurestubzmq.h:33
virtual void hideAxis() override
Definition: figurestubzmq.h:278
virtual void clear() override
Definition: figurestubzmq.h:262
virtual void patch(std::string hashtag, double *X, double *Y, double *Z, int size, std::string options) override
Definition: figurestubzmq.h:162
void send_paint(PLComPaint::PLComPaintType comtype, std::string hashtag, std::string options, int target, int size, double *X, double *Y, double *Z=0)
Definition: figurestubzmq.h:39
virtual void setViewPortOffsets(double targetX, double targetY, double orientDeg) override
Definition: figurestubzmq.h:342
virtual void showGrid() override
Definition: figurestubzmq.h:282
virtual void erase(std::string hashtag) override
Definition: figurestubzmq.h:310
void send_paint(PLComPaint::PLComPaintType comtype, std::string hashtag, std::string options, int target, int size, double *X, double *Y, double z)
Definition: figurestubzmq.h:81
virtual void setName(std::string value) override
Definition: figurestubzmq.h:330
virtual void setFixAxis(double x0, double y0, double z0, double x1, double y1, double z1) override
Definition: figurestubzmq.h:290
virtual void patch(std::string hashtag, double *X, double *Y, int size, std::string options) override
Definition: figurestubzmq.h:166
void send_other(PLComOther::PLComOtherType comtype, std::string hashtag, std::string options, int target)
Definition: figurestubzmq.h:98
virtual void setXLabel(std::string value) override
Definition: figurestubzmq.h:318
int m_id
Definition: figurestubzmq.h:32
FigureStubZMQ(zmq::context_t &context, std::string url, int id)
Definition: figurestubzmq.h:131
virtual void erase_similar(std::string hashtag) override
Definition: figurestubzmq.h:314
FigureStubZMQ(zmq::context_t &context, int id)
Definition: figurestubzmq.h:124
void send_view(PLComView::PLComViewType viewtype, int target, double targetX, double targetY, double orientDeg, double zoom)
Definition: figurestubzmq.h:112
virtual void tristrip(std::string hashtag, double *X, double *Y, double z, int size, std::string options) override
Definition: figurestubzmq.h:158
virtual void plotText(std::string hashtag, double x, double y, std::string text) override
Definition: figurestubzmq.h:254
virtual void tristrip(std::string hashtag, double *X, double *Y, int size, std::string options) override
Definition: figurestubzmq.h:154
virtual void setZLabel(std::string value) override
Definition: figurestubzmq.h:326
Definition: zmqobjectprovider.h:21
static const int max_size_hashtag
Definition: plcommands.h:26
static const int max_size_points
Definition: plcommands.h:24
static const int max_size_options
Definition: plcommands.h:25
Definition: afigurestub.h:20
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
z0
Definition: adore_set_goal.py:27
y
Definition: adore_set_goal.py:31
z
Definition: adore_set_goal.py:32
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
z1
Definition: adore_set_pose.py:30
w
Definition: adore_set_pose.py:40
Definition: plcommands.h:71
int target
Definition: plcommands.h:74
char options[PLCom::max_size_options]
Definition: plcommands.h:72
enum DLR_TS::PlotLab::PLComOther::PLComOtherType comtype
PLComOtherType
Definition: plcommands.h:76
@ hide
Definition: plcommands.h:79
@ title
Definition: plcommands.h:89
@ zlabel
Definition: plcommands.h:88
@ showAxis
Definition: plcommands.h:82
@ xlabel
Definition: plcommands.h:86
@ name
Definition: plcommands.h:90
@ hideAxis
Definition: plcommands.h:83
@ showGrid
Definition: plcommands.h:84
@ ylabel
Definition: plcommands.h:87
@ erase_similar
Definition: plcommands.h:81
@ show
Definition: plcommands.h:78
@ erase
Definition: plcommands.h:80
@ clear
Definition: plcommands.h:77
@ hideGrid
Definition: plcommands.h:85
char hashtag[PLCom::max_size_hashtag]
Definition: plcommands.h:73
Definition: plcommands.h:51
double Y[PLCom::max_size_points]
Definition: plcommands.h:53
PLComPaintType
Definition: plcommands.h:60
@ circlestrip
Definition: plcommands.h:67
@ patch
Definition: plcommands.h:62
@ texture
Definition: plcommands.h:65
@ text
Definition: plcommands.h:64
@ line
Definition: plcommands.h:61
@ append
Definition: plcommands.h:63
@ tristrip
Definition: plcommands.h:66
enum DLR_TS::PlotLab::PLComPaint::PLComPaintType comtype
double X[PLCom::max_size_points]
Definition: plcommands.h:52
char options[PLCom::max_size_options]
Definition: plcommands.h:55
double Z[PLCom::max_size_points]
Definition: plcommands.h:54
int size
Definition: plcommands.h:57
int target
Definition: plcommands.h:58
char hashtag[PLCom::max_size_hashtag]
Definition: plcommands.h:56
Definition: plcommands.h:29
double fix_zwidth
Definition: plcommands.h:35
double orientDeg
Definition: plcommands.h:36
enum DLR_TS::PlotLab::PLComView::PLComViewType viewType
int target
Definition: plcommands.h:48
double targetX
Definition: plcommands.h:30
PLComViewType
Definition: plcommands.h:39
@ disable
Definition: plcommands.h:46
@ AutoAxis
Definition: plcommands.h:45
@ ViewPosOnly
Definition: plcommands.h:40
@ ViewPosOrientZoom
Definition: plcommands.h:42
@ ViewPosOrientation
Definition: plcommands.h:41
@ FixedAxis
Definition: plcommands.h:44
double fix_xwidth
Definition: plcommands.h:33
double fix_ywidth
Definition: plcommands.h:34
double targetY
Definition: plcommands.h:31
double zoom
Definition: plcommands.h:37
double targetZ
Definition: plcommands.h:32