path_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 
32 // C++ standard libraries
33 #include <cstdio>
34 #include <vector>
35 
36 // QT libraries
37 #include <QDialog>
38 #include <QGLWidget>
39 
40 // ROS libraries
41 #include <ros/master.h>
42 
44 
45 // Declare plugin
48 
49 namespace mapviz_plugins
50 {
51  PathPlugin::PathPlugin() : config_widget_(new QWidget())
52  {
53  ui_.setupUi(config_widget_);
54  ui_.path_color->setColor(Qt::green);
55 
56  // Set background white
57  QPalette p(config_widget_->palette());
58  p.setColor(QPalette::Background, Qt::white);
59  config_widget_->setPalette(p);
60 
61  // Set status text red
62  QPalette p3(ui_.status->palette());
63  p3.setColor(QPalette::Text, Qt::red);
64  ui_.status->setPalette(p3);
65 
66  connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic()));
67  connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited()));
68  connect(ui_.path_color, SIGNAL(colorEdited(const QColor&)), this,
69  SLOT(SetColor(const QColor&)));
70 
71  }
72 
74  {
75  }
76 
78  {
81 
82  if (!topic.name.empty())
83  {
84  ui_.topic->setText(QString::fromStdString(topic.name));
85  TopicEdited();
86  }
87  }
88 
90  {
91  std::string topic = ui_.topic->text().trimmed().toStdString();
92  if (topic != topic_)
93  {
94  initialized_ = false;
95  ClearPoints();
96  has_message_ = false;
97  PrintWarning("No messages received.");
98 
100 
101  topic_ = topic;
102  if (!topic.empty())
103  {
105 
106  ROS_INFO("Subscribing to %s", topic_.c_str());
107  }
108  }
109  }
110 
111  void PathPlugin::pathCallback(const nav_msgs::PathConstPtr& path)
112  {
113  if (!has_message_)
114  {
115  initialized_ = true;
116  has_message_ = true;
117  }
118 
119  ClearPoints();
120 
121  for (unsigned int i = 0; i < path->poses.size(); i++)
122  {
123  StampedPoint stamped_point;
124  stamped_point.stamp = path->header.stamp;
125  stamped_point.source_frame = path->header.frame_id;
126  stamped_point.point = tf::Point(path->poses[i].pose.position.x,
127  path->poses[i].pose.position.y, 0);
128 
129  pushPoint( std::move(stamped_point) );
130  }
131  }
132 
133  void PathPlugin::PrintError(const std::string& message)
134  {
135  PrintErrorHelper(ui_.status, message);
136  }
137 
138  void PathPlugin::PrintInfo(const std::string& message)
139  {
140  PrintInfoHelper(ui_.status, message);
141  }
142 
143  void PathPlugin::PrintWarning(const std::string& message)
144  {
145  PrintWarningHelper(ui_.status, message);
146  }
147 
148  QWidget* PathPlugin::GetConfigWidget(QWidget* parent)
149  {
150  config_widget_->setParent(parent);
151 
152  return config_widget_;
153  }
154 
155  bool PathPlugin::Initialize(QGLWidget* canvas)
156  {
157  canvas_ = canvas;
158  DrawIcon();
159  return true;
160  }
161 
162  void PathPlugin::Draw(double x, double y, double scale)
163  {
164  bool lines;
165  bool points;
166  QColor old_color = ui_.path_color->color();
167  QColor color = old_color.dark(200);
168  SetDrawStyle( LINES );
169  lines = DrawPoints(scale);
170  SetColor(color);
171  SetDrawStyle( POINTS );
172  points = DrawPoints(scale);
173  SetColor(old_color);
174  if (lines && points)
175  {
176  PrintInfo("OK");
177  }
178  }
179 
180  void PathPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
181  {
182  if (swri_yaml_util::FindValue(node, "topic"))
183  {
184  std::string topic;
185  node["topic"] >> topic;
186  ui_.topic->setText(topic.c_str());
187  TopicEdited();
188  }
189 
190  if (swri_yaml_util::FindValue(node, "color"))
191  {
192  std::string color;
193  node["color"] >> color;
194  QColor qcolor(color.c_str());
195  SetColor(qcolor);
196  ui_.path_color->setColor(qcolor);
197  }
198  }
199 
200  void PathPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
201  {
202  std::string topic = ui_.topic->text().toStdString();
203  emitter << YAML::Key << "topic" << YAML::Value << topic;
204 
205  std::string color = ui_.path_color->color().name().toStdString();
206  emitter << YAML::Key << "color" << YAML::Value << color;
207  }
208 }
void PrintInfo(const std::string &message)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
ros::NodeHandle node_
ros::Subscriber path_sub_
Definition: path_plugin.h:92
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool Initialize(QGLWidget *canvas)
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)
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void PrintWarning(const std::string &message)
bool FindValue(const YAML::Node &node, const std::string &name)
#define ROS_INFO(...)
lines
QWidget * GetConfigWidget(QWidget *parent)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
virtual void SetColor(const QColor &color)
void LoadConfig(const YAML::Node &node, const std::string &path)
void pathCallback(const nav_msgs::PathConstPtr &path)
void PrintError(const std::string &message)
virtual void SetDrawStyle(QString style)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void Draw(double x, double y, double scale)
const std::deque< StampedPoint > & points() const


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