screw_visual.cpp
Go to the documentation of this file.
2 #include <OgreSceneNode.h>
3 #include <OgreSceneManager.h>
4 
7 
8 #include <ros/ros.h>
9 
10 #include "screw_visual.h"
11 
12 namespace rviz
13 {
14 ScrewVisual::ScrewVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
15 {
16  scene_manager_ = scene_manager;
17 
18  // Ogre::SceneNode s form a tree, with each node storing the transform (position and orientation)
19  // of itself relative to its parent. Ogre does the math of combining those transforms for rendering.
20  // Here we create a node to store the pose of the screw's header frame relative to the RViz fixed frame.
21  frame_node_ = parent_node->createChildSceneNode();
22  linear_node_ = frame_node_->createChildSceneNode();
23  angular_node_ = frame_node_->createChildSceneNode();
24  hide_small_values_ = true;
25 
26  // We create the arrow object within the frame node so that we can
27  // set its position and direction relative to its header frame.
32 }
33 
35 {
36  // Delete the arrow to make it disappear.
37  delete arrow_linear_;
38  delete arrow_angular_;
39  delete circle_angular_;
40  delete circle_arrow_angular_;
41 
42  // Destroy the frame node since we don't need it anymore.
43  scene_manager_->destroySceneNode(frame_node_);
44 }
45 
46 
47 void ScrewVisual::setScrew(const geometry_msgs::Vector3& linear, const geometry_msgs::Vector3& angular)
48 {
49  setScrew(Ogre::Vector3(linear.x, linear.y, linear.z), Ogre::Vector3(angular.x, angular.y, angular.z));
50 }
51 
52 void ScrewVisual::setScrew(const Ogre::Vector3& linear, const Ogre::Vector3& angular)
53 {
54  double linear_length = linear.length() * linear_scale_;
55  double angular_length = angular.length() * angular_scale_;
56  // hide markers if they get too short and hide_small_values_ is activated
57  // "too short" is defined as "linear_length > width_"
58  bool show_linear = (linear_length > width_) || !hide_small_values_;
59  bool show_angular = (angular_length > width_) || !hide_small_values_;
60 
61  if (show_linear)
62  {
63  arrow_linear_->setScale(Ogre::Vector3(linear_length, width_, width_));
65  }
66  linear_node_->setVisible(show_linear);
67 
68  if (show_angular)
69  {
70  arrow_angular_->setScale(Ogre::Vector3(angular_length, width_, width_));
72  Ogre::Vector3 axis_z(0, 0, 1);
73  Ogre::Quaternion orientation = axis_z.getRotationTo(angular);
74  if (std::isnan(orientation.x) || std::isnan(orientation.y) || std::isnan(orientation.z))
75  orientation = Ogre::Quaternion::IDENTITY;
76  // circle_arrow_angular_->setScale(Ogre::Vector3(width_, width_, 0.05));
77  circle_arrow_angular_->set(0, width_ * 0.1, width_ * 0.1 * 1.0, width_ * 0.1 * 2.0);
78  circle_arrow_angular_->setDirection(orientation * Ogre::Vector3(0, 1, 0));
79  circle_arrow_angular_->setPosition(orientation *
80  Ogre::Vector3(angular_length / 4, 0, angular_length / 2));
83  for (int i = 4; i <= 32; i++)
84  {
85  Ogre::Vector3 point =
86  Ogre::Vector3((angular_length / 4) * cos(i * 2 * M_PI / 32),
87  (angular_length / 4) * sin(i * 2 * M_PI / 32), angular_length / 2);
88  circle_angular_->addPoint(orientation * point);
89  }
90  }
91  angular_node_->setVisible(show_angular);
92 }
93 
94 // Position and orientation are passed through to the SceneNode.
95 void ScrewVisual::setFramePosition(const Ogre::Vector3& position)
96 {
97  frame_node_->setPosition(position);
98 }
99 
100 void ScrewVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
101 {
102  frame_node_->setOrientation(orientation);
103 }
104 
105 // Color is passed through to the rviz object.
106 void ScrewVisual::setLinearColor(float r, float g, float b, float a)
107 {
108  arrow_linear_->setColor(r, g, b, a);
109 }
110 // Color is passed through to the rviz object.
111 void ScrewVisual::setAngularColor(float r, float g, float b, float a)
112 {
113  arrow_angular_->setColor(r, g, b, a);
114  circle_angular_->setColor(r, g, b, a);
115  circle_arrow_angular_->setColor(r, g, b, a);
116 }
117 
119 {
120  linear_scale_ = s;
121 }
122 
124 {
125  angular_scale_ = s;
126 }
127 
129 {
130  width_ = w;
131 }
132 
134 {
135  hide_small_values_ = h;
136 }
137 
138 
139 void ScrewVisual::setVisible(bool visible)
140 {
141  frame_node_->setVisible(visible);
142 }
143 
144 } // end namespace rviz
rviz::ScrewVisual::setAngularColor
void setAngularColor(float r, float g, float b, float a)
Definition: screw_visual.cpp:111
rviz::Arrow::set
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
Definition: arrow.cpp:74
rviz::BillboardLine::clear
void clear()
Definition: billboard_line.cpp:105
rviz::BillboardLine
An object that displays a multi-segment line strip rendered as billboards.
Definition: billboard_line.h:58
rviz::BillboardLine::setLineWidth
void setLineWidth(float width)
Definition: billboard_line.cpp:237
rviz::ScrewVisual::ScrewVisual
ScrewVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
Definition: screw_visual.cpp:14
rviz::Arrow
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:57
linear
constexpr const char * linear()
Definition: screw_display.cpp:18
rviz::ScrewVisual::arrow_linear_
rviz::Arrow * arrow_linear_
Definition: screw_visual.h:49
s
XmlRpcServer s
rviz::ScrewVisual::frame_node_
Ogre::SceneNode * frame_node_
Definition: screw_visual.h:57
rviz::ScrewVisual::setFrameOrientation
void setFrameOrientation(const Ogre::Quaternion &orientation)
Definition: screw_visual.cpp:100
ros.h
rviz::ScrewVisual::setScrew
void setScrew(const Ogre::Vector3 &linear, const Ogre::Vector3 &angular)
Definition: screw_visual.cpp:52
rviz::ScrewVisual::angular_scale_
float angular_scale_
Definition: screw_visual.h:53
rviz::ScrewVisual::angular_node_
Ogre::SceneNode * angular_node_
Definition: screw_visual.h:60
screw_visual.h
rviz::ScrewVisual::linear_node_
Ogre::SceneNode * linear_node_
Definition: screw_visual.h:59
rviz::ScrewVisual::scene_manager_
Ogre::SceneManager * scene_manager_
Definition: screw_visual.h:63
rviz::ScrewVisual::setWidth
void setWidth(float w)
Definition: screw_visual.cpp:128
rviz::ScrewVisual::~ScrewVisual
virtual ~ScrewVisual()
Definition: screw_visual.cpp:34
rviz
Definition: add_display_dialog.cpp:54
rviz::ScrewVisual::setLinearScale
void setLinearScale(float s)
Definition: screw_visual.cpp:118
rviz::ScrewVisual::circle_arrow_angular_
rviz::Arrow * circle_arrow_angular_
Definition: screw_visual.h:52
arrow.h
ogre_vector.h
angular
constexpr const char * angular()
Definition: screw_display.cpp:23
billboard_line.h
rviz::Arrow::setDirection
void setDirection(const Ogre::Vector3 &direction)
Set the direction of the arrow.
Definition: arrow.cpp:126
rviz::ScrewVisual::linear_scale_
float linear_scale_
Definition: screw_visual.h:53
rviz::ScrewVisual::hide_small_values_
bool hide_small_values_
Definition: screw_visual.h:54
rviz::ScrewVisual::setHideSmallValues
void setHideSmallValues(bool h)
Definition: screw_visual.cpp:133
rviz::ScrewVisual::circle_angular_
rviz::BillboardLine * circle_angular_
Definition: screw_visual.h:51
rviz::ScrewVisual::width_
float width_
Definition: screw_visual.h:53
rviz::ScrewVisual::setVisible
void setVisible(bool visible)
Definition: screw_visual.cpp:139
rviz::ScrewVisual::setAngularScale
void setAngularScale(float s)
Definition: screw_visual.cpp:123
rviz::ScrewVisual::setLinearColor
void setLinearColor(float r, float g, float b, float a)
Definition: screw_visual.cpp:106
rviz::Arrow::setPosition
void setPosition(const Ogre::Vector3 &position) override
Set the position of the base of the arrow.
Definition: arrow.cpp:114
rviz::Arrow::setColor
void setColor(float r, float g, float b, float a) override
Set the color of this arrow. Sets both the head and shaft color to the same value....
Definition: arrow.cpp:89
rviz::ScrewVisual::setFramePosition
void setFramePosition(const Ogre::Vector3 &position)
Definition: screw_visual.cpp:95
rviz::ScrewVisual::arrow_angular_
rviz::Arrow * arrow_angular_
Definition: screw_visual.h:50
rviz::BillboardLine::setColor
void setColor(float r, float g, float b, float a) override
Set the color of the object. Values are in the range [0, 1].
Definition: billboard_line.cpp:271
rviz::Arrow::setScale
void setScale(const Ogre::Vector3 &scale) override
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: arrow.cpp:134
rviz::BillboardLine::addPoint
void addPoint(const Ogre::Vector3 &point)
Definition: billboard_line.cpp:208


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