tf_frame_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 <algorithm>
35 #include <vector>
36 
37 // QT libraries
38 #include <QGLWidget>
39 #include <QPalette>
40 
41 // ROS libraries
42 #include <ros/master.h>
43 
45 
46 // Declare plugin
49 
50 namespace mapviz_plugins
51 {
52  TfFramePlugin::TfFramePlugin() : config_widget_(new QWidget())
53  {
54  ui_.setupUi(config_widget_);
55 
56  ui_.color->setColor(Qt::green);
57 
58  // Set background white
59  QPalette p(config_widget_->palette());
60  p.setColor(QPalette::Background, Qt::white);
61  config_widget_->setPalette(p);
62 
63  // Set status text red
64  QPalette p3(ui_.status->palette());
65  p3.setColor(QPalette::Text, Qt::red);
66  ui_.status->setPalette(p3);
67 
68  QObject::connect(ui_.selectframe, SIGNAL(clicked()), this,
69  SLOT(SelectFrame()));
70  QObject::connect(ui_.frame, SIGNAL(editingFinished()), this,
71  SLOT(FrameEdited()));
72  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
73  SLOT(PositionToleranceChanged(double)));
74  QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this,
75  SLOT(BufferSizeChanged(int)));
76  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
77  SLOT(SetDrawStyle(QString)));
78  QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
79  this, SLOT(SetStaticArrowSizes(bool)));
80  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
81  this, SLOT(SetArrowSize(int)));
82  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
83  SLOT(SetColor(const QColor&)));
84  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
85  SLOT(ClearPoints()));
86  }
87 
89  {
90  }
91 
93  {
94  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
95  if (!frame.empty())
96  {
97  ui_.frame->setText(QString::fromStdString(frame));
98  FrameEdited();
99  }
100  }
101 
103  {
104  source_frame_ = ui_.frame->text().toStdString();
105  PrintWarning("Waiting for transform.");
106 
107  ROS_INFO("Setting target frame to to %s", source_frame_.c_str());
108 
109  initialized_ = true;
110  }
111 
113  {
115  if (GetTransform(ros::Time(), transform))
116  {
117  StampedPoint stamped_point;
118  stamped_point.point = transform.GetOrigin();
119  stamped_point.orientation = transform.GetOrientation();
120  stamped_point.source_frame = target_frame_;
121  stamped_point.stamp = transform.GetStamp();
122  stamped_point.transformed = false;
123 
124  pushPoint( std::move(stamped_point) );
125  }
126  }
127 
128  void TfFramePlugin::PrintError(const std::string& message)
129  {
130  PrintErrorHelper(ui_.status, message);
131  }
132 
133  void TfFramePlugin::PrintInfo(const std::string& message)
134  {
135  PrintInfoHelper(ui_.status, message);
136  }
137 
138  void TfFramePlugin::PrintWarning(const std::string& message)
139  {
140  PrintWarningHelper(ui_.status, message);
141  }
142 
143  QWidget* TfFramePlugin::GetConfigWidget(QWidget* parent)
144  {
145  config_widget_->setParent(parent);
146 
147  return config_widget_;
148  }
149 
150  bool TfFramePlugin::Initialize(QGLWidget* canvas)
151  {
152  canvas_ = canvas;
153 
156 
157  SetColor(ui_.color->color());
158 
159  return true;
160  }
161 
162  void TfFramePlugin::Draw(double x, double y, double scale)
163  {
164  if (DrawPoints(scale))
165  {
166  PrintInfo("OK");
167  }
168  }
169  void TfFramePlugin::LoadConfig(const YAML::Node& node,
170  const std::string& path)
171  {
172  if (node["frame"])
173  {
174  node["frame"] >> source_frame_;
175  ui_.frame->setText(source_frame_.c_str());
176  }
177 
178  if (node["color"])
179  {
180  std::string color;
181  node["color"] >> color;
182  QColor qcolor(color.c_str());
183  SetColor(qcolor);
184  ui_.color->setColor(qcolor);
185  }
186 
187  if (node["draw_style"])
188  {
189  std::string draw_style;
190  node["draw_style"] >> draw_style;
191 
192  if (draw_style == "lines")
193  {
194  ui_.drawstyle->setCurrentIndex(0);
195  SetDrawStyle( LINES );
196  }
197  else if (draw_style == "points")
198  {
199  ui_.drawstyle->setCurrentIndex(1);
200  SetDrawStyle( POINTS );
201  }
202  else if (draw_style == "arrows")
203  {
204  ui_.drawstyle->setCurrentIndex(2);
205  SetDrawStyle( ARROWS );
206  }
207  }
208 
209  if (node["position_tolerance"])
210  {
211  double position_tolerance;
212  node["position_tolerance"] >> position_tolerance;
213  ui_.positiontolerance->setValue(position_tolerance);
214  PositionToleranceChanged(position_tolerance);
215  }
216 
217  if (node["buffer_size"])
218  {
219  double buffer_size;
220  node["buffer_size"] >> buffer_size;
221  ui_.buffersize->setValue(buffer_size);
222  BufferSizeChanged(buffer_size);
223  }
224 
225  if (node["static_arrow_sizes"])
226  {
227  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
228  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
229  SetStaticArrowSizes(static_arrow_sizes);
230  }
231 
232  if (node["arrow_size"])
233  {
234  int arrow_size = node["arrow_size"].as<int>();
235  ui_.arrow_size->setValue(arrow_size);
236  SetArrowSize(arrow_size);
237  }
238 
239  FrameEdited();
240  }
241 
242  void TfFramePlugin::SaveConfig(YAML::Emitter& emitter,
243  const std::string& path)
244  {
245  emitter << YAML::Key << "frame" << YAML::Value
246  << ui_.frame->text().toStdString();
247  emitter << YAML::Key << "color" << YAML::Value
248  << ui_.color->color().name().toStdString();
249 
250  std::string draw_style = ui_.drawstyle->currentText().toStdString();
251  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
252 
253  emitter << YAML::Key << "position_tolerance" <<
254  YAML::Value << positionTolerance();
255 
256  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
257 
258  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
259 
260  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
261  }
262 }
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
ros::NodeHandle node_
tf::Vector3 GetOrigin() const
void Draw(double x, double y, double scale)
virtual void PositionToleranceChanged(double value)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
virtual void SetArrowSize(int arrowSize)
void PrintWarning(const std::string &message)
void PrintError(const std::string &message)
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)
QWidget * GetConfigWidget(QWidget *parent)
std::string target_frame_
static std::string selectFrame(boost::shared_ptr< tf::TransformListener > tf_listener, QWidget *parent=0)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
std::string source_frame_
void TimerCallback(const ros::TimerEvent &event)
#define ROS_INFO(...)
void LoadConfig(const YAML::Node &node, const std::string &path)
tf::Quaternion GetOrientation() const
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
boost::shared_ptr< tf::TransformListener > tf_
void PrintInfo(const std::string &message)
virtual void SetDrawStyle(QString style)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool Initialize(QGLWidget *canvas)


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