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