footstep_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 "footstep_display.h"
37 #include <rviz/validate_floats.h>
39 
40 namespace jsk_rviz_plugins
41 {
43  {
44  alpha_property_ = new rviz::FloatProperty( "Alpha", 0.5,
45  "0 is fully transparent, 1.0 is fully opaque.",
46  this, SLOT( updateAlpha() ));
48  "Show Name", true,
49  "Show name of each footstep",
50  this, SLOT(updateShowName()));
52  "Use Group Coloring", false,
53  "Use footstep_group field to colorize footsteps",
54  this, SLOT(updateUseGroupColoring()));
56  "Width", 0.15,
57  "width of the footstep, it's not used if the dimensions is specified in Footstep message.",
58  this, SLOT( updateWidth() ));
60  "height", 0.01,
61  "height of the footstep, it's not used if the dimensions is specified in Footstep message.",
62  this, SLOT( updateHeight() ));
63 
65  "depth", 0.3,
66  "depth of the footstep, it's not used if the dimensions is specified in Footstep message.",
67  this, SLOT( updateDepth() ));
68  }
69 
71  {
72  delete alpha_property_;
73  delete width_property_;
74  delete height_property_;
75  delete depth_property_;
76  delete show_name_property_;
78  delete line_;
79  // remove all the nodes
80  for (size_t i = 0; i < text_nodes_.size(); i++) {
81  Ogre::SceneNode* node = text_nodes_[i];
82  node->removeAndDestroyAllChildren();
83  node->detachAllObjects();
84  scene_manager_->destroySceneNode(node);
85  }
86  }
87 
89  {
91  }
92 
94  {
96  }
97 
99  {
101  }
102 
104  {
106  }
107 
109  {
111  }
112 
114  {
116  }
117 
118 
120  {
121  MFDClass::reset();
122  shapes_.clear();
123  line_->clear();
124  allocateTexts(0);
125  }
126 
127  bool FootstepDisplay::validateFloats( const jsk_footstep_msgs::FootstepArray& msg )
128  {
129  for (std::vector<jsk_footstep_msgs::Footstep>::const_iterator it = msg.footsteps.begin();
130  it != msg.footsteps.end();
131  ++it) {
132  if (!rviz::validateFloats((*it).pose.position.x)
133  || !rviz::validateFloats((*it).pose.position.y)
134  || !rviz::validateFloats((*it).pose.position.z)
135  || !rviz::validateFloats((*it).pose.orientation.x)
136  || !rviz::validateFloats((*it).pose.orientation.y)
137  || !rviz::validateFloats((*it).pose.orientation.z)
138  || !rviz::validateFloats((*it).pose.orientation.w)
139  ) {
140  return false;
141  }
142  }
143  return true;
144  }
145 
147  {
149  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
151  updateShowName();
152  updateWidth();
153  updateHeight();
154  updateDepth();
155  updateAlpha();
157  }
158 
160  if (num > shapes_.size()) {
161  // need to allocate
162  for (size_t i = shapes_.size(); i < num; i++) {
163  ShapePtr shape;
165  scene_node_));
166  shapes_.push_back(shape);
167  }
168  }
169  else if (num < shapes_.size()) {
170  // need to remove
171  shapes_.resize(num);
172  }
173  }
174 
176  if (num > texts_.size()) {
177  // need to allocate
178  for (size_t i = texts_.size(); i < num; i++) {
179  // create nodes
180  Ogre::SceneNode* node = scene_node_->createChildSceneNode();
182  = new rviz::MovableText("not initialized", "Liberation Sans", 0.05);
183  text->setVisible(false);
186  node->attachObject(text);
187  texts_.push_back(text);
188  text_nodes_.push_back(node);
189  }
190  }
191  else if (num < texts_.size()) {
192  for (int i = texts_.size() - 1; i >= (int)num; i--) {
193  Ogre::SceneNode* node = text_nodes_[i];
194  node->detachAllObjects();
195  node->removeAndDestroyAllChildren();
196  scene_manager_->destroySceneNode(node);
197  }
198  text_nodes_.resize(num);
199  texts_.resize(num);
200 
201  }
202  }
203 
204  double FootstepDisplay::minNotZero(double a, double b) {
205  if (a == 0.0) {
206  return b;
207  }
208  else if (b == 0.0) {
209  return a;
210  }
211  else {
212  return std::min(a, b);
213  }
214  }
215 
216  void FootstepDisplay::update(float wall_dt, float ros_dt)
217  {
218  for (size_t i = 0; i < shapes_.size(); i++) {
219  ShapePtr shape = shapes_[i];
220  texts_[i]->setVisible(show_name_); // TODO
221  jsk_footstep_msgs::Footstep footstep = latest_footstep_->footsteps[i];
222  // color
223  if (use_group_coloring_) {
224  std_msgs::ColorRGBA color
225  = jsk_topic_tools::colorCategory20(footstep.footstep_group);
226  shape->setColor(color.r, color.g, color.b, alpha_);
227  }
228  else {
229  if (footstep.leg == jsk_footstep_msgs::Footstep::LLEG) {
230  shape->setColor(0, 1, 0, alpha_);
231  }
232  else if (footstep.leg == jsk_footstep_msgs::Footstep::RLEG) {
233  shape->setColor(1, 0, 0, alpha_);
234  }
235  else if (footstep.leg == jsk_footstep_msgs::Footstep::LARM) {
236  shape->setColor(0, 1, 1, alpha_);
237  }
238  else if (footstep.leg == jsk_footstep_msgs::Footstep::RARM) {
239  shape->setColor(1, 0, 1, alpha_);
240  }
241  else {
242  shape->setColor(1, 1, 1, alpha_);
243  }
244  }
245 
246  }
247  }
248 
250  const jsk_footstep_msgs::Footstep& footstep)
251  {
252  if (footstep.dimensions.x == 0 &&
253  footstep.dimensions.y == 0 &&
254  footstep.dimensions.z == 0) {
255  return std::max(minNotZero(minNotZero(footstep.dimensions.x,
256  footstep.dimensions.y),
257  footstep.dimensions.z),
258  0.1);
259  }
260  else {
261  return std::max(minNotZero(minNotZero(depth_property_->getFloat(),
264  0.1);
265 
266  }
267  }
268 
269 
270  void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
271  {
272  if (!validateFloats(*msg)) {
273  setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
274  return;
275  }
276  latest_footstep_ = msg;
277  Ogre::Quaternion orientation;
278  Ogre::Vector3 position;
279  if(!context_->getFrameManager()->getTransform( msg->header.frame_id,
280  msg->header.stamp,
281  position, orientation)) {
282  std::ostringstream oss;
283  oss << "Error transforming pose";
284  oss << " from frame '" << msg->header.frame_id << "'";
285  oss << " to frame '" << qPrintable(fixed_frame_) << "'";
286  ROS_ERROR_STREAM(oss.str());
287  setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
288  return;
289  }
290 
291  // check thhe length of the shapes_
292  allocateCubes(msg->footsteps.size());
293  allocateTexts(msg->footsteps.size());
294  line_->clear();
295  line_->setLineWidth(0.01);
296  line_->setNumLines(1);
297  line_->setMaxPointsPerLine(1024);
298 
299  for (size_t i = 0; i < msg->footsteps.size(); i++)
300  {
301  ShapePtr shape = shapes_[i];
303  Ogre::SceneNode* node = text_nodes_[i];
304  jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
305  Ogre::Vector3 step_position;
306  Ogre::Vector3 shape_position;
307  Ogre::Quaternion step_quaternion;
308  if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
309  step_position,
310  step_quaternion ))
311  {
312  std::ostringstream oss;
313  oss << "Error transforming pose";
314  oss << " from frame '" << msg->header.frame_id << "'";
315  oss << " to frame '" << qPrintable(fixed_frame_) << "'";
316  ROS_ERROR_STREAM(oss.str());
317  setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
318  return;
319  }
320  // add offset
321  Ogre::Vector3 step_offset (footstep.offset.x, footstep.offset.y, footstep.offset.z);
322  shape_position = step_position + (step_quaternion * step_offset);
323 
324  shape->setPosition(shape_position);
325  shape->setOrientation(step_quaternion);
326  // size of shape
327  Ogre::Vector3 scale;
328  if (footstep.dimensions.x == 0 &&
329  footstep.dimensions.y == 0 &&
330  footstep.dimensions.z == 0) {
331  scale[0] = depth_;
332  scale[1] = width_;
333  scale[2] = height_;
334  }
335  else {
336  scale[0] = footstep.dimensions.x;
337  scale[1] = footstep.dimensions.y;
338  scale[2] = footstep.dimensions.z;
339  }
340  shape->setScale(scale);
341  // update the size of text
342  if (footstep.leg == jsk_footstep_msgs::Footstep::LLEG) {
343  text->setCaption("LLEG");
344  }
345  else if (footstep.leg == jsk_footstep_msgs::Footstep::RLEG) {
346  text->setCaption("RLEG");
347  }
348  else if (footstep.leg == jsk_footstep_msgs::Footstep::LARM) {
349  text->setCaption("LARM");
350  }
351  else if (footstep.leg == jsk_footstep_msgs::Footstep::RARM) {
352  text->setCaption("RARM");
353  }
354  else {
355  text->setCaption("unknown");
356  }
357  text->setCharacterHeight(estimateTextSize(footstep));
358  node->setPosition(shape_position);
359  node->setOrientation(step_quaternion);
360  text->setVisible(show_name_); // TODO
361  line_->addPoint(step_position);
362 
363  }
364  //updateFootstepSize();
365  updateAlpha();
367  }
368 
369 }
370 
jsk_footstep_msgs::FootstepArray::ConstPtr latest_footstep_
std::vector< Ogre::SceneNode * > text_nodes_
rviz::FloatProperty * height_property_
void addPoint(const Ogre::Vector3 &point)
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
virtual void processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr &msg)
virtual void allocateCubes(size_t num)
rviz::BoolProperty * show_name_property_
virtual void update(float wall_dt, float ros_dt)
void setCharacterHeight(Ogre::Real height)
Ogre::SceneNode * scene_node_
QString fixed_frame_
rviz::BoolProperty * use_group_coloring_property_
virtual void allocateTexts(size_t num)
void setCaption(const Ogre::String &caption)
std::vector< ShapePtr > shapes_
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual FrameManager * getFrameManager() const=0
virtual bool validateFloats(const jsk_footstep_msgs::FootstepArray &msg)
virtual double estimateTextSize(const jsk_footstep_msgs::Footstep &footstep)
bool validateFloats(const visualization_msgs::InteractiveMarker &msg)
void setTextAlignment(const HorizontalAlignment &horizontalAlignment, const VerticalAlignment &verticalAlignment)
void setNumLines(uint32_t num)
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
rviz::FloatProperty * width_property_
rviz::FloatProperty * depth_property_
virtual Ogre::SceneManager * getSceneManager() const=0
virtual void queueRender()=0
std_msgs::ColorRGBA colorCategory20(int i)
virtual float getFloat() const
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
#define ROS_ERROR_STREAM(args)
rviz::FloatProperty * alpha_property_
virtual bool getBool() const
std::vector< rviz::MovableText * > texts_
virtual double minNotZero(double a, double b)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
void setLineWidth(float width)


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