effort_visual.cpp
Go to the documentation of this file.
1 #include <OgreVector3.h>
2 #include <OgreSceneNode.h>
3 #include <OgreSceneManager.h>
4 
7 #include <rviz/validate_floats.h>
8 
9 #include <ros/ros.h>
10 
11 #include <urdf/model.h>
12 #include "effort_visual.h"
13 
14 namespace rviz
15 {
16 EffortVisual::EffortVisual(Ogre::SceneManager* scene_manager,
17  Ogre::SceneNode* parent_node,
19 {
20  scene_manager_ = scene_manager;
21 
22  // Ogre::SceneNode s form a tree, with each node storing the
23  // transform (position and orientation) of itself relative to its
24  // parent. Ogre does the math of combining those transforms when it
25  // is time to render.
26  //
27  // Here we create a node to store the pose of the Effort's header frame
28  // relative to the RViz fixed frame.
29  frame_node_ = parent_node->createChildSceneNode();
30 
31  //
32  urdf_model_ = urdf_model;
33 
34  // We create the arrow object within the frame node so that we can
35  // set its position and direction relative to its header frame.
36  for (std::map<std::string, urdf::JointSharedPtr>::iterator it = urdf_model_->joints_.begin();
37  it != urdf_model_->joints_.end(); it++)
38  {
39  if (it->second->type == urdf::Joint::REVOLUTE)
40  {
41  std::string joint_name = it->first;
42  effort_enabled_[joint_name] = true;
43  }
44  }
45 }
46 
48 {
49  // Delete the arrow to make it disappear.
50  for (std::map<std::string, rviz::BillboardLine*>::iterator it = effort_circle_.begin();
51  it != effort_circle_.end(); it++)
52  {
53  delete (it->second);
54  }
55  for (std::map<std::string, rviz::Arrow*>::iterator it = effort_arrow_.begin();
56  it != effort_arrow_.end(); it++)
57  {
58  delete (it->second);
59  }
60 
61  // Destroy the frame node since we don't need it anymore.
62  scene_manager_->destroySceneNode(frame_node_);
63 }
64 
65 
66 void EffortVisual::getRainbowColor(float value, Ogre::ColourValue& color)
67 {
68  value = std::min(value, 1.0f);
69  value = std::max(value, 0.0f);
70 
71  float h = value * 5.0f + 1.0f;
72  int i = floor(h);
73  float f = h - i;
74  if (!(i & 1))
75  f = 1 - f; // if i is even
76  float n = 1 - f;
77 
78  if (i <= 1)
79  color[0] = n, color[1] = 0, color[2] = 1;
80  else if (i == 2)
81  color[0] = 0, color[1] = n, color[2] = 1;
82  else if (i == 3)
83  color[0] = 0, color[1] = 1, color[2] = n;
84  else if (i == 4)
85  color[0] = n, color[1] = 1, color[2] = 0;
86  else if (i >= 5)
87  color[0] = 1, color[1] = n, color[2] = 0;
88 }
89 
90 void EffortVisual::setMessage(const sensor_msgs::JointStateConstPtr& msg)
91 {
92  // for all joints...
93  int joint_num = msg->name.size();
94  for (int i = 0; i < joint_num; i++)
95  {
96  std::string joint_name = msg->name[i];
97  double effort = msg->effort[i];
98  if (!validateFloats(effort))
99  {
100  ROS_WARN_STREAM("Invalid effort for joint '" << joint_name << "': " << effort);
101  continue;
102  }
103  const urdf::Joint* joint = urdf_model_->getJoint(joint_name).get();
104  int joint_type = joint->type;
105  if (joint_type == urdf::Joint::REVOLUTE)
106  {
107  // enable or disable draw
108  if (effort_circle_.find(joint_name) != effort_circle_.end() &&
109  !effort_enabled_[joint_name]) // enable->disable
110  {
111  delete (effort_circle_[joint_name]);
112  delete (effort_arrow_[joint_name]);
113  effort_circle_.erase(joint_name);
114  effort_arrow_.erase(joint_name);
115  }
116  if (effort_circle_.find(joint_name) == effort_circle_.end() &&
117  effort_enabled_[joint_name]) // disable -> enable
118  {
121  }
122 
123  if (!effort_enabled_[joint_name])
124  continue;
125 
126  // tf::Transform offset = poseFromJoint(joint);
127  urdf::JointLimitsSharedPtr limit = joint->limits;
128  double max_effort = limit->effort, effort_value = 0.05;
129 
130  if (max_effort != 0.0)
131  {
132  effort_value = std::min(fabs(effort) / max_effort, 1.0) + 0.05;
133  }
134  else
135  {
136  effort_value = fabs(effort) + 0.05;
137  }
138 
139  effort_arrow_[joint_name]->set(0, width_ * 2, width_ * 2 * 1.0, width_ * 2 * 2.0);
140  if (effort > 0)
141  {
142  effort_arrow_[joint_name]->setDirection(orientation_[joint_name] * Ogre::Vector3(-1, 0, 0));
143  }
144  else
145  {
146  effort_arrow_[joint_name]->setDirection(orientation_[joint_name] * Ogre::Vector3(1, 0, 0));
147  }
148  effort_arrow_[joint_name]->setPosition(
149  orientation_[joint_name] * Ogre::Vector3(0, 0.05 + effort_value * scale_ * 0.5, 0) +
150  position_[joint_name]);
151  effort_circle_[joint_name]->clear();
152  effort_circle_[joint_name]->setLineWidth(width_);
153  for (int i = 0; i < 30; i++)
154  {
155  Ogre::Vector3 point =
156  Ogre::Vector3((0.05 + effort_value * scale_ * 0.5) * sin(i * 2 * M_PI / 32),
157  (0.05 + effort_value * scale_ * 0.5) * cos(i * 2 * M_PI / 32), 0);
158  if (effort < 0)
159  point.x = -point.x;
160  effort_circle_[joint_name]->addPoint(orientation_[joint_name] * point + position_[joint_name]);
161  }
162  Ogre::ColourValue color;
163  getRainbowColor(effort_value, color);
164  effort_arrow_[joint_name]->setColor(color.r, color.g, color.b, color.a);
165  effort_circle_[joint_name]->setColor(color.r, color.g, color.b, color.a);
166  }
167  }
168 }
169 
170 void EffortVisual::setFrameEnabled(const std::string joint_name, const bool e)
171 {
172  effort_enabled_[joint_name] = e;
173 }
174 
175 // Position and orientation are passed through to the SceneNode.
176 void EffortVisual::setFramePosition(const Ogre::Vector3& position)
177 {
178  frame_node_->setPosition(position);
179 }
180 
181 void EffortVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
182 {
183  frame_node_->setOrientation(orientation);
184 }
185 // Position and orientation are passed through to the SceneNode.
186 void EffortVisual::setFramePosition(const std::string joint_name, const Ogre::Vector3& position)
187 {
188  position_[joint_name] = position;
189 }
190 
191 void EffortVisual::setFrameOrientation(const std::string joint_name, const Ogre::Quaternion& orientation)
192 {
193  orientation_[joint_name] = orientation;
194 }
195 
197 {
198  width_ = w;
199 }
200 
202 {
203  scale_ = s;
204 }
205 
206 } // end namespace rviz
std::map< std::string, Ogre::Quaternion > orientation_
Definition: effort_visual.h:83
void getRainbowColor(float value, Ogre::ColourValue &color)
void setMessage(const sensor_msgs::JointStateConstPtr &msg)
f
An object that displays a multi-segment line strip rendered as billboards.
static int limit(int i)
Definition: parse_color.cpp:34
void setFramePosition(const Ogre::Vector3 &position)
std::map< std::string, rviz::Arrow * > effort_arrow_
Definition: effort_visual.h:71
Ogre::SceneNode * frame_node_
Definition: effort_visual.h:76
std::map< std::string, Ogre::Vector3 > position_
Definition: effort_visual.h:82
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void setScale(float s)
void setWidth(float w)
std::map< std::string, rviz::BillboardLine * > effort_circle_
Definition: effort_visual.h:70
Ogre::SceneManager * scene_manager_
Definition: effort_visual.h:80
#define ROS_WARN_STREAM(args)
std::map< std::string, bool > effort_enabled_
Definition: effort_visual.h:72
EffortVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node, boost::shared_ptr< urdf::Model > urdf_model)
void setFrameEnabled(const std::string joint_name, const bool e)
void setFrameOrientation(const Ogre::Quaternion &orientation)
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
boost::shared_ptr< urdf::Model > urdf_model_
Definition: effort_visual.h:88
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Aug 15 2022 02:05:16