twist_stamped_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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 "twist_stamped_display.h"
37 
38 namespace jsk_rviz_plugins
39 {
40  TwistStampedDisplay::TwistStampedDisplay()
41  {
42  linear_scale_property_ = new rviz::FloatProperty("linear scale", 1.0,
43  "linear velocity scale",
44  this, SLOT(updateLinearScale()));
45  angular_scale_property_ = new rviz::FloatProperty("angular scale", 1.0,
46  "angular velocity scale",
47  this, SLOT(updateAngularScale()));
48  linear_color_property_ = new rviz::ColorProperty("linear color", QColor(0, 255, 0),
49  "linear velocity color",
50  this, SLOT(updateLinearColor()));
51  angular_color_property_ = new rviz::ColorProperty("angular color", QColor(255, 0, 0),
52  "angular velocity color",
53  this, SLOT(updateAngularColor()));
54  linear_scale_property_->setMin(0.0);
55  angular_scale_property_->setMin(0.0);
56  }
57 
58  TwistStampedDisplay::~TwistStampedDisplay()
59  {
60  delete linear_color_property_;
61  delete angular_color_property_;
62  }
63 
64  void TwistStampedDisplay::onInitialize()
65  {
66  MFDClass::onInitialize();
67  linear_arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_));
68  x_rotate_circle_.reset(new rviz::BillboardLine(scene_manager_, scene_node_));
69  y_rotate_circle_.reset(new rviz::BillboardLine(scene_manager_, scene_node_));
70  z_rotate_circle_.reset(new rviz::BillboardLine(scene_manager_, scene_node_));
71  x_rotate_arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_));
72  y_rotate_arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_));
73  z_rotate_arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_));
74  updateLinearScale();
75  updateAngularScale();
76  updateLinearColor();
77  updateAngularColor();
78  Ogre::Vector3 zero_scale(0, 0, 0);
79  linear_arrow_->setScale(zero_scale);
80  x_rotate_arrow_->set(0, 0, 0, 0);
81  y_rotate_arrow_->set(0, 0, 0, 0);
82  z_rotate_arrow_->set(0, 0, 0, 0);
83  }
84 
85  void TwistStampedDisplay::reset()
86  {
87  MFDClass::reset();
88  }
89 
90  void TwistStampedDisplay::processMessage(
91  const geometry_msgs::TwistStamped::ConstPtr& msg)
92  {
93  // move scene_node_ to the frame pose
94  Ogre::Vector3 position;
95  Ogre::Quaternion orientation;
96  if(!context_->getFrameManager()->getTransform(
97  msg->header, position, orientation)) {
98  ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
99  msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
100  return;
101  }
102  scene_node_->setPosition(position);
103  scene_node_->setOrientation(orientation);
104  // linear velocity
105  linear_arrow_->setColor(rviz::qtToOgre(linear_color_));
106  Ogre::Vector3 linear_direction(msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z);
107  Ogre::Vector3 linear_scale(linear_scale_ * linear_direction.length(),
108  linear_scale_ * linear_direction.length(),
109  linear_scale_ * linear_direction.length());
110  linear_arrow_->setScale(linear_scale);
111  linear_arrow_->setDirection(linear_direction);
112 
113  // rotate velocity
114  updateRotationVelocity(x_rotate_circle_,
115  x_rotate_arrow_,
116  Ogre::Vector3(0, 1, 0),
117  Ogre::Vector3(0, 0, 1),
118  Ogre::Vector3(1, 0, 0),
119  std::abs(msg->twist.angular.x),
120  msg->twist.angular.x > 0);
121  updateRotationVelocity(y_rotate_circle_,
122  y_rotate_arrow_,
123  Ogre::Vector3(0, 0, 1),
124  Ogre::Vector3(1, 0, 0),
125  Ogre::Vector3(0, 1, 0),
126  std::abs(msg->twist.angular.y),
127  msg->twist.angular.y > 0);
128  updateRotationVelocity(z_rotate_circle_,
129  z_rotate_arrow_,
130  Ogre::Vector3(1, 0, 0),
131  Ogre::Vector3(0, 1, 0),
132  Ogre::Vector3(0, 0, 1),
133  std::abs(msg->twist.angular.z),
134  msg->twist.angular.z > 0);
135  Ogre::ColourValue c = rviz::qtToOgre(angular_color_);
136  x_rotate_circle_->setColor(c.r, c.g, c.b, 1.0);
137  y_rotate_circle_->setColor(c.r, c.g, c.b, 1.0);
138  z_rotate_circle_->setColor(c.r, c.g, c.b, 1.0);
139  x_rotate_arrow_->setColor(c);
140  y_rotate_arrow_->setColor(c);
141  z_rotate_arrow_->setColor(c);
142  }
143 
144  void TwistStampedDisplay::updateRotationVelocity(
145  BillboardLinePtr circle,
146  ArrowPtr arrow,
147  const Ogre::Vector3& ux,
148  const Ogre::Vector3& uy,
149  const Ogre::Vector3& uz,
150  const double r,
151  bool positive)
152  {
153  circle->clear();
154  if (r < 1.0e-9) { // too small to visualize it
155  arrow->set(0, 0, 0, 0);
156  return;
157  }
158  const double step = 10; // per 10 deg
159  const double start_theta = 20;
160  const double end_theta = 340;
161  circle->setMaxPointsPerLine((end_theta - start_theta) / step + 1); // +1?
162  circle->setLineWidth(r * angular_scale_ / 2 * 0.1);
163  for (double theta = start_theta; theta < end_theta; theta += step) {
164  double rad = theta / 180 * M_PI;
165  Ogre::Vector3 p = ux * cos(rad) * r * angular_scale_ + uy * sin(rad) * r * angular_scale_;
166  circle->addPoint(p);
167  }
168  // put arrow
169  if (positive) {
170  double end_rad = (end_theta - step) / 180 * M_PI;
171  Ogre::Vector3 endpoint = ux * cos(end_rad) * r * angular_scale_ + uy * sin(end_rad) * r * angular_scale_;
172  Ogre::Vector3 direction = ux * (- sin(end_rad)) + uy * cos(end_rad);
173  arrow->setPosition(endpoint);
174  arrow->setDirection(direction);
175  }
176  else {
177  double end_rad = (start_theta + step) / 180 * M_PI;
178  Ogre::Vector3 endpoint = ux * cos(end_rad) * r * angular_scale_ + uy * sin(end_rad) * r * angular_scale_;
179  Ogre::Vector3 direction = - ux * (- sin(end_rad)) - uy * cos(end_rad);
180  arrow->setPosition(endpoint);
181  arrow->setDirection(direction);
182  }
183  arrow->set(0, 0, r * angular_scale_ / 2, r * angular_scale_ / 2);
184  }
185 
187  // update methods
189  void TwistStampedDisplay::updateLinearScale()
190  {
191  linear_scale_ = linear_scale_property_->getFloat();
192  }
193 
194  void TwistStampedDisplay::updateAngularScale()
195  {
196  angular_scale_ = angular_scale_property_->getFloat();
197  }
198 
199  void TwistStampedDisplay::updateLinearColor()
200  {
201  linear_color_ = linear_color_property_->getColor();
202  }
203 
204  void TwistStampedDisplay::updateAngularColor()
205  {
206  angular_color_ = angular_color_property_->getColor();
207  }
208 }
209 
210 #include <pluginlib/class_list_macros.h>
211 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::TwistStampedDisplay, rviz::Display )
rviz::BillboardLine
ScrewDisplay< geometry_msgs::TwistStamped >::linear_color_property_
rviz::ColorProperty * linear_color_property_
rviz::ColorProperty::getColor
virtual QColor getColor() const
MessageFilterDisplay< geometry_msgs::TwistStamped >::reset
void reset() override
rviz::qtToOgre
Ogre::ColourValue qtToOgre(const QColor &c)
msg
msg
ScrewDisplay< geometry_msgs::TwistStamped >::onInitialize
void onInitialize() override
rviz::Arrow
p
p
overlay_sample.theta
int theta
Definition: overlay_sample.py:22
step
unsigned int step
ScrewDisplay< geometry_msgs::TwistStamped >::linear_scale_property_
rviz::FloatProperty * linear_scale_property_
rviz::FloatProperty::setMin
void setMin(float min)
rviz::ColorProperty
rviz::Display
rviz::FloatProperty
class_list_macros.h
rviz::FloatProperty::getFloat
virtual float getFloat() const
ROS_DEBUG
#define ROS_DEBUG(...)
ScrewDisplay< geometry_msgs::TwistStamped >::angular_color_property_
rviz::ColorProperty * angular_color_property_
ScrewDisplay< geometry_msgs::TwistStamped >::angular_scale_property_
rviz::FloatProperty * angular_scale_property_
bounding_box_sample.r
r
Definition: bounding_box_sample.py:10
MessageFilterDisplay< geometry_msgs::TwistStamped >::onInitialize
void onInitialize() override
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
rviz::TwistStampedDisplay
twist_stamped_display.h
rviz::TwistStampedDisplay::processMessage
void processMessage(const geometry_msgs::TwistStamped::ConstPtr &msg) override
Definition: twist_stamped_display.cpp:122
jsk_rviz_plugins
Definition: __init__.py:1
ScrewDisplay< geometry_msgs::TwistStamped >::reset
void reset() override

jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Mon Jan 22 2024 03:47:13