def update_overlay_menu(self): menu = OverlayMenu() menu.title = "HaroSystemMode" menu.menus = ["Sleep", "Searching", "MovingInCircles","Waiting"] menu.current_index = self.counter % len(menu.menus) return menu
def publishMenu(self, index, close=False): menu = OverlayMenu() menu.menus = [p.name for p in self.plugin_instances] menu.current_index = index menu.title = "Joystick" if close: menu.action = OverlayMenu.ACTION_CLOSE self.menu_pub.publish(menu)
def publishMenu(self, close=False): menu = OverlayMenu() menu.menus = self.CANCELED_MENUS menu.current_index = self.current_selecting_index menu.title = "Canceled" if close: menu.action = OverlayMenu.ACTION_CLOSE self.menu_pub.publish(menu)
def publishMenu(self, index, close=False): menu = OverlayMenu() menu.menus = self.menus menu.current_index = index menu.title = "JSK teleop menu" if close: menu.action = OverlayMenu.ACTION_CLOSE self.menu_pub.publish(menu)
def publishMenu(self, close=False): menu = OverlayMenu() menu.menus = [m[0] for m in self.menus] menu.current_index = self.current_index menu.title = self.name if close: menu.action = OverlayMenu.ACTION_CLOSE self.menu_pub.publish(menu)
def update_overlay_menu_haro_tf(self): menu = OverlayMenu() menu.title = "HaroDistanceFromPerson" menu.menus = ["FarAway", "CloseBy", "Target", "OtherWayRound"] fraction = 10.0 if self.piechart_value < (math.pi/fraction): if self.plot_value >= self.max_distance_from_object: index = 0 elif self.plot_value >= (self.max_distance_from_object/ fraction) and self.plot_value < self.max_distance_from_object: index = 1 elif self.plot_value < (self.max_distance_from_object/fraction): index = 2 else: index = 3 menu.current_index = index return menu
def __init__(self): self.recipe = ["No Recipe"] '''self.recipe = [ "1. chop the spam", # "put the potato", "2. turn on the induction", "3. put the oil", "4. chop the onion", "5. chop the carrot", "6. fry for 30 sec", # "7. fry for 10 sec", "7. put the rice", "8. put the salt", "9. fry for 30 sec", "10. turn off the induction" ]''' self.p = rospy.Publisher("recipe", OverlayMenu, queue_size=1) # self.r = rospy.Rate(10) # self.current_index = 0 self.menu = OverlayMenu() self.menu.title = "Recipe" # menu.menus = ["John Lennon", "Paul McCartney", "George Harrison", # "Ringo Starr"] self.menu.menus = self.recipe self.menu.current_index = 0 #(counter/10) % len(menu.menus) # if counter % 100 == 0: # menu.action = OverlayMenu.ACTION_CLOSE self.menu.fg_color.r = 1.0 self.menu.fg_color.g = 1.0 self.menu.fg_color.b = 1.0 self.menu.fg_color.a = 1.0 self.menu.bg_color.r = 0.0 self.menu.bg_color.g = 0.0 self.menu.bg_color.b = 0.0 self.menu.bg_color.a = 1.0 self.p.publish(self.menu)
def joyCB(self, status, history): now = rospy.Time.from_sec(time.time()) latest = history.latest() if not latest: return # menu mode if self.menu != None: if status.up and not latest.up: self.menu.current_index = (self.menu.current_index - 1) % len(self.menu.menus) self.pub.publish(self.menu) elif status.down and not latest.down: self.menu.current_index = (self.menu.current_index + 1) % len(self.menu.menus) self.pub.publish(self.menu) elif status.circle and not latest.circle: self.menu.action = OverlayMenu.ACTION_CLOSE if self.menu.current_index == self.menu.menus.index("Yes"): try: self.execute_footstep_srv() except rospy.ServiceException as e: rospy.logwarn("Execute Footsteps failed: %s", e) self.pub.publish(self.menu) self.menu = None elif status.cross and not latest.cross: self.menu.action = OverlayMenu.ACTION_CLOSE self.pub.publish(self.menu) self.menu = None else: self.pub.publish(self.menu) return # control mode JoyPose6D.joyCB(self, status, history) if status.circle and not latest.circle: # go into execute footsteps menu self.menu = OverlayMenu() self.menu.title = "Execute Footsteps?" self.menu.menus = ["Yes", "No"] self.menu.current_index = 1 self.pub.publish(self.menu) elif status.cross and not latest.cross: # reset marker self.reset_marker_srv() marker_pose = self.getCurrentMarkerPose("initial_footstep_marker") if marker_pose != None: self.pre_pose = marker_pose else: self.pre_pose = PoseStamped() self.pre_pose.pose.orientation.w = 1 elif status.triangle and not latest.triangle: #req = SetPoseRequest(self.pre_pose) res = self.get_stack_marker_pose_srv(self.pre_pose, []) elif status.start and not latest.start: # toggle footstep_marker mode self.toggle_footstep_marker_mode_srv() # synchronize marker_pose, which may be modified by interactive marker if self.current_goal_pose == None or not isSamePose(self.current_goal_pose.pose, self.pre_pose.pose): if self.pose_updated == False: marker_pose = self.getCurrentMarkerPose("movable_footstep_marker") # synchronize marker_pose only at first of pose update if marker_pose != None and not isSamePose(self.pre_pose.pose, marker_pose.pose): self.pre_pose = marker_pose self.pose_updated = True if (now - self.prev_time).to_sec() > 1 / 30.0: self.pose_pub.publish(self.pre_pose) self.prev_time = now self.current_goal_pose = copy.deepcopy(self.pre_pose) else: self.pose_updated = False
class JoyFootstepMarker(JoyPose6D): ''' Usage: Refer JoyPose6D circle: execute footstep cross: reset marker triangle: stack footstep (only used in stack mode) start: toggle footstep marker mode (single -> continuous -> stack) Args: use_tf [Boolean, default: True]: update marker pose using tf publish_pose : forced False frame_id [String, default: map]: frame_id of publishing pose, this is overwritten by parameter, ~frame_id pose [String, default: pose]: topic name for publishing pose set_pose [String, default: set_pose]: topic name for setting pose by topic ''' def __init__(self, name, args): args['publish_pose'] = False # supress publishing pose of joy_pose_6d JoyPose6D.__init__(self, name, args) self.pose_pub = rospy.Publisher(self.getArg('pose', 'pose'), PoseStamped) self.current_goal_pose = None self.use_tf = self.getArg('use_tf', True) self.pose_updated = False # make publisher self.pub = rospy.Publisher("joy_footstep_menu", OverlayMenu) self.menu = None # make service proxy rospy.wait_for_service('/footstep_marker/reset_marker') self.reset_marker_srv = rospy.ServiceProxy( '/footstep_marker/reset_marker', Empty) rospy.wait_for_service('/footstep_marker/toggle_footstep_marker_mode') self.toggle_footstep_marker_mode_srv = rospy.ServiceProxy( '/footstep_marker/toggle_footstep_marker_mode', Empty) rospy.wait_for_service('/footstep_marker/execute_footstep') self.execute_footstep_srv = rospy.ServiceProxy( '/footstep_marker/execute_footstep', Empty) rospy.wait_for_service('/footstep_marker/get_footstep_marker_pose') self.get_footstep_marker_pose_srv = rospy.ServiceProxy( '/footstep_marker/get_footstep_marker_pose', GetTransformableMarkerPose) rospy.wait_for_service('/footstep_marker/stack_marker_pose') self.get_stack_marker_pose_srv = rospy.ServiceProxy( '/footstep_marker/stack_marker_pose', SetPose) # initialize maker pose marker_pose = self.getCurrentMarkerPose("movable_footstep_marker") if marker_pose != None: self.pre_pose = marker_pose def joyCB(self, status, history): now = rospy.Time.from_sec(time.time()) latest = history.latest() if not latest: return # menu mode if self.menu != None: if status.up and not latest.up: self.menu.current_index = (self.menu.current_index - 1) % len( self.menu.menus) self.pub.publish(self.menu) elif status.down and not latest.down: self.menu.current_index = (self.menu.current_index + 1) % len( self.menu.menus) self.pub.publish(self.menu) elif status.circle and not latest.circle: self.menu.action = OverlayMenu.ACTION_CLOSE if self.menu.current_index == self.menu.menus.index("Yes"): try: self.execute_footstep_srv() except rospy.ServiceException, e: rospy.logwarn("Execute Footsteps failed: %s", e) self.pub.publish(self.menu) self.menu = None elif status.cross and not latest.cross: self.menu.action = OverlayMenu.ACTION_CLOSE self.pub.publish(self.menu) self.menu = None else: self.pub.publish(self.menu) return # control mode JoyPose6D.joyCB(self, status, history) if status.circle and not latest.circle: # go into execute footsteps menu self.menu = OverlayMenu() self.menu.title = "Execute Footsteps?" self.menu.menus = ["Yes", "No"] self.menu.current_index = 1 self.pub.publish(self.menu) elif status.cross and not latest.cross: # reset marker self.reset_marker_srv() marker_pose = self.getCurrentMarkerPose("initial_footstep_marker") if marker_pose != None: self.pre_pose = marker_pose else: self.pre_pose = PoseStamped() self.pre_pose.pose.orientation.w = 1 elif status.triangle and not latest.triangle: #req = SetPoseRequest(self.pre_pose) res = self.get_stack_marker_pose_srv(self.pre_pose, []) elif status.start and not latest.start: # toggle footstep_marker mode self.toggle_footstep_marker_mode_srv() # synchronize marker_pose, which may be modified by interactive marker if self.current_goal_pose == None or not isSamePose( self.current_goal_pose.pose, self.pre_pose.pose): if self.pose_updated == False: marker_pose = self.getCurrentMarkerPose( "movable_footstep_marker" ) # synchronize marker_pose only at first of pose update if marker_pose != None and not isSamePose( self.pre_pose.pose, marker_pose.pose): self.pre_pose = marker_pose self.pose_updated = True if (now - self.prev_time).to_sec() > 1 / 30.0: self.pose_pub.publish(self.pre_pose) self.prev_time = now self.current_goal_pose = copy.deepcopy(self.pre_pose) else: self.pose_updated = False
#!/usr/bin/env python import rospy from jsk_rviz_plugins.msg import OverlayMenu rospy.init_node("test_menu") p = rospy.Publisher("test_menu", OverlayMenu) r = rospy.Rate(5) counter = 0 while not rospy.is_shutdown(): menu = OverlayMenu() menu.title = "The Beatles" menu.menus = [ "John Lennon", "Paul McCartney", "George Harrison", "Ringo Starr" ] menu.current_index = counter % len(menu.menus) if counter % 100 == 0: menu.action = OverlayMenu.ACTION_CLOSE p.publish(menu) counter = counter + 1 r.sleep()
#!/usr/bin/env python import rospy from jsk_rviz_plugins.msg import OverlayMenu rospy.init_node("test_menu") p = rospy.Publisher("test_menu", OverlayMenu) r = rospy.Rate(5) counter = 0 while not rospy.is_shutdown(): menu = OverlayMenu() menu.title = "The Beatles" menu.menus = ["John Lennon", "Paul McCartney", "George Harrison", "Ringo Starr"] menu.current_index = counter % len(menu.menus) if counter % 100 == 0: menu.action = OverlayMenu.ACTION_CLOSE p.publish(menu) counter = counter + 1 r.sleep()