jsk_teleop_joy: environment_plane_modeling

Go to the documentation of this file.
00001 from jsk_teleop_joy.joy_plugin import JSKJoyPlugin
00002 from jsk_rviz_plugins.msg import OverlayMenu
00003 from std_srvs.srv import Empty
00004 
00005 import rospy
00006 
00007 class EnvironmentPlaneModelingClient(JSKJoyPlugin):
00008     def __init__(self, name, args):
00009         JSKJoyPlugin.__init__(self, name, args)
00010         self.menu_pub = rospy.Publisher("/overlay_menu", OverlayMenu)
00011         self.menus = [
00012             ["Append Pre-defined Polygon", self.appendPreDefinedPolygon],
00013             ["Register Current GridMap", self.registerCurrentGridMap],
00014             ["Clear Environment", self.clearEnvironment],
00015             ["Exit", self.exit]]
00016         self.clear_maps_srv = rospy.ServiceProxy(
00017             self.getArg("clear_maps", "/env_server/clear_maps"), Empty)
00018         self.register_current_map_srv = rospy.ServiceProxy(
00019             self.getArg("register_current_map", "/env_server/register_to_hisotry"), Empty)
00020         self.register_completed_map_srv = rospy.ServiceProxy(
00021             self.getArg("register_completed_map", "/env_server/register_completion_to_hisotry"), Empty)
00022         self.current_index = 0
00023     def joyCB(self, status, history):
00024         # self.pub.publish(status.orig_msg)
00025         if history.new(status, 'up'):
00026             self.upCursor()
00027         elif history.new(status, 'down'):
00028             self.downCursor()
00029         #publish menu anyway
00030         if history.new(status, 'cross'):
00031             self.publishMenu(close=True)
00032             rospy.sleep(0.2)
00033             self.manager.forceToPluginMenu(publish_menu=False)
00034         elif history.new(status, 'circle'):
00035             # call the function
00036             func = self.menus[self.current_index][1]
00037             func()
00038         else:
00039             self.publishMenu() 
00040     def downCursor(self):
00041         if self.current_index == len(self.menus) - 1:
00042             self.current_index = 0
00043         else:
00044             self.current_index = self.current_index + 1
00045     def upCursor(self):
00046         if self.current_index == 0:
00047             self.current_index = len(self.menus) - 1
00048         else:
00049             self.current_index = self.current_index - 1
00050     def exit(self):                       #same to cross button
00051         self.publishMenu(close=True)
00052         rospy.sleep(0.2)
00053         self.manager.forceToPluginMenu(publish_menu=False)
00054     def registerCurrentGridMap(self):
00055         try:
00056             self.register_current_map_srv()
00057         except rospy.ServiceException, e:
00058             rospy.logfatal("failed to register current map: %s" % (e))
00059     def appendPreDefinedPolygon(self):
00060         try:
00061             self.register_completed_map_srv()
00062         except rospy.ServiceException, e:
00063             rospy.logfatal("failed to register completed current map: %s" % (e))
00064     def clearEnvironment(self):
00065         try:
00066             self.clear_maps_srv()
00067         except rospy.ServiceException, e:
00068             rospy.logfatal("failed to clear_map: %s" % (e))
00069     def publishMenu(self, close=False):
00070         menu = OverlayMenu()
00071         menu.menus = [m[0] for m in self.menus]
00072         menu.current_index = self.current_index
00073         menu.title = self.name
00074         if close:
00075             menu.action = OverlayMenu.ACTION_CLOSE
00076         self.menu_pub.publish(menu)
00077