people_position_measurement_array_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
38 #include <rviz/view_manager.h>
39 #include <rviz/render_panel.h>
40 #include <OGRE/OgreCamera.h>
41 #include <QPainter>
43 #include <OGRE/OgreRenderSystem.h>
44 
45 #include <algorithm>
46 #include <boost/lambda/lambda.hpp>
47 
48 namespace jsk_rviz_plugins
49 {
51  {
52  size_property_ = new rviz::FloatProperty("size", 0.3,
53  "size of the visualizer", this,
54  SLOT(updateSize()));
56  "timeout", 10.0, "timeout seconds", this, SLOT(updateTimeout()));
58  "anonymous", false,
59  "anonymous",
60  this, SLOT(updateAnonymous()));
62  "text", "person found here person found here",
63  "text to rotate",
64  this, SLOT(updateText()));
65  }
66 
67 
69  {
70  delete size_property_;
71  }
72 
74  {
76  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
77  updateSize();
78  updateTimeout();
80  updateText();
81  }
82 
84  {
85  faces_.clear();
86  visualizers_.clear();
87  }
88 
90  {
92  clearObjects();
93  }
94 
96  const people_msgs::PositionMeasurementArray::ConstPtr& msg)
97  {
98  boost::mutex::scoped_lock lock(mutex_);
99  static int count = 0;
100  static int square_count = 0;
101  faces_ = msg->people;
102  // check texture is ready or not
103  if (faces_.size() > visualizers_.size()) {
104  for (size_t i = visualizers_.size(); i < faces_.size(); i++) {
107  scene_node_,
108  size_,
109  text_)));
110  visualizers_[visualizers_.size() - 1]->setAnonymous(anonymous_);
111  visualizers_[visualizers_.size() - 1]->update(0, 0);
112  QColor color(25.0, 255.0, 240.0);
113  visualizers_[visualizers_.size() - 1]->setColor(color);
114  }
115  }
116  else {
117  visualizers_.resize(faces_.size());
118  }
119  // move scene_node
120  for (size_t i = 0; i < faces_.size(); i++) {
121  Ogre::Quaternion orientation;
122  Ogre::Vector3 position;
123  geometry_msgs::Pose pose;
124  pose.position = faces_[i].pos;
125  pose.orientation.w = 1.0;
126  if(!context_->getFrameManager()->transform(msg->header,
127  pose,
128  position, orientation))
129  {
130  std::ostringstream oss;
131  oss << "Error transforming pose";
132  oss << " from frame '" << msg->header.frame_id << "'";
133  oss << " to frame '" << qPrintable(fixed_frame_) << "'";
134  ROS_ERROR_STREAM(oss.str());
135  setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
136  }
137  else {
138  visualizers_[i]->setPosition(position);
139  }
140  }
141  latest_time_ = msg->header.stamp;
142  }
143 
145  float wall_dt, float ros_dt)
146  {
147  boost::mutex::scoped_lock lock(mutex_);
148  if (faces_.size() == 0) {
149  return;
150  }
151  if ((ros::Time::now() - latest_time_).toSec() > timeout_) {
152  ROS_WARN("timeout face recognition result");
153  clearObjects();
154  return;
155  }
156  for (size_t i = 0; i < visualizers_.size(); i++) {
157  visualizers_[i]->setOrientation(context_);
158  }
159  for (size_t i = 0; i < visualizers_.size(); i++) {
160  visualizers_[i]->update(wall_dt, ros_dt);
161  }
162  }
163 
165  {
166  boost::mutex::scoped_lock lock(mutex_);
168  }
169 
171  {
172  boost::mutex::scoped_lock lock(mutex_);
174  visualizers_.clear();
175  }
176 
178  {
179  boost::mutex::scoped_lock lock(mutex_);
181  for (size_t i = 0; i < visualizers_.size(); i++) {
182  visualizers_[i]->setAnonymous(anonymous_);
183  }
184  }
185 
187  {
188  boost::mutex::scoped_lock lock(mutex_);
190  for (size_t i = 0; i < visualizers_.size(); i++) {
191  visualizers_[i]->setText(text_);
192  }
193  }
194 
195 }
196 
197 
200 
void processMessage(const people_msgs::PositionMeasurementArray::ConstPtr &msg)
pose
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
#define ROS_WARN(...)
Ogre::SceneNode * scene_node_
std::string getStdString()
QString fixed_frame_
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual FrameManager * getFrameManager() const=0
Ogre::SceneManager * scene_manager_
virtual float getFloat() const
static Time now()
#define ROS_ERROR_STREAM(args)
virtual bool getBool() const
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)


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