point_click_publisher_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/point_click_publisher_plugin.h"
00031 #include <geometry_msgs/PointStamped.h>
00032 #include <swri_transform_util/frames.h>
00033 #include <swri_yaml_util/yaml_util.h>
00034 
00035 #include <boost/shared_ptr.hpp>
00036 
00037 // Declare plugin
00038 #include <pluginlib/class_list_macros.h>
00039 PLUGINLIB_DECLARE_CLASS(
00040     mapviz_plugins,
00041     point_click_publisher,
00042     mapviz_plugins::PointClickPublisherPlugin,
00043     mapviz::MapvizPlugin)
00044 
00045 namespace mapviz_plugins
00046 {
00047   PointClickPublisherPlugin::PointClickPublisherPlugin() :
00048     config_widget_(new QWidget()),
00049     canvas_(NULL)
00050   {
00051     ui_.setupUi(config_widget_);
00052 
00053     connect(&click_filter_, SIGNAL(pointClicked(const QPointF&)),
00054             this, SLOT(pointClicked(const QPointF&)));
00055     connect(ui_.topic, SIGNAL(textEdited(const QString&)),
00056             this, SLOT(topicChanged(const QString&)));
00057 
00058     frame_timer_.start(1000);
00059     connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(updateFrames()));
00060   }
00061 
00062   PointClickPublisherPlugin::~PointClickPublisherPlugin()
00063   {
00064     if (canvas_)
00065     {
00066       canvas_->removeEventFilter(&click_filter_);
00067     }
00068   }
00069 
00070   bool PointClickPublisherPlugin::Initialize(QGLWidget* canvas)
00071   {
00072     canvas_ = static_cast<mapviz::MapCanvas*>(canvas);
00073     canvas_->installEventFilter(&click_filter_);
00074 
00075     PrintInfo("Ready.");
00076 
00077     return true;
00078   }
00079 
00080   void PointClickPublisherPlugin::Draw(double x, double y, double scale)
00081   {
00082   }
00083 
00084   void PointClickPublisherPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
00085   {
00086     std::string tmp;
00087     if (swri_yaml_util::FindValue(node, "topic"))
00088     {
00089       node["topic"] >> tmp;
00090       ui_.topic->setText(QString(tmp.c_str()));
00091       topicChanged(ui_.topic->text());
00092     }
00093 
00094     if (swri_yaml_util::FindValue(node, "output_frame"))
00095     {
00096       node["output_frame"] >> tmp;
00097       ui_.outputframe->addItem(QString(tmp.c_str()));
00098     }
00099   }
00100 
00101   void PointClickPublisherPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
00102   {
00103     emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString();
00104     emitter << YAML::Key << "output_frame" << YAML::Value << ui_.outputframe->currentText().toStdString();
00105   }
00106 
00107   QWidget* PointClickPublisherPlugin::GetConfigWidget(QWidget* parent)
00108   {
00109     config_widget_->setParent(parent);
00110 
00111     return config_widget_;
00112   }
00113 
00114 
00115   void PointClickPublisherPlugin::pointClicked(const QPointF& point)
00116   {
00117     QPointF transformed = canvas_->MapGlCoordToFixedFrame(point);
00118 
00119     std::string output_frame = ui_.outputframe->currentText().toStdString();
00120 
00121     if (target_frame_ != output_frame)
00122     {
00123       swri_transform_util::Transform tf;
00124       tf::Point tfPoint(transformed.x(), transformed.y(), 0.0);
00125       if (tf_manager_.GetTransform(output_frame, target_frame_, tf))
00126       {
00127         tfPoint = tf * tfPoint;
00128       }
00129       else
00130       {
00131         std::stringstream error;
00132         error << "Unable to find transform from " << target_frame_ << " to " << output_frame << ".";
00133         PrintError(error.str());
00134         return;
00135       }
00136       transformed.setX(tfPoint.x());
00137       transformed.setY(tfPoint.y());
00138     }
00139 
00140     std::stringstream ss;
00141     ss << "Point in " << output_frame.c_str() << ": " << transformed.x() << "," << transformed.y();
00142     PrintInfo(ss.str());
00143 
00144     boost::shared_ptr<geometry_msgs::PointStamped> stamped = boost::make_shared<geometry_msgs::PointStamped>();
00145     stamped->header.frame_id = output_frame;
00146     stamped->header.stamp = ros::Time::now();
00147     stamped->point.x = transformed.x();
00148     stamped->point.y = transformed.y();
00149     stamped->point.z = 0.0;
00150 
00151     point_publisher_.publish(stamped);
00152   }
00153 
00154   void PointClickPublisherPlugin::SetNode(const ros::NodeHandle& node)
00155   {
00156     mapviz::MapvizPlugin::SetNode(node);
00157 
00158     // We override this method so that we can initialize our publisher after
00159     // our node has been set, ensuring that it's in mapviz's namespace.
00160     topicChanged(ui_.topic->text());
00161   }
00162 
00163   void PointClickPublisherPlugin::PrintError(const std::string& message)
00164   {
00165     if (message == ui_.status->text().toStdString())
00166     {
00167       return;
00168     }
00169 
00170     ROS_ERROR("Error: %s", message.c_str());
00171     QPalette p(ui_.status->palette());
00172     p.setColor(QPalette::Text, Qt::red);
00173     ui_.status->setPalette(p);
00174     ui_.status->setText(message.c_str());
00175   }
00176 
00177   void PointClickPublisherPlugin::PrintInfo(const std::string& message)
00178   {
00179     if (message == ui_.status->text().toStdString())
00180     {
00181       return;
00182     }
00183 
00184     ROS_INFO("%s", message.c_str());
00185     QPalette p(ui_.status->palette());
00186     p.setColor(QPalette::Text, Qt::green);
00187     ui_.status->setPalette(p);
00188     ui_.status->setText(message.c_str());
00189   }
00190 
00191   void PointClickPublisherPlugin::PrintWarning(const std::string& message)
00192   {
00193     if (message == ui_.status->text().toStdString())
00194     {
00195       return;
00196     }
00197 
00198     ROS_WARN("%s", message.c_str());
00199     QPalette p(ui_.status->palette());
00200     p.setColor(QPalette::Text, Qt::darkYellow);
00201     ui_.status->setPalette(p);
00202     ui_.status->setText(message.c_str());
00203   }
00204 
00205 
00206   void PointClickPublisherPlugin::topicChanged(const QString& topic)
00207   {
00208     std::stringstream ss;
00209     ss << "Publishing points to topic: " << topic.toStdString().c_str();
00210     PrintInfo(ss.str());
00211 
00212     if (!topic.isEmpty())
00213     {
00214       point_publisher_ = node_.advertise<geometry_msgs::PointStamped>(topic.toStdString(), 1000);
00215     }
00216   }
00217 
00218   void PointClickPublisherPlugin::updateFrames()
00219   {
00220     std::vector<std::string> frames;
00221     tf_->getFrameStrings(frames);
00222 
00223     bool supports_wgs84 = tf_manager_.SupportsTransform(
00224         swri_transform_util::_local_xy_frame,
00225         swri_transform_util::_wgs84_frame);
00226 
00227     if (supports_wgs84)
00228     {
00229       frames.push_back(swri_transform_util::_wgs84_frame);
00230     }
00231 
00232     if (ui_.outputframe->count() >= 0 &&
00233         static_cast<size_t>(ui_.outputframe->count()) == frames.size())
00234     {
00235       bool changed = false;
00236       for (size_t i = 0; i < frames.size(); i++)
00237       {
00238         if (frames[i] != ui_.outputframe->itemText(static_cast<int>(i)).toStdString())
00239         {
00240           changed = true;
00241         }
00242       }
00243 
00244       if (!changed)
00245         return;
00246     }
00247 
00248     std::string current_output = ui_.outputframe->currentText().toStdString();
00249 
00250     ui_.outputframe->clear();
00251     for (size_t i = 0; i < frames.size(); i++)
00252     {
00253       ui_.outputframe->addItem(frames[i].c_str());
00254     }
00255 
00256     if (current_output != "")
00257     {
00258       int index = ui_.outputframe->findText(current_output.c_str());
00259       if (index < 0)
00260       {
00261         ui_.outputframe->addItem(current_output.c_str());
00262       }
00263 
00264       index = ui_.outputframe->findText(current_output.c_str());
00265       ui_.outputframe->setCurrentIndex(index);
00266     }
00267   }
00268 }


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