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,
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 
rviz::BoolProperty::getBool
virtual bool getBool() const
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::reset
virtual void reset()
Definition: people_position_measurement_array_display.cpp:121
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::size_property_
rviz::FloatProperty * size_property_
Definition: people_position_measurement_array_display.h:138
rviz::MessageFilterDisplay< MessageType >::reset
void reset() override
msg
msg
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
boost::shared_ptr
rviz::StatusProperty::Error
Error
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::anonymous_property_
rviz::BoolProperty * anonymous_property_
Definition: people_position_measurement_array_display.h:140
rviz_mouse_point_to_tablet.lock
lock
Definition: rviz_mouse_point_to_tablet.py:11
rviz::BoolProperty
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::onInitialize
virtual void onInitialize()
Definition: people_position_measurement_array_display.cpp:105
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay
Definition: people_position_measurement_array_display.h:93
rviz::Display::fixed_frame_
QString fixed_frame_
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::processMessage
void processMessage(const people_msgs::PositionMeasurementArray::ConstPtr &msg)
Definition: people_position_measurement_array_display.cpp:127
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::updateTimeout
void updateTimeout()
Definition: people_position_measurement_array_display.cpp:196
rviz::Display
render_system.h
rviz::FloatProperty
torus_array_sample.pose
pose
Definition: torus_array_sample.py:23
class_list_macros.h
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::visualizers_
std::vector< GISCircleVisualizer::Ptr > visualizers_
Definition: people_position_measurement_array_display.h:148
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::size_
double size_
Definition: people_position_measurement_array_display.h:143
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::update
void update(float wall_dt, float ros_dt)
Definition: people_position_measurement_array_display.cpp:176
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::~PeoplePositionMeasurementArrayDisplay
virtual ~PeoplePositionMeasurementArrayDisplay()
Definition: people_position_measurement_array_display.cpp:100
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::text_property_
rviz::StringProperty * text_property_
Definition: people_position_measurement_array_display.h:141
rviz::FloatProperty::getFloat
virtual float getFloat() const
render_panel.h
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::text_
std::string text_
Definition: people_position_measurement_array_display.h:146
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
rviz::StringProperty
rviz::StringProperty::getStdString
std::string getStdString()
people_position_measurement_array_display.h
rviz::Display::scene_manager_
Ogre::SceneManager * scene_manager_
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::updateText
void updateText()
Definition: people_position_measurement_array_display.cpp:218
ROS_WARN
#define ROS_WARN(...)
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::latest_time_
ros::Time latest_time_
Definition: people_position_measurement_array_display.h:149
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const=0
view_manager.h
rviz::MessageFilterDisplay< MessageType >::onInitialize
void onInitialize() override
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::timeout_
double timeout_
Definition: people_position_measurement_array_display.h:144
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::updateSize
void updateSize()
Definition: people_position_measurement_array_display.cpp:202
rviz::Display::context_
DisplayContext * context_
point_test.count
int count
Definition: point_test.py:15
rviz::FrameManager::transform
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::PeoplePositionMeasurementArrayDisplay
PeoplePositionMeasurementArrayDisplay()
Definition: people_position_measurement_array_display.cpp:82
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::clearObjects
void clearObjects()
Definition: people_position_measurement_array_display.cpp:115
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::updateAnonymous
void updateAnonymous()
Definition: people_position_measurement_array_display.cpp:209
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::anonymous_
bool anonymous_
Definition: people_position_measurement_array_display.h:145
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::timeout_property_
rviz::FloatProperty * timeout_property_
Definition: people_position_measurement_array_display.h:139
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::faces_
std::vector< people_msgs::PositionMeasurement > faces_
Definition: people_position_measurement_array_display.h:147
jsk_rviz_plugins::GISCircleVisualizer
Definition: facing_visualizer.h:252
jsk_rviz_plugins
Definition: __init__.py:1
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay::mutex_
boost::mutex mutex_
Definition: people_position_measurement_array_display.h:142
uniform_string_stream.h
ros::Time::now
static Time now()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Fri Dec 13 2024 03:49:56