RadarCanvas.cpp
Go to the documentation of this file.
1 
5 #include <sstream>
6 #include <cmath>
7 
9 
10 #include <QPainter>
11 #include <QRectF>
12 #include <QString>
13 #include <QVector>
14 #include <QTransform>
15 #include <QResizeEvent>
16 #include <QPolygon>
17 #include <QCheckBox>
18 
19 // ROS Utilities
20 #include <ros/package.h>
21 
22 // ROS messages
23 #include <hri_msgs/IdsList.h>
24 
25 #include "ui_radar_tabs.h"
26 
27 std::vector<double> SPECIAL_ANGLES = {0, 30, 60, 90, 120, 150, 180};
28 double SVG_SIZE_RATIO = 1.4857;
29 double SVG_PERSON_WIDTH = 70;
30 
31 namespace rqt_human_radar {
32 
33 RadarCanvas::RadarCanvas(QWidget *parent, Ui::RadarTabs* ui) :
34  QWidget(parent){
35  timer_ = new QTimer(this);
36  connect(timer_, &QTimer::timeout, this, QOverload<>::of(&RadarCanvas::update));
37  timer_->start(100);
38 
39  this->ui_ = ui;
40 
41  connect(ui_->ppmSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &RadarCanvas::updatePixelPerMeter);
42  connect(ui_->idCheckbox, QOverload<int>::of(&QCheckBox::stateChanged), this, &RadarCanvas::showId);
43 
44  // Retrieving robot and person icons
45  package_ = ros::package::getPath("rqt_human_radar");
46  robotImageFile_ = package_ + "/img/ARI_icon.png";
47  personSvgFile_ = package_ + "/img/adult_standing_disengaging.svg";
48  robotImageFound = robotImage_.load(QString::fromStdString(robotImageFile_));
49 
50  svgRendererInitialized_ = svgRenderer_.load(QString::fromStdString(personSvgFile_));
51 
52  if (!robotImageFound){
53  ROS_WARN("Robot icon not found");
54  }
55 
57  ROS_WARN("Person icon not found");
58  }
59 
60  // xOffset is meant to be fixed, while yOffset depends
61  // on the window size
62  xOffset_ = 50;
63  yOffset_ = parent->size().height()/2;
64 
65  // Initial value, it is possible to modify it through settings
66  pixelPerMeter_ = 300;
67 
68  // Color definition
69  QColor lightGrey(232, 232, 233);
70  QColor lighterGrey(237, 238, 239);
71  QColor midGrey(175, 175, 175);
72 
73  // Brush definition. Used to paint the background
74  evenBrush_ = QBrush(lightGrey);
75  oddBrush_ = QBrush(lighterGrey);
76  rangePen_ = QPen(midGrey);
77 
78  // Initialization of a constant versor
79  // which is later used to orient person icons.
80  versor_.vector.x = 1.0;
81  versor_.vector.y = 0.0;
82  versor_.vector.z = 0.0;
83 
84  // Computation of the number of arcs to draw
85  double distanceFromTopRightCorner =
86  std::sqrt(std::pow((ui_->tab->size().width() - xOffset_), 2) + std::pow(yOffset_, 2));
87  arcsToDraw_ = std::ceil(distanceFromTopRightCorner/pixelPerMeter_);
88 
89  // Activating mouse events
90  this->setMouseTracking(true);
91 
92  // Setting to "" the name of the hovered person
93  idClicked_ = "";
94 
95  // Setting the reference frame to "camera_link"
96  // To be changed in following versions
97  referenceFrame_ = "camera_link";
98 
99  // Reading the value representing whether we should display people ids or not
100  showIdValue_ = ui_->idCheckbox->checkState();
101 
102  update();
103 }
104 
106 }
107 
108 void RadarCanvas::paintEvent(QPaintEvent *event){
109  QPainter painter(this);
110  painter.setRenderHint(QPainter::Antialiasing);
111  font_ = painter.font();
112 
113  versor_.header.stamp = ros::Time(0);
114 
115  // Ranges painting process
116  painter.setPen(QPen(Qt::transparent));
117 
118  for(int arcN = arcsToDraw_; arcN > 0; arcN--){
119 
120  // Setting circles and text parameters and support structures
121  double circleRange = pixelPerMeter_*arcN;
122  double circleRectOriginX = xOffset_ - circleRange;
123  double circleRectOriginY = yOffset_ - circleRange;
124  double circleRectWidth = 2*circleRange;
125  double circleRectHeight = 2*circleRange;
126 
127  QRectF circleRect(
128  circleRectOriginX, circleRectOriginY, circleRectWidth, circleRectHeight);
129  QPointF textPoint(
130  xOffset_ + circleRange + 5, yOffset_ + (font_.pointSize()/2));
131  QString rangeDescription =
132  QString::fromStdString(std::to_string(arcN)+"m");
133 
134  // Different brush, whether it is an odd or even circle
135  if ((arcN%2) == 1){
136  painter.setBrush(oddBrush_);
137  }
138  else{
139  painter.setBrush(evenBrush_);
140  }
141 
142  painter.drawEllipse(circleRect);
143  painter.setPen(rangePen_);
144  painter.translate(textPoint);
145  painter.rotate(90);
146  painter.drawText(QPoint(0, 0), rangeDescription);
147  painter.rotate(-90);
148  painter.translate(-textPoint);
149 
150  // Printing angle values
151  double distToPrint = circleRange - (0.5*pixelPerMeter_);
152  for(double angle: SPECIAL_ANGLES){
153  if (angle == 90)
154  continue;
155  angle -= 90;
156  angle *= (M_PI/180);
157  double xToPrint = distToPrint*std::cos(angle) + xOffset_;
158  double yToPrint = distToPrint*std::sin(angle) + yOffset_;
159  if (inScreen(xToPrint, yToPrint)){
160  painter.translate(QPointF(xToPrint, yToPrint));
161  painter.rotate(angle*180/M_PI);
162  painter.drawText(
163  QPointF(0, -2),
164  QString::fromStdString(
165  std::to_string(int(std::round(angle*180/M_PI))*-1)+"°"));
166  painter.rotate(-angle*180/M_PI);
167  painter.translate(-QPointF(xToPrint, yToPrint));
168  }
169  }
170 
171  painter.setPen(QPen(Qt::transparent));
172  }
173 
174  // Writing special angles values
175  painter.setPen(rangePen_);
176 
177  for(double& angle: SPECIAL_ANGLES){
178  // Handling the process taking into account
179  // the left handed Qt reference system
180  double m = std::tan(angle/180*M_PI);
181  double candidateYMaxWidth = (this->size().width() - xOffset_)/m + yOffset_;
182  if(angle < 90){
183  double candidateXMaxHeight =
184  m*(this->size().height() - yOffset_) + xOffset_;
185  if(candidateYMaxWidth > this->size().height())
186  painter.drawLine(
187  xOffset_, yOffset_, candidateXMaxHeight, this->size().height());
188  else
189  painter.drawLine(
190  xOffset_, yOffset_, this->size().width(), candidateYMaxWidth);
191  }
192  else if(angle > 90){
193  double candidateXMaxHeight = -m*yOffset_ + xOffset_;
194  if(candidateYMaxWidth < 0)
195  painter.drawLine(xOffset_, yOffset_, candidateXMaxHeight, 0);
196  else
197  painter.drawLine(
198  xOffset_, yOffset_, this->size().width(), candidateYMaxWidth);
199  }else{
200  painter.drawLine(xOffset_, yOffset_, this->size().width(), yOffset_);
201  }
202  }
203 
204  painter.setBrush(QBrush(Qt::transparent));
205  painter.setPen(QPen(Qt::black));
206 
207  // Drawing people. Using body_<body_id> frames
208  // to define people's position and orientation
209  auto bodies = hriListener_.getBodies();
210  peoplePosition_.clear();
211  for(auto& body: bodies){
212  geometry_msgs::Vector3Stamped rotatedVersor;
213 
214  std::string id = body.first;
215  std::string bodyFrame = "body_" + id;
216  versor_.header.frame_id = bodyFrame;
217  tf::StampedTransform bodyTrans;
218 
219  try{
221  referenceFrame_, bodyFrame, ros::Time(0), bodyTrans);
223  double distance =
224  std::sqrt(std::pow(bodyTrans.getOrigin().x(), 2)
225  + std::pow(bodyTrans.getOrigin().y(), 2));
226  double theta =
227  std::atan2(-(rotatedVersor.vector.y), -(rotatedVersor.vector.x));
228  theta += M_PI/2;
229 
230  // Left-handed rotation of the rectangle containing the person's icon
231  // to draw
232 
233  // The rectangle used here is initially managed as it was
234  // centered in (0, 0) and subsequently translated.
235  // When referencing to "width" and "height", it is actually
236  // half of the width and height of the rectangle containing
237  // the person's icon.
238 
239  double rotatedWidthX = (SVG_PERSON_WIDTH/2*std::cos(theta));
240  double rotatedWidthY = (SVG_PERSON_WIDTH/2*-std::sin(theta));
241 
242  double rotatedHeightX =
243  (SVG_PERSON_WIDTH/SVG_SIZE_RATIO/2*std::sin(theta));
244  double rotatedHeightY =
245  (SVG_PERSON_WIDTH/SVG_SIZE_RATIO/2*std::cos(theta));
246 
247  // Computing vertices of the rotated rectangle
248 
249  double personRectTopLeftX =
250  xOffset_
251  + (bodyTrans.getOrigin().x()*pixelPerMeter_)
252  - rotatedHeightX
253  - rotatedWidthX;
254  double personRectTopLeftY =
255  yOffset_
256  - (bodyTrans.getOrigin().y()*pixelPerMeter_)
257  - rotatedHeightY
258  - rotatedWidthY;
259 
260  double personRectBottomRightX =
261  xOffset_
262  + (bodyTrans.getOrigin().x()*pixelPerMeter_)
263  + rotatedHeightX
264  + rotatedWidthX;
265  double personRectBottomRightY =
266  yOffset_
267  - (bodyTrans.getOrigin().y()*pixelPerMeter_)
268  + rotatedHeightY
269  + rotatedWidthY;
270 
271  double personRectBottomLeftX =
272  xOffset_
273  + (bodyTrans.getOrigin().x()*pixelPerMeter_)
274  + rotatedHeightX
275  - rotatedWidthX;
276  double personRectBottomLeftY =
277  yOffset_
278  - (bodyTrans.getOrigin().y()*pixelPerMeter_)
279  + rotatedHeightY
280  - rotatedWidthY;
281 
282  double personRectTopRightX =
283  xOffset_
284  + (bodyTrans.getOrigin().x()*pixelPerMeter_)
285  - rotatedHeightX
286  + rotatedWidthX;
287  double personRectTopRightY =
288  yOffset_
289  - (bodyTrans.getOrigin().y()*pixelPerMeter_)
290  - rotatedHeightY
291  + rotatedWidthY;
292 
293  QPointF topLeftCorner(personRectTopLeftX, personRectTopLeftY);
294  QPointF bottomRightCorner(personRectBottomRightX, personRectBottomRightY);
295  QPointF bottomLeftCorner(personRectBottomLeftX, personRectBottomLeftY);
296  QPointF topRightCorner(personRectTopRightX, personRectTopRightY);
297 
298  // Image translation and rotation (via QPainter methods)
299 
300  painter.translate(topLeftCorner);
301  painter.rotate(-(theta*180)/M_PI);
302  svgRenderer_.render(
303  &painter, QRectF(QPointF(0, 0), QPointF(SVG_PERSON_WIDTH, SVG_PERSON_WIDTH/SVG_SIZE_RATIO)));
304  painter.rotate((theta*180)/M_PI);
305  painter.translate(-topLeftCorner);
306 
307  // Storing the person's containing polygon
308  // A rectangle object can not represent rotated rectangles
309 
310  QVector<QPoint> points;
311  points.append(topLeftCorner.toPoint());
312  points.append(bottomLeftCorner.toPoint());
313  points.append(bottomRightCorner.toPoint());
314  points.append(topRightCorner.toPoint());
315 
316  peoplePosition_.insert(
317  std::pair<std::string, QPolygon>(id, QPolygon(points)));
318 
319  // Showing people ID when option selected in
320  // radar settings
321  // Showing people distance when clicking on them
322  QString identificator = QString::fromStdString(id);
323  if(showIdValue_ == Qt::Checked)
324  painter.drawText(bottomRightCorner, identificator);
325  if(idClicked_ == id){
326  std::ostringstream distanceStream;
327  distanceStream << std::fixed << std::setprecision(2) << distance;
328  std::string distanceString = distanceStream.str();
329  QString distanceInfo =
330  QString::fromStdString("Distance: " + distanceString);
331  painter.drawText(bottomRightCorner, identificator);
332  painter.drawText(bottomLeftCorner, distanceInfo);
333  }
334  }
335  catch(tf::TransformException ex){
336  ROS_WARN("%s", ex.what());
337  }
338  }
339 
340  painter.drawImage(
341  QRectF(
342  QPointF(xOffset_-50, yOffset_-50), QPointF(xOffset_+50, yOffset_+50)), robotImage_);
343 }
344 
345 void RadarCanvas::resizeEvent(QResizeEvent *event){
346  yOffset_ = ui_->tab->size().height()/2;
347 
349 }
350 
352  pixelPerMeter_ = ui_->ppmSpinBox->value();
353  updateArcsToDraw();
354  update();
355 }
356 
358  showIdValue_ = ui_->idCheckbox->checkState();
359  update();
360 }
361 
362 void RadarCanvas::mousePressEvent(QMouseEvent *event){
363  for(auto& elem: peoplePosition_){
364  if(elem.second.containsPoint(QPoint(event->x(), event->y()), Qt::OddEvenFill)){
365  idClicked_ = (idClicked_ != elem.first)?elem.first:"";
366  return; // No more than one clicked person at a time
367  }
368  }
369 }
370 
371 bool RadarCanvas::inScreen(double& x, double& y) const{
372  return (x > 0) && (y > 0) && (x < this->size().width()) && (y < this->size().height());
373 }
374 
376  double distanceFromTopRightCorner =
377  std::sqrt(std::pow((ui_->tab->size().width() - xOffset_), 2) + std::pow(yOffset_, 2));
378  arcsToDraw_ = std::ceil(distanceFromTopRightCorner/pixelPerMeter_);
379 }
380 
381 } /* namespace */
rqt_human_radar::RadarCanvas::hriListener_
hri::HRIListener hriListener_
Definition: RadarCanvas.hpp:105
rqt_human_radar::RadarCanvas::package_
std::string package_
Definition: RadarCanvas.hpp:117
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
SPECIAL_ANGLES
std::vector< double > SPECIAL_ANGLES
Definition: RadarCanvas.cpp:27
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
rqt_human_radar::RadarCanvas::robotImageFound
bool robotImageFound
Definition: RadarCanvas.hpp:116
rqt_human_radar::RadarCanvas::peoplePosition_
std::map< std::string, QPolygon > peoplePosition_
Definition: RadarCanvas.hpp:118
rqt_human_radar::RadarCanvas::mousePressEvent
void mousePressEvent(QMouseEvent *event) override
overriding the mousePressEvent virtual function
Definition: RadarCanvas.cpp:362
rqt_human_radar::RadarCanvas::tfListener_
tf::TransformListener tfListener_
Definition: RadarCanvas.hpp:106
rqt_human_radar::RadarCanvas::ui_
Ui::RadarTabs * ui_
Definition: RadarCanvas.hpp:135
tf::TransformListener::transformVector
void transformVector(const std::string &target_frame, const geometry_msgs::Vector3Stamped &stamped_in, geometry_msgs::Vector3Stamped &stamped_out) const
rqt_human_radar::RadarCanvas::personSvgFile_
std::string personSvgFile_
Definition: RadarCanvas.hpp:117
rqt_human_radar::RadarCanvas::timer_
QTimer * timer_
Definition: RadarCanvas.hpp:103
rqt_human_radar::RadarCanvas::font_
QFont font_
Definition: RadarCanvas.hpp:112
body
body
rqt_human_radar::RadarCanvas::updatePixelPerMeter
void updatePixelPerMeter()
Updating the pixel-per-meter value.
Definition: RadarCanvas.cpp:351
tf::StampedTransform
rqt_human_radar::RadarCanvas::idClicked_
std::string idClicked_
Definition: RadarCanvas.hpp:138
rqt_human_radar::RadarCanvas::paintEvent
void paintEvent(QPaintEvent *event) override
overriding the paintEvent virtual function.
Definition: RadarCanvas.cpp:108
hri::HRIListener::getBodies
std::map< ID, BodyWeakConstPtr > getBodies() const
rqt_human_radar::RadarCanvas::showIdValue_
Qt::CheckState showIdValue_
Definition: RadarCanvas.hpp:128
rqt_human_radar::RadarCanvas::updateArcsToDraw
void updateArcsToDraw()
updates the number of arcs to draw.
Definition: RadarCanvas.cpp:375
rqt_human_radar::RadarCanvas::arcsToDraw_
int arcsToDraw_
Definition: RadarCanvas.hpp:126
rqt_human_radar::RadarCanvas::resizeEvent
void resizeEvent(QResizeEvent *event) override
overriding the resizeEvent virtual function.
Definition: RadarCanvas.cpp:345
rqt_human_radar::RadarCanvas::RadarCanvas
RadarCanvas(QWidget *parent, Ui::RadarTabs *ui)
Constructor.
Definition: RadarCanvas.cpp:33
rqt_human_radar::RadarCanvas::~RadarCanvas
virtual ~RadarCanvas()
Destructor.
Definition: RadarCanvas.cpp:105
update
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
SVG_SIZE_RATIO
double SVG_SIZE_RATIO
Definition: RadarCanvas.cpp:28
rqt_human_radar::RadarCanvas::rangePen_
QPen rangePen_
Definition: RadarCanvas.hpp:110
rqt_human_radar::RadarCanvas::versor_
geometry_msgs::Vector3Stamped versor_
Definition: RadarCanvas.hpp:107
ROS_WARN
#define ROS_WARN(...)
rqt_human_radar::RadarCanvas::robotImageFile_
std::string robotImageFile_
Definition: RadarCanvas.hpp:117
rqt_human_radar::RadarCanvas::svgRenderer_
QSvgRenderer svgRenderer_
Definition: RadarCanvas.hpp:121
rqt_human_radar::RadarCanvas::robotImage_
QImage robotImage_
Definition: RadarCanvas.hpp:115
rqt_human_radar::RadarCanvas::evenBrush_
QBrush evenBrush_
Definition: RadarCanvas.hpp:111
package.h
rqt_human_radar::RadarCanvas::pixelPerMeter_
int pixelPerMeter_
Definition: RadarCanvas.hpp:124
ros::Time
rqt_human_radar
Definition: Radar.hpp:13
rqt_human_radar::RadarCanvas::xOffset_
double xOffset_
Definition: RadarCanvas.hpp:131
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const
rqt_human_radar::RadarCanvas::referenceFrame_
std::string referenceFrame_
Definition: RadarCanvas.hpp:141
tf2::TransformException
rqt_human_radar::RadarCanvas::svgRendererInitialized_
bool svgRendererInitialized_
Definition: RadarCanvas.hpp:122
rqt_human_radar::RadarCanvas::showId
void showId()
Reading the user preference about showing or not people ID.
Definition: RadarCanvas.cpp:357
rqt_human_radar::RadarCanvas::oddBrush_
QBrush oddBrush_
Definition: RadarCanvas.hpp:111
SVG_PERSON_WIDTH
double SVG_PERSON_WIDTH
Definition: RadarCanvas.cpp:29
rqt_human_radar::RadarCanvas::yOffset_
double yOffset_
Definition: RadarCanvas.hpp:131
RadarCanvas.hpp
Defines the class for the RadarCanvas widget.
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
rqt_human_radar::RadarCanvas::inScreen
bool inScreen(double &x, double &y) const
returns whether or not a point is inside the canvas.
Definition: RadarCanvas.cpp:371


rqt_human_radar
Author(s): Lorenzo Ferrini
autogenerated on Fri Dec 16 2022 03:43:38