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 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 __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 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 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):
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._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