def make_interactive_marker(pose, model): int_marker = InteractiveMarker() int_marker.name = model int_marker.description = model int_marker.header.frame_id = "map" int_marker.pose = pose int_marker.pose.position.z = max(int_marker.pose.position.z, 0.01) # ensure marker is above ground int_marker.pose.orientation = quaternion_msg_from_yaw( yaw(pose)) # discard all but yaw to ensure marker is flat int_marker.scale = 1 make_model_control(int_marker, model) control = InteractiveMarkerControl() control.name = "drag" control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 normalize_quaternion(control.orientation) control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE int_marker.controls.append(copy.deepcopy(control)) control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) server.insert(int_marker, process_feedback) menu_handler.apply(server, int_marker.name) return int_marker
def makeInteractiveMarker(name, description): global fixed_frame interactive_marker = InteractiveMarker() interactive_marker.header.frame_id = fixed_frame interactive_marker.name = name interactive_marker.description = description return interactive_marker
def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) self.gripper_pose_pub = rospy.Publisher("~gripper_pose", PoseStamped) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker); #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik self.planning_waiting_time = 10.0 rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def __init__(self, whicharm): self.whicharm = whicharm self.robot_state = pr2_control_utilities.RobotState() self.joint_controller = pr2_control_utilities.PR2JointMover( self.robot_state) self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller) self.server = InteractiveMarkerServer("~trajectory_markers") self.tf_listener = self.planner.tf_listener self.visualizer_pub = rospy.Publisher("~trajectory_markers_path", MarkerArray) self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray) rospy.Subscriber("~overwrite_trajectory", PoseArray, self.overwrite_trajectory) rospy.Service("~execute_trajectory", Empty, self.execute_trajectory_srv) # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/base_link" int_marker.pose.position.x = 0.5 int_marker.pose.position.z = 1.0 int_marker.name = "move_" + whicharm + "_arm" int_marker.description = "Move the " + whicharm + " arm" int_marker.scale = 0.2 self.server.insert(int_marker, self.main_callback) # create the main marker shape #color = (1,0,0,1) color = None self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #add the controls utils.make_6DOF_marker(int_marker) self.int_marker = int_marker self.create_menu() self.server.applyChanges() self.trajectory = PoseArray() self.trajectory.header.frame_id = "/base_link" self.last_gripper_pose = None if whicharm == "right": self.ik_utils = self.planner.right_ik else: self.ik_utils = self.planner.left_ik rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def create_marker(self, object_name): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/" + object_name int_marker.name = object_name int_marker.description = "Select this object" int_marker.pose.position.z = -0.3 self.int_markers[object_name] = int_marker # create a marker over the object object_marker = Marker() object_marker.type = Marker.SPHERE object_marker.pose.position.z = 0.6 object_marker.scale.x = 0.1 object_marker.scale.y = 0.1 object_marker.scale.z = 0.1 object_marker.color.r = 0.6 object_marker.color.g = 0.02 object_marker.color.b = 1.0 object_marker.color.a = 1.0 self.object_markers[object_name] = object_marker # create a non-interactive control which contains the box object_control = InteractiveMarkerControl() object_control.interaction_mode = InteractiveMarkerControl.BUTTON object_control.always_visible = True object_control.markers.append(object_marker) self.object_controls[object_name] = object_control # add the control to the interactive marker self.int_markers[object_name].controls.append(object_control) # add the interactive marker to our collection & # tell the server to call processFeedback() when feedback arrives for it self.server.insert(self.int_markers[object_name], self.processFeedback) # 'commit' changes and send to all clients self.server.applyChanges()