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)
