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 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 __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()
