pose_array_plugin.cpp
Go to the documentation of this file.
1 
32 
33 // Declare plugin
36 
37 namespace mapviz_plugins
38 {
39  PoseArrayPlugin::PoseArrayPlugin() : config_widget_(new QWidget())
40  {
41  ui_.setupUi(config_widget_);
42 
43  ui_.color->setColor(Qt::green);
44 
45  // Set background white
46  QPalette p(config_widget_->palette());
47  p.setColor(QPalette::Background, Qt::white);
48  config_widget_->setPalette(p);
49 
50  // Set status text red
51  QPalette p3(ui_.status->palette());
52  p3.setColor(QPalette::Text, Qt::red);
53  ui_.status->setPalette(p3);
54 
55  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this,
56  SLOT(SelectTopic()));
57  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this,
58  SLOT(TopicEdited()));
59  QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this,
60  SLOT(PositionToleranceChanged(double)));
61  QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this,
62  SLOT(SetDrawStyle(QString)));
63  QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)),
64  this, SLOT(SetStaticArrowSizes(bool)));
65  QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)),
66  this, SLOT(SetArrowSize(int)));
67  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
68  SLOT(SetColor(const QColor&)));
69  }
70 
72  {
73  }
74 
76  {
78  mapviz::SelectTopicDialog::selectTopic("geometry_msgs/PoseArray");
79 
80  if (!topic.name.empty())
81  {
82  ui_.topic->setText(QString::fromStdString(topic.name));
83  TopicEdited();
84  }
85  }
86 
88  {
89  std::string topic = ui_.topic->text().trimmed().toStdString();
90  if (topic != topic_)
91  {
92  initialized_ = false;
93  ClearPoints();
94  has_message_ = false;
95  PrintWarning("No messages received.");
96 
98 
99  topic_ = topic;
100  if (!topic.empty())
101  {
103 
104  ROS_INFO("Subscribing to %s", topic_.c_str());
105  }
106  }
107  }
108 
109  void PoseArrayPlugin::PoseArrayCallback(const geometry_msgs::PoseArrayConstPtr& msg)
110  {
111  if (!has_message_)
112  {
113  initialized_ = true; // callback won't draw till this is true
114  has_message_ = true;
115  }
116 
117  ClearPoints();
118 
119  StampedPoint stamped_point;
120  for (unsigned int i=0 ; i < msg->poses.size(); i++)
121  {
122  stamped_point.stamp = msg->header.stamp;
123  stamped_point.source_frame = msg->header.frame_id;
124  geometry_msgs::Pose pose = msg->poses[i];
125 
126  stamped_point.point = tf::Point(pose.position.x,
127  pose.position.y,
128  pose.position.z);
129 
130  stamped_point.orientation = tf::Quaternion(
131  pose.orientation.x,
132  pose.orientation.y,
133  pose.orientation.z,
134  pose.orientation.w);
135 
136  pushPoint( std::move( stamped_point) );
137  }
138  }
139 
140  void PoseArrayPlugin::PrintError(const std::string& message)
141  {
142  PrintErrorHelper(ui_.status, message);
143  }
144 
145  void PoseArrayPlugin::PrintInfo(const std::string& message)
146  {
147  PrintInfoHelper(ui_.status, message);
148  }
149 
150  void PoseArrayPlugin::PrintWarning(const std::string& message)
151  {
152  PrintWarningHelper(ui_.status, message);
153  }
154 
155  QWidget* PoseArrayPlugin::GetConfigWidget(QWidget* parent)
156  {
157  config_widget_->setParent(parent);
158 
159  return config_widget_;
160  }
161 
162  bool PoseArrayPlugin::Initialize(QGLWidget* canvas)
163  {
164  canvas_ = canvas;
165  SetColor(ui_.color->color());
166 
167  return true;
168  }
169 
170  void PoseArrayPlugin::Draw(double x, double y, double scale)
171  {
172  if (DrawPoints(scale))
173  {
174  PrintInfo("OK");
175  }
176  }
177 
178  void PoseArrayPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
179  {
180  if (node["topic"])
181  {
182  std::string topic;
183  node["topic"] >> topic;
184  ui_.topic->setText(topic.c_str());
185  }
186 
187  if (node["color"])
188  {
189  std::string color;
190  node["color"] >> color;
191  QColor qcolor(color.c_str());
192  SetColor(qcolor);
193  ui_.color->setColor(qcolor);
194  }
195 
196  if (node["draw_style"])
197  {
198  std::string draw_style;
199  node["draw_style"] >> draw_style;
200 
201  if (draw_style == "arrows")
202  {
203  ui_.drawstyle->setCurrentIndex(0);
204  SetDrawStyle( ARROWS );
205  }
206  else if (draw_style == "points")
207  {
208  ui_.drawstyle->setCurrentIndex(1);
209  SetDrawStyle( POINTS );
210  }
211  }
212 
213  if (node["position_tolerance"])
214  {
215  double position_tolerance;
216  node["position_tolerance"] >> position_tolerance;
217  ui_.positiontolerance->setValue(position_tolerance);
218  PositionToleranceChanged(position_tolerance);
219  }
220 
221  if (node["static_arrow_sizes"])
222  {
223  bool static_arrow_sizes = node["static_arrow_sizes"].as<bool>();
224  ui_.static_arrow_sizes->setChecked(static_arrow_sizes);
225  SetStaticArrowSizes(static_arrow_sizes);
226  }
227 
228  if (node["arrow_size"])
229  {
230  int arrow_size = node["arrow_size"].as<int>();
231  ui_.arrow_size->setValue(arrow_size);
232  SetArrowSize(arrow_size);
233  }
234 
235  TopicEdited(); // forces redraw
236  }
237 
238  void PoseArrayPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
239  {
240  std::string topic = ui_.topic->text().toStdString();
241  emitter << YAML::Key << "topic" << YAML::Value << topic;
242 
243  emitter << YAML::Key << "color" << YAML::Value
244  << ui_.color->color().name().toStdString();
245 
246  std::string draw_style = ui_.drawstyle->currentText().toStdString();
247  emitter << YAML::Key << "draw_style" << YAML::Value << draw_style;
248 
249  emitter << YAML::Key << "position_tolerance" <<
250  YAML::Value << positionTolerance();
251 
252  emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked();
253 
254  emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value();
255  }
256 }
void LoadConfig(const YAML::Node &node, const std::string &path)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
QWidget * GetConfigWidget(QWidget *parent)
ros::NodeHandle node_
virtual void PositionToleranceChanged(double value)
virtual void SetArrowSize(int arrowSize)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool Initialize(QGLWidget *canvas)
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)
tf::Vector3 Point
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void Draw(double x, double y, double scale)
#define ROS_INFO(...)
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
void PrintInfo(const std::string &message)
void PoseArrayCallback(const geometry_msgs::PoseArrayConstPtr &msg)
virtual void SetDrawStyle(QString style)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void PrintWarning(const std::string &message)
void PrintError(const std::string &message)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Mar 19 2021 02:44:32