attitude_indicator_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
31 #include <GL/glut.h>
32 
33 // C++ standard libraries
34 #include <algorithm>
35 #include <cstdio>
36 #include <vector>
37 
38 // QT libraries
39 #include <QDebug>
40 #include <QDialog>
41 #include <QGLWidget>
42 
43 // ROS libraries
44 #include <ros/master.h>
45 
48 
49 // Declare plugin
51 
53 
54 namespace mapviz_plugins
55 {
56 #define IS_INSTANCE(msg, type) \
57  (msg->getDataType() == ros::message_traits::datatype<type>())
58 
60  config_widget_(new QWidget())
61  {
62  ui_.setupUi(config_widget_);
63 
64  // Set background white
65  QPalette p(config_widget_->palette());
66  p.setColor(QPalette::Background, Qt::white);
67  config_widget_->setPalette(p);
68  roll_ = pitch_ = yaw_ = 0;
69  topics_.push_back("nav_msgs/Odometry");
70  topics_.push_back("geometry_msgs/Pose");
71  topics_.push_back("sensor_msgs/Imu");
72  // Set status text red
73  QPalette p3(ui_.status->palette());
74  p3.setColor(QPalette::Text, Qt::red);
75  ui_.status->setPalette(p3);
76 
77  placer_.setRect(QRect(0, 0, 100, 100));
78  QObject::connect(this, SIGNAL(VisibleChanged(bool)),
79  &placer_, SLOT(setVisible(bool)));
80 
81  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic()));
82  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited()));
83  }
84 
86  {
87  }
88 
90  {
92  topics_);
93  if (topic.name.empty())
94  {
95  return;
96  }
97 
98  ui_.topic->setText(QString::fromStdString(topic.name));
99  TopicEdited();
100  }
101 
103  {
104  std::string topic = ui_.topic->text().trimmed().toStdString();
105  if (topic != topic_)
106  {
107  initialized_ = true;
108  PrintWarning("No messages received.");
109 
111  topic_ = topic;
112  if (!topic_.empty())
113  {
116 
117  ROS_INFO("Subscribing to %s", topic_.c_str());
118  }
119  }
120  }
121 
123  {
124  if (IS_INSTANCE(msg, nav_msgs::Odometry))
125  {
126  AttitudeCallbackOdom(msg->instantiate<nav_msgs::Odometry>());
127  }
128  else if (IS_INSTANCE(msg, sensor_msgs::Imu))
129  {
130  AttitudeCallbackImu(msg->instantiate<sensor_msgs::Imu>());
131  }
132  else if (IS_INSTANCE(msg, geometry_msgs::Pose))
133  {
134  AttitudeCallbackPose(msg->instantiate<geometry_msgs::Pose>());
135  }
136  else
137  {
138  PrintError("Unknown message type: " + msg->getDataType());
139  }
140  }
141 
142  void AttitudeIndicatorPlugin::AttitudeCallbackOdom(const nav_msgs::OdometryConstPtr& odometry)
143  {
144  applyAttitudeOrientation(odometry->pose.pose.orientation);
145  }
146 
147  void AttitudeIndicatorPlugin::AttitudeCallbackImu(const sensor_msgs::ImuConstPtr& imu)
148  {
149  applyAttitudeOrientation(imu->orientation);
150  }
151 
152  void AttitudeIndicatorPlugin::AttitudeCallbackPose(const geometry_msgs::PoseConstPtr& pose)
153  {
154  applyAttitudeOrientation(pose->orientation);
155  }
156 
157  void AttitudeIndicatorPlugin::applyAttitudeOrientation(const geometry_msgs::Quaternion &orientation)
158  {
159  tf::Quaternion attitude_orientation(
160  orientation.x,
161  orientation.y,
162  orientation.z,
163  orientation.w);
164 
165  tf::Matrix3x3 m(attitude_orientation);
166  m.getRPY(roll_, pitch_, yaw_);
167  roll_ = roll_ * (180.0 / M_PI);
168  pitch_ = pitch_ * (180.0 / M_PI);
169  yaw_ = yaw_ * (180.0 / M_PI);
170 
171  canvas_->update();
172  }
173 
174  void AttitudeIndicatorPlugin::PrintError(const std::string& message)
175  {
176  PrintErrorHelper(ui_.status, message);
177  }
178 
179  void AttitudeIndicatorPlugin::PrintInfo(const std::string& message)
180  {
181  PrintInfoHelper(ui_.status, message);
182  }
183 
184  void AttitudeIndicatorPlugin::PrintWarning(const std::string& message)
185  {
186  PrintWarningHelper(ui_.status, message);
187  }
188 
190  {
191  config_widget_->setParent(parent);
192  return config_widget_;
193  }
194 
195  bool AttitudeIndicatorPlugin::Initialize(QGLWidget* canvas)
196  {
197  initialized_ = true;
198  canvas_ = canvas;
200  startTimer(50);
201  return true;
202  }
203 
205  {
207  }
208 
210  {
211  canvas_->update();
212  }
213 
215  {
216  GLdouble eqn[4] = {0.0, 0.0, 1.0, 0.0};
217  GLdouble eqn2[4] = {0.0, 0.0, -1.0, 0.0};
218  GLdouble eqn4[4] = {0.0, 0.0, 1.0, 0.05};
219  GLdouble eqn3[4] = {0.0, 0.0, -1.0, 0.05};
220 
221  glEnable(GL_DEPTH_TEST);
222  glDepthFunc(GL_LESS);
223 
224  glPushMatrix();
225 
226  glColor3f(0.392156863f, 0.584313725f, 0.929411765f);
227  glRotated(90.0 + pitch_, 1.0, 0.0, 0.0);
228 
229  glRotated(roll_, 0.0, 1.0, 0.0);
230  glRotated(yaw_, 0.0, 0.0, 1.0);
231  glClipPlane(GL_CLIP_PLANE1, eqn2);
232  glEnable(GL_CLIP_PLANE1);
233  glutSolidSphere(.8, 20, 16);
234  glDisable(GL_CLIP_PLANE1);
235  glPopMatrix();
236 
237  glPushMatrix();
238 
239  glLineWidth(2);
240  glColor3f(1.0f, 1.0f, 1.0f);
241  glRotated(90.0 + pitch_, 1.0, 0.0, 0.0);
242  glRotated(roll_, 0.0, 1.0, 0.0);
243  glRotated(yaw_, 0.0, 0.0, 1.0);
244  glClipPlane(GL_CLIP_PLANE3, eqn4);
245  glClipPlane(GL_CLIP_PLANE2, eqn3);
246  glEnable(GL_CLIP_PLANE2);
247  glEnable(GL_CLIP_PLANE3);
248  glutWireSphere(.801, 10, 16);
249  glDisable(GL_CLIP_PLANE2);
250  glDisable(GL_CLIP_PLANE3);
251  glPopMatrix();
252 
253  glPushMatrix();
254  glColor3f(0.62745098f, 0.321568627f, 0.176470588f);
255  glRotated(90.0 + pitch_, 1.0, 0.0, 0.0);//x
256  glRotated(roll_, 0.0, 1.0, 0.0);//y
257  glRotated(yaw_, 0.0, 0.0, 1.0);//z
258  glClipPlane(GL_CLIP_PLANE0, eqn);
259  glEnable(GL_CLIP_PLANE0);
260  glutSolidSphere(.8, 20, 16);
261  glDisable(GL_CLIP_PLANE0);
262  glPopMatrix();
263  glDisable(GL_DEPTH_TEST);
264  }
265 
266  void AttitudeIndicatorPlugin::Draw(double x, double y, double scale)
267  {
268  glPushAttrib(GL_ALL_ATTRIB_BITS);
269  glMatrixMode(GL_PROJECTION);
270  glPushMatrix();
271  glLoadIdentity();
272  glOrtho(0, canvas_->width(), canvas_->height(), 0, -1.0f, 1.0f);
273  glMatrixMode(GL_MODELVIEW);
274  glPushMatrix();
275  glLoadIdentity();
276  // Setup coordinate system so that we have a [-1,1]x[1,1] cube on
277  // the screen.
278  QRect rect = placer_.rect();
279  double s_x = rect.width() / 2.0;
280  double s_y = -rect.height() / 2.0;
281  double t_x = rect.right() - s_x;
282  double t_y = rect.top() - s_y;
283 
284  double m[16] = {
285  s_x, 0, 0, 0,
286  0, s_y, 0, 0,
287  0, 0, 1.0, 0,
288  t_x, t_y, 0, 1.0};
289  glMultMatrixd(m);
290 
291  // Placed in a separate function so that we don't forget to pop the
292  // GL state back.
293 
294  drawBackground();
295  drawBall();
296 
297  drawPanel();
298 
299  glPopMatrix();
300  glMatrixMode(GL_PROJECTION);
301  glPopMatrix();
302  glMatrixMode(GL_MODELVIEW);
303  glPopAttrib();
304  PrintInfo("OK!");
305  }
306 
308  {
309  glBegin(GL_TRIANGLES);
310  glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
311 
312  glVertex2d(-1.0, -1.0);
313  glVertex2d(-1.0, 1.0);
314  glVertex2d(1.0, 1.0);
315 
316  glVertex2d(-1.0, -1.0);
317  glVertex2d(1.0, 1.0);
318  glVertex2d(1.0, -1.0);
319 
320  glEnd();
321  }
322 
324  {
325  glLineWidth(2);
326 
327  glBegin(GL_LINE_STRIP);
328  glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
329 
330  glVertex2d(-0.9, 0.0);
331  glVertex2d(-0.2, 0.0);
332 
333  int divisions = 20;
334  for (int i = 1; i < divisions; i++)
335  {
336  glVertex2d(-0.2 * std::cos(M_PI * i / divisions),
337  -0.2 * std::sin(M_PI * i / divisions));
338  }
339 
340  glVertex2f(0.2, 0.0);
341  glVertex2f(0.9, 0.0);
342  glEnd();
343 
344  glBegin(GL_LINES);
345  glVertex2f(0.0, -0.2f);
346  glVertex2f(0.0, -0.9f);
347  glEnd();
348  }
349 
350  void AttitudeIndicatorPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
351  {
352  if (node["topic"])
353  {
354  std::string topic;
355  node["topic"] >> topic;
356  ui_.topic->setText(topic.c_str());
357  }
358 
359  QRect current = placer_.rect();
360  int x = current.x();
361  int y = current.y();
362  int width = current.width();
363  int height = current.height();
364 
365  if (swri_yaml_util::FindValue(node, "x"))
366  {
367  node["x"] >> x;
368  }
369 
370  if (swri_yaml_util::FindValue(node, "y"))
371  {
372  node["y"] >> y;
373  }
374 
375  if (swri_yaml_util::FindValue(node, "width"))
376  {
377  node["width"] >> width;
378  }
379 
380  if (swri_yaml_util::FindValue(node, "height"))
381  {
382  node["height"] >> height;
383  }
384 
385  QRect position(x, y, width, height);
386  placer_.setRect(position);
387 
388  TopicEdited();
389  }
390 
391  void AttitudeIndicatorPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
392  {
393  emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString();
394 
395  QRect position = placer_.rect();
396 
397  emitter << YAML::Key << "x" << YAML::Value << position.x();
398  emitter << YAML::Key << "y" << YAML::Value << position.y();
399  emitter << YAML::Key << "width" << YAML::Value << position.width();
400  emitter << YAML::Key << "height" << YAML::Value << position.height();
401  }
402 }
#define NULL
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
void LoadConfig(const YAML::Node &node, const std::string &path)
ros::NodeHandle node_
void AttitudeCallbackImu(const sensor_msgs::ImuConstPtr &imu)
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void Draw(double x, double y, double scale)
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void applyAttitudeOrientation(const geometry_msgs::Quaternion &orientation)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
#define IS_INSTANCE(msg, type)
bool FindValue(const YAML::Node &node, const std::string &name)
#define ROS_INFO(...)
void handleMessage(const topic_tools::ShapeShifter::ConstPtr &msg)
void VisibleChanged(bool visible)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
void AttitudeCallbackOdom(const nav_msgs::OdometryConstPtr &odometry)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void AttitudeCallbackPose(const geometry_msgs::PoseConstPtr &pose)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:33