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_.use_latest_transforms, SIGNAL(clicked(bool)),
81  this, SLOT(SetUseLatestTransforms(bool)));
82  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
83  this, SLOT(SetArrowSize(int)));
84  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
85  SLOT(SetColor(const QColor&)));
86  QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this,
87  SLOT(ClearPoints()));
88  QObject::connect(this,
89  SIGNAL(TargetFrameChanged(const std::string&)),
90  this,
91  SLOT(ClearPoints()));
92  }
93 
95  {
96  }
97 
99  {
100  std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_);
101  if (!frame.empty())
102  {
103  ui_.frame->setText(QString::fromStdString(frame));
104  FrameEdited();
105  }
106  }
107 
109  {
110  source_frame_ = ui_.frame->text().toStdString();
111  PrintWarning("Waiting for transform.");
112 
113  ROS_INFO("Setting target frame to to %s", source_frame_.c_str());
114 
115  ClearPoints();
116 
117  initialized_ = true;
118  }
119 
121  {
124  {
125  StampedPoint stamped_point;
126  stamped_point.point = transform.GetOrigin();
127  stamped_point.orientation = transform.GetOrientation();
128  stamped_point.source_frame = target_frame_;
129  stamped_point.stamp = transform.GetStamp();
130  stamped_point.transformed = false;
131 
132  pushPoint( std::move(stamped_point) );
133  }
134  }
135 
136  void TfFramePlugin::PrintError(const std::string& message)
137  {
138  PrintErrorHelper(ui_.status, message);
139  }
140 
141  void TfFramePlugin::PrintInfo(const std::string& message)
142  {
143  PrintInfoHelper(ui_.status, message);
144  }
145 
146  void TfFramePlugin::PrintWarning(const std::string& message)
147  {
148  PrintWarningHelper(ui_.status, message);
149  }
150 
151  QWidget* TfFramePlugin::GetConfigWidget(QWidget* parent)
152  {
153  config_widget_->setParent(parent);
154 
155  return config_widget_;
156  }
157 
158  bool TfFramePlugin::Initialize(QGLWidget* canvas)
159  {
160  canvas_ = canvas;
161 
164 
165  SetColor(ui_.color->color());
166 
167  return true;
168  }
169 
170  void TfFramePlugin::Draw(double x, double y, double scale)
171  {
172  if (DrawPoints(scale))
173  {
174  PrintInfo("OK");
175  }
176  }
177  void TfFramePlugin::LoadConfig(const YAML::Node& node,
178  const std::string& path)
179  {
180  if (node["frame"])
181  {
182  node["frame"] >> source_frame_;
183  ui_.frame->setText(source_frame_.c_str());
184  }
185 
186  if (node["color"])
187  {
188  std::string color;
189  node["color"] >> color;
190  QColor qcolor(color.c_str());
191  SetColor(qcolor);
192  ui_.color->setColor(qcolor);
193  }
194 
195  if (node["draw_style"])
196  {
197  std::string draw_style;
198  node["draw_style"] >> draw_style;
199 
200  if (draw_style == "lines")
201  {
202  ui_.drawstyle->setCurrentIndex(0);
203  SetDrawStyle( LINES );
204  }
205  else if (draw_style == "points")
206  {
207  ui_.drawstyle->setCurrentIndex(1);
208  SetDrawStyle( POINTS );
209  }
210  else if (draw_style == "arrows")
211  {
212  ui_.drawstyle->setCurrentIndex(2);
213  SetDrawStyle( ARROWS );
214  }
215  }
216 
217  if (node["position_tolerance"])
218  {
219  double position_tolerance;
220  node["position_tolerance"] >> position_tolerance;
221  ui_.positiontolerance->setValue(position_tolerance);
222  PositionToleranceChanged(position_tolerance);
223  }
224 
225  if (node["buffer_size"])
226  {
227  double buffer_size;
228  node["buffer_size"] >> buffer_size;
229  ui_.buffersize->setValue(buffer_size);
230  BufferSizeChanged(buffer_size);
231  }
232 
233  if (node["static_arrow_sizes"])
234  {
235  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
236  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
237  SetStaticArrowSizes(static_arrow_sizes);
238  }
239 
240  if (node["arrow_size"])
241  {
242  int arrow_size = node["arrow_size"].as<int>();
243  ui_.arrow_size->setValue(arrow_size);
244  SetArrowSize(arrow_size);
245  }
246 
247  if (node["use_latest_transforms"])
248  {
249  bool use_latest_transforms = node["use_latest_transforms"].as<bool>();
250  ui_.use_latest_transforms->setChecked(use_latest_transforms);
251  SetUseLatestTransforms(use_latest_transforms);
252  }
253 
254  FrameEdited();
255  }
256 
257  void TfFramePlugin::SaveConfig(YAML::Emitter& emitter,
258  const std::string& path)
259  {
260  emitter << YAML::Key << "frame" << YAML::Value
261  << ui_.frame->text().toStdString();
262  emitter << YAML::Key << "color" << YAML::Value
263  << ui_.color->color().name().toStdString();
264 
265  std::string draw_style = ui_.drawstyle->currentText().toStdString();
266  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
267 
268  emitter << YAML::Key << "position_tolerance" <<
269  YAML::Value << positionTolerance();
270 
271  emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize();
272 
273  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
274 
275  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
276 
277  emitter << YAML::Key << "use_latest_transforms" << YAML::Value << ui_.use_latest_transforms->isChecked();
278  }
279 }
mapviz_plugins::PointDrawingPlugin::StampedPoint
Definition: point_drawing_plugin.h:56
mapviz::MapvizPlugin::tf_
boost::shared_ptr< tf::TransformListener > tf_
mapviz_plugins::TfFramePlugin::TimerCallback
void TimerCallback(const ros::TimerEvent &event)
Definition: tf_frame_plugin.cpp:120
mapviz_plugins::PointDrawingPlugin::DrawPoints
virtual bool DrawPoints(double scale)
Definition: point_drawing_plugin.cpp:258
mapviz_plugins::TfFramePlugin::PrintError
void PrintError(const std::string &message)
Definition: tf_frame_plugin.cpp:136
mapviz_plugins::PointDrawingPlugin::ClearPoints
void ClearPoints()
Definition: point_drawing_plugin.cpp:218
mapviz_plugins::PointDrawingPlugin::BufferSizeChanged
virtual void BufferSizeChanged(int value)
Definition: point_drawing_plugin.cpp:245
mapviz_plugins::TfFramePlugin::SelectFrame
void SelectFrame()
Definition: tf_frame_plugin.cpp:98
mapviz::MapvizPlugin::PrintErrorHelper
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
mapviz_plugins::PointDrawingPlugin::SetColor
virtual void SetColor(const QColor &color)
Definition: point_drawing_plugin.cpp:405
mapviz_plugins::TfFramePlugin::~TfFramePlugin
virtual ~TfFramePlugin()
Definition: tf_frame_plugin.cpp:94
mapviz::SelectFrameDialog::selectFrame
static std::string selectFrame(boost::shared_ptr< tf::TransformListener > tf_listener, QWidget *parent=0)
mapviz_plugins::PointDrawingPlugin::SetStaticArrowSizes
virtual void SetStaticArrowSizes(bool isChecked)
Definition: point_drawing_plugin.cpp:149
mapviz_plugins::PointDrawingPlugin::SetUseLatestTransforms
virtual void SetUseLatestTransforms(bool checked)
Definition: point_drawing_plugin.cpp:175
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::PointDrawingPlugin::positionTolerance
double positionTolerance() const
Definition: point_drawing_plugin.cpp:235
mapviz_plugins::TfFramePlugin::GetConfigWidget
QWidget * GetConfigWidget(QWidget *parent)
Definition: tf_frame_plugin.cpp:151
class_list_macros.h
mapviz_plugins::PointDrawingPlugin::LINES
@ LINES
Definition: point_drawing_plugin.h:76
transform
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
mapviz_plugins::TfFramePlugin::FrameEdited
void FrameEdited()
Definition: tf_frame_plugin.cpp:108
mapviz_plugins::TfFramePlugin::SaveConfig
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
Definition: tf_frame_plugin.cpp:257
mapviz_plugins::TfFramePlugin::config_widget_
QWidget * config_widget_
Definition: tf_frame_plugin.h:88
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
mapviz_plugins::TfFramePlugin::PrintInfo
void PrintInfo(const std::string &message)
Definition: tf_frame_plugin.cpp:141
mapviz_plugins::PointDrawingPlugin::PositionToleranceChanged
virtual void PositionToleranceChanged(double value)
Definition: point_drawing_plugin.cpp:155
mapviz_plugins::TfFramePlugin::TfFramePlugin
TfFramePlugin()
Definition: tf_frame_plugin.cpp:52
swri_transform_util::Transform
mapviz::MapvizPlugin::PrintWarningHelper
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
tf_frame_plugin.h
mapviz_plugins::PointDrawingPlugin::bufferSize
double bufferSize() const
Definition: point_drawing_plugin.cpp:223
mapviz_plugins::TfFramePlugin::Draw
void Draw(double x, double y, double scale)
Definition: tf_frame_plugin.cpp:170
mapviz_plugins::TfFramePlugin::ui_
Ui::tf_frame_config ui_
Definition: tf_frame_plugin.h:87
ros::TimerEvent
mapviz_plugins::TfFramePlugin::LoadConfig
void LoadConfig(const YAML::Node &node, const std::string &path)
Definition: tf_frame_plugin.cpp:177
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::TfFramePlugin::PrintWarning
void PrintWarning(const std::string &message)
Definition: tf_frame_plugin.cpp:146
mapviz::MapvizPlugin::canvas_
QGLWidget * canvas_
mapviz_plugins::PointDrawingPlugin::StampedPoint::orientation
tf::Quaternion orientation
Definition: point_drawing_plugin.h:61
mapviz_plugins::TfFramePlugin::Initialize
bool Initialize(QGLWidget *canvas)
Definition: tf_frame_plugin.cpp:158
ros::Time
mapviz_plugins::PointDrawingPlugin::ARROWS
@ ARROWS
Definition: point_drawing_plugin.h:78
mapviz_plugins::PointDrawingPlugin::SetArrowSize
virtual void SetArrowSize(int arrowSize)
Definition: point_drawing_plugin.cpp:119
mapviz::MapvizPlugin::target_frame_
std::string target_frame_
mapviz_plugins::PointDrawingPlugin::SetDrawStyle
virtual void SetDrawStyle(QString style)
Definition: point_drawing_plugin.cpp:125
mapviz::MapvizPlugin::TargetFrameChanged
void TargetFrameChanged(const std::string &target_frame)
mapviz_plugins::PointDrawingPlugin::StampedPoint::transformed
bool transformed
Definition: point_drawing_plugin.h:67
mapviz_plugins::PointDrawingPlugin::StampedPoint::stamp
ros::Time stamp
Definition: point_drawing_plugin.h:68
select_frame_dialog.h
mapviz::MapvizPlugin
mapviz::MapvizPlugin::initialized_
bool initialized_
mapviz_plugins::TfFramePlugin
Definition: tf_frame_plugin.h:57
mapviz::MapvizPlugin::source_frame_
std::string source_frame_
ROS_INFO
#define ROS_INFO(...)
mapviz_plugins::PointDrawingPlugin::StampedPoint::point
tf::Point point
Definition: point_drawing_plugin.h:60
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
mapviz_plugins
Definition: attitude_indicator_plugin.h:61
master.h
mapviz::MapvizPlugin::node_
ros::NodeHandle node_
ros::Duration
mapviz::MapvizPlugin::GetTransform
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform)
mapviz_plugins::TfFramePlugin::timer_
ros::Timer timer_
Definition: tf_frame_plugin.h:90


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