move_base_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, 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 <vector>
35 
36 // QT libraries
37 #include <QDateTime>
38 #include <QDialog>
39 #include <QGLWidget>
40 #include <QMouseEvent>
41 #include <QPainter>
42 #include <QPalette>
43 #include <QStaticText>
44 #include <QDebug>
45 #include <QSettings>
46 #include <fstream>
47 
48 // ROS libraries
49 #include <ros/master.h>
51 
52 // Declare plugin
54 
56 
57 namespace mapviz_plugins
58 {
59 
61  config_widget_(new QWidget()),
62  map_canvas_(NULL),
63  is_mouse_down_(false),
64  move_base_client_("move_base", true),
65  monitoring_action_state_(false)
66 {
67  init_pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, false);
68 
69  ui_.setupUi(config_widget_);
70 
71  // Set background white
72  QPalette p(config_widget_->palette());
73  p.setColor(QPalette::Background, Qt::white);
74  config_widget_->setPalette(p);
75  // Set status text red
76 
77  ui_.status->setText("OK");
78  QPalette p3(ui_.status->palette());
79  p3.setColor(QPalette::Text, Qt::green);
80  ui_.status->setPalette(p3);
81 
82  QObject::connect(ui_.pushButtonInitialPose, SIGNAL(&QPushButton::toggled),
84 
85  QObject::connect(ui_.pushButtonGoalPose, SIGNAL(&QPushButton::toggled),
87 
88  QObject::connect(ui_.pushButtonAbort, SIGNAL(&QPushButton::clicked),
90 
92 
93 }
94 
96 {
97  if (map_canvas_)
98  {
99  map_canvas_->removeEventFilter(this);
100  }
101 }
102 
103 void MoveBasePlugin::PrintError(const std::string& message)
104 {
105  PrintErrorHelper( ui_.status, message);
106 }
107 
108 void MoveBasePlugin::PrintInfo(const std::string& message)
109 {
110  PrintInfoHelper( ui_.status, message);
111 }
112 
113 void MoveBasePlugin::PrintWarning(const std::string& message)
114 {
115  PrintWarningHelper( ui_.status, message);
116 }
117 
118 QWidget* MoveBasePlugin::GetConfigWidget(QWidget* parent)
119 {
120  config_widget_->setParent(parent);
121  return config_widget_;
122 }
123 
124 bool MoveBasePlugin::Initialize(QGLWidget* canvas)
125 {
126  map_canvas_ = static_cast<mapviz::MapCanvas*>(canvas);
127  map_canvas_->installEventFilter(this);
128  initialized_ = true;
129  return true;
130 }
131 
132 bool MoveBasePlugin::eventFilter(QObject *object, QEvent* event)
133 {
134  switch (event->type())
135  {
136  case QEvent::MouseButtonPress:
137  return handleMousePress(static_cast<QMouseEvent*>(event));
138  case QEvent::MouseButtonRelease:
139  return handleMouseRelease(static_cast<QMouseEvent*>(event));
140  case QEvent::MouseMove:
141  return handleMouseMove(static_cast<QMouseEvent*>(event));
142  default:
143  return false;
144  }
145 }
146 
148 {
149  bool connected = move_base_client_.isServerConnected();
150  ui_.pushButtonAbort->setEnabled( connected );
151  ui_.pushButtonGoalPose->setEnabled( connected );
152  ui_.pushButtonInitialPose->setEnabled( connected );
153 
154  if(!connected)
155  {
156  PrintErrorHelper( ui_.status, "[move_base] server not connected");
157  }
158  else if( !monitoring_action_state_ ){
159  PrintInfoHelper( ui_.status, "Ready to send command");
160  }
161  else{
163  switch( state.state_ )
164  {
166  PrintWarningHelper( ui_.status, state.toString() );
167  break;
168 
170  PrintWarningHelper( ui_.status, state.toString() );
171  monitoring_action_state_ = false;
172  break;
173 
175  PrintInfoHelper( ui_.status, state.toString() );
176  break;
177 
179  PrintInfoHelper( ui_.status, state.toString() );
180  monitoring_action_state_ = false;
181  break;
182 
187  PrintErrorHelper( ui_.status, state.toString() );
188  monitoring_action_state_ = false;
189  break;
190  }
191  }
192 }
193 
194 
195 bool MoveBasePlugin::handleMousePress(QMouseEvent* event)
196 {
197  bool init_checked = ui_.pushButtonInitialPose->isChecked();
198  bool goal_checked = ui_.pushButtonGoalPose->isChecked();
199  if( !init_checked && !goal_checked)
200  {
201  return false;
202  }
203 
204  if (event->button() == Qt::LeftButton)
205  {
206  is_mouse_down_ = true;
207  arrow_angle_ = 0;
208 #if QT_VERSION >= 0x050000
210 #else
212 #endif
213  return true;
214  }
215  return false;
216 }
217 
218 bool MoveBasePlugin::handleMouseMove(QMouseEvent* event)
219 {
220  if (is_mouse_down_)
221  {
222 #if QT_VERSION >= 0x050000
223  QPointF head_pos = map_canvas_->MapGlCoordToFixedFrame( event->localPos() );
224 #else
225  QPointF head_pos = map_canvas_->MapGlCoordToFixedFrame( event->posF() );
226 #endif
227  arrow_angle_ = atan2( head_pos.y() - arrow_tail_position_.y(),
228  head_pos.x() - arrow_tail_position_.x() );
229  }
230  return false;
231 }
232 
233 bool MoveBasePlugin::handleMouseRelease(QMouseEvent* event)
234 {
235  if( !is_mouse_down_ )
236  {
237  return false;
238  }
239 
240  is_mouse_down_ = false;
241 
242  bool init_checked = ui_.pushButtonInitialPose->isChecked();
243  bool goal_checked = ui_.pushButtonGoalPose->isChecked();
244  if( !init_checked && !goal_checked)
245  {
246  return false;
247  }
248 
250 
251  if( goal_checked ){
252 
253  move_base_msg_.action_goal.header.frame_id = target_frame_;
254  move_base_msg_.action_goal.header.stamp = ros::Time::now();
255  move_base_msg_.action_goal.goal_id.stamp = move_base_msg_.action_goal.header.stamp;
256  move_base_msg_.action_goal.goal_id.id = "mapviz_goal";
257  move_base_msg_.action_goal.goal.target_pose.header = move_base_msg_.action_goal.header;
258 
259  geometry_msgs::Pose& pose = move_base_msg_.action_goal.goal.target_pose.pose;
260  pose.position.x = arrow_tail_position_.x();
261  pose.position.y = arrow_tail_position_.y();
262  pose.position.z = 0.0;
263  tf::quaternionTFToMsg( quat, pose.orientation );
264 
265  move_base_client_.sendGoal(move_base_msg_.action_goal.goal);
266  ui_.pushButtonGoalPose->setChecked(false);
268  }
269  if( init_checked ){
270  geometry_msgs::PoseWithCovarianceStamped initpose;
271  initpose.header.frame_id = target_frame_;
272  initpose.header.stamp = ros::Time::now();
273  initpose.pose.pose.position.x = arrow_tail_position_.x();
274  initpose.pose.pose.position.y = arrow_tail_position_.y();
275  initpose.pose.pose.position.z = 0.0;
276  tf::quaternionTFToMsg( quat, initpose.pose.pose.orientation );
277 
278  init_pose_pub_.publish(initpose);
279  ui_.pushButtonInitialPose->setChecked(false);
280  }
281  return true;
282 }
283 
284 
285 void MoveBasePlugin::Draw(double x, double y, double scale)
286 {
287  std::array<QPointF, 7> arrow_points;
288  arrow_points[0] = QPointF(10, 0);
289  arrow_points[1] = QPointF(6, -2.5);
290  arrow_points[2] = QPointF(6.5, -1);
291  arrow_points[3] = QPointF(0, -1);
292  arrow_points[4] = QPointF(0, 1);
293  arrow_points[5] = QPointF(6.5, 1);
294  arrow_points[6] = QPointF(6, 2.5);
295 
296  if( is_mouse_down_ )
297  {
298  QPointF transformed_points[7];
299  for (size_t i=0; i<7; i++ )
300  {
301  tf::Vector3 point(arrow_points[i].x(), arrow_points[i].y(), 0);
302  point *= scale*10;
304  transformed_points[i] = QPointF(point.x() + arrow_tail_position_.x(),
305  point.y() + arrow_tail_position_.y() );
306  }
307  glColor3f(0.1, 0.9, 0.1);
308  glLineWidth(2);
309  glBegin(GL_TRIANGLE_FAN);
310  for (const QPointF& point: transformed_points )
311  {
312  glVertex2d(point.x(), point.y());
313  }
314  glEnd();
315 
316  glColor3f(0.0, 0.6, 0.0);
317  glBegin(GL_LINE_LOOP);
318  for (const QPointF& point: transformed_points )
319  {
320  glVertex2d(point.x(), point.y());
321  }
322  glEnd();
323  }
324 }
325 
326 
327 void MoveBasePlugin::LoadConfig(const YAML::Node& node, const std::string& path)
328 {
329 
330 }
331 
332 void MoveBasePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
333 {
334 
335 }
336 
338 {
339  const bool other_checked = ui_.pushButtonGoalPose->isChecked();
340 
341  if(checked){
342  if(other_checked){
343  ui_.pushButtonGoalPose->setChecked(false);
344  }
345  else{
346  QPixmap cursor_pixmap = QPixmap(":/images/green-arrow.png");
347  QApplication::setOverrideCursor(QCursor(cursor_pixmap));
348  }
349  }
350  if( !checked && !other_checked )
351  {
352  QApplication::restoreOverrideCursor();
353  }
354 }
355 
357 {
358  const bool other_checked = ui_.pushButtonInitialPose->isChecked();
359  if(checked){
360  if( other_checked){
361  ui_.pushButtonInitialPose->setChecked(false);
362  }
363  else{
364  QPixmap cursor_pixmap = QPixmap(":/images/green-arrow.png");
365  QApplication::setOverrideCursor(QCursor(cursor_pixmap));
366  }
367  }
368  if( !checked && !other_checked )
369  {
370  QApplication::restoreOverrideCursor();
371  }
372 
373 }
374 
376 {
378 }
379 
380 }
move_base_msgs::MoveBaseAction move_base_msg_
#define NULL
QPointF MapGlCoordToFixedFrame(const QPointF &point)
QWidget * GetConfigWidget(QWidget *parent)
virtual bool eventFilter(QObject *object, QEvent *event) override
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
virtual void PrintError(const std::string &message) override
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void timerCallback(const ros::TimerEvent &ev=ros::TimerEvent())
std::string toString() const
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)
void Draw(double x, double y, double scale)
std::string target_frame_
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
bool handleMousePress(QMouseEvent *)
void publish(const boost::shared_ptr< M > &message) const
SimpleClientGoalState getState() const
static Quaternion createQuaternionFromYaw(double yaw)
void on_pushButtonInitialPose_toggled(bool checked)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void PrintInfo(const std::string &message) override
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
virtual void PrintWarning(const std::string &message) override
static Time now()
bool Initialize(QGLWidget *canvas)
void on_pushButtonGoalPose_toggled(bool checked)
bool handleMouseRelease(QMouseEvent *)
void LoadConfig(const YAML::Node &node, const std::string &path)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


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