def make_sphere_control(name, scale):
control = ims.InteractiveMarkerControl()
control.name = name + '_sphere'
control.always_visible = True
control.markers.append(make_rviz_marker(scale))
control.interaction_mode = ims.InteractiveMarkerControl.BUTTON
return control
def __init__(self, tagid, pose_in_map_frame, action_marker_manager, server_lock, marker_server, scale=.2):
self.tagid = tagid
self.action_marker_manager = action_marker_manager
self.server_lock = server_lock
self.marker_server = marker_server
self.marker_name = 'ar_marker_' + str(self.tagid)
int_marker = interactive_marker(self.marker_name, pose_in_map_frame, scale)
int_marker.scale = .6
self.marker_obj = int_marker
int_marker.description = 'Tag #%d' % self.tagid
#int_marker.controls += []
#int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0,1,0,.5)
sph = make_sphere_control(self.marker_name, scale)
sph.markers[0].color = stdm.ColorRGBA(0,1,0,.5)
self.menu = mh.MenuHandler()
self.menu.insert('Create New Action', parent=None, callback=self.create_new_action_cb)
menu_control = ims.InteractiveMarkerControl()
menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU
menu_control.description=""
menu_control.name = 'menu_tag_' + str(self.tagid)
menu_control.markers.append(copy.deepcopy(sph.markers[0]))
menu_control.always_visible = True
int_marker.controls += [menu_control]
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.menu.apply(self.marker_server, int_marker.name)
self.server_lock.release()
def make_control_marker(orientation=[0,0,0,1.]):
control = ims.InteractiveMarkerControl()
control.orientation.x = orientation[0]
control.orientation.y = orientation[1]
control.orientation.z = orientation[2]
control.orientation.w = orientation[3]
control.interaction_mode = ims.InteractiveMarkerControl.MOVE_AXIS
return control
def make_control_marker():
control = ims.InteractiveMarkerControl()
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 0
control.orientation.w = 1
control.interaction_mode = ims.InteractiveMarkerControl.MOVE_AXIS
return control
def _AddWaypointCallback(self, pose):
name = 'waypoint_%d' % self._current_waypoint_index
description = 'waypoint %d' % self._current_waypoint_index
self._current_waypoint_index += 1
self._AddWaypoint(name, pose)
waypoint_int_marker = interactive_marker_server.InteractiveMarker()
waypoint_int_marker.header.frame_id = pose.header.frame_id
waypoint_int_marker.name = name
waypoint_int_marker.description = description
waypoint_int_marker.pose = copy.deepcopy(pose.pose)
waypoint_int_marker.pose.position.z += 0.01
waypoint_marker = interactive_marker_server.Marker()
waypoint_marker.type = interactive_marker_server.Marker.ARROW
waypoint_marker.scale.x = 0.8
waypoint_marker.scale.y = 0.8
waypoint_marker.scale.z = 0.8
waypoint_marker.color.r = 0.0
waypoint_marker.color.g = 0.8
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_control = interactive_marker_server.InteractiveMarkerControl()
waypoint_control.always_visible = True
waypoint_control.markers.append(waypoint_marker)
waypoint_int_marker.controls.append(waypoint_control)
translate_control = interactive_marker_server.InteractiveMarkerControl(
)
translate_control.name = 'move_rotate_plane'
translate_control.interaction_mode = interactive_marker_server.InteractiveMarkerControl.MOVE_ROTATE
translate_control.orientation.x = 0
translate_control.orientation.y = 1
translate_control.orientation.z = 0
translate_control.orientation.w = 1
translate_control.always_visible = True
waypoint_int_marker.controls.append(translate_control)
self.server.insert(waypoint_int_marker, self._MarkerFeedbackCallback)
self.menu_handler.apply(self.server, name)
self.server.applyChanges()
def _make_menu(self):
menu_control = ims.InteractiveMarkerControl()
menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU
menu_control.description = ""
menu_control.name = self.tagid + '_menu'
menu_control.markers.append(
copy.deepcopy(self.target_marker.controls[0].markers[0]))
menu_control.always_visible = True
self.target_marker.controls.append(copy.deepcopy(menu_control))
self.server.insert(self.target_marker, self.process_feedback)
self.menu.apply(self.server, self.target_marker.name)
def _make_menu(self):
menu_control = ims.InteractiveMarkerControl()
menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU
menu_control.description=""
menu_control.name = 'menu_' + self.actionid
menu_control.markers.append(copy.deepcopy(self.marker_obj.controls[0].markers[0]))
menu_control.always_visible = True
self.marker_obj.controls.append(copy.deepcopy(menu_control))
self.server_lock.acquire()
self.marker_server.insert(self.marker_obj, self.marker_cb)
self.menu_handler.apply(self.marker_server, self.marker_obj.name)
self.server_lock.release()
def create_head_menu(self):
if self.head_menu_marker != None:
self.marker_server_lock.acquire()
self.marker_server.erase('behavior_server_refresh')
self.marker_server_lock.release()
int_marker = interactive_marker('behavior_server_refresh', HEAD_MARKER_LOC, .2)
int_marker.header.frame_id = 'torso_lift_link'
int_marker.description = ''
int_marker.controls.append(make_sphere_control(int_marker.name, .2))
menu_handler = mh.MenuHandler()
menu_handler.insert('Rescan Behaviors', parent=None, callback=self.rescan_behaviors_cb)
menu_handler.insert('-----------------', parent=None, callback=None)
#Insert calls to learnable behaviors
behavior_folder = find_folder(self.actions_tree, 'Behaviors')
if behavior_folder != None:
learnable_folder = copy.deepcopy(find_folder(behavior_folder, 'Learnable'))
if learnable_folder != None:
tree = {'path':'root', 'actions':[learnable_folder]}
menu_handler_from_action_tree(tree, self.learnable_behavior_menu_cb, menu_handler)
menu_control = ims.InteractiveMarkerControl()
menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU
menu_control.name = 'menu_rescan'
menu_control.markers.append(copy.deepcopy(int_marker.controls[0].markers[0]))
menu_control.always_visible = True
int_marker.controls.append(menu_control)
self.marker_server_lock.acquire()
self.marker_server.insert(int_marker, None)
menu_handler.apply(self.marker_server, int_marker.name)
self.marker_server.applyChanges()
self.marker_server_lock.release()
self.head_menu_marker = int_marker