src
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
36
#include "
people_position_measurement_array_display.h
"
37
#include <
rviz/uniform_string_stream.h
>
38
#include <
rviz/view_manager.h
>
39
#include <
rviz/render_panel.h
>
40
#include <OGRE/OgreCamera.h>
41
#include <QPainter>
42
#include <
rviz/ogre_helpers/render_system.h
>
43
#include <OGRE/OgreRenderSystem.h>
44
45
#include <algorithm>
46
#include <boost/lambda/lambda.hpp>
47
48
namespace
jsk_rviz_plugins
49
{
50
PeoplePositionMeasurementArrayDisplay::PeoplePositionMeasurementArrayDisplay
()
51
{
52
size_property_
=
new
rviz::FloatProperty
(
"size"
, 0.3,
53
"size of the visualizer"
,
this
,
54
SLOT(
updateSize
()));
55
timeout_property_
=
new
rviz::FloatProperty
(
56
"timeout"
, 10.0,
"timeout seconds"
,
this
, SLOT(
updateTimeout
()));
57
anonymous_property_
=
new
rviz::BoolProperty
(
58
"anonymous"
,
false
,
59
"anonymous"
,
60
this
, SLOT(
updateAnonymous
()));
61
text_property_
=
new
rviz::StringProperty
(
62
"text"
,
"person found here person found here"
,
63
"text to rotate"
,
64
this
, SLOT(
updateText
()));
65
}
66
67
68
PeoplePositionMeasurementArrayDisplay::~PeoplePositionMeasurementArrayDisplay
()
69
{
70
delete
size_property_
;
71
}
72
73
void
PeoplePositionMeasurementArrayDisplay::onInitialize
()
74
{
75
MFDClass::onInitialize
();
76
scene_node_
=
scene_manager_
->getRootSceneNode()->createChildSceneNode();
77
updateSize
();
78
updateTimeout
();
79
updateAnonymous
();
80
updateText
();
81
}
82
83
void
PeoplePositionMeasurementArrayDisplay::clearObjects
()
84
{
85
faces_
.clear();
86
visualizers_
.clear();
87
}
88
89
void
PeoplePositionMeasurementArrayDisplay::reset
()
90
{
91
MFDClass::reset
();
92
clearObjects
();
93
}
94
95
void
PeoplePositionMeasurementArrayDisplay::processMessage
(
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++) {
105
visualizers_
.push_back(
GISCircleVisualizer::Ptr
(
new
GISCircleVisualizer
(
106
scene_manager_
,
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
144
void
PeoplePositionMeasurementArrayDisplay::update
(
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
164
void
PeoplePositionMeasurementArrayDisplay::updateTimeout
()
165
{
166
boost::mutex::scoped_lock
lock
(
mutex_
);
167
timeout_
=
timeout_property_
->
getFloat
();
168
}
169
170
void
PeoplePositionMeasurementArrayDisplay::updateSize
()
171
{
172
boost::mutex::scoped_lock
lock
(
mutex_
);
173
size_
=
size_property_
->
getFloat
();
174
visualizers_
.clear();
175
}
176
177
void
PeoplePositionMeasurementArrayDisplay::updateAnonymous
()
178
{
179
boost::mutex::scoped_lock
lock
(
mutex_
);
180
anonymous_
=
anonymous_property_
->
getBool
();
181
for
(
size_t
i = 0; i <
visualizers_
.size(); i++) {
182
visualizers_
[i]->setAnonymous(
anonymous_
);
183
}
184
}
185
186
void
PeoplePositionMeasurementArrayDisplay::updateText
()
187
{
188
boost::mutex::scoped_lock
lock
(
mutex_
);
189
text_
=
text_property_
->
getStdString
();
190
for
(
size_t
i = 0; i <
visualizers_
.size(); i++) {
191
visualizers_
[i]->setText(
text_
);
192
}
193
}
194
195
}
196
197
198
#include <
pluginlib/class_list_macros.h
>
199
PLUGINLIB_EXPORT_CLASS
(
jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay
,
rviz::Display
)
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 Tue Dec 10 2024 03:48:24