wrench_visual.cpp
Go to the documentation of this file.
1 #include <OgreVector3.h>
2 #include <OgreSceneNode.h>
3 #include <OgreSceneManager.h>
4 
7 
8 #include <ros/ros.h>
9 
10 #include "wrench_visual.h"
11 
12 namespace rviz
13 {
14 
15 WrenchVisual::WrenchVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
16 {
17  scene_manager_ = scene_manager;
18 
19  // Ogre::SceneNode s form a tree, with each node storing the
20  // transform (position and orientation) of itself relative to its
21  // parent. Ogre does the math of combining those transforms when it
22  // is time to render.
23  //
24  // Here we create a node to store the pose of the WrenchStamped's header frame
25  // relative to the RViz fixed frame.
26  frame_node_ = parent_node->createChildSceneNode();
27  force_node_ = frame_node_->createChildSceneNode();
28  torque_node_ = frame_node_->createChildSceneNode();
29 
30  // We create the arrow object within the frame node so that we can
31  // set its position and direction relative to its header frame.
36 }
37 
39 {
40  // Delete the arrow to make it disappear.
41  delete arrow_force_;
42  delete arrow_torque_;
43  delete circle_torque_;
44  delete circle_arrow_torque_;
45 
46  // Destroy the frame node since we don't need it anymore.
47  scene_manager_->destroySceneNode( frame_node_ );
48 }
49 
50 
51 void WrenchVisual::setWrench( const geometry_msgs::Wrench& wrench )
52 {
53  Ogre::Vector3 force(wrench.force.x, wrench.force.y, wrench.force.z);
54  Ogre::Vector3 torque(wrench.torque.x, wrench.torque.y, wrench.torque.z);
55  setWrench(force, torque);
56 }
57 
58 void WrenchVisual::setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque )
59 {
60  double force_length = force.length() * force_scale_;
61  double torque_length = torque.length() * torque_scale_;
62  // hide markers if they get too short
63  bool show_force = (force_length > width_);
64  bool show_torque = (torque_length > width_);
65  if (show_force) {
66  arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_));
67  arrow_force_->setDirection(force);
68  }
69  force_node_->setVisible(show_force);
70 
71  if (show_torque) {
72  arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_));
73  arrow_torque_->setDirection(torque);
74  Ogre::Vector3 axis_z(0,0,1);
75  Ogre::Quaternion orientation = axis_z.getRotationTo(torque);
76  if ( std::isnan(orientation.x) ||
77  std::isnan(orientation.y) ||
78  std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY;
79  //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05));
80  circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0);
81  circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0));
82  circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(torque_length/4, 0, torque_length/2));
85  for (int i = 4; i <= 32; i++) {
86  Ogre::Vector3 point = Ogre::Vector3((torque_length/4)*cos(i*2*M_PI/32),
87  (torque_length/4)*sin(i*2*M_PI/32),
88  torque_length/2);
89  circle_torque_->addPoint(orientation * point);
90  }
91  }
92  torque_node_->setVisible(show_torque);
93 }
94 
95 // Position and orientation are passed through to the SceneNode.
96 void WrenchVisual::setFramePosition( const Ogre::Vector3& position )
97 {
98  frame_node_->setPosition( position );
99 }
100 
101 void WrenchVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
102 {
103  frame_node_->setOrientation( orientation );
104 }
105 
106 // Color is passed through to the rviz object.
107 void WrenchVisual::setForceColor( float r, float g, float b, float a )
108 {
109  arrow_force_->setColor( r, g, b, a );
110 }
111 // Color is passed through to the rviz object.
112 void WrenchVisual::setTorqueColor( float r, float g, float b, float a )
113 {
114  arrow_torque_->setColor( r, g, b, a );
115  circle_torque_->setColor( r, g, b, a );
116  circle_arrow_torque_->setColor( r, g, b, a );
117 }
118 
120 {
121  force_scale_ = s;
122 }
123 
125 {
126  torque_scale_ = s;
127 }
128 
129 void WrenchVisual::setWidth( float w )
130 {
131  width_ = w;
132 }
133 
134 void WrenchVisual::setVisible(bool visible)
135 {
136  frame_node_->setVisible(visible);
137 }
138 
139 } // end namespace rviz
rviz::BillboardLine * circle_torque_
Definition: wrench_visual.h:62
void setFrameOrientation(const Ogre::Quaternion &orientation)
void setTorqueScale(float s)
rviz::Arrow * circle_arrow_torque_
Definition: wrench_visual.h:63
Ogre::SceneManager * scene_manager_
Definition: wrench_visual.h:75
void addPoint(const Ogre::Vector3 &point)
Ogre::SceneNode * frame_node_
Definition: wrench_visual.h:68
An object that displays a multi-segment line strip rendered as billboards.
void setWidth(float w)
void setDirection(const Ogre::Vector3 &direction)
Set the direction of the arrow.
Definition: arrow.cpp:123
void setTorqueColor(float r, float g, float b, float a)
void setFramePosition(const Ogre::Vector3 &position)
Ogre::SceneNode * force_node_
Definition: wrench_visual.h:70
void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter)
Set the parameters for this arrow.
Definition: arrow.cpp:71
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
Ogre::SceneNode * torque_node_
Definition: wrench_visual.h:71
void setWrench(const Ogre::Vector3 &force, const Ogre::Vector3 &torque)
void setForceScale(float s)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of the base of the arrow.
Definition: arrow.cpp:111
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: arrow.cpp:131
void setVisible(bool visible)
virtual void setColor(float r, float g, float b, float a)
Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1].
Definition: arrow.cpp:86
rviz::Arrow * arrow_torque_
Definition: wrench_visual.h:61
void setForceColor(float r, float g, float b, float a)
rviz::Arrow * arrow_force_
Definition: wrench_visual.h:60
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
WrenchVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
void setLineWidth(float width)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42