point_click_publisher_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 <geometry_msgs/PointStamped.h>
34 
35 #include <boost/shared_ptr.hpp>
36 
37 // Declare plugin
40 
41 namespace mapviz_plugins
42 {
44  config_widget_(new QWidget()),
45  canvas_(NULL)
46  {
47  ui_.setupUi(config_widget_);
48 
49  connect(&click_filter_, SIGNAL(pointClicked(const QPointF&)),
50  this, SLOT(pointClicked(const QPointF&)));
51  connect(ui_.topic, SIGNAL(textEdited(const QString&)),
52  this, SLOT(topicChanged(const QString&)));
53 
54  frame_timer_.start(1000);
55  connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(updateFrames()));
56  }
57 
59  {
60  if (canvas_)
61  {
62  canvas_->removeEventFilter(&click_filter_);
63  }
64  }
65 
66  bool PointClickPublisherPlugin::Initialize(QGLWidget* canvas)
67  {
68  canvas_ = static_cast<mapviz::MapCanvas*>(canvas);
69  canvas_->installEventFilter(&click_filter_);
70 
71  PrintInfo("Ready.");
72 
73  return true;
74  }
75 
76  void PointClickPublisherPlugin::Draw(double x, double y, double scale)
77  {
78  }
79 
80  void PointClickPublisherPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
81  {
82  std::string tmp;
83  if (swri_yaml_util::FindValue(node, "topic"))
84  {
85  node["topic"] >> tmp;
86  ui_.topic->setText(QString(tmp.c_str()));
87  topicChanged(ui_.topic->text());
88  }
89 
90  if (swri_yaml_util::FindValue(node, "output_frame"))
91  {
92  node["output_frame"] >> tmp;
93  ui_.outputframe->addItem(QString(tmp.c_str()));
94  }
95  }
96 
97  void PointClickPublisherPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
98  {
99  emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString();
100  emitter << YAML::Key << "output_frame" << YAML::Value << ui_.outputframe->currentText().toStdString();
101  }
102 
104  {
105  config_widget_->setParent(parent);
106 
107  return config_widget_;
108  }
109 
110 
111  void PointClickPublisherPlugin::pointClicked(const QPointF& point)
112  {
113  QPointF transformed = canvas_->MapGlCoordToFixedFrame(point);
114 
115  std::string output_frame = ui_.outputframe->currentText().toStdString();
116 
117  if (target_frame_ != output_frame)
118  {
120  tf::Point tfPoint(transformed.x(), transformed.y(), 0.0);
121  if (tf_manager_->GetTransform(output_frame, target_frame_, tf))
122  {
123  tfPoint = tf * tfPoint;
124  }
125  else
126  {
127  std::stringstream error;
128  error << "Unable to find transform from " << target_frame_ << " to " << output_frame << ".";
129  PrintError(error.str());
130  return;
131  }
132  transformed.setX(tfPoint.x());
133  transformed.setY(tfPoint.y());
134  }
135 
136  boost::shared_ptr<geometry_msgs::PointStamped> stamped = boost::make_shared<geometry_msgs::PointStamped>();
137  stamped->header.frame_id = output_frame;
138  stamped->header.stamp = ros::Time::now();
139  stamped->point.x = transformed.x();
140  stamped->point.y = transformed.y();
141  stamped->point.z = 0.0;
142 
143  std::stringstream ss;
144  ss << "Point in " << output_frame.c_str() << ": " << transformed.x() << "," << transformed.y();
145 
146  // Only publish if this plugin is visible
147  if(this->Visible())
148  {
149  point_publisher_.publish(stamped);
150  }
151  else
152  {
153  ss << " (but not publishing since plugin is hidden)";
154  }
155 
156  PrintInfo(ss.str());
157  }
158 
160  {
162 
163  // We override this method so that we can initialize our publisher after
164  // our node has been set, ensuring that it's in mapviz's namespace.
165  topicChanged(ui_.topic->text());
166  }
167 
168  void PointClickPublisherPlugin::PrintError(const std::string& message)
169  {
170  PrintErrorHelper(ui_.status, message);
171  }
172 
173  void PointClickPublisherPlugin::PrintInfo(const std::string& message)
174  {
175  PrintInfoHelper(ui_.status, message);
176  }
177 
178  void PointClickPublisherPlugin::PrintWarning(const std::string& message)
179  {
180  PrintWarningHelper(ui_.status, message);
181  }
182 
183 
184  void PointClickPublisherPlugin::topicChanged(const QString& topic)
185  {
186  std::stringstream ss;
187  ss << "Publishing points to topic: " << topic.toStdString().c_str();
188  PrintInfo(ss.str());
189 
190  if (!topic.isEmpty())
191  {
192  point_publisher_ = node_.advertise<geometry_msgs::PointStamped>(topic.toStdString(), 1000);
193  }
194  }
195 
197  {
198  std::vector<std::string> frames;
199  tf_->getFrameStrings(frames);
200 
201  bool supports_wgs84 = tf_manager_->SupportsTransform(
204 
205  if (supports_wgs84)
206  {
207  frames.push_back(swri_transform_util::_wgs84_frame);
208  }
209 
210  if (ui_.outputframe->count() >= 0 &&
211  static_cast<size_t>(ui_.outputframe->count()) == frames.size())
212  {
213  bool changed = false;
214  for (size_t i = 0; i < frames.size(); i++)
215  {
216  if (frames[i] != ui_.outputframe->itemText(static_cast<int>(i)).toStdString())
217  {
218  changed = true;
219  }
220  }
221 
222  if (!changed)
223  return;
224  }
225 
226  std::string current_output = ui_.outputframe->currentText().toStdString();
227 
228  ui_.outputframe->clear();
229  for (size_t i = 0; i < frames.size(); i++)
230  {
231  ui_.outputframe->addItem(frames[i].c_str());
232  }
233 
234  if (current_output != "")
235  {
236  int index = ui_.outputframe->findText(current_output.c_str());
237  if (index < 0)
238  {
239  ui_.outputframe->addItem(current_output.c_str());
240  }
241 
242  index = ui_.outputframe->findText(current_output.c_str());
243  ui_.outputframe->setCurrentIndex(index);
244  }
245  }
246 }
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
#define NULL
QPointF MapGlCoordToFixedFrame(const QPointF &point)
static const std::string _wgs84_frame
virtual void SetNode(const ros::NodeHandle &node)
ros::NodeHandle node_
virtual void SetNode(const ros::NodeHandle &node)
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)
std::string target_frame_
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Draw(double x, double y, double scale)
void LoadConfig(const YAML::Node &node, const std::string &path)
bool Visible() const
static const std::string _local_xy_frame
void publish(const boost::shared_ptr< M > &message) const
bool FindValue(const YAML::Node &node, const std::string &name)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void PrintError(const std::string &message)
virtual void PrintInfo(const std::string &message)
virtual void PrintWarning(const std::string &message)
static Time now()
boost::shared_ptr< tf::TransformListener > tf_
swri_transform_util::TransformManagerPtr tf_manager_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


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