def goto_pose(self, name, T, timeout):
self.server.setPose("move_arm_marker", convert_to_message(T))
self.server.applyChanges()
self.pub_command.publish(convert_to_trans_message(T))
print 'Goal published'
start_time = time.time()
done = False
while not done and not rospy.is_shutdown():
self.mutex.acquire()
last_joint_state = deepcopy(self.joint_state)
self.mutex.release()
if not self.check_validity(last_joint_state):
print 'COLLISION!'
try:
(trans,rot) = self.listener.lookupTransform('world_link','lwr_arm_7_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF Exception!"
continue
TR = numpy.dot(tf.transformations.translation_matrix(trans),
tf.transformations.quaternion_matrix(rot))
if (is_same(T, TR)):
print name + ": Reached goal"
done = True
if (time.time() - start_time > timeout) :
done = True
print name + ": Robot took too long to reach goal. Grader timed out"
else:
rospy.sleep(0.05)
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = self.robot.get_root()
control_marker.name = "move_arm_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
box = Marker()
box.type = Marker.CUBE
box.scale.x = 0.15
box.scale.y = 0.03
box.scale.z = 0.03
box.color.r = 0.5
box.color.g = 0.5
box.color.b = 0.5
box.color.a = 1.0
menu_control.markers.append(box)
box2 = deepcopy(box)
box2.scale.x = 0.03
box2.scale.z = 0.1
box2.pose.position.z = 0.05
menu_control.markers.append(box2)
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Move Arm", callback=self.move_arm_cb)
obs_entry = self.menu_handler.insert("Obstacles")
self.menu_handler.insert("No Obstacle",
callback=self.no_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Simple Obstacle",
callback=self.simple_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Hard Obstacle",
callback=self.complex_obs_cb,
parent=obs_entry)
self.menu_handler.insert("Super-hard Obstacle",
callback=self.super_obs_cb,
parent=obs_entry)
self.menu_handler.apply(
self.server,
"move_arm_marker",
)
self.server.applyChanges()
Ttrans = tf.transformations.translation_matrix((0.6, 0.2, 0.5))
Rtrans = tf.transformations.rotation_matrix(1.57, (0, 1, 0))
self.server.setPose("move_arm_marker",
convert_to_message(numpy.dot(Ttrans, Rtrans)))
self.server.applyChanges()