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 
54  QSize TabletCmdVelArea::sizeHint() const
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 
170  TabletControllerPanel::TabletControllerPanel(QWidget* parent): rviz::Panel(parent)
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);
196  cmd_vel_area_ = new TabletCmdVelArea(this, pub_cmd_vel_);
197  layout_->addWidget(cmd_vel_area_);
198 
199 
200  setLayout(layout_);
201  setBackgroundRole(QPalette::Base);
202  setAutoFillBackground(true);
203  }
204 
206  {
207 
208  }
209 
210  void TabletControllerPanel::load(const rviz::Config& config)
211  {
212  rviz::Panel::load(config);
213  }
214 
215  void TabletControllerPanel::save(rviz::Config config) const
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 
jsk_rviz_plugins::TabletControllerPanel::listStyleSheet
virtual QString listStyleSheet()
Definition: tablet_controller_panel.cpp:197
jsk_rviz_plugins::TabletControllerPanel
Definition: tablet_controller_panel.h:120
overlay_sample.line_width
line_width
Definition: overlay_sample.py:29
jsk_rviz_plugins::TabletControllerPanel::spot_dialog_
QDialog * spot_dialog_
Definition: tablet_controller_panel.h:156
jsk_rviz_plugins::TabletControllerPanel::task_execute_button_
QPushButton * task_execute_button_
Definition: tablet_controller_panel.h:151
ros::Publisher
jsk_rviz_plugins::TabletCmdVelArea::minimumSizeHint
virtual QSize minimumSizeHint() const
Definition: tablet_controller_panel.cpp:81
rviz::Panel
tablet_controller_panel.h
jsk_rviz_plugins::TabletCmdVelArea::sizeHint
virtual QSize sizeHint() const
Definition: tablet_controller_panel.cpp:86
jsk_rviz_plugins::TabletControllerPanel::TabletControllerPanel
TabletControllerPanel(QWidget *parent=0)
Definition: tablet_controller_panel.cpp:202
jsk_rviz_plugins::TabletControllerPanel::save
virtual void save(rviz::Config config) const
Definition: tablet_controller_panel.cpp:247
jsk_rviz_plugins::TabletControllerPanel::spotButtonClicked
void spotButtonClicked()
Definition: tablet_controller_panel.cpp:340
rviz_mouse_point_to_tablet.lock
lock
Definition: rviz_mouse_point_to_tablet.py:11
command
ROSLIB_DECL std::string command(const std::string &cmd)
jsk_rviz_plugins::TabletControllerPanel::taskExecuteClicked
void taskExecuteClicked()
Definition: tablet_controller_panel.cpp:323
jsk_rviz_plugins::TabletControllerPanel::taskButtonClicked
void taskButtonClicked()
Definition: tablet_controller_panel.cpp:270
overlay_sample.theta
int theta
Definition: overlay_sample.py:22
jsk_rviz_plugins::TabletControllerPanel::~TabletControllerPanel
virtual ~TabletControllerPanel()
Definition: tablet_controller_panel.cpp:237
jsk_rviz_plugins::TabletControllerPanel::task_dialog_layout_
QVBoxLayout * task_dialog_layout_
Definition: tablet_controller_panel.h:149
jsk_rviz_plugins::TabletControllerPanel::pub_start_demo_
ros::Publisher pub_start_demo_
Definition: tablet_controller_panel.h:167
jsk_rviz_plugins::TabletControllerPanel::spot_cancel_button_
QPushButton * spot_cancel_button_
Definition: tablet_controller_panel.h:160
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
jsk_rviz_plugins::TabletControllerPanel::spotGoClicked
void spotGoClicked()
Definition: tablet_controller_panel.cpp:384
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
jsk_rviz_plugins::TabletControllerPanel::task_dialog_button_layout_
QHBoxLayout * task_dialog_button_layout_
Definition: tablet_controller_panel.h:150
overlay_sample.text
text
Definition: overlay_sample.py:21
jsk_rviz_plugins::TabletControllerPanel::spot_go_button_
QPushButton * spot_go_button_
Definition: tablet_controller_panel.h:159
class_list_macros.h
jsk_rviz_plugins::TabletCmdVelArea::mouse_y_
int mouse_y_
Definition: tablet_controller_panel.h:148
dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
jsk_rviz_plugins::TabletControllerPanel::spots_
std::vector< std::string > spots_
Definition: tablet_controller_panel.h:155
jsk_rviz_plugins::TabletControllerPanel::mutex_
boost::mutex mutex_
Definition: tablet_controller_panel.h:169
jsk_rviz_plugins::TabletCmdVelArea::pub_cmd_vel_
ros::Publisher pub_cmd_vel_
Definition: tablet_controller_panel.h:149
jsk_rviz_plugins::TabletControllerPanel::taskCancelClicked
void taskCancelClicked()
Definition: tablet_controller_panel.cpp:318
jsk_rviz_plugins::TabletControllerPanel::executeButtonStyleSheet
virtual QString executeButtonStyleSheet()
Definition: tablet_controller_panel.cpp:187
rviz
jsk_rviz_plugins::TabletControllerPanel::task_dialog_
QDialog * task_dialog_
Definition: tablet_controller_panel.h:148
jsk_rviz_plugins::TabletCmdVelArea::publishVelocity
virtual void publishVelocity(int mouse_x, int mouse_y, int cx, int cy)
Definition: tablet_controller_panel.cpp:144
bounding_box_sample.x
x
Definition: bounding_box_sample.py:30
jsk_rviz_plugins::TabletCmdVelArea::mouseReleaseEvent
virtual void mouseReleaseEvent(QMouseEvent *event)
Definition: tablet_controller_panel.cpp:101
jsk_rviz_plugins::TabletCmdVelArea::TabletCmdVelArea
TabletCmdVelArea(QWidget *parent, ros::Publisher &pub_cmd_vel)
Definition: tablet_controller_panel.cpp:74
jsk_rviz_plugins::TabletCmdVelArea::paintEvent
virtual void paintEvent(QPaintEvent *event)
Definition: tablet_controller_panel.cpp:108
jsk_rviz_plugins::TabletCmdVelArea::publishCmdVel
virtual void publishCmdVel(double x, double y, double theta)
Definition: tablet_controller_panel.cpp:171
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
jsk_rviz_plugins::TabletControllerPanel::spotCallback
virtual void spotCallback(const visualization_msgs::MarkerArray::ConstPtr &marker)
Definition: tablet_controller_panel.cpp:255
bounding_box_sample.y
y
Definition: bounding_box_sample.py:31
jsk_rviz_plugins::TabletControllerPanel::spot_list_
QListWidget * spot_list_
Definition: tablet_controller_panel.h:161
jsk_rviz_plugins::TabletCmdVelArea::mouse_x_
int mouse_x_
Definition: tablet_controller_panel.h:147
width
width
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::TabletControllerPanel, rviz::Panel)
jsk_rviz_plugins::TabletCmdVelArea
Definition: tablet_controller_panel.h:101
jsk_rviz_plugins::TabletControllerPanel::radioButtonStyleSheet
virtual QString radioButtonStyleSheet()
Definition: tablet_controller_panel.cpp:192
jsk_rviz_plugins::TabletControllerPanel::spot_dialog_button_layout_
QHBoxLayout * spot_dialog_button_layout_
Definition: tablet_controller_panel.h:158
rviz::Panel::load
virtual void load(const Config &config)
jsk_rviz_plugins::TabletControllerPanel::load
virtual void load(const rviz::Config &config)
Definition: tablet_controller_panel.cpp:242
rviz::Panel::save
virtual void save(Config config) const
jsk_rviz_plugins::TabletControllerPanel::defaultButtonStyleSheet
virtual QString defaultButtonStyleSheet()
Definition: tablet_controller_panel.cpp:182
jsk_rviz_plugins::TabletControllerPanel::spot_dialog_layout_
QVBoxLayout * spot_dialog_layout_
Definition: tablet_controller_panel.h:157
jsk_rviz_plugins::TabletControllerPanel::pub_spot_
ros::Publisher pub_spot_
Definition: tablet_controller_panel.h:166
jsk_rviz_plugins::TabletCmdVelArea::mousePressEvent
virtual void mousePressEvent(QMouseEvent *event)
Definition: tablet_controller_panel.cpp:96
height
height
ROS_INFO
#define ROS_INFO(...)
pictogram.size
size
Definition: pictogram.py:1713
jsk_rviz_plugins::TabletCmdVelArea::mouseMoveEvent
virtual void mouseMoveEvent(QMouseEvent *event)
Definition: tablet_controller_panel.cpp:91
jsk_rviz_plugins
Definition: __init__.py:1
rviz::Config
ros::NodeHandle
jsk_rviz_plugins::TabletControllerPanel::spotCancelClicked
void spotCancelClicked()
Definition: tablet_controller_panel.cpp:379
jsk_rviz_plugins::TabletControllerPanel::task_cancel_button_
QPushButton * task_cancel_button_
Definition: tablet_controller_panel.h:152
ros::Time::now
static Time now()
jsk_rviz_plugins::TabletControllerPanel::task_radio_buttons_
std::vector< QRadioButton * > task_radio_buttons_
Definition: tablet_controller_panel.h:153


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Fri Dec 13 2024 03:49:57