command_widget.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2015, Tokyo Opensource Robotics Kyokai Association
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 Tokyo Opensource Robotics Kyokai Association (TORK).
19 # nor the names of its contributors may be used to endorse or promote
20 # products derived 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 import os
36 try: # Python2
37  from urlparse import urlparse
38 except ImportError: # Python3
39  from urllib.parse import urlparse
40 
41 from hironx_ros_bridge.hironx_client import HIRONX
42 from hrpsys import rtm
43 from hrpsys_ros_bridge.hrpsys_dashboard import HrpsysDashboard
44 from python_qt_binding import loadUi
45 from python_qt_binding.QtCore import Qt, Signal
46 
47 try: #14.04
48  from python_qt_binding.QtGui import (QHeaderView, QItemSelectionModel,
49  QWidget)
50 except: #16.04
51  from python_qt_binding.QtWidgets import (QHeaderView, QWidget)
52  from python_qt_binding.QtCore import QItemSelectionModel
53 
54 from rosgraph import Master
55 from rospkg import RosPack
56 import rospy
57 from rospy.exceptions import ROSException
58 import rosservice
59 from rqt_robot_dashboard.widgets import MenuDashWidget
60 
61 PKG_NAME = 'hironx_ros_bridge'
62 
63 
64 class HironxoCommandPanel(QWidget):
65  '''
66  Design decisions:
67 
68  - joint_state_publisher is dropped, since hrpsys_ros_bridge seems to set
69  `robot_description` param using COLLADA (see https://goo.gl/aLpILa)
70  instead of URDF, which joint_state_publisher is not capable of.
71  '''
72 
73  def __init__(self, parent, guicontext):
74  '''
75  A GUi panel to list common operation commands for Hironx / NEXTAGE Open robot.
76 
77  @param guicontext: The plugin context to create the monitor in.
78  @type guicontext: qt_gui.plugin_context.PluginContext
79  '''
80  super(HironxoCommandPanel, self).__init__()
81  self._parent = parent
82  self._guicontext = guicontext
83 
84  # RTM Client
85  rtm.nshost = rospy.get_param('rtmnameserver_host', 'localhost')
86  rtm.nsport = rospy.get_param('rtmnameserver_port', '15005')
87  robotname = rospy.get_param('rtmnameserver_robotname', 'HiroNX(Robot)0')
88  rospy.loginfo('Connecting to RTM nameserver. host={}, port={}, robotname={} using rtmnameserver_{{host,port,robotname}} ROS param'.format(rtm.nshost, rtm.nsport, robotname))
89 
90  self._rtm = HIRONX()
91  self._rtm.init(robotname=robotname, url='')
92 
93  rospack = RosPack()
94  ui_file = os.path.join(rospack.get_path(PKG_NAME), 'resource',
95  'hironx_commandpanel_main.ui')
96  loadUi(ui_file, self, {'HironxoCommandPanel': HironxoCommandPanel})
97 
98  self._precision_output = 4 # Default degree order to be used to print values
99 
100  # Assign callback methods
101  self.pushButton_checkEncoders.clicked[bool].connect(self._check_encoders)
102  self.pushButton_goInitial.clicked[bool].connect(self._go_initial)
103  self.pushButton_goInitial_factoryval.clicked[bool].connect(self._go_initial_factoryval)
104  self.pushButton_goOffPose.clicked[bool].connect(self._go_offPose)
105  self.pushButton_startImpedance.clicked[bool].connect(self._impedance_on)
106  self.pushButton_stopImpedance.clicked[bool].connect(self._impedance_off)
107  self.pushButton_actualPose_l.clicked[bool].connect(self._actual_pose_l)
108  self.pushButton_actualPose_r.clicked[bool].connect(self._actual_pose_r)
109  self.spinBox_precision_output.valueChanged[int].connect(self._get_precision_output)
110  self.pushButton_groups.clicked[bool].connect(self._show_groups)
111 
113  '''
114  Workaround for rosgraph.Master.getUri() that does NOT return
115  a domain name with ".local".
116  '''
117  master = Master('/hironxo_command_widget')
118  #masteruri_http = master.getUri() # This does not obtain a hostname with ".local",
119  # regardless ROS_MASTER_URI actually contains it.
120  masteruri_http = os.environ['ROS_MASTER_URI']
121  urlparsed = urlparse(masteruri_http)
122  return urlparsed
123 
124  def _print_command(self, command_str):
125  self.textBrowser_output.append('***Command used***\n\t' + command_str)
126 
127  def _get_duration(self):
128  '''
129  @rtype float
130  '''
131  return float(self.doubleSpinBox_duration.text())
132 
134  self._precision_output = self.spinBox_precision_output.value()
135 
137  '''
138  @rtype str
139  @return: Returns a name of arm group that is checked on GUI. None by default.
140  '''
141  checked_arm = None
142  if self.radioButton_impedance_left.isChecked():
143  checked_arm = 'larm'
144  elif self.radioButton_impedance_right.isChecked():
145  checked_arm = 'rarm'
146  return checked_arm
147 
148  def _check_encoders(self):
149  self._print_command('checkEncoders()')
150  self._rtm.checkEncoders()
151 
152  def _go_initial(self):
153  self._print_command('goInitial(tm={})'.format(self._get_duration()))
154  self._rtm.goInitial(tm=self._get_duration())
155 
157  self._print_command('goInitial(init_pose_type=1, tm={})'.format(self._get_duration()))
158  self._rtm.goInitial(init_pose_type=self._rtm.INITPOS_TYPE_FACTORY,
159  tm=self._get_duration())
160 
161  def _go_offPose(self):
162  self._print_command('goOffPose(tm={})'.format(self._get_duration()))
163  self._rtm.goOffPose(tm=self._get_duration())
164 
165  def _impedance_on_off(self, do_on=True):
166  '''
167  Start/stop impedance control for the specified arm group.
168  Arm group to operate impedance control is automatically obtained from
169  GUI internally within this method.
170  @raise AttributeError: When no arm group is specified.
171  @type do_on: bool
172  @param do_on: On/off of impedance control
173  '''
174  armgroup = self._get_arm_impedance()
175  if not armgroup:
176  raise AttributeError('No arm is specified for impedance control to start.')
177  if do_on:
178  self._print_command('startImpedance({})'.format(armgroup))
179  self._rtm.startImpedance(armgroup)
180  elif not do_on:
181  self._print_command('stopImpedance({})'.format(armgroup))
182  self._rtm.stopImpedance(armgroup)
183 
184  def _impedance_on(self):
185  self._impedance_on_off()
186 
187  def _impedance_off(self):
188  self._impedance_on_off(do_on=False)
189 
190  def _show_actual_pose(self, list_pose):
191  '''
192  @type list_pose: [str]
193  @param list_pose: list of str that represent angles (in radian)
194  '''
195  # Print the section line. This creates '---- ---- ---- ---- '
196  section_line_piece = '-'
197  text_single_line = section_line_piece * self._precision_output + '\t'
198  self.textBrowser_output.append(text_single_line * 4)
199 
200  text_single_line = ''
201  i = 0
202  for fl in list_pose:
203  val = str(round(fl, self._precision_output))
204 
205  # Format the diagonal
206  text_single_line += val
207  print('DEBUG) #{}: text_single_line: {}'.format(i, text_single_line))
208  # If num of elements in a single line reaches 4,
209  # or if cursor reaches the end of the list.
210  # Also, We want to add the 1st element into the 1st line.
211  if (i != 0 and i % 4 == 3) or i+1 == len(list_pose):
212  self.textBrowser_output.append(text_single_line)
213  text_single_line = '' # Clear the text for a single line
214  else:
215  text_single_line += '\t'
216  i += 1
217 
218  def _actual_pose_l(self):
219  target_joint = 'LARM_JOINT5'
220  self._print_command('getCurrentPose({})'.format(target_joint))
221  self._show_actual_pose(self._rtm.getCurrentPose(target_joint))
222 
223  def _actual_pose_r(self):
224  target_joint = 'LARM_JOINT5'
225  self._print_command('getCurrentPose({})'.format(target_joint))
226  self._show_actual_pose(self._rtm.getCurrentPose(target_joint))
227 
228  def _show_groups(self):
229  groups = self._rtm.Groups
230  text = ''
231  for g in groups:
232  text += g[0]
233  str_elems = ''.join(str('\t' + e + '\n') for e in g[1])
234  text += str_elems
235 
236  self._print_command('Groups')
237  self.textBrowser_output.append(text)
def getCurrentPose(self, lname=None, frame_name=None)
Returns the current physical pose of the specified joint.


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15