message: use wait_for_message to get initial pose in interactive_marker.py
diff: @@ -12,7 +12,6 @@ from geometry_msgs.msg import PoseStamped
  from franka_msgs.msg import FrankaState
  marker_pose = PoseStamped()
  -initial_pose_found = False
  pose_pub = None
  # [[min_x, max_x], [min_y, max_y], [min_z, max_z]]
  position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
  @@ -24,24 +23,6 @@ def publisher_callback(msg, link_name):
  pose_pub.publish(marker_pose)
  -def franka_state_callback(msg):
  - initial_quaternion = \
  - tf.transformations.quaternion_from_matrix(
  - np.transpose(np.reshape(msg.O_T_EE,
  - (4, 4))))
  - initial_quaternion = initial_quaternion / \
  - np.linalg.norm(initial_quaternion)
  - marker_pose.pose.orientation.x = initial_quaternion[0]
  - marker_pose.pose.orientation.y = initial_quaternion[1]
  - marker_pose.pose.orientation.z = initial_quaternion[2]
  - marker_pose.pose.orientation.w = initial_quaternion[3]
  - marker_pose.pose.position.x = msg.O_T_EE[12]
  - marker_pose.pose.position.y = msg.O_T_EE[13]
  - marker_pose.pose.position.z = msg.O_T_EE[14]
  - global initial_pose_found
  - initial_pose_found = True
  -
  -
  def process_feedback(feedback):
  if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
  marker_pose.pose.position.x = max([min([feedback.pose.position.x,
  @@ -57,17 +38,31 @@ def process_feedback(feedback):
  server.applyChanges()
  +def wait_for_initial_pose():
  + msg = rospy.wait_for_message("franka_state_controller/franka_states",
  + FrankaState) # type: FrankaState
  +
  + initial_quaternion = \
  + tf.transformations.quaternion_from_matrix(
  + np.transpose(np.reshape(msg.O_T_EE,
  + (4, 4))))
  + initial_quaternion = initial_quaternion / \
  + np.linalg.norm(initial_quaternion)
  + marker_pose.pose.orientation.x = initial_quaternion[0]
  + marker_pose.pose.orientation.y = initial_quaternion[1]
  + marker_pose.pose.orientation.z = initial_quaternion[2]
  + marker_pose.pose.orientation.w = initial_quaternion[3]
  + marker_pose.pose.position.x = msg.O_T_EE[12]
  + marker_pose.pose.position.y = msg.O_T_EE[13]
  + marker_pose.pose.position.z = msg.O_T_EE[14]
  +
  +
  if __name__ == "__main__":
  rospy.init_node("equilibrium_pose_node")
  - state_sub = rospy.Subscriber("franka_state_controller/franka_states",
  - FrankaState, franka_state_callback)
  listener = tf.TransformListener()
  link_name = rospy.get_param("~link_name")
  - # Get initial pose for the interactive marker
  - while not initial_pose_found and not rospy.is_shutdown():
  - rospy.sleep(1)
  - state_sub.unregister()
  + wait_for_initial_pose()
  pose_pub = rospy.Publisher(
  "equilibrium_pose", PoseStamped, queue_size=10)
  