moveitjoy_module.py
Go to the documentation of this file.
1 # ********************************************************************
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2014, JSK, The University of Tokyo.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the JSK, The University of Tokyo nor the names of its
18 # nor the names of its contributors may be
19 # 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 # Author: Ryohei Ueda, Dave Coleman
37 # Desc: Interface between PS3/XBox controller and MoveIt! Motion Planning Rviz Plugin
38 
39 from __future__ import print_function
40 
41 import xml.dom.minidom
42 from operator import add
43 import sys
44 import threading
45 from moveit_ros_planning_interface._moveit_robot_interface import RobotInterface
46 
47 import rospy
48 import roslib
49 import numpy
50 import time
51 import tf
52 from std_msgs.msg import Empty, String
53 from sensor_msgs.msg import Joy
54 from geometry_msgs.msg import PoseStamped
55 from visualization_msgs.msg import InteractiveMarkerInit
56 
57 
58 def signedSquare(val):
59  if val > 0:
60  sign = 1
61  else:
62  sign = -1
63  return val * val * sign
64 
65 
66 # classes to use joystick of xbox, ps3(wired) and ps3(wireless).
67 
68 
69 class JoyStatus:
70  def __init__(self):
71  self.center = False
72  self.select = False
73  self.start = False
74  self.L3 = False
75  self.R3 = False
76  self.square = False
77  self.up = False
78  self.down = False
79  self.left = False
80  self.right = False
81  self.triangle = False
82  self.cross = False
83  self.circle = False
84  self.L1 = False
85  self.R1 = False
86  self.L2 = False
87  self.R2 = False
88  self.left_analog_x = 0.0
89  self.left_analog_y = 0.0
90  self.right_analog_x = 0.0
91  self.right_analog_y = 0.0
92 
93 
95  def __init__(self, msg):
96  JoyStatus.__init__(self)
97  if msg.buttons[8] == 1:
98  self.center = True
99  else:
100  self.center = False
101  if msg.buttons[6] == 1:
102  self.select = True
103  else:
104  self.select = False
105  if msg.buttons[7] == 1:
106  self.start = True
107  else:
108  self.start = False
109  if msg.buttons[9] == 1:
110  self.L3 = True
111  else:
112  self.L3 = False
113  if msg.buttons[10] == 1:
114  self.R3 = True
115  else:
116  self.R3 = False
117  if msg.buttons[2] == 1:
118  self.square = True
119  else:
120  self.square = False
121  if msg.buttons[1] == 1:
122  self.circle = True
123  else:
124  self.circle = False
125  if msg.axes[7] > 0.1:
126  self.up = True
127  else:
128  self.up = False
129  if msg.axes[7] < -0.1:
130  self.down = True
131  else:
132  self.down = False
133  if msg.axes[6] > 0.1:
134  self.left = True
135  else:
136  self.left = False
137  if msg.axes[6] < -0.1:
138  self.right = True
139  else:
140  self.right = False
141  if msg.buttons[3] == 1:
142  self.triangle = True
143  else:
144  self.triangle = False
145  if msg.buttons[0] == 1:
146  self.cross = True
147  else:
148  self.cross = False
149  if msg.buttons[4] == 1:
150  self.L1 = True
151  else:
152  self.L1 = False
153  if msg.buttons[5] == 1:
154  self.R1 = True
155  else:
156  self.R1 = False
157  if msg.axes[2] < -0.5:
158  self.L2 = True
159  else:
160  self.L2 = False
161  if msg.axes[5] < -0.5:
162  self.R2 = True
163  else:
164  self.R2 = False
165  self.left_analog_x = msg.axes[0]
166  self.left_analog_y = msg.axes[1]
167  self.right_analog_x = msg.axes[3]
168  self.right_analog_y = msg.axes[4]
169  self.orig_msg = msg
170 
171 
173  def __init__(self, msg):
174  JoyStatus.__init__(self)
175  # creating from sensor_msgs/Joy
176  if msg.buttons[16] == 1:
177  self.center = True
178  else:
179  self.center = False
180  if msg.buttons[0] == 1:
181  self.select = True
182  else:
183  self.select = False
184  if msg.buttons[3] == 1:
185  self.start = True
186  else:
187  self.start = False
188  if msg.buttons[1] == 1:
189  self.L3 = True
190  else:
191  self.L3 = False
192  if msg.buttons[2] == 1:
193  self.R3 = True
194  else:
195  self.R3 = False
196  if msg.axes[15] < 0:
197  self.square = True
198  else:
199  self.square = False
200  if msg.axes[4] < 0:
201  self.up = True
202  else:
203  self.up = False
204  if msg.axes[6] < 0:
205  self.down = True
206  else:
207  self.down = False
208  if msg.axes[7] < 0:
209  self.left = True
210  else:
211  self.left = False
212  if msg.axes[5] < 0:
213  self.right = True
214  else:
215  self.right = False
216  if msg.axes[12] < 0:
217  self.triangle = True
218  else:
219  self.triangle = False
220  if msg.axes[14] < 0:
221  self.cross = True
222  else:
223  self.cross = False
224  if msg.axes[13] < 0:
225  self.circle = True
226  else:
227  self.circle = False
228  if msg.axes[10] < 0:
229  self.L1 = True
230  else:
231  self.L1 = False
232  if msg.axes[11] < 0:
233  self.R1 = True
234  else:
235  self.R1 = False
236  if msg.axes[8] < 0:
237  self.L2 = True
238  else:
239  self.L2 = False
240  if msg.axes[9] < 0:
241  self.R2 = True
242  else:
243  self.R2 = False
244  self.left_analog_x = msg.axes[0]
245  self.left_analog_y = msg.axes[1]
246  self.right_analog_x = msg.axes[2]
247  self.right_analog_y = msg.axes[3]
248  self.orig_msg = msg
249 
250 
252  def __init__(self, msg):
253  JoyStatus.__init__(self)
254  # creating from sensor_msgs/Joy
255  if msg.buttons[16] == 1:
256  self.center = True
257  else:
258  self.center = False
259  if msg.buttons[0] == 1:
260  self.select = True
261  else:
262  self.select = False
263  if msg.buttons[3] == 1:
264  self.start = True
265  else:
266  self.start = False
267  if msg.buttons[1] == 1:
268  self.L3 = True
269  else:
270  self.L3 = False
271  if msg.buttons[2] == 1:
272  self.R3 = True
273  else:
274  self.R3 = False
275  if msg.buttons[15] == 1:
276  self.square = True
277  else:
278  self.square = False
279  if msg.buttons[4] == 1:
280  self.up = True
281  else:
282  self.up = False
283  if msg.buttons[6] == 1:
284  self.down = True
285  else:
286  self.down = False
287  if msg.buttons[7] == 1:
288  self.left = True
289  else:
290  self.left = False
291  if msg.buttons[5] == 1:
292  self.right = True
293  else:
294  self.right = False
295  if msg.buttons[12] == 1:
296  self.triangle = True
297  else:
298  self.triangle = False
299  if msg.buttons[14] == 1:
300  self.cross = True
301  else:
302  self.cross = False
303  if msg.buttons[13] == 1:
304  self.circle = True
305  else:
306  self.circle = False
307  if msg.buttons[10] == 1:
308  self.L1 = True
309  else:
310  self.L1 = False
311  if msg.buttons[11] == 1:
312  self.R1 = True
313  else:
314  self.R1 = False
315  if msg.buttons[8] == 1:
316  self.L2 = True
317  else:
318  self.L2 = False
319  if msg.buttons[9] == 1:
320  self.R2 = True
321  else:
322  self.R2 = False
323  self.left_analog_x = msg.axes[0]
324  self.left_analog_y = msg.axes[1]
325  self.right_analog_x = msg.axes[2]
326  self.right_analog_y = msg.axes[3]
327  self.orig_msg = msg
328 
329 
331  def __init__(self, msg):
332  JoyStatus.__init__(self)
333  # creating from sensor_msg/Joy
334  if msg.buttons[12] == 1:
335  self.center = True
336  else:
337  self.center = False
338  if msg.buttons[8] == 1:
339  self.select = True
340  else:
341  self.select = False
342  if msg.buttons[9] == 1:
343  self.start = True
344  else:
345  self.start = False
346  if msg.buttons[10] == 1:
347  self.L3 = True
348  else:
349  self.L3 = False
350  if msg.buttons[11] == 1:
351  self.R3 = True
352  else:
353  self.R3 = False
354  if msg.buttons[0] == 1:
355  self.square = True
356  else:
357  self.square = False
358  if msg.axes[10] < 0:
359  self.up = True
360  else:
361  self.up = False
362  if msg.axes[10] > 0:
363  self.down = True
364  else:
365  self.down = False
366  if msg.axes[9] < 0:
367  self.left = True
368  else:
369  self.left = False
370  if msg.axes[9] > 0:
371  self.right = True
372  else:
373  self.right = False
374  if msg.buttons[3] == 1:
375  self.triangle = True
376  else:
377  self.triangle = False
378  if msg.buttons[1] == 1:
379  self.cross = True
380  else:
381  self.cross = False
382  if msg.buttons[2] == 1:
383  self.circle = True
384  else:
385  self.circle = False
386  if msg.buttons[4] == 1:
387  self.L1 = True
388  else:
389  self.L1 = False
390  if msg.buttons[5] == 1:
391  self.R1 = True
392  else:
393  self.R1 = False
394  if msg.buttons[6] == 1:
395  self.L2 = True
396  else:
397  self.L2 = False
398  if msg.buttons[7] == 1:
399  self.R2 = True
400  else:
401  self.R2 = False
402  self.left_analog_x = msg.axes[0]
403  self.left_analog_y = msg.axes[1]
404  self.right_analog_x = msg.axes[5]
405  self.right_analog_y = msg.axes[2]
406  self.orig_msg = msg
407 
408 
410  def __init__(self, msg):
411  JoyStatus.__init__(self)
412  # creating from sensor_msg/Joy
413  if msg.buttons[10] == 1:
414  self.center = True
415  else:
416  self.center = False
417  if msg.buttons[8] == 1:
418  self.select = True
419  else:
420  self.select = False
421  if msg.buttons[9] == 1:
422  self.start = True
423  else:
424  self.start = False
425  if msg.buttons[11] == 1:
426  self.L3 = True
427  else:
428  self.L3 = False
429  if msg.buttons[12] == 1:
430  self.R3 = True
431  else:
432  self.R3 = False
433  if msg.buttons[3] == 1:
434  self.square = True
435  else:
436  self.square = False
437  if msg.axes[7] < 0:
438  self.up = True
439  else:
440  self.up = False
441  if msg.axes[7] > 0:
442  self.down = True
443  else:
444  self.down = False
445  if msg.axes[6] < 0:
446  self.left = True
447  else:
448  self.left = False
449  if msg.axes[6] > 0:
450  self.right = True
451  else:
452  self.right = False
453  if msg.buttons[2] == 1:
454  self.triangle = True
455  else:
456  self.triangle = False
457  if msg.buttons[0] == 1:
458  self.cross = True
459  else:
460  self.cross = False
461  if msg.buttons[1] == 1:
462  self.circle = True
463  else:
464  self.circle = False
465  if msg.buttons[4] == 1:
466  self.L1 = True
467  else:
468  self.L1 = False
469  if msg.buttons[5] == 1:
470  self.R1 = True
471  else:
472  self.R1 = False
473  if msg.buttons[6] == 1:
474  self.L2 = True
475  else:
476  self.L2 = False
477  if msg.buttons[7] == 1:
478  self.R2 = True
479  else:
480  self.R2 = False
481  self.left_analog_x = msg.axes[0]
482  self.left_analog_y = msg.axes[1]
483  self.right_analog_x = msg.axes[3]
484  self.right_analog_y = msg.axes[4]
485  self.orig_msg = msg
486 
487 
489  def __init__(self, max_length=10):
490  self.max_length = max_length
491  self.buffer = []
492 
493  def add(self, status):
494  self.buffer.append(status)
495  if len(self.buffer) > self.max_length:
496  self.buffer = self.buffer[1 : self.max_length + 1]
497 
498  def all(self, proc):
499  for status in self.buffer:
500  if not proc(status):
501  return False
502  return True
503 
504  def latest(self):
505  if len(self.buffer) > 0:
506  return self.buffer[-1]
507  else:
508  return None
509 
510  def length(self):
511  return len(self.buffer)
512 
513  def new(self, status, attr):
514  if len(self.buffer) == 0:
515  return getattr(status, attr)
516  else:
517  return getattr(status, attr) and not getattr(self.latest(), attr)
518 
519 
520 class MoveitJoy:
521  def parseSRDF(self):
522  ri = RobotInterface("/robot_description")
523  planning_groups = {}
524  for g in ri.get_group_names():
525  self.planning_groups_tips[g] = ri.get_group_joint_tips(g)
526  planning_groups[g] = [
527  "/rviz/moveit/move_marker/goal_" + l
528  for l in self.planning_groups_tips[g]
529  ]
530  for name in planning_groups.keys():
531  if len(planning_groups[name]) == 0:
532  del planning_groups[name]
533  else:
534  print(name, planning_groups[name])
535  self.planning_groups = planning_groups
537  planning_groups.keys()
538  ) # we'd like to store the 'order'
539  self.frame_id = ri.get_planning_frame()
540 
541  def __init__(self):
542  self.initial_poses = {}
545  self.marker_lock = threading.Lock()
546  self.prev_time = rospy.Time.now()
547  self.counter = 0
548  self.history = StatusHistory(max_length=10)
549  self.pre_pose = PoseStamped()
550  self.pre_pose.pose.orientation.w = 1
553  self.initialize_poses = False
554  self.initialized = False
555  self.parseSRDF()
556  self.plan_group_pub = rospy.Publisher(
557  "/rviz/moveit/select_planning_group", String, queue_size=5
558  )
559  self.updatePlanningGroup(0)
560  self.updatePoseTopic(0, False)
561  self.joy_pose_pub = rospy.Publisher("/joy_pose", PoseStamped, queue_size=1)
562  self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty, queue_size=5)
563  self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty, queue_size=5)
564  self.update_start_state_pub = rospy.Publisher(
565  "/rviz/moveit/update_start_state", Empty, queue_size=5
566  )
567  self.update_goal_state_pub = rospy.Publisher(
568  "/rviz/moveit/update_goal_state", Empty, queue_size=5
569  )
570  self.interactive_marker_sub = rospy.Subscriber(
571  "/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full",
572  InteractiveMarkerInit,
573  self.markerCB,
574  queue_size=1,
575  )
576  self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
577 
578  def updatePlanningGroup(self, next_index):
579  if next_index >= len(self.planning_groups_keys):
581  elif next_index < 0:
583  else:
584  self.current_planning_group_index = next_index
585  next_planning_group = None
586  try:
587  next_planning_group = self.planning_groups_keys[
589  ]
590  except IndexError:
591  msg = "Check if you started movegroups. Exiting."
592  rospy.logfatal(msg)
593  raise rospy.ROSInitException(msg)
594  rospy.loginfo("Changed planning group to " + next_planning_group)
595  self.plan_group_pub.publish(next_planning_group)
596 
597  def updatePoseTopic(self, next_index, wait=True):
598  planning_group = self.planning_groups_keys[self.current_planning_group_index]
599  topics = self.planning_groups[planning_group]
600  if next_index >= len(topics):
601  self.current_eef_index = 0
602  elif next_index < 0:
603  self.current_eef_index = len(topics) - 1
604  else:
605  self.current_eef_index = next_index
606  next_topic = topics[self.current_eef_index]
607 
608  rospy.loginfo(
609  "Changed controlled end effector to "
610  + self.planning_groups_tips[planning_group][self.current_eef_index]
611  )
612  self.pose_pub = rospy.Publisher(next_topic, PoseStamped, queue_size=5)
613  if wait:
614  self.waitForInitialPose(next_topic)
615  self.current_pose_topic = next_topic
616 
617  def markerCB(self, msg):
618  try:
619  self.marker_lock.acquire()
620  if not self.initialize_poses:
621  return
622  self.initial_poses = {}
623  for marker in msg.markers:
624  if marker.name.startswith("EE:goal_"):
625  # resolve tf
626  if marker.header.frame_id != self.frame_id:
627  ps = PoseStamped(header=marker.header, pose=marker.pose)
628  try:
629  transformed_pose = self.tf_listener.transformPose(
630  self.frame_id, ps
631  )
632  self.initial_poses[marker.name[3:]] = transformed_pose.pose
633  except (
637  e,
638  ):
639  rospy.logerr("tf error when resolving tf: %s" % e)
640  else:
641  self.initial_poses[
642  marker.name[3:]
643  ] = marker.pose # tf should be resolved
644  finally:
645  self.marker_lock.release()
646 
647  def waitForInitialPose(self, next_topic, timeout=None):
648  counter = 0
649  while not rospy.is_shutdown():
650  counter = counter + 1
651  if timeout and counter >= timeout:
652  return False
653  try:
654  self.marker_lock.acquire()
655  self.initialize_poses = True
656  topic_suffix = next_topic.split("/")[-1]
657  if topic_suffix in self.initial_poses:
658  self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix])
659  self.initialize_poses = False
660  return True
661  else:
662  rospy.logdebug(self.initial_poses.keys())
663  rospy.loginfo(
664  "Waiting for pose topic of '%s' to be initialized", topic_suffix
665  )
666  rospy.sleep(1)
667  finally:
668  self.marker_lock.release()
669 
670  def joyCB(self, msg):
671  if len(msg.axes) == 27 and len(msg.buttons) == 19:
672  status = PS3WiredStatus(msg)
673  elif len(msg.axes) == 8 and len(msg.buttons) == 11:
674  status = XBoxStatus(msg)
675  elif len(msg.axes) == 20 and len(msg.buttons) == 17:
676  status = PS3Status(msg)
677  elif len(msg.axes) == 14 and len(msg.buttons) == 14:
678  status = PS4Status(msg)
679  elif len(msg.axes) == 8 and len(msg.buttons) == 13:
680  status = PS4WiredStatus(msg)
681  else:
682  raise Exception("Unknown joystick")
683  self.run(status)
684  self.history.add(status)
685 
686  def computePoseFromJoy(self, pre_pose, status):
687  new_pose = PoseStamped()
688  new_pose.header.frame_id = self.frame_id
689  new_pose.header.stamp = rospy.Time(0.0)
690  # move in local
691  dist = (
692  status.left_analog_y * status.left_analog_y
693  + status.left_analog_x * status.left_analog_x
694  )
695  scale = 200.0
696  x_diff = signedSquare(status.left_analog_y) / scale
697  y_diff = signedSquare(status.left_analog_x) / scale
698  # z
699  if status.L2:
700  z_diff = 0.005
701  elif status.R2:
702  z_diff = -0.005
703  else:
704  z_diff = 0.0
705  if self.history.all(lambda s: s.L2) or self.history.all(lambda s: s.R2):
706  z_scale = 4.0
707  else:
708  z_scale = 2.0
709  local_move = numpy.array((x_diff, y_diff, z_diff * z_scale, 1.0))
710  q = numpy.array(
711  (
712  pre_pose.pose.orientation.x,
713  pre_pose.pose.orientation.y,
714  pre_pose.pose.orientation.z,
715  pre_pose.pose.orientation.w,
716  )
717  )
718  xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q), local_move)
719  new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
720  new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
721  new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
722  roll = 0.0
723  pitch = 0.0
724  yaw = 0.0
725  DTHETA = 0.005
726  if status.L1:
727  if self.history.all(lambda s: s.L1):
728  yaw = yaw + DTHETA * 2
729  else:
730  yaw = yaw + DTHETA
731  elif status.R1:
732  if self.history.all(lambda s: s.R1):
733  yaw = yaw - DTHETA * 2
734  else:
735  yaw = yaw - DTHETA
736  if status.up:
737  if self.history.all(lambda s: s.up):
738  pitch = pitch + DTHETA * 2
739  else:
740  pitch = pitch + DTHETA
741  elif status.down:
742  if self.history.all(lambda s: s.down):
743  pitch = pitch - DTHETA * 2
744  else:
745  pitch = pitch - DTHETA
746  if status.right:
747  if self.history.all(lambda s: s.right):
748  roll = roll + DTHETA * 2
749  else:
750  roll = roll + DTHETA
751  elif status.left:
752  if self.history.all(lambda s: s.left):
753  roll = roll - DTHETA * 2
754  else:
755  roll = roll - DTHETA
756  diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
757  new_q = tf.transformations.quaternion_multiply(q, diff_q)
758  new_pose.pose.orientation.x = new_q[0]
759  new_pose.pose.orientation.y = new_q[1]
760  new_pose.pose.orientation.z = new_q[2]
761  new_pose.pose.orientation.w = new_q[3]
762  return new_pose
763 
764  def run(self, status):
765  if not self.initialized:
766  # when not initialized, we will force to change planning_group
767  while True:
769  planning_group = self.planning_groups_keys[
771  ]
772  topics = self.planning_groups[planning_group]
773  next_topic = topics[self.current_eef_index]
774  if not self.waitForInitialPose(next_topic, timeout=3):
775  rospy.logwarn(
776  "Unable to initialize planning group "
777  + planning_group
778  + ". Trying different group."
779  )
780  rospy.logwarn(
781  "Is 'Allow External Comm.' enabled in Rviz? Is the 'Query Goal State' robot enabled?"
782  )
783  else:
784  rospy.loginfo("Initialized planning group")
785  self.initialized = True
787  return
788  # Try to initialize with different planning group
791  self.current_planning_group_index = 0 # reset loop
792  if self.history.new(status, "select"): # increment planning group
794  self.current_eef_index = 0 # force to reset
796  return
797  elif self.history.new(status, "start"): # decrement planning group
799  self.current_eef_index = 0 # force to reset
801  return
802  elif self.history.new(status, "triangle"):
803  self.updatePoseTopic(self.current_eef_index + 1)
804  return
805  elif self.history.new(status, "cross"):
806  self.updatePoseTopic(self.current_eef_index - 1)
807  return
808  elif self.history.new(status, "square"): # plan
809  rospy.loginfo("Plan")
810  self.plan_pub.publish(Empty())
811  return
812  elif self.history.new(status, "circle"): # execute
813  rospy.loginfo("Execute")
814  self.execute_pub.publish(Empty())
815  return
816  self.marker_lock.acquire()
817  pre_pose = self.pre_pose
818  new_pose = self.computePoseFromJoy(pre_pose, status)
819  now = rospy.Time.from_sec(time.time())
820  # placement.time_from_start = now - self.prev_time
821  if (now - self.prev_time).to_sec() > 1 / 30.0:
822  # rospy.loginfo(new_pose)
823  self.pose_pub.publish(new_pose)
824  self.joy_pose_pub.publish(new_pose)
825  self.prev_time = now
826  # sync start state to the real robot state
827  self.counter = self.counter + 1
828  if self.counter % 10:
829  self.update_start_state_pub.publish(Empty())
830  self.pre_pose = new_pose
831  self.marker_lock.release()
832  # update self.initial_poses
833  self.marker_lock.acquire()
834  self.initial_poses[self.current_pose_topic.split("/")[-1]] = new_pose.pose
835  self.marker_lock.release()
def updatePoseTopic(self, next_index, wait=True)
def waitForInitialPose(self, next_topic, timeout=None)


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Nov 8 2021 03:52:26