def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose # rospy.loginfo("Updating Marker: %d", feedback.event_type) # rospy.loginfo("POS: %s", feedback.pose.position) if self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1, 0, 0, 1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) # rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges()
def main_callback(self, feedback): """ If the mouse button is released change the gripper color according to the IK result. Quite awkward, trying to get a nicer way to do it. """ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.last_gripper_pose = feedback.pose #rospy.loginfo("Updating Marker: %d", feedback.event_type) #rospy.loginfo("POS: %s", feedback.pose.position) if (self.last_gripper_pose and feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP): self.last_gripper_pose = None pos = PoseStamped() pos.header.frame_id = feedback.header.frame_id pos.pose = feedback.pose if self.whicharm == "right": ik = self.planner.check_ik_right_arm else: ik = self.planner.check_ik_left_arm if ik(pos): color = None else: color = (1, 0, 0, 1) int_marker = self.server.get(self.int_marker.name) int_marker.controls.remove(self.gripper_marker) self.gripper_marker = utils.makeGripperMarker(color=color) int_marker.controls.append(self.gripper_marker) #rospy.loginfo("Control: %s", int_marker.controls) self.server.insert(int_marker, self.main_callback) self.server.setPose(int_marker.name, self.last_gripper_pose) self.server.applyChanges()
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 publish_gripper_pose(self, pose): assert isinstance(pose, PoseStamped) gripper = utils.makeGripperMarker(pose) self.gripper_pub.publish(gripper)