tablet_controller_panel.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 
38 #include <Eigen/Core>
39 #include <Eigen/Geometry>
40 namespace jsk_rviz_plugins
41 {
42  TabletCmdVelArea::TabletCmdVelArea(QWidget* parent, ros::Publisher& pub_cmd_vel):
43  QWidget(parent), mouse_x_(-1), mouse_y_(-1), pub_cmd_vel_(pub_cmd_vel)
44  {
45  setBackgroundRole(QPalette::Base);
46  setAutoFillBackground(true);
47  }
48 
50  {
51  return QSize(300, 300);
52  }
53 
55  {
56  return QSize(300, 300);
57  }
58 
59  void TabletCmdVelArea::mouseMoveEvent(QMouseEvent* event){
60  mouse_x_ = event->x();
61  mouse_y_ = event->y();
62  repaint();
63  }
64  void TabletCmdVelArea::mousePressEvent(QMouseEvent* event){
65  mouse_x_ = event->x();
66  mouse_y_ = event->y();
67  repaint();
68  }
69  void TabletCmdVelArea::mouseReleaseEvent(QMouseEvent* event){
70  mouse_x_ = -1;
71  mouse_y_ = -1;
72  repaint();
73  publishCmdVel(0, 0, 0);
74  }
75 
76  void TabletCmdVelArea::paintEvent(QPaintEvent* event)
77  {
78  QSize widget_size = size();
79  int line_width = 20;
80  int width = widget_size.width() - line_width * 2;
81  int height = widget_size.height() - line_width * 2;
82  // out circle
83  int radius = std::min(width, height) / 2;
84  int center_x = width / 2 + line_width;
85  int center_y = height / 2 + line_width;
86  QPainter painter(this);
87  painter.setRenderHint(QPainter::Antialiasing);
88  QPen pen;
89  pen.setColor(QColor(130, 177, 255));
90  pen.setWidth(10);
91  painter.setPen(pen);
92  painter.drawArc(center_x - radius, center_y - radius,
93  radius * 2, radius * 2, 0, (360 + 1) * 16);
94  // position of mouse
95  QPen inner_pen;
96  inner_pen.setColor(QColor(33, 150, 243));
97  int inner_size = 40;
98  inner_pen.setWidth(inner_size);
99  painter.setPen(inner_pen);
100  if (mouse_x_ == -1 && mouse_y_ == -1) {
101  mouse_x_ = center_x;
102  mouse_y_ = center_y;
103  }
104  else {
105  publishVelocity(mouse_x_, mouse_y_, center_x, center_y);
106  }
107  painter.drawArc(mouse_x_ - inner_size / 2,
108  mouse_y_ - inner_size / 2,
109  inner_size, inner_size, 0, (360 + 1) * 16);
110  }
111 
113  int mouse_x, int mouse_y, int cx, int cy)
114  {
115  double diff_x = mouse_x - cx;
116  double diff_y = mouse_y - cy;
117  Eigen::Vector3d ex(0, -1, 0);
118  Eigen::Vector3d vel(diff_x / cx, diff_y / cy, 0);
119 
120  int sign = 1;
121  if (ex.cross(vel).dot(Eigen::Vector3d(0, 0, -1)) < 0) {
122  sign = -1;
123  }
124  double dot = ex.dot(vel) / ex.norm() / vel.norm();
125  if (dot < -1) {
126  dot = -1.0;
127  }
128  else if (dot > 1) {
129  dot = 1.0;
130  }
131  double theta = sign * acos(dot);
132  if (!std::isnan(theta)) {
133  Eigen::Vector3d vel_refined(-vel[1], -vel[0], 0);
134 
135  publishCmdVel(vel_refined[0] * 0.2 , vel_refined[1] * 0.2, theta * 0.2);
136  }
137  }
138 
139  void TabletCmdVelArea::publishCmdVel(double x, double y, double theta)
140  {
141  ROS_INFO("(%f, %f)", x, y);
142  ROS_INFO("theta: %f", theta);
143  geometry_msgs::Twist twist;
144  twist.linear.x = x;
145  twist.linear.y = y;
146  twist.angular.z = theta;
147  pub_cmd_vel_.publish(twist);
148  }
149 
151  {
152  return "QPushButton {background-color: #FF5252; color: white; font-size: 30pt;}";
153  }
154 
156  {
157  return "QPushButton {background-color: white; color: #424242; font-size: 30pt;}";
158  }
159 
161  {
162  return "QRadioButton {font-size: 20pt; color: #424242;}";
163  }
164 
166  {
167  return "QListWidget {font-size: 20pt; color: #424242;}";
168  }
169 
171  {
172  ros::NodeHandle nh;
173  pub_start_demo_ = nh.advertise<jsk_rviz_plugins::StringStamped>(
174  "/Tablet/StartDemo", 1);
175  pub_spot_ = nh.advertise<jsk_rviz_plugins::StringStamped>(
176  "/Tablet/MoveToSpot", 1);
177  sub_spots_ = nh.subscribe("/spots_marker_array",
179  pub_cmd_vel_ = nh.advertise<geometry_msgs::Twist>(
180  "/navigation/unsafe_vel", 1);
181  layout_ = new QVBoxLayout();
182  layout_->addStretch();
183  task_button_ = new QPushButton("Task", this);
184  task_button_->setMinimumHeight(100);
185  task_button_->setStyleSheet(defaultButtonStyleSheet());
186 
187  connect(task_button_, SIGNAL(released()), this, SLOT(taskButtonClicked()));
188  layout_->addWidget(task_button_);
189  layout_->addSpacing(10);
190  spot_button_ = new QPushButton("Move to spot", this);
191  spot_button_->setMinimumHeight(100);
192  spot_button_->setStyleSheet(defaultButtonStyleSheet());
193  connect(spot_button_, SIGNAL(released()), this, SLOT(spotButtonClicked()));
194  layout_->addWidget(spot_button_);
195  layout_->addSpacing(10);
197  layout_->addWidget(cmd_vel_area_);
198 
199 
200  setLayout(layout_);
201  setBackgroundRole(QPalette::Base);
202  setAutoFillBackground(true);
203  }
204 
206  {
207 
208  }
209 
211  {
212  rviz::Panel::load(config);
213  }
214 
216  {
217  rviz::Panel::save(config);
218  }
219 
221  // callbacks
224  const visualization_msgs::MarkerArray::ConstPtr& marker)
225  {
226  boost::mutex::scoped_lock lock(mutex_);
227  spots_.clear();
228  for (size_t i = 0; i < marker->markers.size(); i++) {
229  std::string text = marker->markers[i].text;
230  if (!text.empty()) {
231  spots_.push_back(text);
232  }
233  }
234  }
235 
236 
237 
239  {
240  task_dialog_ = new QDialog();
241  task_dialog_->setBackgroundRole(QPalette::Base);
242  task_dialog_->setAutoFillBackground(true);
243  task_dialog_layout_ = new QVBoxLayout();
244  task_radio_buttons_.clear();
245  std::vector<std::string> tasks;
246  tasks.push_back("/Tablet/other/GetGeorgia");
247  tasks.push_back("/Tablet/chen/GoToElevator");
248  tasks.push_back("/Tablet/chen/Greeting1");
249  tasks.push_back("/Tablet/chen/Greeting2");
250  tasks.push_back("/Tablet/chen/Greeting3");
251  tasks.push_back("/Tablet/chen/Greeting4");
252  tasks.push_back("/Tablet/chen/Greeting5");
253  tasks.push_back("/Tablet/chen/HandOver");
254  for (size_t i = 0; i < tasks.size(); i++) {
255  QRadioButton* task = new QRadioButton(QString::fromStdString(tasks[i]), this);
256  task->setMinimumHeight(50);
257  if (i == 0) {
258  task->setChecked(true);
259  }
260  task->setStyleSheet(radioButtonStyleSheet());
261  task_radio_buttons_.push_back(task);
262  }
263 
264  for (size_t i = 0; i < task_radio_buttons_.size(); i++) {
266  }
267  task_dialog_button_layout_ = new QHBoxLayout();
268  task_execute_button_ = new QPushButton("Execute", this);
270  task_execute_button_->setMinimumHeight(100);
271  task_execute_button_->setMinimumWidth(300);
273  connect(task_execute_button_, SIGNAL(released()), this, SLOT(taskExecuteClicked()));
274  task_cancel_button_ = new QPushButton("Cancel", this);
276  task_cancel_button_->setMinimumHeight(100);
277  task_cancel_button_->setMinimumWidth(300);
278  connect(task_cancel_button_, SIGNAL(released()), this, SLOT(taskCancelClicked()));
281  task_dialog_->setLayout(task_dialog_layout_);
282  task_dialog_->show();
283 
284  }
285 
287  {
288  task_dialog_->reject();
289  }
290 
292  {
293  for (size_t i = 0; i < task_radio_buttons_.size(); i++) {
294  QRadioButton* radio = task_radio_buttons_[i];
295  if (radio->isChecked()) {
296  std::string task = radio->text().toStdString();
297  ROS_INFO("task: %s", task.c_str());
298  task_dialog_->reject();
299  jsk_rviz_plugins::StringStamped command;
300  command.data = task;
301  command.header.stamp = ros::Time::now();
302  pub_start_demo_.publish(command);
303  return;
304  }
305  }
306  }
307 
309  {
310  boost::mutex::scoped_lock lock(mutex_);
311  spot_dialog_ = new QDialog();
312  spot_dialog_->setBackgroundRole(QPalette::Base);
313  spot_dialog_->setAutoFillBackground(true);
314  spot_dialog_layout_ = new QVBoxLayout();
315 
316  spot_list_ = new QListWidget();
317  spot_list_->setSortingEnabled(true);
318  spot_list_->setStyleSheet(listStyleSheet());
319  for (size_t i = 0; i < spots_.size(); i++) {
320  QListWidgetItem* item = new QListWidgetItem(
321  QString::fromStdString(spots_[i]));
322  item->setSizeHint(QSize(item->sizeHint().width(), 30));
323  spot_list_->addItem(item);
324  }
325  spot_dialog_layout_->addWidget(spot_list_);
326  spot_dialog_button_layout_ = new QHBoxLayout();
327  spot_go_button_ = new QPushButton("Go", this);
328  spot_go_button_->setStyleSheet(executeButtonStyleSheet());
329  spot_go_button_->setMinimumHeight(50);
330  spot_go_button_->setMinimumWidth(300);
331  connect(spot_go_button_, SIGNAL(released()),
332  this, SLOT(spotGoClicked()));
334 
335  spot_cancel_button_ = new QPushButton("Cancel", this);
336  spot_cancel_button_->setMinimumHeight(50);
337  spot_cancel_button_->setMinimumWidth(300);
339  connect(spot_cancel_button_, SIGNAL(released()),
340  this, SLOT(spotCancelClicked()));
343  spot_dialog_->setLayout(spot_dialog_layout_);
344  spot_dialog_->show();
345  }
346 
348  {
349  spot_dialog_->reject();
350  }
351 
353  {
354  QListWidgetItem* item = spot_list_->currentItem();
355  if (item) {
356  std::string spot = item->text().toStdString();
357  jsk_rviz_plugins::StringStamped spot_command;
358  spot_command.data = spot;
359  spot_command.header.stamp = ros::Time::now();
360  pub_spot_.publish(spot_command);
361  }
362  spot_dialog_->reject();
363  }
364 
365 }
366 
virtual void spotCallback(const visualization_msgs::MarkerArray::ConstPtr &marker)
double sign(double arg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual void load(const rviz::Config &config)
width
virtual void save(Config config) const
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
std::vector< QRadioButton * > task_radio_buttons_
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::TabletControllerPanel, rviz::Panel)
void publish(const boost::shared_ptr< M > &message) const
virtual void publishCmdVel(double x, double y, double theta)
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
virtual void mousePressEvent(QMouseEvent *event)
virtual void save(rviz::Config config) const
static Time now()
virtual void mouseMoveEvent(QMouseEvent *event)
height
virtual void load(const Config &config)
virtual void paintEvent(QPaintEvent *event)
TabletCmdVelArea(QWidget *parent, ros::Publisher &pub_cmd_vel)
virtual void publishVelocity(int mouse_x, int mouse_y, int cx, int cy)
virtual void mouseReleaseEvent(QMouseEvent *event)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58