def __init__(self, parent_instance_handle, server_name, ik_solver): self.parent_instance_handle = parent_instance_handle self.server = InteractiveMarkerServer(server_name) self.ik_solver = ik_solver self.ik_position_xyz_bounds = [0.01, 0.01, 0.01] self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK self._menu_handlers = {} self._menu_cmds = {} self.menu_handler = MenuHandler() del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) self._int_marker_name_list = [] self.markers_created_cnt = 0 self.marker_cnt = 0 self.name_to_marker_dict = {} self.waypoint_to_marker_dict = {} self.name_to_waypoint_dict = {}
def __init__(self, name, pose, dimensions, db_id): """ Constructs a WorldLandmark object. Use bounding_box() or cloud_box() instead. Args: name (str): Name of this landmark pose (Pose): Pose of the landmark, in base frame dimensions (Vector3): Size of bounding box db_id (str): The MongoDB id if this is a cloud box """ self._name = name self._pose = pose self._dimensions = dimensions self._db_id = db_id if self.is_bounding_box(): self.object = Landmark(type=Landmark.TABLE_TOP, name=name, pose=pose, dimensions=dimensions, db_id='') elif self.is_cloud_box(): self.object = Landmark(type=Landmark.CLOUD_BOX, name=name, pose=pose, dimensions=dimensions, db_id=db_id) self.int_marker = None self.is_removed = False # TODO(jstn): Move this outside. self.menu_handler = MenuHandler() self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self, root_tips, suffix=u''): """ :param root_tips: list containing root->tip tuple for each interactive marker. :type root_tips: list :param suffix: the marker will be called 'eef_control{}'.format(suffix) :type suffix: str """ self.enable_self_collision = rospy.get_param(u'~enable_self_collision', True) self.giskard = GiskardWrapper() if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.markers = {} # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, root, tip, self.giskard, self.enable_self_collision)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def init_menu(self): self.menu_handler = MenuHandler() # Remove Goal entry self.menu_remove_goal_title = "Remove Goal" self.menu_remove_goal_handle = self.menu_handler.insert( self.menu_remove_goal_title, callback=self.process_feedback)
def __init__(self, from_frame, to_frame, position, orientation): self.server = InteractiveMarkerServer("posestamped_im") o = orientation r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("Publishing transform and PoseStamped from: " + from_frame + " to " + to_frame + " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) + " and orientation " + str(o.x) + " " + str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " + str(r) + " " + str(p) + " " + str(y)) self.menu_handler = MenuHandler() self.from_frame = from_frame self.to_frame = to_frame # Transform broadcaster self.tf_br = tf.TransformBroadcaster() self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) pose = Pose() pose.position = position pose.orientation = orientation self.last_pose = pose self.print_commands(pose) self.makeGraspIM(pose) self.server.applyChanges()
def create_menu(self): """ Create and populates all the menu entries """ menu_handler = MenuHandler() menu_handler.insert("Point the head", callback=self.move_head) menu_handler.insert("Add position to trajectory", callback=self.add_point) menu_handler.insert("Place marker over gripper", callback=self.place_gripper) menu_handler.insert("Execute trjectory", callback=self.execute_trajectory) menu_handler.insert("Clear trajectory", callback=self.clear_trajectory) menu_handler.insert("Publish trajectory", callback=self.publish_trajectory) menu_handler.insert("Move the arm (planning)", callback=self.plan_arm) menu_handler.insert("Move the arm (collision-free)", callback=self.collision_free_arm) menu_handler.insert( "Move the arm to trajectory start (collision-free)", callback=self.arm_trajectory_start) menu_handler.insert("Update planning scene", callback=self.update_planning) menu_handler.apply(self.server, self.int_marker.name)
def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1): int_marker = InteractiveMarker() int_marker.header.frame_id = path_msg.header.frame_id int_marker.name = str(path_id) int_marker.description = "Path {0}".format(path_id) # line_marker = Marker() # line_marker.type = Marker.LINE_STRIP # line_marker.scale.x = width # line_marker.color = color_msg # line_marker.points = [p.pose.position for p in path_msg.poses] # for point in line_marker.points: # point.z += delta_z control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.MENU # control.markers.append(line_marker) points = [node(pose, delta_z) for pose in path_msg.poses] for p1, p2 in zip(points[:-1], points[1:]): control.markers.append(cylinder_between(p1, p2, color_msg, width)) for p in points: control.markers.append(sphere_at(p, color_msg, width)) int_marker.controls.append(copy.deepcopy(control)) menu_handler = MenuHandler() # put all the information in the main menu #d = menu_handler.insert("Description") for line in description: menu_handler.insert(line)#, parent=d) return menu_handler, int_marker
def __init__(self, root_link, tip_links): # tf self.tfBuffer = Buffer(rospy.Duration(1)) self.tf_listener = TransformListener(self.tfBuffer) # giskard goal client # self.client = SimpleActionClient('move', ControllerListAction) self.client = SimpleActionClient('qp_controller/command', ControllerListAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer("eef_control") self.menu_handler = MenuHandler() for tip_link in tip_links: int_marker = self.make6DofMarker( InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link) self.server.insert( int_marker, self.process_feedback(self.server, self.client, root_link, tip_link)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def _update_menu(self): '''Recreates the menu when something has changed''' self._menu_handler = MenuHandler() frame_entry = self._menu_handler.insert('Reference frame') self._sub_entries = [None] * len(ActionStepMarker._ref_names) for i in range(len(ActionStepMarker._ref_names)): self._sub_entries[i] = self._menu_handler.insert( ActionStepMarker._ref_names[i], parent=frame_entry, callback=self.change_ref_cb) self._menu_handler.insert('Move arm here', callback=self.move_to_cb) self._menu_handler.insert('Move to current arm pose', callback=self.move_pose_to_cb) self._menu_handler.insert('Delete', callback=self.delete_step_cb) for i in range(len(ActionStepMarker._ref_names)): self._menu_handler.setCheckState(self._sub_entries[i], MenuHandler.UNCHECKED) menu_id = self._get_menu_id(self._get_ref_name()) if menu_id == None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
def __init__(self): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() # self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1) # self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1) self.pub = rospy.Publisher('/pose_to_follow', PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name)) #self.global_frame_id = 'world' self.global_frame_id = 'base_footprint' pose = Pose() pose.position.x = 0.2 # pose.position.y = 0.35 pose.position.y = -0.35 #pose.position.z = 0.1 pose.position.z = 1.1 pose.orientation.w = 1.0 # pose.orientation.x = 0.35762 # pose.orientation.y = 0.50398 # pose.orientation.z = 0.45213 # pose.orientation.w = 0.64319 #self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
def __init__(self, server): self.server = server self.pose = Pose() self.publish = True self.make_marker() self.menu_handler = MenuHandler() item = self.menu_handler.insert("publish", callback=self.menu_callback) self.menu_handler.setCheckState(item, MenuHandler.CHECKED) self.menu_handler.apply(self.server, self.marker_name)
def __init__(self, server, id_, name, initial_pose=None): self._menu_handler = MenuHandler() self._menu_handler.insert("Delete", callback=self.deleteCallback) self._server = server self._key = name + str(id_) self._marker_map = {} self._callback_map = {} # not all templates will have a dynamic reconfigure server self._dserver = None self._initial_pose = initial_pose
def __init__(self, pose, index, dimensions, is_recognized): ''' Initialization of objects''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Object(Object.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self, side_prefix): self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers') self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges() # Code for moving the robots joints switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server()
def __init__(self, server): self.server = server self.pose = Pose() self.active = True self.covariance = 0.1 self.int_marker = None self.make_marker() self.menu_handler = MenuHandler() item = self.menu_handler.insert("active", callback=self.menu_callback) self.menu_handler.setCheckState(item, MenuHandler.CHECKED) high_conf = self.menu_handler.insert("high confidence", callback=self.conf_callback) self.menu_handler.setCheckState(high_conf, MenuHandler.CHECKED) self.menu_handler.apply(self.server, self.marker_name)
def _update_menu(self): """Recreates the menu when something has changed.""" self._menu_handler = MenuHandler() # Insert sub entries. self._sub_entries = [] self._menu_options = [] frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref']) refs = ActionStepMarker._ref_names for ref in refs: if ref != BASE_LINK and "Obj" not in str(ref): subent = self._menu_handler.insert(ref + ' center', parent=frame_entry, callback=self.subent_cb) self._sub_entries += [subent] self._menu_options += [subent] elif ref == BASE_LINK: subent = self._menu_handler.insert(ref, parent=frame_entry, callback=self.subent_cb) self._sub_entries += [subent] self._menu_options += [subent] select_one = self._menu_handler.insert('Select an object side', parent=frame_entry, callback=self.select_one_cb) self._menu_options += [select_one] # Inset main menu entries. self._menu_handler.insert(MENU_OPTIONS['move_here'], callback=self.move_to_cb) self._menu_handler.insert(MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb) self._menu_handler.insert(MENU_OPTIONS['del'], callback=self.delete_step_cb) # Make all unchecked to start. for subent in self._sub_entries: self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED) self._menu_handler.setCheckState(select_one, MenuHandler.UNCHECKED) # Check if necessary. Set base_link to checked menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) # Update. self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient( r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient( l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik
def init(self): # The parameter /robot_description must exist if not rospy.has_param('/robot_description'): rospy.logerr( "Parameter '/robot_description' must exist to continue") sys.exit(1) self.config = loadJSONConfig(self.args['calibration_file']) if self.config is None: sys.exit(1) # loadJSON should tell you why. # self.config = CalibConfig() # ok = self.config.loadJSON(self.args['calibration_file']) # # Exit if it fails to open a valid calibration file # if not ok: # sys.exit(1) # Load the urdf from /robot_description self.urdf = URDF.from_parameter_server() # ok = validateLinks(self.config.world_link, self.config.sensors, self.urdf) # if not ok: # sys.exit(1) print('Number of sensors: ' + str(len(self.config['sensors']))) # Init interaction self.server = InteractiveMarkerServer("interactive_first_guess") self.menu = MenuHandler() self.menu.insert("Save sensors configuration", callback=self.onSaveFirstGuess) self.menu.insert("Reset to initial configuration", callback=self.onReset) # For each node generate an interactive marker. for name, sensor in self.config['sensors'].items(): print(Fore.BLUE + '\nSensor name is ' + name + Style.RESET_ALL) params = { "frame_world": self.config['world_link'], "frame_opt_parent": sensor['parent_link'], "frame_opt_child": sensor['child_link'], "frame_sensor": sensor['link'], "marker_scale": self.args['marker_scale'] } # Append to the list of sensors self.sensors.append(Sensor(name, self.server, self.menu, **params)) self.server.applyChanges()
def __init__(self, side_prefix): if GripperMarkers._tf_listener is None: GripperMarkers._tf_listener = TransformListener() self.ik = IK(side_prefix) self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers') self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.is_control_visible = False self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker') self._im_server.applyChanges()
def __init__(self, server, cam_info): self.cam_info = cam_info self.marker_name = "goal" self.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE self.absolute_publisher = rospy.Publisher("goal_absolute", PoseWithCertaintyArray, queue_size=1) self.relative_publisher = rospy.Publisher("goal_relative", PoseWithCertaintyArray, queue_size=1) self.relative_posts_publisher = rospy.Publisher("goal_posts_relative", PoseWithCertaintyArray, queue_size=1) super(GoalMarker, self).__init__(server) self.menu_handler = MenuHandler() self.pose.position.x = 3.0
def __init__(self, pose, index, dimensions, is_recognized): ''' Args: pose (Pose): Position of bounding box index (int): For naming object in world (e.g. "thing 0") dimensions (Vector3): Size of bounding box is_recognized (bool): Result of object recognition. ''' self.index = index self.assigned_name = None self.is_recognized = is_recognized self.object = Landmark( Landmark.TABLE_TOP, self.get_name(), pose, dimensions) self.menu_handler = MenuHandler() self.int_marker = None self.is_removed = False self.menu_handler.insert('Remove from scene', callback=self.remove)
def update(self): '''Updates marker/arm loop''' self._menu_handler = MenuHandler() # Inset main menu entries. self._menu_handler.insert('Move gripper here', callback=self._move_to_cb) self._menu_handler.insert('Move marker to current gripper pose', callback=self._move_pose_to_cb) if self._is_hand_open(): self._menu_handler.insert('Close gripper', callback=self._close_gripper_cb) else: self._menu_handler.insert('Open gripper', callback=self._open_gripper_cb) frame_id = REF_FRAME pose = self.get_pose() # if self._marker_moved() or self._menu_control is None: rospy.loginfo("Marker moved") menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) self._menu_control = menu_control # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = frame_id int_marker.pose = pose.pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(self._menu_control) ArmControlMarker._im_server.insert(int_marker, self._marker_feedback_cb) self._menu_handler.apply(ArmControlMarker._im_server, self._get_name()) ArmControlMarker._im_server.applyChanges()
def __init__(self, topic, initial_x, initial_y, initial_z, initial_roll, initial_pitch, initial_yaw, frame_id): self.server = InteractiveMarkerServer("posestamped_im") self.menu_handler = MenuHandler() self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1) rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.resolved_name)) pose = Pose() pose.position.x = initial_x pose.position.y = initial_y pose.position.z = initial_z quat = quaternion_from_euler(initial_roll, initial_pitch, initial_yaw) pose.orientation = Quaternion(*quat) self.initial_pose = pose self.frame_id = frame_id # self.makeMenuMarker(pose) self.makeGraspIM(pose) self.server.applyChanges()
def start_once(self): # giskard goal client self.client = SimpleActionClient(u'qp_controller/command', MoveAction) self.client.wait_for_server() # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() all_goals = {} for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, self.client, root, tip, all_goals)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self, server, name, X_WM, markers): self._server = server self.name = name self._X_WM = X_WM self._menu = MenuHandler() self._edit_id = self._menu.insert("Edit", callback=self._edit_callback) self._delete_id = self._menu.insert("Delete", callback=self._delete_callback) self._menu.insert(self.name, callback=noop) # For annotation. self._t_click_prev = -float('inf') # Make template marker. template = InteractiveMarker() template.name = self.name template.description = name template.header.frame_id = "world" viz = InteractiveMarkerControl() viz.interaction_mode = InteractiveMarkerControl.BUTTON viz.always_visible = True viz.markers.extend(markers) template.controls.append(viz) self._template = template # Initialize. self._edit = False self._update()
def _update_menu(self): '''Recreates the menu when something has changed.''' self._menu_handler = MenuHandler() # Insert sub entries. self._sub_entries = [] frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref']) refs = ActionStepMarker._ref_names for ref in refs: subent = self._menu_handler.insert(ref, parent=frame_entry, callback=self.change_ref_cb) self._sub_entries += [subent] # Inset main menu entries. self._menu_handler.insert(MENU_OPTIONS['move_here'], callback=self.move_to_cb) self._menu_handler.insert(MENU_OPTIONS['move_current'], callback=self.move_pose_to_cb) self._menu_handler.insert(MENU_OPTIONS['del'], callback=self.delete_step_cb) # Make all unchecked to start. for subent in self._sub_entries: self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED) # Check if necessary. menu_id = self._get_menu_id(self._get_ref_name()) if menu_id is None: self.has_object = False else: self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED) # Update. self._update_viz_core() self._menu_handler.apply(ActionStepMarker._im_server, self._get_name()) ActionStepMarker._im_server.applyChanges()
def create_menus(self): self.global_menu_handler = MenuHandler() g_art = self.global_menu_handler.insert("Artificial") self.global_menu_handler.insert("Add primitive", parent=g_art, callback=self.global_menu_add_prim_cb) self.global_menu_handler.insert("Clear all", parent=g_art, callback=self.global_menu_clear_all_cb) self.global_menu_handler.insert( "Clear all (permanent)", parent=g_art, callback=self.global_menu_clear_all_perm_cb) self.global_menu_handler.insert("Save all", parent=g_art, callback=self.global_menu_save_all_cb) self.global_menu_handler.insert("Reload", parent=g_art, callback=self.global_menu_reload_cb) g_det = self.global_menu_handler.insert("Detected") self.global_menu_handler.insert("Pause", parent=g_det, callback=self.global_menu_pause_cb) self.global_menu_handler.insert( "Clear all", parent=g_det, callback=self.global_menu_det_clear_all_cb) self.a_obj_menu_handler = MenuHandler() mov = self.a_obj_menu_handler.insert("Moveable", callback=self.menu_moveable_cb) self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED) sc = self.a_obj_menu_handler.insert("Scaleable", callback=self.menu_scaleable_cb) self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED) self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb) self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb) self.d_obj_menu_handler = MenuHandler() self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb) int_marker = InteractiveMarker() int_marker.header.frame_id = self.world_frame int_marker.pose.position.z = 1.2 int_marker.scale = 0.5 int_marker.name = "global_menu" int_marker.description = "Global Menu" control = InteractiveMarkerControl() control.interaction_mode = InteractiveMarkerControl.BUTTON control.always_visible = True marker = Marker() marker.type = Marker.CUBE marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 control.markers.append(marker) int_marker.controls.append(control) self.im_server.insert(int_marker, self.ignored_cb) self.global_menu_handler.apply(self.im_server, int_marker.name) self.im_server.applyChanges()
# ------------------------ # IMPORT MODULES # # ------------------------ import argparse import rospkg import rospy from visualization_msgs.msg import InteractiveMarkerControl, Marker, InteractiveMarker from interactive_markers.interactive_marker_server import InteractiveMarkerServer from interactive_markers.menu_handler import MenuHandler from interactive_calibration.data_collector_and_labeler import DataCollectorAndLabeler server = None menu_handler = MenuHandler() def menuFeedback(feedback): handle = feedback.menu_entry_id if handle == 1: # collect snapshot print('Save collection selected') data_collector.saveCollection() def initMenu(): menu_handler.insert("Save collection", callback=menuFeedback) def createInteractiveMarker(world_link): marker = InteractiveMarker()
def add_marker(self, initial_pose, frame="base_link", description=None): self.markers_created_cnt += 1 self.marker_cnt += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = frame int_marker.pose.position = initial_pose.position int_marker.scale = 0.1 int_marker.name = rospy.get_name() + str(self.markers_created_cnt) if description is None: int_marker.description = int_marker.name else: int_marker.description = description self.name_to_marker_dict[int_marker.name] = int_marker self.name_to_position_only_flag[int_marker.name] = True # insert a box # TODO: may change geometry / eff mesh make_box_control_marker(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 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 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 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 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 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 int_marker.controls.append(control) self.server.insert(int_marker, self._process_feedback) # add menus menu_handler = MenuHandler() pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback) self._menu_cmds['position_only'] = pos_opt_handle menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED) menu_handler.apply(self.server, int_marker.name) self._menu_handlers[int_marker.name] = menu_handler self.server.applyChanges() self._int_marker_name_list.append(int_marker.name) return int_marker.name
def add_waypoint_marker(self, waypoint, description=None): self.markers_created_cnt += 1 self.marker_cnt += 1 int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.pose.position = waypoint.end_effector_pose.position int_marker.scale = 0.1 int_marker.name = str(self.markers_created_cnt) if description is None: int_marker.description = int_marker.name else: int_marker.description = description self.name_to_marker_dict[int_marker.name] = int_marker self.waypoint_to_marker_dict[waypoint] = int_marker self.name_to_waypoint_dict[int_marker.name] = waypoint # insert a box # TODO: may change geometry / eff mesh make_box_control_marker(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 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 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 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 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 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 int_marker.controls.append(control) self.server.insert(int_marker, self._process_feedback) # add menus menu_handler = MenuHandler() del_sub_menu_handle = menu_handler.insert("Delete Waypoint") self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback) ins_sub_menu_handle = menu_handler.insert("Insert Waypoint") self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback) self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback) menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback) # Set time time_sub_menu_handle = menu_handler.insert("Set Waypoint Time") menu_time_cmds = {} time = self.name_to_waypoint_dict[int_marker.name].time time_list = [float(num) for num in range((int(m.ceil(time))))] time_list.append(time) time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)]) self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds} for t in time_list: time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback) menu_time_cmds[time_opt_handle] = t if t == time: menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED) else: menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED) menu_handler.apply(self.server, int_marker.name) self._menu_handlers[int_marker.name] = menu_handler self.server.applyChanges() if int(int_marker.description)-1 < len(self._int_marker_name_list): self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name) else: self._int_marker_name_list.append(int_marker.name)