rqt_joint_trajectory_controller.joint_trajectory

71 """ 72 Graphical frontend for a C{JointTrajectoryController}. 73 74 There are two modes for interacting with a controller: 75 1. B{Monitor mode} Joint displays are updated with the state reported 76 by the controller. This is a read-only mode and does I{not} send 77 control commands. Every time a new controller is selected, it starts 78 in monitor mode for safety reasons. 79 80 2. B{Control mode} Joint displays update the control command that is 81 sent to the controller. Commands are sent periodically evan if the 82 displays are not being updated by the user. 83 84 To control the aggressiveness of the motions, the maximum speed of the 85 sent commands can be scaled down using the C{Speed scaling control} 86 87 This plugin is able to detect and keep track of all active controller 88 managers, as well as the JointTrajectoryControllers that are I{running} 89 in each controller manager. 90 For a controller to be compatible with this plugin, it must comply with 91 the following requisites: 92 - The controller type contains the C{JointTrajectoryController} 93 substring, e.g., C{position_controllers/JointTrajectoryController} 94 - The controller exposes the C{command} and C{state} topics in its 95 ROS interface. 96 97 Additionally, there must be a URDF loaded with a valid joint limit 98 specification, namely position (when applicable) and velocity limits. 99 100 A reference implementation of the C{JointTrajectoryController} is 101 available in the C{joint_trajectory_controller} ROS package. 102 """ 103 _cmd_pub_freq = 10.0 # Hz 104 _widget_update_freq = 30.0 # Hz 105 _ctrlrs_update_freq = 1 # Hz 106 _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration 107 108 jointStateChanged = Signal([dict]) 109

110 - def __init__(self, context):

111 super(JointTrajectoryController, self).__init__(context) 112 self.setObjectName('JointTrajectoryController') 113 114 # Create QWidget and extend it with all the attributes and children 115 # from the UI file 116 self._widget = QWidget() 117 rp = rospkg.RosPack() 118 ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'), 119 'resource', 120 'joint_trajectory_controller.ui') 121 loadUi(ui_file, self._widget) 122 self._widget.setObjectName('JointTrajectoryControllerUi') 123 124 # Setup speed scaler 125 speed_scaling = DoubleEditor(1.0, 100.0) 126 speed_scaling.spin_box.setSuffix('%') 127 speed_scaling.spin_box.setValue(50.0) 128 speed_scaling.spin_box.setDecimals(0) 129 speed_scaling.setEnabled(False) 130 self._widget.speed_scaling_layout.addWidget(speed_scaling) 131 self._speed_scaling_widget = speed_scaling 132 speed_scaling.valueChanged.connect(self._on_speed_scaling_change) 133 self._on_speed_scaling_change(speed_scaling.value()) 134 135 # Show _widget.windowTitle on left-top of each plugin (when 136 # it's set in _widget). This is useful when you open multiple 137 # plugins at once. Also if you open multiple instances of your 138 # plugin at once, these lines add number to make it easy to 139 # tell from pane to pane. 140 if context.serial_number() > 1: 141 self._widget.setWindowTitle(self._widget.windowTitle() + 142 (' (%d)' % context.serial_number())) 143 # Add widget to the user interface 144 context.add_widget(self._widget) 145 146 # Initialize members 147 self._jtc_name = [] # Name of selected joint trajectory controller 148 self._cm_ns = [] # Namespace of the selected controller manager 149 self._joint_pos = {} # name->pos map for joints of selected controller 150 self._joint_names = [] # Ordered list of selected controller joints 151 self._robot_joint_limits = {} # Lazily evaluated on first use 152 153 # Timer for sending commands to active controller 154 self._update_cmd_timer = QTimer(self) 155 self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq) 156 self._update_cmd_timer.timeout.connect(self._update_cmd_cb) 157 158 # Timer for updating the joint widgets from the controller state 159 self._update_act_pos_timer = QTimer(self) 160 self._update_act_pos_timer.setInterval(1000.0 / 161 self._widget_update_freq) 162 self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) 163 164 # Timer for controller manager updates 165 self._list_cm = ControllerManagerLister() 166 self._update_cm_list_timer = QTimer(self) 167 self._update_cm_list_timer.setInterval(1000.0 / 168 self._ctrlrs_update_freq) 169 self._update_cm_list_timer.timeout.connect(self._update_cm_list) 170 self._update_cm_list_timer.start() 171 172 # Timer for running controller updates 173 self._update_jtc_list_timer = QTimer(self) 174 self._update_jtc_list_timer.setInterval(1000.0 / 175 self._ctrlrs_update_freq) 176 self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) 177 self._update_jtc_list_timer.start() 178 179 # Signal connections 180 w = self._widget 181 w.enable_button.toggled.connect(self._on_jtc_enabled) 182 w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) 183 w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) 184 185 self._cmd_pub = None # Controller command publisher 186 self._state_sub = None # Controller state subscriber 187 188 self._list_controllers = None

