path_plugin.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <mapviz_plugins/path_plugin.h>
00031 
00032 // C++ standard libraries
00033 #include <cstdio>
00034 #include <vector>
00035 
00036 // QT libraries
00037 #include <QDialog>
00038 #include <QGLWidget>
00039 
00040 // ROS libraries
00041 #include <ros/master.h>
00042 
00043 #include <mapviz/select_topic_dialog.h>
00044 
00045 // Declare plugin
00046 #include <pluginlib/class_list_macros.h>
00047 PLUGINLIB_DECLARE_CLASS(mapviz_plugins,
00048                         path,
00049                         mapviz_plugins::PathPlugin,
00050                         mapviz::MapvizPlugin)
00051 
00052 namespace mapviz_plugins
00053 {
00054   PathPlugin::PathPlugin() : config_widget_(new QWidget())
00055   {
00056     ui_.setupUi(config_widget_);
00057     ui_.path_color->setColor(Qt::green);
00058 
00059     // Set background white
00060     QPalette p(config_widget_->palette());
00061     p.setColor(QPalette::Background, Qt::white);
00062     config_widget_->setPalette(p);
00063 
00064     // Set status text red
00065     QPalette p3(ui_.status->palette());
00066     p3.setColor(QPalette::Text, Qt::red);
00067     ui_.status->setPalette(p3);
00068 
00069     connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic()));
00070     connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited()));
00071     connect(ui_.path_color, SIGNAL(colorEdited(const QColor&)), this,
00072             SLOT(SetColor(const QColor&)));
00073   }
00074 
00075   PathPlugin::~PathPlugin()
00076   {
00077   }
00078 
00079   void PathPlugin::SelectTopic()
00080   {
00081     ros::master::TopicInfo topic =
00082         mapviz::SelectTopicDialog::selectTopic("nav_msgs/Path");
00083 
00084     if (!topic.name.empty())
00085     {
00086       ui_.topic->setText(QString::fromStdString(topic.name));
00087       TopicEdited();
00088     }
00089   }
00090 
00091   void PathPlugin::TopicEdited()
00092   {
00093     std::string topic = ui_.topic->text().trimmed().toStdString();
00094     if (topic != topic_)
00095     {
00096       initialized_ = false;
00097       points_.clear();
00098       has_message_ = false;
00099       PrintWarning("No messages received.");
00100 
00101       path_sub_.shutdown();
00102 
00103       topic_ = topic;
00104       if (!topic.empty())
00105       {
00106         path_sub_ = node_.subscribe(topic_, 1, &PathPlugin::pathCallback, this);
00107 
00108         ROS_INFO("Subscribing to %s", topic_.c_str());
00109       }
00110     }
00111   }
00112 
00113   void PathPlugin::pathCallback(const nav_msgs::PathConstPtr& path)
00114   {
00115     if (!has_message_)
00116     {
00117       initialized_ = true;
00118       has_message_ = true;
00119     }
00120 
00121     points_.clear();
00122 
00123     for (unsigned int i = 0; i < path->poses.size(); i++)
00124     {
00125       StampedPoint stamped_point;
00126       stamped_point.stamp = path->header.stamp;
00127       stamped_point.source_frame = path->header.frame_id;
00128       stamped_point.point = tf::Point(path->poses[i].pose.position.x,
00129                                       path->poses[i].pose.position.y, 0);
00130 
00131       points_.push_back(stamped_point);
00132     }
00133   }
00134 
00135   void PathPlugin::PrintError(const std::string& message)
00136   {
00137     if (message == ui_.status->text().toStdString())
00138     {
00139       return;
00140     }
00141 
00142     ROS_ERROR("Error: %s", message.c_str());
00143     QPalette p(ui_.status->palette());
00144     p.setColor(QPalette::Text, Qt::red);
00145     ui_.status->setPalette(p);
00146     ui_.status->setText(message.c_str());
00147   }
00148 
00149   void PathPlugin::PrintInfo(const std::string& message)
00150   {
00151     if (message == ui_.status->text().toStdString())
00152     {
00153       return;
00154     }
00155 
00156     ROS_INFO("%s", message.c_str());
00157     QPalette p(ui_.status->palette());
00158     p.setColor(QPalette::Text, Qt::green);
00159     ui_.status->setPalette(p);
00160     ui_.status->setText(message.c_str());
00161   }
00162 
00163   void PathPlugin::PrintWarning(const std::string& message)
00164   {
00165     if (message == ui_.status->text().toStdString())
00166     {
00167       return;
00168     }
00169 
00170     ROS_WARN("%s", message.c_str());
00171     QPalette p(ui_.status->palette());
00172     p.setColor(QPalette::Text, Qt::darkYellow);
00173     ui_.status->setPalette(p);
00174     ui_.status->setText(message.c_str());
00175   }
00176 
00177   QWidget* PathPlugin::GetConfigWidget(QWidget* parent)
00178   {
00179     config_widget_->setParent(parent);
00180 
00181     return config_widget_;
00182   }
00183 
00184   bool PathPlugin::Initialize(QGLWidget* canvas)
00185   {
00186     canvas_ = canvas;
00187     DrawIcon();
00188     return true;
00189   }
00190 
00191   void PathPlugin::Draw(double x, double y, double scale)
00192   {
00193     bool lines;
00194     bool points;
00195     QColor old_color = ui_.path_color->color();
00196     draw_style_ = LINES;
00197     lines = DrawPoints(scale);
00198     color_ = color_.dark(200);
00199     draw_style_ = POINTS;
00200     points = DrawPoints(scale);
00201     color_ = old_color;
00202     if (lines && points)
00203     {
00204       PrintInfo("OK");
00205     }
00206   }
00207 
00208   void PathPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
00209   {
00210     if (swri_yaml_util::FindValue(node, "color"))
00211     {
00212       std::string topic;
00213       node["topic"] >> topic;
00214       ui_.topic->setText(topic.c_str());
00215       TopicEdited();
00216     }
00217 
00218     if (swri_yaml_util::FindValue(node, "color"))
00219     {
00220       std::string color;
00221       node["color"] >> color;
00222       SetColor(QColor(color.c_str()));
00223       ui_.path_color->setColor(color_);
00224     }
00225   }
00226 
00227   void PathPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
00228   {
00229     std::string topic = ui_.topic->text().toStdString();
00230     emitter << YAML::Key << "topic" << YAML::Value << topic;
00231 
00232     std::string color = ui_.path_color->color().name().toStdString();
00233     emitter << YAML::Key << "color" << YAML::Value << color;
00234   }
00235 }


mapviz_plugins
Author(s): Marc Alban
autogenerated on Thu Aug 24 2017 02:46:09