jsk_teleop_joy: joy_footstep_planner.py Source File

Go to the documentation of this file.
00001 import rospy
00002 
00003 import imp
00004 try:
00005   imp.find_module("actionlib")
00006 except:
00007   import roslib; roslib.load_manifest('jsk_teleop_joy')
00008 import threading
00009 import actionlib
00010 from joy_pose_6d import JoyPose6D
00011 from actionlib_msgs.msg import GoalStatusArray
00012 from jsk_footstep_msgs.msg import PlanFootstepsAction, PlanFootstepsGoal, Footstep, FootstepArray, ExecFootstepsAction, ExecFootstepsGoal
00013 from jsk_rviz_plugins.msg import OverlayMenu, OverlayText
00014 from std_msgs.msg import UInt8, Empty
00015 from std_srvs.srv import Empty as EmptySrv
00016 import std_srvs.srv
00017 import tf
00018 from tf.transformations import *
00019 from geometry_msgs.msg import PoseStamped
00020 import jsk_teleop_joy.tf_ext as tf_ext
00021 from jsk_footstep_planner.srv import ChangeSuccessor
00022 class JoyFootstepPlanner(JoyPose6D):
00023   EXECUTING = 1
00024   PLANNING = 2
00025   WAIT_FOR_CANCEL = 3
00026   CANCELED = 4
00027   CANCELED_MENUS = ["Cancel", 
00028                     "Ignore and proceed",
00029                     "Use larger footsteps",
00030                     "Use middle footsteps",
00031                     "Use smaller footsteps"]
00032   def __init__(self, name, args):
00033     JoyPose6D.__init__(self, name, args)
00034     self.usage_pub = rospy.Publisher("/joy/usage", OverlayText)
00035     self.supportFollowView(True)
00036     self.mode = self.PLANNING
00037     self.snapped_pose = None
00038     self.ignore_next_status_flag = False
00039     self.prev_mode = self.PLANNING
00040     self.frame_id = self.getArg('frame_id', '/map')
00041     self.lfoot_frame_id = rospy.get_param('~lfoot_frame_id', '/LLEG_LINK5')
00042     self.rfoot_frame_id = rospy.get_param('~rfoot_frame_id', '/RLEG_LINK5')
00043     self.lfoot_offset = tf_ext.xyzxyzwToMatrix(rospy.get_param('~lfoot_offset'))
00044     self.rfoot_offset = tf_ext.xyzxyzwToMatrix(rospy.get_param('~rfoot_offset'))
00045     self.lock_furutaractive = self.getArg("lock_furutaractive", False)
00046     self.furutaractive_namespace = self.getArg("furutaractive_namespace", "urdf_model_marker")
00047     self.command_pub = rospy.Publisher('/menu_command', UInt8)
00048     self.execute_pub = rospy.Publisher(self.getArg('execute', 'execute'), Empty)
00049     self.resume_pub = rospy.Publisher(self.getArg('resume', 'resume'), Empty)
00050     self.tf_listener = tf.TransformListener()
00051     self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
00052     # initialize self.pre_pose
00053     rospy.loginfo("waiting %s" % (self.lfoot_frame_id))
00054     self.tf_listener.waitForTransform(self.frame_id, self.lfoot_frame_id,
00055                                       rospy.Time(0.0), rospy.Duration(100.0))
00056     rospy.loginfo("waiting %s" % (self.rfoot_frame_id))
00057     self.tf_listener.waitForTransform(self.frame_id, self.rfoot_frame_id, 
00058                                       rospy.Time(0.0), rospy.Duration(100.0))
00059     self.exec_client = actionlib.SimpleActionClient('/footstep_controller',
00060                                                     ExecFootstepsAction)
00061     self.status_sub = rospy.Subscriber("/footstep_controller/status", GoalStatusArray,
00062                                        self.statusCB, queue_size=1)
00063     self.snap_sub = rospy.Subscriber(self.getArg("snapped_pose", "/footstep_marker/snapped_pose"), 
00064                                      PoseStamped,
00065                                      self.snapCB,
00066                                      queue_size=1)
00067     self.cancel_menu_sub = rospy.Subscriber("/footstep_cancel_broadcast", Empty, 
00068                                             self.cancelMenuCB, queue_size=1)
00069     self.status_lock = threading.Lock()
00070     self.current_selecting_index = 0
00071     self.resetGoalPose()
00072   def lockFurutaractive(self):
00073     try:
00074       lock = rospy.ServiceProxy(self.furutaractive_namespace + '/lock_joint_states', EmptySrv)
00075       lock()
00076     except rospy.ServiceException, e:
00077       rospy.logerror("failed to call service: %s" % (e.message))
00078   def unlockFurutaractive(self):
00079     try:
00080       unlock = rospy.ServiceProxy(self.furutaractive_namespace + '/unlock_joint_states', EmptySrv)
00081       unlock()
00082     except rospy.ServiceException, e:
00083       rospy.logerror("failed to call service: %s" % (e.message))
00084   def cancelMenuCB(self, msg):
00085     with self.status_lock:
00086       self.mode = self.CANCELED
00087   def snapCB(self, msg):
00088     self.snapped_pose = msg
00089   def statusCB(self, msg):
00090     if len(msg.status_list) == 0:
00091       return                    # do nothing
00092     self.status_lock.acquire()
00093     # msg.status_list[0].status == 0 -> done
00094     if self.mode == self.EXECUTING and msg.status_list[0].status == 0: # ???
00095       # done
00096       if self.ignore_next_status_flag: # hack!!
00097         self.ignore_next_status_flag = False
00098       else:
00099         self.mode = self.PLANNING
00100         if self.lock_furutaractive:
00101           self.unlockFurutaractive()
00102     elif self.mode == self.WAIT_FOR_CANCEL and msg.status_list[0].status == 0:
00103       self.mode = self.CANCELED
00104     self.status_lock.release()
00105   def publishUsage(self):
00106     overlay_text = OverlayText()
00107     overlay_text.text = """
00108 Left Analog: x/y
00109 L2/R2      : +-z
00110 L1/R1      : +-yaw
00111 Left/Right : +-roll
00112 Up/Down    : +-pitch
00113 circle     : Go
00114 cross      : Reset/Cancel
00115 triangle   : Clear maps and look around ground
00116 up/down    : Move menu cursors
00117 """
00118     overlay_text.width = 500
00119     overlay_text.height = 500
00120     overlay_text.text_size = 12
00121     overlay_text.left = 10
00122     overlay_text.top = 10
00123     overlay_text.font = "Ubuntu Mono Regular"
00124     overlay_text.bg_color.a = 0
00125     overlay_text.fg_color.r = 25 / 255.0
00126     overlay_text.fg_color.g = 1
00127     overlay_text.fg_color.b = 1
00128     overlay_text.fg_color.a = 1
00129     self.usage_pub.publish(overlay_text)
00130   def resetGoalPose(self):
00131     # initial pose will be the center 
00132     # of self.lfoot_frame_id and self.rfoot_frame_id
00133     lfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
00134       self.frame_id, 
00135       self.lfoot_frame_id,
00136       rospy.Time(0.0)))
00137     rfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
00138       self.frame_id, 
00139       self.rfoot_frame_id,
00140       rospy.Time(0.0)))
00141     # apply offset
00142     lfoot_with_offset = numpy.dot(lfoot_pose, self.lfoot_offset)
00143     rfoot_with_offset = numpy.dot(rfoot_pose, self.rfoot_offset)
00144     (lfoot_pos, lfoot_q) = tf_ext.decomposeMatrix(lfoot_with_offset)
00145     (rfoot_pos, rfoot_q) = tf_ext.decomposeMatrix(rfoot_with_offset)
00146     # compute the center of the two transformations
00147     mid_pos = (lfoot_pos + rfoot_pos) / 2.0
00148     mid_quaternion = quaternion_slerp(lfoot_q, rfoot_q,
00149                                       0.5)
00150     self.pre_pose.pose.position.x = mid_pos[0]
00151     self.pre_pose.pose.position.y = mid_pos[1]
00152     self.pre_pose.pose.position.z = mid_pos[2]
00153     self.pre_pose.pose.orientation.x = mid_quaternion[0]
00154     self.pre_pose.pose.orientation.y = mid_quaternion[1]
00155     self.pre_pose.pose.orientation.z = mid_quaternion[2]
00156     self.pre_pose.pose.orientation.w = mid_quaternion[3]
00157   def executePlan(self):
00158     # publish execute with std_msgs/empty 
00159     self.execute_pub.publish(Empty())
00160   def resumePlan(self):
00161     # publish execute with std_msgs/empty 
00162     self.resume_pub.publish(Empty())
00163   def publishMenu(self, close=False):
00164     menu = OverlayMenu()
00165     menu.menus = self.CANCELED_MENUS
00166     menu.current_index = self.current_selecting_index
00167     menu.title = "Canceled"
00168     if close:
00169       menu.action = OverlayMenu.ACTION_CLOSE
00170     self.menu_pub.publish(menu)
00171   def changePlanningSuccessor(self, successor_type):
00172     try:
00173       change_successor = rospy.ServiceProxy('/change_successor', ChangeSuccessor)
00174       change_successor(successor_type)
00175     except rospy.ServiceException, e:
00176       rospy.logerror("failed to call service: %s" % (e.message))
00177   def procCancelMenu(self, index):
00178     selected_title = self.CANCELED_MENUS[index]
00179     if selected_title == "Cancel":
00180       self.status_lock.acquire()
00181       self.mode = self.PLANNING
00182       self.status_lock.release()
00183       self.publishMenu(close=True)
00184     elif selected_title == "Ignore and proceed":
00185       # re-execute the plan left
00186       self.resumePlan()
00187       self.status_lock.acquire()
00188       self.mode = self.EXECUTING
00189       self.status_lock.release()
00190       self.publishMenu(close=True)
00191     elif selected_title == "Use smaller footsteps":
00192       self.status_lock.acquire()
00193       self.mode = self.PLANNING
00194       self.changePlanningSuccessor("small")
00195       self.status_lock.release()
00196       self.publishMenu(close=True)
00197     elif (selected_title == "Use larger footsteps" or 
00198           selected_title == "Use middle footsteps"):
00199       self.status_lock.acquire()
00200       self.mode = self.PLANNING
00201       self.changePlanningSuccessor("normal")
00202       self.status_lock.release()
00203       self.publishMenu(close=True)
00204   def lookAround(self):
00205     try:
00206       clear_maps = rospy.ServiceProxy('/env_server/clear_maps', std_srvs.srv.Empty)
00207       clear_maps()
00208       look_around = rospy.ServiceProxy('/lookaround_ground', std_srvs.srv.Empty)
00209       look_around()
00210     except Exception, e:
00211       rospy.logerr("error when lookaround ground: %s", e.message)
00212   def joyCB(self, status, history):
00213     self.publishUsage()
00214     if self.prev_mode != self.mode:
00215       print self.prev_mode, " -> ", self.mode
00216     if self.mode == self.PLANNING:
00217       JoyPose6D.joyCB(self, status, history)
00218       if history.new(status, "circle"):
00219         self.status_lock.acquire()
00220         self.mode = self.EXECUTING
00221         if self.lock_furutaractive:
00222           self.lockFurutaractive()
00223         self.executePlan()
00224         self.ignore_next_status_flag = True
00225         self.status_lock.release()
00226       elif history.new(status, "cross"):
00227         self.resetGoalPose()
00228       elif history.new(status, "triangle"):
00229         self.lookAround()
00230     elif self.mode == self.CANCELED:
00231       # show menu
00232       if history.new(status, "circle"):
00233         # choosing
00234         self.procCancelMenu(self.current_selecting_index)
00235       else:
00236         if history.new(status, "up"):
00237           self.current_selecting_index = self.current_selecting_index - 1
00238         elif history.new(status, "down"):
00239           self.current_selecting_index = self.current_selecting_index + 1
00240           # menu is only cancel/ignore
00241         if self.current_selecting_index < 0:
00242           self.current_selecting_index = len(self.CANCELED_MENUS) - 1
00243         elif self.current_selecting_index > len(self.CANCELED_MENUS) - 1:
00244           self.current_selecting_index = 0
00245         self.publishMenu()
00246     elif self.mode == self.EXECUTING:
00247       # if history.new(status, "triangle"):
00248       #   self.command_pub.publish(UInt8(1))
00249       if history.new(status, "cross"):
00250         self.status_lock.acquire()
00251         self.exec_client.cancel_all_goals()
00252         self.mode = self.WAIT_FOR_CANCEL
00253         self.status_lock.release()
00254     self.prev_mode = self.mode