human_skeleton_array_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, 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 
36 #include <jsk_topic_tools/color_utils.h>
37 namespace jsk_rviz_plugins
38 {
40  {
42  "coloring", "Auto",
43  "coloring method",
44  this, SLOT(updateColoring()));
45  coloring_property_->addOption("Auto", 0);
46  coloring_property_->addOption("Flat color", 1);
47 
49  "color", QColor(25, 255, 0),
50  "color to draw the edges",
51  this, SLOT(updateColor()));
53  "alpha", 1.0,
54  "alpha value to draw the edges",
55  this, SLOT(updateAlpha()));
57  "line width", 0.005,
58  "line width of the edges",
59  this, SLOT(updateLineWidth()));
60  }
61 
63  {
64  delete color_property_;
65  delete alpha_property_;
66  delete coloring_property_;
67  }
68 
69  QColor HumanSkeletonArrayDisplay::getColor(size_t index)
70  {
71  if (coloring_method_ == "auto") {
72  std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
73  return QColor(ros_color.r * 255.0,
74  ros_color.g * 255.0,
75  ros_color.b * 255.0,
76  ros_color.a * 255.0);
77  }
78  else if (coloring_method_ == "flat") {
79  return color_;
80  }
81  }
82 
84  {
86  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
87 
88  updateColor();
89  updateAlpha();
92  }
93 
95  {
97  if (latest_msg_) {
99  }
100  }
101 
103  {
105  if (latest_msg_) {
107  }
108  }
109 
111  {
113  if (latest_msg_) {
115  }
116  }
117 
119  {
120  if (coloring_property_->getOptionInt() == 0) {
121  coloring_method_ = "auto";
123  }
124  else if (coloring_property_->getOptionInt() == 1) {
125  coloring_method_ = "flat";
127  }
128 
129  if (latest_msg_) {
131  }
132  }
133 
135  {
136  MFDClass::reset();
137  edges_.clear();
138  latest_msg_.reset();
139  }
140 
142  if (num > shapes_.size()) {
143  for (size_t i = shapes_.size(); i < num; i++) {
144  ShapePtr shape;
146  scene_node_));
147  shapes_.push_back(shape);
148  }
149  }
150  else if (num < shapes_.size()) {
151  shapes_.resize(num);
152  }
153  }
154 
156  {
157  if (num > edges_.size()) {
158  for (size_t i = edges_.size(); i < num; i++) {
160  this->context_->getSceneManager(), this->scene_node_));
161  edges_.push_back(line);
162  }
163  }
164  else if (num < edges_.size())
165  {
166  edges_.resize(num); // ok??
167  }
168  }
169 
171  const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr& msg)
172  {
173  int line_num = 0;
174  for (size_t i = 0; i < msg->skeletons.size(); i++) {
175  line_num = line_num + msg->skeletons[i].bones.size();
176  }
177  allocateBillboardLines(line_num);
178  allocateSpheres(2 * line_num);
179  int line_i = 0;
180  for (size_t i = 0; i < msg->skeletons.size(); i++) {
181  for (size_t j = 0; j < msg->skeletons[i].bones.size(); j++) {
182  jsk_recognition_msgs::Segment edge_msg = msg->skeletons[i].bones[j];
183  BillboardLinePtr edge = edges_[line_i];
184  ShapePtr start_shape = shapes_[2 * line_i];
185  ShapePtr end_shape = shapes_[2 * line_i + 1];
186  edge->clear();
187 
188  geometry_msgs::Pose start_pose_local;
189  geometry_msgs::Pose end_pose_local;
190  start_pose_local.position = edge_msg.start_point;
191  start_pose_local.orientation.w = 1.0;
192  end_pose_local.position = edge_msg.end_point;
193  end_pose_local.orientation.w = 1.0;
194 
195  Ogre::Vector3 start_point;
196  Ogre::Vector3 end_point;
197  Ogre::Quaternion quaternion; // not used to visualize
198  bool transform_ret;
199  transform_ret =
200  context_->getFrameManager()->transform(msg->header, start_pose_local, start_point, quaternion)
201  && context_->getFrameManager()->transform(msg->header, end_pose_local, end_point, quaternion);
202  if(!transform_ret) {
203  ROS_ERROR( "Error transforming pose"
204  "'%s' from frame '%s' to frame '%s'",
205  qPrintable( getName() ), msg->header.frame_id.c_str(),
206  qPrintable( fixed_frame_ ));
207  return; // return?
208  }
209  edge->addPoint(start_point);
210  edge->addPoint(end_point);
211  edge->setLineWidth(line_width_);
212  QColor color = getColor(i);
213  edge->setColor(color.red() / 255.0,
214  color.green() / 255.0,
215  color.blue() / 255.0,
216  alpha_);
217 
218  Ogre::Vector3 scale;
219  scale[0] = 2.0 * line_width_;
220  scale[1] = 2.0 * line_width_;
221  scale[2] = 2.0 * line_width_;
222  start_shape->setPosition(start_point);
223  start_shape->setScale(scale);
224  start_shape->setOrientation(quaternion);
225  start_shape->setColor(color.red() / 255.0,
226  color.green() / 255.0,
227  color.blue() / 255.0,
228  alpha_);
229  end_shape->setPosition(end_point);
230  end_shape->setScale(scale);
231  end_shape->setOrientation(quaternion);
232  end_shape->setColor(color.red() / 255.0,
233  color.green() / 255.0,
234  color.blue() / 255.0,
235  alpha_);
236  line_i++;
237  }
238  }
239  }
240 
242  const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr& msg)
243  {
244  // Store latest message
245  latest_msg_ = msg;
246 
247  showEdges(msg);
248  }
249 }
250 
jsk_rviz_plugins::HumanSkeletonArrayDisplay::latest_msg_
jsk_recognition_msgs::HumanSkeletonArray::ConstPtr latest_msg_
Definition: human_skeleton_array_display.h:153
rviz::BillboardLine
rviz::EnumProperty::getOptionInt
virtual int getOptionInt()
rviz::ColorProperty::getColor
virtual QColor getColor() const
rviz::MessageFilterDisplay< MessageType >::reset
void reset() override
jsk_rviz_plugins::HumanSkeletonArrayDisplay
Definition: human_skeleton_array_display.h:87
msg
msg
jsk_rviz_plugins::HumanSkeletonArrayDisplay::alpha_property_
rviz::FloatProperty * alpha_property_
Definition: human_skeleton_array_display.h:144
jsk_rviz_plugins::HumanSkeletonArrayDisplay::line_width_property_
rviz::FloatProperty * line_width_property_
Definition: human_skeleton_array_display.h:145
contact_state_marker.scale
scale
Definition: contact_state_marker.py:91
boost::shared_ptr
jsk_rviz_plugins::HumanSkeletonArrayDisplay::~HumanSkeletonArrayDisplay
virtual ~HumanSkeletonArrayDisplay()
Definition: human_skeleton_array_display.cpp:94
rviz::Property::show
void show()
jsk_rviz_plugins::HumanSkeletonArrayDisplay::shapes_
std::vector< ShapePtr > shapes_
Definition: human_skeleton_array_display.h:151
jsk_rviz_plugins::HumanSkeletonArrayDisplay::color_property_
rviz::ColorProperty * color_property_
Definition: human_skeleton_array_display.h:143
jsk_rviz_plugins::HumanSkeletonArrayDisplay::updateColoring
void updateColoring()
Definition: human_skeleton_array_display.cpp:150
rviz::Display::fixed_frame_
QString fixed_frame_
rviz::DisplayContext::getSceneManager
virtual Ogre::SceneManager * getSceneManager() const=0
rviz::ColorProperty
rviz::Display
jsk_rviz_plugins::HumanSkeletonArrayDisplay::processMessage
void processMessage(const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr &msg)
Definition: human_skeleton_array_display.cpp:273
rviz::EnumProperty
rviz::FloatProperty
class_list_macros.h
rviz::Property::hide
void hide()
rviz::FloatProperty::getFloat
virtual float getFloat() const
rviz::EnumProperty::addOption
virtual void addOption(const QString &option, int value=0)
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
jsk_rviz_plugins::HumanSkeletonArrayDisplay::updateColor
void updateColor()
Definition: human_skeleton_array_display.cpp:134
jsk_rviz_plugins::HumanSkeletonArrayDisplay::edges_
std::vector< BillboardLinePtr > edges_
Definition: human_skeleton_array_display.h:150
rviz::Display::scene_manager_
Ogre::SceneManager * scene_manager_
jsk_rviz_plugins::HumanSkeletonArrayDisplay::allocateSpheres
void allocateSpheres(int num)
Definition: human_skeleton_array_display.cpp:173
jsk_rviz_plugins::HumanSkeletonArrayDisplay::coloring_property_
rviz::EnumProperty * coloring_property_
Definition: human_skeleton_array_display.h:142
human_skeleton_array_display.h
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const=0
rviz::Shape::Sphere
Sphere
jsk_rviz_plugins::HumanSkeletonArrayDisplay::allocateBillboardLines
void allocateBillboardLines(int num)
Definition: human_skeleton_array_display.cpp:187
num
num
jsk_rviz_plugins::HumanSkeletonArrayDisplay::color_
QColor color_
Definition: human_skeleton_array_display.h:146
jsk_rviz_plugins::HumanSkeletonArrayDisplay::updateLineWidth
void updateLineWidth()
Definition: human_skeleton_array_display.cpp:126
rviz::MessageFilterDisplay< MessageType >::onInitialize
void onInitialize() override
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
rviz::Property::getName
virtual QString getName() const
rviz::Display::context_
DisplayContext * context_
jsk_rviz_plugins::HumanSkeletonArrayDisplay::BillboardLinePtr
boost::shared_ptr< rviz::BillboardLine > BillboardLinePtr
Definition: human_skeleton_array_display.h:128
jsk_rviz_plugins::HumanSkeletonArrayDisplay::line_width_
double line_width_
Definition: human_skeleton_array_display.h:149
ROS_ERROR
#define ROS_ERROR(...)
rviz::Shape
jsk_rviz_plugins::HumanSkeletonArrayDisplay::onInitialize
virtual void onInitialize()
Definition: human_skeleton_array_display.cpp:115
rviz::FrameManager::transform
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
jsk_rviz_plugins::HumanSkeletonArrayDisplay::HumanSkeletonArrayDisplay
HumanSkeletonArrayDisplay()
Definition: human_skeleton_array_display.cpp:71
jsk_rviz_plugins::HumanSkeletonArrayDisplay::ShapePtr
boost::shared_ptr< rviz::Shape > ShapePtr
Definition: human_skeleton_array_display.h:129
jsk_rviz_plugins::HumanSkeletonArrayDisplay::alpha_
double alpha_
Definition: human_skeleton_array_display.h:147
jsk_rviz_plugins::HumanSkeletonArrayDisplay::updateAlpha
void updateAlpha()
Definition: human_skeleton_array_display.cpp:142
jsk_rviz_plugins
Definition: __init__.py:1
jsk_rviz_plugins::HumanSkeletonArrayDisplay::reset
virtual void reset()
Definition: human_skeleton_array_display.cpp:166
jsk_rviz_plugins::HumanSkeletonArrayDisplay::coloring_method_
std::string coloring_method_
Definition: human_skeleton_array_display.h:148
jsk_rviz_plugins::HumanSkeletonArrayDisplay::getColor
QColor getColor(size_t index)
Definition: human_skeleton_array_display.cpp:101
jsk_rviz_plugins::HumanSkeletonArrayDisplay::showEdges
virtual void showEdges(const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr &msg)
Definition: human_skeleton_array_display.cpp:202


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Tue Dec 10 2024 03:48:24