def _make_6dof_control(self, name, orientation, is_move, is_fixed): '''Creates and returns one component of the 6dof controller. Args: name (str): Name for hte control orientation (Quaternion): How the control should be oriented. is_move (bool): Looks like whether the marker moves the object (?). Currently passed as True for moving markers, False for rotating ones. is_fixed (bool): Looks like whether position is fixed (?). Currently always passed as True. Returns: InteractiveMarkerControl ''' control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if self.is_control_visible: if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
def makeMarker(self, callback=None, marker=None, pose=[0, 0, 0], controls=[], fixed=False, name=None, frame="map", description="", imode=0, rot=[0, 0, 0, 1]): if marker is None: marker = self.mg.marker() if callback is None: callback = default_callback if name is None: name = "control%d" % self.c self.c += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.pose.position.x = pose[0] int_marker.pose.position.y = pose[1] int_marker.pose.position.z = pose[2] int_marker.pose.orientation.x = rot[0] int_marker.pose.orientation.y = rot[1] int_marker.pose.orientation.z = rot[2] int_marker.pose.orientation.w = rot[3] int_marker.scale = 1 int_marker.name = name int_marker.description = description control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = imode control.markers.append(marker) int_marker.controls.append(control) for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] / SQRT2 control.orientation.x = data[1] / SQRT2 control.orientation.y = data[2] / SQRT2 control.orientation.z = data[3] / SQRT2 control.name = control_name control.interaction_mode = data[4] if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker self.server.applyChanges()
def make_rotate_axis_fixed(axis_name, orientation): controlR = InteractiveMarkerControl() controlR.orientation.w = orientation[0] controlR.orientation.x = orientation[1] controlR.orientation.y = orientation[2] controlR.orientation.z = orientation[3] controlR.name = "rotate_" + axis_name controlR.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed_orientation: controlR.orientation_mode = InteractiveMarkerControl.FIXED controlM = InteractiveMarkerControl() controlM.orientation.w = orientation[0] controlM.orientation.x = orientation[1] controlM.orientation.y = orientation[2] controlM.orientation.z = orientation[3] controlM.name = "move_" + axis_name controlM.interaction_mode = InteractiveMarkerControl.MOVE_AXIS controlM.orientation_mode = InteractiveMarkerControl.FIXED return (controlR, controlM)
def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
def make6DOFGimbal(intMarker): """Creates 6DOF gimbal marker controls.""" pitch = InteractiveMarkerControlMsg() pitch.orientation.x = 1 pitch.orientation.y = 0 pitch.orientation.z = 0 pitch.orientation.w = 1 pitch.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS pitch.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(pitch) yaw = InteractiveMarkerControlMsg() yaw.orientation.x = 0 yaw.orientation.y = 1 yaw.orientation.z = 0 yaw.orientation.w = 1 yaw.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS yaw.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(yaw) roll = InteractiveMarkerControlMsg() roll.orientation.x = 0 roll.orientation.y = 0 roll.orientation.z = 1 roll.orientation.w = 1 roll.interaction_mode = InteractiveMarkerControlMsg.ROTATE_AXIS roll.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(roll) transX = InteractiveMarkerControlMsg() transX.orientation.x = 1 transX.orientation.y = 0 transX.orientation.z = 0 transX.orientation.w = 1 transX.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS transX.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(transX) transY = InteractiveMarkerControlMsg() transY.orientation.x = 0 transY.orientation.y = 1 transY.orientation.z = 0 transY.orientation.w = 1 transY.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS transY.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(transY) transZ = InteractiveMarkerControlMsg() transZ.orientation.x = 0 transZ.orientation.y = 0 transZ.orientation.z = 1 transZ.orientation.w = 1 transZ.interaction_mode = InteractiveMarkerControlMsg.MOVE_AXIS transZ.orientation_mode = InteractiveMarkerControlMsg.FIXED intMarker.controls.append(transZ)
def _create_marker(self, name): self.name_markers[name] = InteractiveMarker() self.name_markers[name].header.frame_id = "map" self.name_markers[name].name = name self.name_markers[name].description = name self.name_markers[ name].pose.position.x = self.pose.pose.pose.position.x self.name_markers[ name].pose.position.y = self.pose.pose.pose.position.y # TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL self.name_markers[name].pose.position.z = 1 box_marker = create_box_marker() self.name_controls[name] = InteractiveMarkerControl() self.name_controls[ name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE self.name_controls[name].always_visible = True self.name_controls[name].orientation.w = 1 self.name_controls[name].orientation.x = 0 self.name_controls[name].orientation.y = 1 self.name_controls[name].orientation.z = 0 new_controller_lol = InteractiveMarkerControl() new_controller_lol.orientation.w = 1 new_controller_lol.orientation.x = 0 new_controller_lol.orientation.y = 1 new_controller_lol.orientation.z = 0 new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED self.name_markers[name].controls.append(new_controller_lol) # self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w # self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x # self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y # self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z self.name_controls[name].markers.append(box_marker) self.name_markers[name].controls.append(self.name_controls[name]) self.server.insert(self.name_markers[name], self._callback) self.server.applyChanges()
def add_marker(self, init_position, callback, controls=[], scale=1., marker=None, name=None, fixed_orientation=False): if name is None: name = 'control%d' % (self.marker_ctr) self.marker_ctr += 1 if marker is None: marker = make_markers.make_marker() marker.header.frame_id = self.frame_id marker.header.frame_id = self.frame_id int_marker = InteractiveMarker() int_marker.header.frame_id = self.frame_id int_marker.pose.position = init_position int_marker.scale = scale int_marker.name = name vm_control = InteractiveMarkerControl() vm_control.always_visible = True vm_control.markers.append(marker) int_marker.controls.append(vm_control) #int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D for control_name in controls: data = TYPEDATA[control_name] control = InteractiveMarkerControl() control.orientation.w = data[0] control.orientation.x = data[1] control.orientation.y = data[2] control.orientation.z = data[3] control.name = control_name control.interaction_mode = data[4] if fixed_orientation: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.server.insert(int_marker, callback) self.markers[name] = int_marker
def make_6dof_gripper(fixed, ps, scale, color, robot_type = "pr2", ignore_rotation = False, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'gripper_6dof' control = InteractiveMarkerControl() control.always_visible = True control.name = 'pr2_gripper_control' if robot_type == "pr2": control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control) int_marker.description = 'pr2_gripper_control' elif robot_type == "cody": control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'cody_ee_control' elif robot_type == "darci": control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'darci_ee_control' int_marker.controls.append( control ) if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def make6DofMarker( fixed = False ): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.position.x = -0.100988589227 int_marker.pose.position.y = 0.035845387727 int_marker.pose.position.z = 0.266128748655 int_marker.name = "paper_sheet" int_marker.description = "Place the sheet of paper" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.21 box_marker.scale.y = 0.297 box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( a4_sheet() ) # add the control to the interactive marker int_marker.controls.append( box_control ) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback)
def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name)
def make_6dof_gripper(fixed, ps, scale, color, robot_type="pr2", ignore_rotation=False, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'gripper_6dof' control = InteractiveMarkerControl() control.always_visible = True control.name = 'pr2_gripper_control' if robot_type == "pr2": control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control) int_marker.description = 'pr2_gripper_control' elif robot_type == "cody": control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'cody_ee_control' elif robot_type == "darci": control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control) int_marker.description = 'darci_ee_control' int_marker.controls.append(control) if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def make_marker_flexible(fixed, ps, scale, color, mtype, ignore_rotation, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'simple_6dof' int_marker.description = '' # insert a marker control = InteractiveMarkerControl() control.always_visible = True control.markers.append(make_marker(scale, color, mtype)) int_marker.controls.append(control) if fixed: int_marker.name += '_fixed' int_marker.description += '\n(fixed orientation)' if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1, 0, 0, 1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 1, 1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 1, 0, 1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def make6DofMarker(fixed=False): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.position.x = -0.100988589227 int_marker.pose.position.y = 0.035845387727 int_marker.pose.position.z = 0.266128748655 int_marker.name = "paper_sheet" int_marker.description = "Place the sheet of paper" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = 0.21 box_marker.scale.y = 0.297 box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(a4_sheet()) # add the control to the interactive marker int_marker.controls.append(box_control) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback)
def createMoveControls(fixed=False): controls = [] ## rotate control x control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## rotate control y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## rotate control z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control x control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control y control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) ## move control z control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED controls.append(control) return controls
def make6DofMarker( fixed = False ): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.orientation.x = frame_pose.orientation.x int_marker.pose.orientation.y = frame_pose.orientation.y int_marker.pose.orientation.z = frame_pose.orientation.z int_marker.pose.orientation.w = frame_pose.orientation.w int_marker.pose.position.x = frame_pose.position.x int_marker.pose.position.y = frame_pose.position.y int_marker.pose.position.z = frame_pose.position.z int_marker.name = FRAME_ID int_marker.description = "Place the writing surface" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = SURFACE_WIDTH box_marker.scale.y = SURFACE_HEIGHT box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( writing_surface() ) # add the control to the interactive marker int_marker.controls.append( box_control ) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback)
def talker(): rospy.init_node('talker', anonymous=True) # desired_pose_publisher = rospy.Publisher('/equilibrium_pose', Pose, queue_size=10) interactive_marker_init_publisher = rospy.Publisher( '/equilibrium_pose_marker/update_full', InteractiveMarkerInit, queue_size=10) rate = rospy.Rate(30) # Hz seq_num = 1693 # interactive_marker.type = 1 # MAY NEED TO UNCOMMENT IF IT DOESNT WORK # Topic to be published interactive_marker_init = InteractiveMarkerInit() interactive_marker_init.server_id = "/interactive_marker" interactive_marker = InteractiveMarker() interactive_marker.name = "equilibrium_pose" interactive_marker.header.frame_id = "panda_link0" interactive_marker.scale = 0.3 interactive_marker.description = "Equilibrium Pose\nBE CAREFUL! If you move the \nequilibrium pose the robot will follow\ \ it\nso be aware of potential collisions" # Controls - X direction imc = InteractiveMarkerControl() imc.name = "rotate_x" imc.orientation.x = 1.0 imc.orientation.y = 0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_x" imc.orientation.x = 1.0 imc.orientation.y = 0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) # Controls - Y direction imc = InteractiveMarkerControl() imc.name = "rotate_y" imc.orientation.x = 0 imc.orientation.y = 1.0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_y" imc.orientation.x = 0 imc.orientation.y = 1.0 imc.orientation.z = 0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) # Controls - Z direction imc = InteractiveMarkerControl() imc.name = "rotate_z" imc.orientation.x = 0 imc.orientation.y = 0 imc.orientation.z = 1.0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 5 interactive_marker.controls.append(imc) imc = InteractiveMarkerControl() imc.name = "move_z" imc.orientation.x = 0 imc.orientation.y = 0 imc.orientation.z = 1.0 imc.orientation.w = 1.0 imc.orientation_mode = 0 imc.interaction_mode = 3 interactive_marker.controls.append(imc) desired_pose = Pose() desired_pose.position.x = 0.441047421421 desired_pose.position.y = 0.00039597222295 desired_pose.position.z = 0.428872718226 desired_pose.orientation.x = 0.998920857906 desired_pose.orientation.y = -0.0855322959381 desired_pose.orientation.z = 0.0364367915829 desired_pose.orientation.w = -0.00293869199231 interactive_marker.pose = desired_pose interactive_marker_init.markers.append(InteractiveMarker()) positive_y_thresh = 0.20 negative_y_thresh = -0.10 rospy.loginfo("Publishing!!") while desired_pose.position.y < positive_y_thresh: desired_pose.position.y += 0.001 rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)", desired_pose.position.x, desired_pose.position.y, desired_pose.position.z, desired_pose.orientation.x, desired_pose.orientation.y, desired_pose.orientation.z, desired_pose.orientation.w) interactive_marker_init.seq_num = seq_num interactive_marker.pose = desired_pose interactive_marker_init.markers[0] = interactive_marker interactive_marker_init_publisher.publish(interactive_marker_init) seq_num = seq_num + 1 rate.sleep() while desired_pose.position.y > negative_y_thresh: desired_pose.position.y -= 0.001 rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)", desired_pose.position.x, desired_pose.position.y, desired_pose.position.z, desired_pose.orientation.x, desired_pose.orientation.y, desired_pose.orientation.z, desired_pose.orientation.w) interactive_marker_init.seq_num = seq_num interactive_marker.pose = desired_pose interactive_marker_init.markers[0] = interactive_marker interactive_marker_init_publisher.publish(interactive_marker_init) seq_num = seq_num + 1 rate.sleep()
def make6DofMarker(fixed, interaction_mode, position, show_6dof=False): int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link_zero" int_marker.pose.position = position #orientation with Z down (as the gripper) int_marker.pose.orientation.x = 0.707106 int_marker.pose.orientation.y = 0.707106 int_marker.pose.orientation.z = 0 int_marker.pose.orientation.w = 0 int_marker.scale = 0.05 int_marker.name = "simple_3dof" int_marker.description = "Goal for the VSA arm" # insert a box makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: "MOVE_3D", InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] int_marker.description = "3D Control" if show_6dof: int_marker.description += " + 6-DOF controls" int_marker.description += "\n" + control_modes_dict[interaction_mode] if show_6dof: #control = InteractiveMarkerControl() #control.orientation.w = 1 #control.orientation.x = 1 #control.orientation.y = 0 #control.orientation.z = 0 #control.name = "rotate_x" #control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS #if fixed: #control.orientation_mode = InteractiveMarkerControl.FIXED #int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) #control = InteractiveMarkerControl() #control.orientation.w = 1 #control.orientation.x = 0 #control.orientation.y = 1 #control.orientation.z = 0 #control.name = "move_z" #control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS #if fixed: #control.orientation_mode = InteractiveMarkerControl.FIXED #int_marker.controls.append(control) #control = InteractiveMarkerControl() #control.orientation.w = 1 #control.orientation.x = 0 #control.orientation.y = 0 #control.orientation.z = 1 #control.name = "rotate_y" #control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS #if fixed: #control.orientation_mode = InteractiveMarkerControl.FIXED #int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback) menu_handler.apply(server, int_marker.name)
def make6DofMarker( fixed, interaction_mode, position, show_6dof = False): global parentFrame, frame_name int_marker = InteractiveMarker() int_marker.header.frame_id = parentFrame int_marker.pose.position = position int_marker.scale = 0.5 int_marker.name = frame_name int_marker.description = "Place a frame with 6-DOF!" # insert a box makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D : "MOVE_3D", InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] int_marker.description = "3D Control" if show_6dof: int_marker.description += " + 6-DOF controls" int_marker.description += "\n" + control_modes_dict[interaction_mode] if show_6dof: control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback) menu_handler.apply( server, int_marker.name )
def createInteractiveMarkerRGBD(self, x=0, y=0, z=0): self.marker = InteractiveMarker() self.marker.header.frame_id = self.parent self.marker.pose.position.x = x self.marker.pose.position.y = y self.marker.pose.position.z = z self.marker.pose.orientation.x = 0 self.marker.pose.orientation.y = 0 self.marker.pose.orientation.z = 0 self.marker.pose.orientation.w = 1 self.marker.scale = self.marker_scale self.marker.name = self.name self.marker.description = self.name + '_labeler' # insert a box control = InteractiveMarkerControl() control.always_visible = True marker_box = Marker() marker_box.type = Marker.SPHERE marker_box.scale.x = self.marker.scale * 0.3 marker_box.scale.y = self.marker.scale * 0.3 marker_box.scale.z = self.marker.scale * 0.3 marker_box.color.r = 1 marker_box.color.g = 0 marker_box.color.b = 0 marker_box.color.a = 0.2 control.markers.append(marker_box) self.marker.controls.append(control) self.marker.controls[ 0].interaction_mode = InteractiveMarkerControl.MOVE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED self.marker.controls.append(control) self.server.insert(self.marker, self.markerFeedback) self.menu_handler.apply(self.server, self.marker.name)
def make_marker_flexible(fixed, ps, scale, color, mtype, ignore_rotation, ignore_x=False, ignore_y=False, ignore_z=False): int_marker = InteractiveMarker() int_marker.header.frame_id = ps.header.frame_id int_marker.pose = ps.pose int_marker.scale = scale int_marker.name = 'simple_6dof' int_marker.description = '' # insert a marker control = InteractiveMarkerControl() control.always_visible = True control.markers.append(make_marker(scale, color, mtype)) int_marker.controls.append(control) if fixed: int_marker.name += '_fixed' int_marker.description += '\n(fixed orientation)' if not ignore_x: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'move_x' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_y: control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_z: control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'move_z' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) if not ignore_rotation: control = InteractiveMarkerControl() control.orientation = Quaternion(1,0,0,1) control.name = 'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,0,1,1) control.name = 'rotate_y' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0,1,0,1) control.name = 'rotate_z' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def createInteractiveMarker(world_link): marker = InteractiveMarker() marker.header.frame_id = world_link trans = (1, 0, 1) marker.pose.position.x = trans[0] marker.pose.position.y = trans[1] marker.pose.position.z = trans[2] quat = (0, 0, 0, 1) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale = 0.2 marker.name = 'menu' marker.description = 'menu' # insert a box control = InteractiveMarkerControl() control.always_visible = True marker_box = Marker() marker_box.type = Marker.SPHERE marker_box.scale.x = marker.scale * 0.7 marker_box.scale.y = marker.scale * 0.7 marker_box.scale.z = marker.scale * 0.7 marker_box.color.r = 0 marker_box.color.g = 1 marker_box.color.b = 0 marker_box.color.a = 0.2 control.markers.append(marker_box) marker.controls.append(control) marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED marker.controls.append(control) server.insert(marker, markerFeedback) menu_handler.apply(server, marker.name)
def make6DofMarker(fixed=False): int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.scale = 0.05 int_marker.pose.orientation.x = frame_pose.orientation.x int_marker.pose.orientation.y = frame_pose.orientation.y int_marker.pose.orientation.z = frame_pose.orientation.z int_marker.pose.orientation.w = frame_pose.orientation.w int_marker.pose.position.x = frame_pose.position.x int_marker.pose.position.y = frame_pose.position.y int_marker.pose.position.z = frame_pose.position.z int_marker.name = FRAME_ID int_marker.description = "Place the writing surface" # create a grey box marker box_marker = Marker() box_marker.type = Marker.CUBE box_marker.scale.x = SURFACE_WIDTH box_marker.scale.y = SURFACE_HEIGHT box_marker.scale.z = 0.001 box_marker.color.r = 1.0 box_marker.color.g = 1.0 box_marker.color.b = 1.0 box_marker.color.a = 1.0 # create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append(writing_surface()) # add the control to the interactive marker int_marker.controls.append(box_control) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) server.insert(int_marker, processFeedback)
def make6DofMarker(frame_id, fixed=False, description="Simple 6-DOF Control"): int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.scale = 1 int_marker.name = "simple_6dof" int_marker.description = description # insert a box makeBoxControl(int_marker) if fixed: int_marker.name += "_fixed" int_marker.description += "\n(fixed orientation)" control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) return int_marker
def makeGraspIM(self, pose): """ :type pose: Pose """ int_marker = InteractiveMarker() int_marker.header.frame_id = self.from_frame int_marker.pose = pose int_marker.scale = 0.3 int_marker.name = "6dof_eef" int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame # insert a box, well, an arrow self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 0 control.orientation.z = 1 control.name = "move_3d" control.interaction_mode = InteractiveMarkerControl.MOVE_3D control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) self.menu_handler.insert("Publish transform", callback=self.processFeedback) self.menu_handler.insert("Stop publishing transform", callback=self.processFeedback) self.server.insert(int_marker, self.processFeedback) self.menu_handler.apply(self.server, int_marker.name)