pictogram_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK Lab
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK Lab nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include "pictogram_display.h"
36 #include <QPainter>
37 #include <QFontDatabase>
38 #include <ros/package.h>
39 
41 // read Entypo fonts
42 // http://mempko.wordpress.com/2014/11/28/using-entypo-fonts-as-icons-in-your-qt-application/
44 
45 #include "Entypo.dat"
46 #include "Entypo_Social.dat"
47 #include "fontawesome.dat"
48 #include "pictogram_font_mapping.h"
49 
50 namespace jsk_rviz_plugins
51 {
52 
53  int addFont(unsigned char* data, unsigned int data_len)
54  {
55  // register font
56  QByteArray entypo =
57  QByteArray::fromRawData(
58  reinterpret_cast<const char*>(data), data_len);
59  int id =
60  QFontDatabase::addApplicationFontFromData(entypo);
61  if (id == -1) {
62  ROS_WARN("failed to load font");
63  }
64  return id;
65  }
66 
67  bool epsEqual(double a, double b)
68  {
69  return (std::abs(a - b) < 0.01);
70  }
71 
72  bool isCharacterSupported(std::string character)
73  {
74  return ((entypo_social_character_map.find(character)
75  != entypo_social_character_map.end()) ||
76  (entypo_character_map.find(character)
77  != entypo_character_map.end()) ||
78  (fontawesome_character_map.find(character)
79  != fontawesome_character_map.end()));
80  }
81 
82  QFont getFont(std::string character)
83  {
84  if (entypo_social_character_map.find(character)
85  != entypo_social_character_map.end()) {
86  return QFont("Entypo Social");
87  }
88  else if (entypo_character_map.find(character)
89  != entypo_character_map.end()) {
90  return QFont("Entypo");
91  }
92  else {
93  return QFont("Font Awesome 5 Free");
94  }
95  }
96 
97  QString lookupPictogramText(std::string character)
98  {
99  if (entypo_social_character_map.find(character)
100  != entypo_social_character_map.end()) {
102  }
103  else if (entypo_character_map.find(character)
104  != entypo_character_map.end()){
106  }
107  else {
109  }
110  }
111 
112  bool isEntypo(std::string text) {
113  return ((entypo_social_character_map.find(text)
114  != entypo_social_character_map.end()) ||
115  (entypo_character_map.find(text)
116  != entypo_character_map.end()));
117  }
118 
119  bool isFontAwesome(std::string text) {
120  return (fontawesome_character_map.find(text)
121  != fontawesome_character_map.end());
122  }
123 
124 
125 
126  PictogramObject::PictogramObject(Ogre::SceneManager* manager,
127  Ogre::SceneNode* parent,
128  double size):
129  FacingTexturedObject(manager, parent, size),
130  need_to_update_(false),
131  action_(jsk_rviz_plugins::Pictogram::ADD)
132  {
133  square_object_->setPolygonType(SquareObject::SQUARE);
134  square_object_->rebuildPolygon();
135 
136  // for (std::map<std::string, QString>::iterator it = fontawesome_character_map.begin();
137  // it != fontawesome_character_map.end();
138  // ++it) {
139  // ROS_INFO("%s", it->first.c_str());
140  // }
141  }
142 
143  void PictogramObject::setEnable(bool enable)
144  {
145  if (enable && !enable_) {
146  need_to_update_ = true;
147  }
149  }
150 
152  {
154  }
155 
157  {
158  if (size_ != size) {
159  need_to_update_ = true;
161  }
162  }
163 
165  {
166  speed_ = speed;
167  }
168 
169  void PictogramObject::setPose(const geometry_msgs::Pose& pose,
170  const std::string& frame_id)
171  {
172  pose_ = pose;
174  }
175 
177  {
178  context_ = context;
179  }
180 
182  {
183  mode_ = mode;
184  }
185 
186  void PictogramObject::setTTL(double ttl)
187  {
188  ttl_ = ttl;
189  }
190 
192  {
193  action_ = type;
194  if (action_ == jsk_rviz_plugins::Pictogram::DELETE) {
195  setEnable(false);
196  }
197  else{
198  start();
199  }
200  }
201 
202  void PictogramObject::updatePose(float wall_dt)
203  {
204  Ogre::Vector3 position;
205  Ogre::Quaternion quaternion;
206  std_msgs::Header header;
207  header.frame_id = frame_id_;
208  if(!context_->getFrameManager()->transform(header,
209  pose_,
210  position,
211  quaternion)) {
212  ROS_ERROR( "Error transforming pose from frame '%s'",
213  frame_id_.c_str());
214  return;
215  }
216 
217  if (action_ == jsk_rviz_plugins::Pictogram::ADD) {
218  setPosition(position);
219  setOrientation(quaternion);
220  }
221  else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Z ||
222  action_ == jsk_rviz_plugins::Pictogram::ROTATE_X ||
223  action_ == jsk_rviz_plugins::Pictogram::ROTATE_Y) {
224  Ogre::Vector3 axis;
225  if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Z) {
226  axis = Ogre::Vector3(0, 0, 1);
227  }
228  else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_X) {
229  axis = Ogre::Vector3(1, 0, 0);
230  }
231  else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Y) {
232  axis = Ogre::Vector3(0, 1, 0);
233  }
234  time_ = time_ + ros::WallDuration(wall_dt);
235  // time_ -> theta
236  Ogre::Radian theta(M_PI * 2 * fmod(time_.toSec() * speed_, 1.0));
237 
238  Ogre::Quaternion offset;
239  offset.FromAngleAxis(theta, axis);
240  Ogre::Quaternion final_rot = quaternion * offset;
241  setPosition(position);
242  setOrientation(final_rot);
243  }
244  else if (action_ == jsk_rviz_plugins::Pictogram::JUMP ||
245  action_ == jsk_rviz_plugins::Pictogram::JUMP_ONCE) {
246  bool jumpingp = false;
247  if (action_ == jsk_rviz_plugins::Pictogram::JUMP) {
248  jumpingp = true;
249  }
250  else if (action_ == jsk_rviz_plugins::Pictogram::JUMP_ONCE &&
251  (ros::WallTime::now() - time_).toSec() < 2) {
252  jumpingp = true;
253  }
254 
255  if (!jumpingp) {
256  setPosition(position);
257  }
258  else {
259  // t(2-t) * size
260  double t = fmod((ros::WallTime::now() - time_).toSec(), 2.0);
261  double height = size_ * t * (2 - t);
262  Ogre::Vector3 new_pos = position + quaternion * Ogre::Vector3(height, 0, 0);
263  setPosition(new_pos);
264  }
265  setOrientation(quaternion);
266  }
267 
268  double exceeded_time;
269  if( ttl_ && (exceeded_time = (ros::WallTime::now() - time_).toSec()) > ttl_) {
270  setAlpha( std::max(1.0 - 1.0 * (ros::WallTime::now() - (time_ + ros::WallDuration(ttl_))).toSec() / 5.0, 0.0) );
271  if( 1.0 - 1.0 * (ros::WallTime::now() - (time_ + ros::WallDuration(ttl_))).toSec() / 3.0 < 0)
272  setAction(jsk_rviz_plugins::Pictogram::DELETE);
273  }
274  }
275 
276  void PictogramObject::update(float wall_dt, float ros_dt)
277  {
278  if (text_.empty()) {
279  // not yet setted
280  return;
281  }
282  // update position and orientation
283  if (!context_) {
284  return;
285  }
286  updatePose(wall_dt);
287  if (!need_to_update_) {
288  return;
289  }
290  need_to_update_ = false;
291  ScopedPixelBuffer buffer = texture_object_->getBuffer();
292  QColor transparent(255, 255, 255, 0);
293  QImage Hud = buffer.getQImage(128, 128, transparent); // should change according to size
294  QPainter painter( &Hud );
295  painter.setRenderHint(QPainter::Antialiasing, true);
296  QColor foreground = rviz::ogreToQt(color_);
297  painter.setPen(QPen(foreground, 5, Qt::SolidLine));
298 
299  if (isCharacterSupported(text_) && mode_ == jsk_rviz_plugins::Pictogram::PICTOGRAM_MODE) {
300  QFont font = getFont(text_);
301  QString pictogram_text = lookupPictogramText(text_);
302  if (isEntypo(text_)) {
303  font.setPointSize(100);
304  }
305  else if (isFontAwesome(text_)) {
306  font.setPointSize(45);
307  }
308  painter.setFont(font);
309  painter.drawText(0, 0, 128, 128,
310  Qt::AlignHCenter | Qt::AlignVCenter,
311  pictogram_text);
312  painter.end();
313  }else if( mode_ == jsk_rviz_plugins::Pictogram::STRING_MODE){
314  QFont font("Liberation Sans");
315  font.setPointSize(32);
316  font.setBold(true);
317  painter.setFont(font);
318  painter.drawText(0, 0, 128, 128,
319  Qt::TextWordWrap | Qt::AlignHCenter | Qt::AlignVCenter,
320  text_.c_str());
321  painter.end();
322  }
323  else {
324  ROS_WARN("%s is not supported", text_.c_str());
325  }
326  }
327 
329  {
330  }
331 
333  {
334  }
335 
336  void PictogramObject::setColor(QColor color)
337  {
338  if (!epsEqual(color_.r * 255.0, color.red()) ||
339  !epsEqual(color_.g * 255.0, color.green()) ||
340  !epsEqual(color_.b * 255.0, color.blue())) {
342  need_to_update_ = true;
343  }
344  }
345 
346  void PictogramObject::setText(std::string text)
347  {
348  if (text_ != text) {
350  need_to_update_ = true;
351  }
352  }
353 
355  {
356  if (!epsEqual(color_.a, alpha)) {
357  need_to_update_ = true;
359  }
360  }
361 
363  {
364  setupFont();
365  }
366 
368  {
369 
370  }
371 
373  {
374  MFDClass::onInitialize();
375  pictogram_.reset(new PictogramObject(scene_manager_,
376  scene_node_,
377  1.0));
378 
379  pictogram_->setContext(context_);
380  pictogram_->setEnable(false);
381  pictogram_->start();
382  // initial setting
383  pictogram_->setColor(QColor(25, 255, 240));
384  pictogram_->setAlpha(1.0);
385  pictogram_->setSpeed(1.0);
386  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
387  }
388 
390  {
391  MFDClass::reset();
392  pictogram_->setEnable(false);
393  }
394 
396  {
397  subscribe();
398  if (pictogram_) {
399  // keep false, it will be true
400  // in side of processMessae callback.
401  pictogram_->setEnable(false);
402  }
403  }
404 
405  void PictogramDisplay::processMessage(const jsk_rviz_plugins::Pictogram::ConstPtr& msg)
406  {
407  boost::mutex::scoped_lock lock(mutex_);
408 
409  pictogram_->setEnable(isEnabled());
410  if (!isEnabled()) {
411  return;
412  }
413  pictogram_->setAction(msg->action);
414  if (msg->action == jsk_rviz_plugins::Pictogram::DELETE) {
415  return;
416  }
417 
418  if (msg->size <= 0.0) {
419  pictogram_->setSize(0.5);
420  }
421  else {
422  pictogram_->setSize(msg->size / 2.0);
423  }
424  pictogram_->setColor(QColor(msg->color.r * 255,
425  msg->color.g * 255,
426  msg->color.b * 255));
427  pictogram_->setAlpha(msg->color.a);
428  pictogram_->setPose(msg->pose, msg->header.frame_id);
429  pictogram_->setText(msg->character);
430  pictogram_->setMode(msg->mode);
431  pictogram_->setTTL(msg->ttl);
432  if (msg->speed)
433  pictogram_->setSpeed(msg->speed);
434  }
435 
436  void PictogramDisplay::update(float wall_dt, float ros_dt)
437  {
438  boost::mutex::scoped_lock lock(mutex_);
439  if (pictogram_) {
440  pictogram_->update(wall_dt, ros_dt);
441  }
442  }
443 }
444 
QString lookupPictogramText(std::string character)
virtual void setPosition(Ogre::Vector3 &pos)
std::map< std::string, QString > entypo_character_map
void processMessage(const jsk_rviz_plugins::Pictogram::ConstPtr &msg)
QFont getFont(std::string character)
virtual QImage getQImage(unsigned int width, unsigned int height)
virtual void setText(std::string text)
std::map< std::string, QString > entypo_social_character_map
virtual void setAlpha(double alpha)
virtual void setAlpha(double alpha)
bool epsEqual(double a, double b)
#define ROS_WARN(...)
virtual void setOrientation(rviz::DisplayContext *context)
virtual void setContext(rviz::DisplayContext *context)
void subscribe()
bool isCharacterSupported(std::string character)
int addFont(unsigned char *data, unsigned int data_len)
std::map< std::string, QString > fontawesome_character_map
virtual void setMode(uint8_t mode)
virtual void update(float wall_dt, float ros_dt)
QColor ogreToQt(const Ogre::ColourValue &c)
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual FrameManager * getFrameManager() const=0
virtual void setSize(double size)
bool isEntypo(std::string text)
bool isFontAwesome(std::string text)
virtual void setColor(QColor color)
void update(float wall_dt, float ros_dt)
static WallTime now()
Ogre::SceneManager * scene_manager_
virtual void setText(std::string text)
virtual void setSpeed(double speed)
virtual void setEnable(bool enable)
virtual void setEnable(bool enable)
virtual void setPose(const geometry_msgs::Pose &pose, const std::string &frame_id)
height
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramDisplay, rviz::Display)
virtual void setColor(QColor color)
#define ROS_ERROR(...)
virtual void setAction(uint8_t action)
PictogramObject(Ogre::SceneManager *manager, Ogre::SceneNode *parent, double size)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58