189

190 - def shutdown_plugin(self):

191 self._update_cmd_timer.stop() 192 self._update_act_pos_timer.stop() 193 self._update_cm_list_timer.stop() 194 self._update_jtc_list_timer.stop() 195 self._unregister_state_sub() 196 self._unregister_cmd_pub()

197

198 - def save_settings(self, plugin_settings, instance_settings):

199 instance_settings.set_value('cm_ns', self._cm_ns) 200 instance_settings.set_value('jtc_name', self._jtc_name)

201

202 - def restore_settings(self, plugin_settings, instance_settings):

203 # Restore last session's controller_manager, if present 204 self._update_cm_list() 205 cm_ns = instance_settings.value('cm_ns') 206 cm_combo = self._widget.cm_combo 207 cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] 208 try: 209 idx = cm_list.index(cm_ns) 210 cm_combo.setCurrentIndex(idx) 211 # Resore last session's controller, if running 212 self._update_jtc_list() 213 jtc_name = instance_settings.value('jtc_name') 214 jtc_combo = self._widget.jtc_combo 215 jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] 216 try: 217 idx = jtc_list.index(jtc_name) 218 jtc_combo.setCurrentIndex(idx) 219 except (ValueError): 220 pass 221 except (ValueError): 222 pass

223 224 # def trigger_configuration(self): 225 # Comment in to signal that the plugin has a way to configure 226 # This will enable a setting button (gear icon) in each dock widget 227 # title bar 228 # Usually used to open a modal configuration dialog 229

230 - def _update_cm_list(self):

231 update_combo(self._widget.cm_combo, self._list_cm())

232

233 - def _update_jtc_list(self):

234 # Clear controller list if no controller information is available 235 if not self._list_controllers: 236 self._widget.jtc_combo.clear() 237 return 238 239 # List of running controllers with a valid joint limits specification 240 # for _all_ their joints 241 running_jtc = self._running_jtc_info() 242 if running_jtc and not self._robot_joint_limits: 243 self._robot_joint_limits = get_joint_limits() # Lazy evaluation 244 valid_jtc = [] 245 for jtc_info in running_jtc: 246 has_limits = all(name in self._robot_joint_limits 247 for name in _jtc_joint_names(jtc_info)) 248 if has_limits: 249 valid_jtc.append(jtc_info); 250 valid_jtc_names = [data.name for data in valid_jtc] 251 252 # Update widget 253 update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))

254

255 - def _on_speed_scaling_change(self, val):

256 self._speed_scale = val / self._speed_scaling_widget.slider.maximum()

257

258 - def _on_joint_state_change(self, actual_pos):

259 assert(len(actual_pos) == len(self._joint_pos)) 260 for name in actual_pos.keys(): 261 self._joint_pos[name]['position'] = actual_pos[name]

262

263 - def _on_cm_change(self, cm_ns):

264 self._cm_ns = cm_ns 265 if cm_ns: 266 self._list_controllers = ControllerLister(cm_ns) 267 # NOTE: Clear below is important, as different controller managers 268 # might have controllers with the same name but different 269 # configurations. Clearing forces controller re-discovery 270 self._widget.jtc_combo.clear() 271 self._update_jtc_list() 272 else: 273 self._list_controllers = None

274

275 - def _on_jtc_change(self, jtc_name):

276 self._unload_jtc() 277 self._jtc_name = jtc_name 278 if self._jtc_name: 279 self._load_jtc()

280

281 - def _on_jtc_enabled(self, val):

282 # Don't allow enabling if there are no controllers selected 283 if not self._jtc_name: 284 self._widget.enable_button.setChecked(False) 285 return 286 287 # Enable/disable joint displays 288 for joint_widget in self._joint_widgets(): 289 joint_widget.setEnabled(val) 290 291 # Enable/disable speed scaling 292 self._speed_scaling_widget.setEnabled(val) 293 294 if val: 295 # Widgets send desired position commands to controller 296 self._update_act_pos_timer.stop() 297 self._update_cmd_timer.start() 298 else: 299 # Controller updates widgets with actual position 300 self._update_cmd_timer.stop() 301 self._update_act_pos_timer.start()

302

303 - def _load_jtc(self):

