def InsertMenue(self, ID, parent_id, title): menue = MenuEntry() menue.id = ID menue.parent_id = parent_id menue.title = title menue.command_type = MenuEntry.FEEDBACK return menue
def start(self): # Get base interactive marker ps = PoseStamped() ps.header.frame_id = 'base_link' ps.pose.position.x = 0.5 ps.pose.position.y = 0 ps.pose.position.z = 0.5 ps.pose.orientation.w = 1 gripper_im = fetch_api.gripper_viz.gripper_interactive_marker(ps, 0.1) gripper_im.name = 'gripper' # Add menu gripper_im.controls[0].interaction_mode = InteractiveMarkerControl.MENU menu_entry = MenuEntry() menu_entry.command_type = MenuEntry.FEEDBACK menu_entry.id = 1 menu_entry.title = 'Move gripper here' gripper_im.menu_entries.append(copy.deepcopy(menu_entry)) menu_entry.id = 2 menu_entry.title = 'Open gripper' gripper_im.menu_entries.append(copy.deepcopy(menu_entry)) menu_entry.id = 3 menu_entry.title = 'Close gripper' gripper_im.menu_entries.append(copy.deepcopy(menu_entry)) # Add 6 DOF controls gripper_im.controls.extend(make_6dof_controls()) self._im_server.insert(gripper_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() self._check_ik(ps)
def makeEntry(self, context, handle, parent_handle): menu_entry = MenuEntry() if context.check_state == self.NO_CHECKBOX: menu_entry.title = context.title elif context.check_state == self.CHECKED: menu_entry.title = '[x] ' + context.title elif context.check_state == self.UNCHECKED: menu_entry.title = '[ ] ' + context.title menu_entry.command = context.command menu_entry.command_type = context.command_type menu_entry.id = handle menu_entry.parent_id = parent_handle return menu_entry
def MakeMenuEntry(self, Menu_Context, ID, parent_ID): Menu_Entry = MenuEntry() if Menu_Context['CheckState'] == self.NO_CHECKBOX: Menu_Entry.title = Menu_Context['Title'] elif Menu_Context['CheckState'] == self.CHECKED: Menu_Entry.title = '[x] ' + Menu_Context['Title'] elif Menu_Context['CheckState'] == self.UNCHECKED: Menu_Entry.title = '[ ] ' + Menu_Context['Title'] else: rospy.loginfo('unexpect CheckState input') Menu_Entry.command = Menu_Context['Command'] Menu_Entry.command_type = Menu_Context['CommandType'] Menu_Entry.id = ID Menu_Entry.parent_id = parent_ID return Menu_Entry
def start(self): obj_marker = Marker() obj_marker.type = Marker.CUBE obj_marker.pose.orientation.w = 1 obj_marker.scale.x = 0.05 obj_marker.scale.y = 0.05 obj_marker.scale.z = 0.05 obj_marker.color.r = 1 obj_marker.color.g = 1 obj_marker.color.a = 0.7 obj_control = InteractiveMarkerControl() obj_control.interaction_mode = InteractiveMarkerControl.MENU obj_control.always_visible = True obj_control.markers.append(obj_marker) menu_entry1 = MenuEntry() menu_entry1.id = 1 menu_entry1.title = 'Pick from front' menu_entry1.command_type = MenuEntry.FEEDBACK menu_entry2 = MenuEntry() menu_entry2.id = 2 menu_entry2.title = 'Open gripper' menu_entry2.command_type = MenuEntry.FEEDBACK obj_im = InteractiveMarker() obj_im.name = 'object' obj_im.header.frame_id = 'base_link' obj_im.pose.position.x = 0.7 obj_im.pose.position.z = 0.5 obj_im.pose.orientation.w = 1 obj_im.scale = 0.25 obj_im.menu_entries.append(menu_entry1) obj_im.menu_entries.append(menu_entry2) obj_im.controls.append(obj_control) obj_im.controls.extend(make_6dof_controls()) self._im_server.insert(obj_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() self._update_grippers()
def __init__(self, arm, gripper, im_server): self.arm = arm self.gripper = gripper self.currentPose = None self.currentPoseFeasible = False self._im_server = im_server gripper_im = InteractiveMarker() gripper_im.name = "da grip" #gripper_im.header.frame_id = "wrist_roll_link" gripper_im.header.frame_id = "base_link" gripper_im.scale = .5 gripper_im.pose.orientation.w = 1.0 markers = init_markers(0.166, 0, 0) # create gripper mesh # Adds the gripper markers to the interactive markers marker_control = InteractiveMarkerControl() for marker in markers: marker_control.markers.append(marker) marker_control.always_visible = True marker_control.interaction_mode = InteractiveMarkerControl.MENU gripper_im.controls.append(marker_control) # Add the 6 degree of freedom controls add_6dof_controls(gripper_im) # Add the menu commands entry1 = MenuEntry() entry1.id = 1 entry1.title = 'open' entry1.command_type = entry1.FEEDBACK gripper_im.menu_entries.append(entry1) entry2 = MenuEntry() entry2.id = 2 entry2.title = 'close' entry2.command_type = entry2.FEEDBACK gripper_im.menu_entries.append(entry2) entry3 = MenuEntry() entry3.id = 3 entry3.title = 'move_gripper' entry3.command_type = entry3.FEEDBACK gripper_im.menu_entries.append(entry3) self._im_server.insert(gripper_im, feedback_cb = self.handle_feedback) self._im_server.applyChanges()
def __init__(self, arm, gripper, im_server): self._im_server = im_server self.arm = arm self.gripper = gripper self.obj_im = InteractiveMarker() self.obj_im.name = "da object" self.obj_im.header.frame_id = "base_link" self.obj_im.scale = 0.5 self.gripper1_shift = [-0.03, 0, 0] self.gripper2_shift = [-0.15, 0, 0] self.gripper3_shift = [-0.1, 0, 0.3] # prepare parameters for moving the robot later self.currentPoseFeasible = False self.currentGripperPose1 = None self.currentGripperPose2 = None self.currentGripperPose3 = None marker_control = InteractiveMarkerControl() box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.05 box_marker.scale.y = 0.05 box_marker.scale.z = 0.05 # box_marker.pose.position.x += xOffset # box_marker.pose.position.y += yOffset # box_marker.pose.position.z += zOffset box_marker.color.r = 1 box_marker.color.g = 1.0 box_marker.color.b = 0.0 box_marker.color.a = 0.5 gripper1 = init_markers(self.gripper1_shift[0], self.gripper1_shift[1], self.gripper1_shift[2]) gripper2 = init_markers(self.gripper2_shift[0], self.gripper2_shift[1], self.gripper2_shift[2]) gripper3 = init_markers(self.gripper3_shift[0], self.gripper3_shift[1], self.gripper3_shift[2]) marker_control.markers.append(box_marker) for marker in gripper1: marker_control.markers.append(marker) for marker in gripper2: marker_control.markers.append(marker) for marker in gripper3: marker_control.markers.append(marker) marker_control.always_visible = True marker_control.interaction_mode = InteractiveMarkerControl.MENU # Add the menu commands entry1 = MenuEntry() entry1.id = 1 entry1.title = 'open' entry1.command_type = entry1.FEEDBACK self.obj_im.menu_entries.append(entry1) entry2 = MenuEntry() entry2.id = 2 entry2.title = 'close' entry2.command_type = entry2.FEEDBACK self.obj_im.menu_entries.append(entry2) entry3 = MenuEntry() entry3.id = 3 entry3.title = 'move_gripper' entry3.command_type = entry3.FEEDBACK self.obj_im.menu_entries.append(entry3) print "Number of markers:", len(marker_control.markers) self.obj_im.controls.append(marker_control) add_6dof_controls(self.obj_im) self._im_server.insert(self.obj_im, feedback_cb = self.handle_feedback) self._im_server.applyChanges()
def getMarkersFromPose(poseStamped, preGraspStatus): int_marker = InteractiveMarker() int_marker.name = 'gripper' int_marker.header.frame_id = poseStamped.header.frame_id int_marker.pose = poseStamped.pose int_control = InteractiveMarkerControl() int_control.always_visible = True int_control.orientation.w = 1 int_control.orientation.x = 0 int_control.orientation.y = 1 int_control.orientation.z = 0 # ADD GRIPPER CONTROLS gripper_markers = createMarker(0, 0) int_control.interaction_mode = InteractiveMarkerControl.MENU int_control.markers.extend(gripper_markers) int_marker.scale = 0.35 # ADD AUTO PICKING if preGraspStatus: #pre-grasp int_control.markers.extend(createMarker(OFFSET_X, 0)) #lift-grasp int_control.markers.extend(createMarker(0, OFFSET_Z)) #sample-box box_marker = Marker() box_marker.type = Marker.CUBE box_marker.pose.position.x += GRIPPER_OFFSET box_marker.scale.x = 0.05 box_marker.scale.y = 0.05 box_marker.scale.z = 0.05 box_marker.color.r = 1 box_marker.color.g = 1.0 box_marker.color.b = 0.0 box_marker.color.a = 0.5 int_control.markers.append(box_marker) int_marker.controls.append(int_control) # ADD DOF dof_controls = make_6dof_controls() int_marker.controls.extend(dof_controls) # ADD MENU ITEMS menu_entries = [] open_gripper = MenuEntry() open_gripper.id = GRIPPER_OPEN open_gripper.command_type = MenuEntry.FEEDBACK open_gripper.title = 'OPEN GRIPPER' menu_entries.append(open_gripper) close_gripper = MenuEntry() close_gripper.id = GRIPPER_CLOSE close_gripper.command_type = MenuEntry.FEEDBACK close_gripper.title = 'CLOSE GRIPPER' menu_entries.append(close_gripper) move_gripper = MenuEntry() move_gripper.id = GRIPPER_MOVETO move_gripper.command_type = MenuEntry.FEEDBACK if preGraspStatus: move_gripper.title = 'Commence AutoPick' else: move_gripper.title = 'MOVE TO THIS POSE' menu_entries.append(move_gripper) int_marker.menu_entries.extend(menu_entries) return int_marker
def menu_entry(id, title): menu_entry = MenuEntry() menu_entry.command_type = MenuEntry.FEEDBACK menu_entry.id = id menu_entry.title = title return menu_entry