screw_display.cpp
Go to the documentation of this file.
1 #include <OgreSceneNode.h>
2 #include <OgreSceneManager.h>
3 
5 #include <rviz/frame_manager.h>
10 #include <rviz/validate_floats.h>
11 
12 #include <boost/foreach.hpp>
13 
14 #include "screw_visual.h"
15 #include "screw_display.h"
16 
17 template <class MessageType>
18 constexpr const char* linear()
19 {
20  return "Linear";
21 }
22 template <class MessageType>
23 constexpr const char* angular()
24 {
25  return "Angular";
26 }
27 
28 template <>
29 constexpr const char* linear<geometry_msgs::WrenchStamped>()
30 {
31  return "Force";
32 }
33 template <>
34 constexpr const char* angular<geometry_msgs::WrenchStamped>()
35 {
36  return "Torque";
37 }
38 
39 namespace rviz
40 {
41 template <class MessageType>
43 {
44  auto lin = linear<MessageType>();
45  auto ang = angular<MessageType>();
46  linear_color_property_ =
47  new ColorProperty(QString("%1 Color").arg(lin), QColor(204, 51, 51),
48  QString("Color to draw the %1 arrows.").arg(QString(lin).toLower()), this);
49  QObject::connect(linear_color_property_, &rviz::Property::changed, this,
51 
52  angular_color_property_ =
53  new ColorProperty(QString("%1 Color").arg(ang), QColor(204, 204, 51),
54  QString("Color to draw the %1 arrows.").arg(QString(ang).toLower()), this);
55  QObject::connect(angular_color_property_, &rviz::Property::changed, this,
57 
58  alpha_property_ =
59  new FloatProperty("Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this);
60  QObject::connect(alpha_property_, &rviz::Property::changed, this,
62 
63  linear_scale_property_ = new FloatProperty(QString("%1 Arrow Scale").arg(lin), 2.0,
64  QString("%1 arrow scale").arg(lin), this);
65  QObject::connect(linear_scale_property_, &rviz::Property::changed, this,
67 
68  angular_scale_property_ = new FloatProperty(QString("%1 Arrow Scale").arg(ang), 2.0,
69  QString("%1 arrow scale").arg(ang), this);
70  QObject::connect(angular_scale_property_, &rviz::Property::changed, this,
72 
73  width_property_ = new FloatProperty("Arrow Width", 0.5, "arrow width", this);
74  QObject::connect(width_property_, &rviz::Property::changed, this,
76 
77  history_length_property_ =
78  new IntProperty("History Length", 1, "Number of prior measurements to display.", this);
79  QObject::connect(history_length_property_, &rviz::Property::changed, this,
81 
82  hide_small_values_property_ = new BoolProperty("Hide Small Values", true, "Hide small values", this);
83  QObject::connect(hide_small_values_property_, &rviz::Property::changed, this,
85 
86  history_length_property_->setMin(1);
87  history_length_property_->setMax(100000);
88 }
89 
90 
91 template <class MessageType>
93 {
95  updateHistoryLength();
96 }
97 
98 // Override rviz::Display's reset() function to add a call to clear().
99 template <class MessageType>
101 {
103  visuals_.clear();
104 }
105 
106 template <class MessageType>
108 {
109  float alpha = alpha_property_->getFloat();
110  float linear_scale = linear_scale_property_->getFloat();
111  float angular_scale = angular_scale_property_->getFloat();
112  float width = width_property_->getFloat();
113  bool hide_small_values = hide_small_values_property_->getBool();
114  Ogre::ColourValue linear_color = linear_color_property_->getOgreColor();
115  Ogre::ColourValue angular_color = angular_color_property_->getOgreColor();
116 
117  for (size_t i = 0; i < visuals_.size(); i++)
118  {
119  visuals_[i]->setLinearColor(linear_color.r, linear_color.g, linear_color.b, alpha);
120  visuals_[i]->setAngularColor(angular_color.r, angular_color.g, angular_color.b, alpha);
121  visuals_[i]->setLinearScale(linear_scale);
122  visuals_[i]->setAngularScale(angular_scale);
123  visuals_[i]->setWidth(width);
124  visuals_[i]->setHideSmallValues(hide_small_values);
125  }
126 }
127 
128 // Set the number of past visuals to show.
129 template <class MessageType>
131 {
132  visuals_.rset_capacity(history_length_property_->getInt());
133 }
134 
135 // This is our callback to handle an incoming message.
136 template <class MessageType>
137 void ScrewDisplay<MessageType>::processMessagePrivate(const std_msgs::Header& header,
138  const geometry_msgs::Vector3& linear,
139  const geometry_msgs::Vector3& angular)
140 {
142  {
144  "Message contained invalid floating point values (nans or infs)");
145  return;
146  }
147 
148  // Here we call the rviz::FrameManager to get the transform from the
149  // fixed frame to the frame in the header of this Imu message. If
150  // it fails, we can't do anything else so we return.
151  Ogre::Quaternion orientation;
152  Ogre::Vector3 position;
153  if (!Display::context_->getFrameManager()->getTransform(header.frame_id, header.stamp, position,
154  orientation))
155  {
156  ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", header.frame_id.c_str(),
157  qPrintable(Display::fixed_frame_));
158  return;
159  }
160 
161  // We are keeping a circular buffer of visual pointers.
162  // This gets the next one, or creates and stores it if the buffer is not full
164  if (visuals_.full())
165  {
166  visual = visuals_.front();
167  }
168  else
169  {
170  visual.reset(new ScrewVisual(Display::context_->getSceneManager(), Display::scene_node_));
171  }
172 
173  // Now set or update the contents of the chosen visual.
174  visual->setScrew(linear, angular);
175  visual->setFramePosition(position);
176  visual->setFrameOrientation(orientation);
177  float alpha = alpha_property_->getFloat();
178  Ogre::ColourValue linear_color = linear_color_property_->getOgreColor();
179  Ogre::ColourValue angular_color = angular_color_property_->getOgreColor();
180  visual->setLinearColor(linear_color.r, linear_color.g, linear_color.b, alpha);
181  visual->setAngularColor(angular_color.r, angular_color.g, angular_color.b, alpha);
182  visual->setLinearScale(linear_scale_property_->getFloat());
183  visual->setAngularScale(angular_scale_property_->getFloat());
184  visual->setWidth(width_property_->getFloat());
185  visual->setHideSmallValues(hide_small_values_property_->getBool());
186 
187  // And send it to the end of the circular buffer
188  visuals_.push_back(visual);
189 }
190 
194 
195 } // end namespace rviz
196 
197 // Tell pluginlib about these classes. It is important to do this in
198 // global scope, outside our package's namespace.
rviz::MessageFilterDisplay::reset
void reset() override
Called to tell the display to clear its state.
Definition: message_filter_display.h:125
validate_floats.h
frame_manager.h
rviz::ScrewDisplay::onInitialize
void onInitialize() override
Override this function to do subclass-specific initialization.
Definition: screw_display.cpp:92
boost::shared_ptr
linear
constexpr const char * linear()
Definition: screw_display.cpp:18
rviz::StatusProperty::Error
@ Error
Definition: status_property.h:46
int_property.h
rviz::BoolProperty
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
rviz::validateFloats
bool validateFloats(const sensor_msgs::CameraInfo &msg)
Definition: camera_display.cpp:72
screw_visual.h
float_property.h
rviz::Display::fixed_frame_
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
rviz::ColorProperty
Definition: color_property.h:40
rviz::Display
Definition: display.h:63
rviz::FloatProperty
Property specialized to enforce floating point max/min.
Definition: float_property.h:37
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:176
screw_display.h
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rviz::WrenchStampedDisplay
Definition: screw_display.h:88
ROS_DEBUG
#define ROS_DEBUG(...)
rviz
Definition: add_display_dialog.cpp:54
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
color_property.h
rviz::ScrewDisplay
Definition: screw_display.h:32
rviz::ScrewDisplay::updateProperties
void updateProperties()
Definition: screw_display.cpp:107
angular
constexpr const char * angular()
Definition: screw_display.cpp:23
visualization_manager.h
rviz::MessageFilterDisplay::onInitialize
void onInitialize() override
Override this function to do subclass-specific initialization.
Definition: message_filter_display.h:105
rviz::Display::context_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz....
Definition: display.h:287
rviz::TwistStampedDisplay
Definition: screw_display.h:77
rviz::AccelStampedDisplay
Definition: screw_display.h:66
class_list_macros.hpp
rviz::Property::changed
void changed()
Emitted by setValue() just after the value has changed.
rviz::ScrewDisplay::updateHistoryLength
void updateHistoryLength()
Definition: screw_display.cpp:130
rviz::ScrewVisual
Definition: screw_visual.h:16
header
const std::string header
rviz::ScrewDisplay::reset
void reset() override
Called to tell the display to clear its state.
Definition: screw_display.cpp:100
rviz::ScrewDisplay::ScrewDisplay
ScrewDisplay()
Definition: screw_display.cpp:42
rviz::ScrewDisplay::processMessagePrivate
void processMessagePrivate(const std_msgs::Header &header, const geometry_msgs::Vector3 &linear, const geometry_msgs::Vector3 &angular)
Definition: screw_display.cpp:137
rviz::IntProperty
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
parse_color.h


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:10