304 # Initialize joint data corresponding to selected controller 305 running_jtc = self._running_jtc_info() 306 self._joint_names = next(_jtc_joint_names(x) for x in running_jtc 307 if x.name == self._jtc_name) 308 for name in self._joint_names: 309 self._joint_pos[name] = {} 310 311 # Update joint display 312 try: 313 layout = self._widget.joint_group.layout() 314 for name in self._joint_names: 315 limits = self._robot_joint_limits[name] 316 joint_widget = DoubleEditor(limits['min_position'], 317 limits['max_position']) 318 layout.addRow(name, joint_widget) 319 # NOTE: Using partial instead of a lambda because lambdas 320 # "will not evaluate/look up the argument values before it is 321 # effectively called, breaking situations like using a loop 322 # variable inside it" 323 from functools import partial 324 par = partial(self._update_single_cmd_cb, name=name) 325 joint_widget.valueChanged.connect(par) 326 except: 327 # TODO: Can we do better than swallow the exception? 328 from sys import exc_info 329 print 'Unexpected error:', exc_info()[0] 330 331 # Enter monitor mode (sending commands disabled) 332 self._on_jtc_enabled(False) 333 334 # Setup ROS interfaces 335 jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) 336 state_topic = jtc_ns + '/state' 337 cmd_topic = jtc_ns + '/command' 338 self._state_sub = rospy.Subscriber(state_topic, 339 JointTrajectoryControllerState, 340 self._state_cb, 341 queue_size=1) 342 self._cmd_pub = rospy.Publisher(cmd_topic, 343 JointTrajectory, 344 queue_size=1) 345 346 # Start updating the joint positions 347 self.jointStateChanged.connect(self._on_joint_state_change)

348

349 - def _unload_jtc(self):

350 # Stop updating the joint positions 351 try: 352 self.jointStateChanged.disconnect(self._on_joint_state_change) 353 except: 354 pass 355 356 # Reset ROS interfaces 357 self._unregister_state_sub() 358 self._unregister_cmd_pub() 359 360 # Clear joint widgets 361 # NOTE: Implementation is a workaround for: 362 # https://bugreports.qt-project.org/browse/QTBUG-15990 :( 363 layout = self._widget.joint_group.layout() 364 if layout is not None: 365 while layout.count(): 366 layout.takeAt(0).widget().deleteLater() 367 # Delete existing layout by reparenting to temporary 368 QWidget().setLayout(layout) 369 self._widget.joint_group.setLayout(QFormLayout()) 370 371 # Reset joint data 372 self._joint_names = [] 373 self._joint_pos = {} 374 375 # Enforce monitor mode (sending commands disabled) 376 self._widget.enable_button.setChecked(False)

377

378 - def _running_jtc_info(self):

379 from controller_manager_msgs.utils\ 380 import filter_by_type, filter_by_state 381 382 controller_list = self._list_controllers() 383 jtc_list = filter_by_type(controller_list, 384 'JointTrajectoryController', 385 match_substring=True) 386 running_jtc_list = filter_by_state(jtc_list, 'running') 387 return running_jtc_list

388

389 - def _unregister_cmd_pub(self):

390 if self._cmd_pub is not None: 391 self._cmd_pub.unregister() 392 self._cmd_pub = None

393

394 - def _unregister_state_sub(self):

395 if self._state_sub is not None: 396 self._state_sub.unregister() 397 self._state_sub = None

398

399 - def _state_cb(self, msg):

400 actual_pos = {} 401 for i in range(len(msg.joint_names)): 402 joint_name = msg.joint_names[i] 403 joint_pos = msg.actual.positions[i] 404 actual_pos[joint_name] = joint_pos 405 self.jointStateChanged.emit(actual_pos)

406

407 - def _update_single_cmd_cb(self, val, name):

408 self._joint_pos[name]['command'] = val

409

410 - def _update_cmd_cb(self):

411 dur = [] 412 traj = JointTrajectory() 413 traj.joint_names = self._joint_names 414 point = JointTrajectoryPoint() 415 for name in traj.joint_names: 416 pos = self._joint_pos[name]['position'] 417 cmd = pos 418 try: 419 cmd = self._joint_pos[name]['command'] 420 except (KeyError): 421 pass 422 max_vel = self._robot_joint_limits[name]['max_velocity'] 423 dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) 424 point.positions.append(cmd) 425 point.time_from_start = rospy.Duration(max(dur) / self._speed_scale) 426 traj.points.append(point) 427 428 self._cmd_pub.publish(traj)

429

430 - def _update_joint_widgets(self):

431 joint_widgets = self._joint_widgets() 432 for id in range(len(joint_widgets)): 433 joint_name = self._joint_names[id] 434 try: 435 joint_pos = self._joint_pos[joint_name]['position'] 436 joint_widgets[id].setValue(joint_pos) 437 except (KeyError): 438 pass # Can happen when first connected to controller

439

440 - def _joint_widgets(self): # TODO: Cache instead of compute every time?

441 widgets = [] 442 layout = self._widget.joint_group.layout() 443 for row_id in range(layout.rowCount()): 444 widgets.append(layout.itemAt(row_id, 445 QFormLayout.FieldRole).widget()) 446 return widgets

Source Code for Module rqt_joint_trajectory_controller.joint_trajectory_controller