39 from __future__
import print_function
41 import xml.dom.minidom
42 from operator
import add
45 from moveit_ros_planning_interface._moveit_robot_interface
import RobotInterface
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
63 return val * val * sign
96 JoyStatus.__init__(self)
99 self.
start = msg.buttons[7] == 1
100 self.
L3 = msg.buttons[9] == 1
101 self.
R3 = msg.buttons[10] == 1
104 self.
up = msg.axes[7] > 0.1
110 self.
L1 = msg.buttons[4] == 1
111 self.
R1 = msg.buttons[5] == 1
112 self.
L2 = msg.axes[2] < -0.5
113 self.
R2 = msg.axes[5] < -0.5
123 JoyStatus.__init__(self)
129 self.
L1 = msg.buttons[4] == 1
130 self.
R1 = msg.buttons[5] == 1
131 self.
L2 = msg.buttons[6] == 1
132 self.
R2 = msg.buttons[7] == 1
146 JoyStatus.__init__(self)
151 self.
L3 = msg.buttons[1] == 1
152 self.
R3 = msg.buttons[2] == 1
154 self.
up = msg.axes[4] < 0
161 self.
L1 = msg.axes[10] < 0
162 self.
R1 = msg.axes[11] < 0
163 self.
L2 = msg.axes[8] < 0
164 self.
R2 = msg.axes[9] < 0
174 JoyStatus.__init__(self)
179 self.
L3 = msg.buttons[1] == 1
180 self.
R3 = msg.buttons[2] == 1
182 self.
up = msg.buttons[4] == 1
183 self.
down = msg.buttons[6] == 1
184 self.
left = msg.buttons[7] == 1
189 self.
L1 = msg.buttons[10] == 1
190 self.
R1 = msg.buttons[11] == 1
191 self.
L2 = msg.buttons[8] == 1
192 self.
R2 = msg.buttons[9] == 1
202 JoyStatus.__init__(self)
207 self.
L3 = msg.buttons[10] == 1
208 self.
R3 = msg.buttons[11] == 1
210 self.
up = msg.axes[10] < 0
217 self.
L1 = msg.buttons[4] == 1
218 self.
R1 = msg.buttons[5] == 1
219 self.
L2 = msg.buttons[6] == 1
220 self.
R2 = msg.buttons[7] == 1
230 JoyStatus.__init__(self)
235 self.
L3 = msg.buttons[11] == 1
236 self.
R3 = msg.buttons[12] == 1
238 self.
up = msg.axes[7] < 0
245 self.
L1 = msg.buttons[4] == 1
246 self.
R1 = msg.buttons[5] == 1
247 self.
L2 = msg.buttons[6] == 1
248 self.
R2 = msg.buttons[7] == 1
262 self.
buffer.append(status)
267 for status
in self.
buffer:
281 def new(self, status, attr):
283 return getattr(status, attr)
285 return getattr(status, attr)
and not getattr(self.
latest(), attr)
290 ri = RobotInterface(
"/robot_description")
292 for g
in ri.get_group_names():
295 planning_groups[g] = [
296 "/rviz/moveit/move_marker/goal_" + l
299 for name
in planning_groups.keys():
300 print(name, planning_groups[name])
303 planning_groups.keys()
316 self.
pre_pose.pose.orientation.w = 1
323 "/rviz/moveit/select_planning_group", String, queue_size=5
327 self.
joy_pose_pub = rospy.Publisher(
"/joy_pose", PoseStamped, queue_size=1)
328 self.
plan_pub = rospy.Publisher(
"/rviz/moveit/plan", Empty, queue_size=5)
329 self.
execute_pub = rospy.Publisher(
"/rviz/moveit/execute", Empty, queue_size=5)
331 "/rviz/moveit/update_start_state", Empty, queue_size=5
334 "/rviz/moveit/update_goal_state", Empty, queue_size=5
337 "/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full",
338 InteractiveMarkerInit,
342 self.
sub = rospy.Subscriber(
"/joy", Joy, self.
joyCB, queue_size=1)
351 next_planning_group =
None
357 msg =
"Check if you started movegroups. Exiting."
359 raise rospy.ROSInitException(msg)
360 rospy.loginfo(
"Changed planning group to " + next_planning_group)
366 if next_index >= len(topics):
375 "Changed controlled end effector to "
378 self.
pose_pub = rospy.Publisher(next_topic, PoseStamped, queue_size=5)
389 for marker
in msg.markers:
390 if marker.name.startswith(
"EE:goal_"):
392 if marker.header.frame_id != self.
frame_id:
393 ps = PoseStamped(header=marker.header, pose=marker.pose)
405 rospy.logerr(
"tf error when resolving tf: %s" % e)
415 while not rospy.is_shutdown():
416 counter = counter + 1
417 if timeout
and counter >= timeout:
422 topic_suffix = next_topic.split(
"/")[-1]
430 "Waiting for pose topic of '%s' to be initialized", topic_suffix
437 axes_amount = len(msg.axes)
438 buttons_amount = len(msg.buttons)
439 if axes_amount == 27
and buttons_amount == 19:
441 elif axes_amount == 8
and buttons_amount == 11:
443 elif axes_amount == 20
and buttons_amount == 17:
445 elif axes_amount == 14
and buttons_amount == 14:
447 elif axes_amount == 8
and buttons_amount == 13:
449 elif axes_amount == 6
and buttons_amount == 17:
453 "Unknown joystick, axes: {}, buttons: {}".format(
454 axes_amount, buttons_amount
461 new_pose = PoseStamped()
462 new_pose.header.frame_id = self.
frame_id
463 new_pose.header.stamp = rospy.Time(0.0)
466 status.left_analog_y * status.left_analog_y
467 + status.left_analog_x * status.left_analog_x
479 if self.
history.all(
lambda s: s.L2)
or self.
history.all(
lambda s: s.R2):
483 local_move = numpy.array((x_diff, y_diff, z_diff * z_scale, 1.0))
486 pre_pose.pose.orientation.x,
487 pre_pose.pose.orientation.y,
488 pre_pose.pose.orientation.z,
489 pre_pose.pose.orientation.w,
492 xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q), local_move)
493 new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
494 new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
495 new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
501 if self.
history.all(
lambda s: s.L1):
502 yaw = yaw + DTHETA * 2
506 if self.
history.all(
lambda s: s.R1):
507 yaw = yaw - DTHETA * 2
511 if self.
history.all(
lambda s: s.up):
512 pitch = pitch + DTHETA * 2
514 pitch = pitch + DTHETA
516 if self.
history.all(
lambda s: s.down):
517 pitch = pitch - DTHETA * 2
519 pitch = pitch - DTHETA
521 if self.
history.all(
lambda s: s.right):
522 roll = roll + DTHETA * 2
526 if self.
history.all(
lambda s: s.left):
527 roll = roll - DTHETA * 2
530 diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
531 new_q = tf.transformations.quaternion_multiply(q, diff_q)
532 new_pose.pose.orientation.x = new_q[0]
533 new_pose.pose.orientation.y = new_q[1]
534 new_pose.pose.orientation.z = new_q[2]
535 new_pose.pose.orientation.w = new_q[3]
550 "Unable to initialize planning group "
552 +
". Trying different group."
555 "Is 'Allow External Comm.' enabled in Rviz? Is the 'Query Goal State' robot enabled?"
558 rospy.loginfo(
"Initialized planning group")
566 if self.
history.new(status,
"select"):
571 elif self.
history.new(status,
"start"):
576 elif self.
history.new(status,
"triangle"):
579 elif self.
history.new(status,
"cross"):
582 elif self.
history.new(status,
"square"):
583 rospy.loginfo(
"Plan")
586 elif self.
history.new(status,
"circle"):
587 rospy.loginfo(
"Execute")
593 now = rospy.Time.from_sec(time.time())
595 if (now - self.
prev_time).to_sec() > 1 / 30.0: