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 }
mapviz_plugins::PathPlugin::Initialize
bool Initialize(QGLWidget *canvas)
Definition: path_plugin.cpp:155
mapviz_plugins::PointDrawingPlugin::StampedPoint
Definition: point_drawing_plugin.h:56
select_topic_dialog.h
mapviz_plugins::PointDrawingPlugin::DrawPoints
virtual bool DrawPoints(double scale)
Definition: point_drawing_plugin.cpp:258
swri_yaml_util::FindValue
bool FindValue(const YAML::Node &node, const std::string &name)
mapviz_plugins::PointDrawingPlugin::ClearPoints
void ClearPoints()
Definition: point_drawing_plugin.cpp:218
mapviz_plugins::PathPlugin::config_widget_
QWidget * config_widget_
Definition: path_plugin.h:88
mapviz_plugins::PathPlugin::PrintError
void PrintError(const std::string &message)
Definition: path_plugin.cpp:133
path_plugin.h
mapviz::MapvizPlugin::PrintErrorHelper
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::PathPlugin::ui_
Ui::path_config ui_
Definition: path_plugin.h:87
mapviz_plugins::PathPlugin::pathCallback
void pathCallback(const nav_msgs::PathConstPtr &path)
Definition: path_plugin.cpp:111
mapviz_plugins::PointDrawingPlugin::SetColor
virtual void SetColor(const QColor &color)
Definition: point_drawing_plugin.cpp:405
mapviz_plugins::PathPlugin::topic_
std::string topic_
Definition: path_plugin.h:90
ros::Subscriber::shutdown
void shutdown()
mapviz_plugins::PointDrawingPlugin::pushPoint
void pushPoint(StampedPoint point)
Definition: point_drawing_plugin.cpp:198
mapviz_plugins::PointDrawingPlugin::StampedPoint::source_frame
std::string source_frame
Definition: point_drawing_plugin.h:66
mapviz_plugins::PathPlugin::LoadConfig
void LoadConfig(const YAML::Node &node, const std::string &path)
Definition: path_plugin.cpp:180
class_list_macros.h
lines
lines
mapviz_plugins::PointDrawingPlugin::LINES
@ LINES
Definition: point_drawing_plugin.h:76
mapviz_plugins::PathPlugin::path_sub_
ros::Subscriber path_sub_
Definition: path_plugin.h:92
tf::Point
tf::Vector3 Point
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
mapviz_plugins::PathPlugin::SelectTopic
void SelectTopic()
Definition: path_plugin.cpp:77
mapviz_plugins::PathPlugin::PrintInfo
void PrintInfo(const std::string &message)
Definition: path_plugin.cpp:138
mapviz_plugins::PathPlugin::has_message_
bool has_message_
Definition: path_plugin.h:93
mapviz_plugins::PathPlugin::PrintWarning
void PrintWarning(const std::string &message)
Definition: path_plugin.cpp:143
mapviz::MapvizPlugin::PrintWarningHelper
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::PathPlugin::GetConfigWidget
QWidget * GetConfigWidget(QWidget *parent)
Definition: path_plugin.cpp:148
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
mapviz_plugins::PointDrawingPlugin::POINTS
@ POINTS
Definition: point_drawing_plugin.h:77
mapviz::MapvizPlugin::PrintInfoHelper
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::PathPlugin::TopicEdited
void TopicEdited()
Definition: path_plugin.cpp:89
mapviz::MapvizPlugin::canvas_
QGLWidget * canvas_
mapviz_plugins::PointDrawingPlugin::SetDrawStyle
virtual void SetDrawStyle(QString style)
Definition: point_drawing_plugin.cpp:125
mapviz_plugins::PathPlugin
Definition: path_plugin.h:57
mapviz_plugins::PointDrawingPlugin::StampedPoint::stamp
ros::Time stamp
Definition: point_drawing_plugin.h:68
mapviz_plugins::PathPlugin::SaveConfig
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
Definition: path_plugin.cpp:200
mapviz::MapvizPlugin
mapviz_plugins::PathPlugin::PathPlugin
PathPlugin()
Definition: path_plugin.cpp:51
mapviz_plugins::PathPlugin::Draw
void Draw(double x, double y, double scale)
Definition: path_plugin.cpp:162
mapviz::MapvizPlugin::initialized_
bool initialized_
mapviz_plugins::PathPlugin::~PathPlugin
virtual ~PathPlugin()
Definition: path_plugin.cpp:73
ROS_INFO
#define ROS_INFO(...)
ros::master::TopicInfo
mapviz_plugins::PointDrawingPlugin::StampedPoint::point
tf::Point point
Definition: point_drawing_plugin.h:60
mapviz_plugins
Definition: attitude_indicator_plugin.h:61
master.h
mapviz::MapvizPlugin::node_
ros::NodeHandle node_
mapviz_plugins::PointDrawingPlugin::DrawIcon
virtual void DrawIcon()
Definition: point_drawing_plugin.cpp:79
mapviz::SelectTopicDialog::selectTopic
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
ros::master::TopicInfo::name
std::string name
mapviz_plugins::PointDrawingPlugin::points
const std::deque< StampedPoint > & points() const
Definition: point_drawing_plugin.cpp:240


mapviz_plugins
Author(s): Marc Alban
autogenerated on Sun Sep 8 2024 02:27:14