def pose2matrix(pose):
p = pose
if hasattr(pose, "pose"):
p = pose.pose
T = tf_conversions.fromMsg(pose.pose)
T = tf_conversions.toMatrix(T)
return T
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time(0), timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
except:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], tf.transformations.quaternion_from_euler(0,math.pi/2,0))
else:
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation([0,0,0], [0,0,0])
actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def callback(self, camera_info_msg, image_msg, camera_pose_msg):
# camera parameters and image
camera_matrix = numpy.float32(camera_info_msg.K).reshape(3, 3)
distortion = numpy.float32(camera_info_msg.D).flatten()
frame = self.cv_bridge.imgmsg_to_cv2(image_msg, 'bgr8')
# camera pose
cam2world = tf_conversions.toMatrix(
tf_conversions.fromMsg(camera_pose_msg.pose))
world2cam = numpy.linalg.inv(cam2world)
tvec = world2cam[:3, 3]
rvec, jacob = cv2.Rodrigues(world2cam[:3, :3])
# estimates the scale of the environment
ret, scale = self.scale_estimater.estimate(tvec)
if not ret:
cv2.putText(frame, 'estimating scale...', (10, 30),
cv2.FONT_HERSHEY_PLAIN, 3.0, (255, 255, 255))
else:
# assuming that the camera moves 20cm in the first 100 frames
scale = scale / 0.2
for cube in self.cubes:
cube.draw(frame, camera_matrix, distortion, rvec, tvec, scale)
self.frame = frame
self.cam2world = cam2world
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time(0), timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(grasp_frame, move_group_commander.get_end_effector_link(),rospy.Time())
except:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s"%(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot( graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def MoveTriad(self, msg): if self.axisbody is None: return f1 = tf_conversions.fromMsg(msg.pose) T = tf_conversions.toMatrix(f1) #T[0:3,3] = [-.5,0,0] self.axisbody.SetTransform(T)
def get_pose_as_tran(moveit_grasp_msg):
"""
type: moveit_grasp_msg: moveit_msgs.msg.Grasp
"""
assert isinstance(moveit_grasp_msg, moveit_msgs.msg.Grasp)
pose_msg = moveit_grasp_msg.grasp_pose.pose
return tf_conversions.toMatrix(tf_conversions.fromMsg(pose_msg))
def get_pose_as_tran(moveit_grasp_msg):
"""
type: moveit_grasp_msg: moveit_msgs.msg.Grasp
"""
assert isinstance(moveit_grasp_msg, moveit_msgs.msg.Grasp)
pose_msg = moveit_grasp_msg.grasp_pose.pose
return tf_conversions.toMatrix(tf_conversions.fromMsg(pose_msg))
def create_occupancy_grid_from_obstacles(obstacles,
mins_xyz,
step_size_xyz,
dims_xyz,
use_binvox=False):
voxel_grid = np.zeros(shape=dims_xyz)
for obstacle in obstacles:
if use_binvox:
vox = binvox_rw.read_as_3d_array(
open(obstacle.mesh_filepath.replace('.ply', '.binvox'), 'r'))
vertices = binvox_to_points(vox)
else:
vertices = read_vertex_points_from_ply_filepath(
obstacle.mesh_filepath)
frame = tf_conversions.fromMsg(obstacle.pose_stamped.pose)
transform = tf_conversions.toMatrix(frame)
vertices_transformed = transform_points(vertices, transform)
if use_binvox:
voxel_grid += add_obstacles_to_reachability_space_full_binvox(
vertices_transformed, mins_xyz, step_size_xyz, dims_xyz)
else:
voxel_grid += add_obstacles_to_reachability_space_full(
vertices_transformed, mins_xyz, step_size_xyz, dims_xyz)
voxel_grid[np.where(voxel_grid > 0)] = 1
return voxel_grid
def make_pointcloud_proto(self, pointcloud_msg):
"""
:param pointcloud_msg: input sensor_msgs.msg.PointCloud2
:type pointcloud_msg: sensor_msgs.msg.PointCloud2
:rtype: gen_proto.GraspitMessage_pb2.GraspitProtobufMessage
"""
pointcloud_transform = tf_conversions.toMatrix(
tf_conversions.fromTf(
self.listener.lookupTransform(self.graspit_frame,
pointcloud_msg.header.frame_id,
rospy.Time(0))))
rospy.loginfo(self.__class__.__name__ + " is relaying message")
if self.downsample_factor != 1:
self.create_uvs(pointcloud_msg)
points = sensor_msgs.point_cloud2.read_points(pointcloud_msg, None,
False, self.uvs)
#points = sensor_msgs.point_cloud2.read_points(pointcloud_msg)
gm = gen_proto.GraspitMessage_pb2.GraspitProtobufMessage()
#gm.renderable.pointCloud.points.add()
gm.renderable.pointCloud.points.extend(
[gen_proto.Renderable_pb2.Renderable.PointXYZRGB()] *
(pointcloud_msg.width * pointcloud_msg.height /
(self.downsample_factor**2)))
#renderable = gen_proto.Renderable_pb2.Renderable()
#pointcloud = gen_proto.Renderable_pb2.Renderable.PointCloudXYZRGB()
for ind, point in enumerate(points):
#pointXYZRGB = gen_proto.Renderable_pb2.Renderable.PointXYZRGB()
color = self.get_color_converted_point(point)
point_location = numpy.dot(
pointcloud_transform,
numpy.array([point[:3] + (1, )]).transpose())
renderable = gm.renderable
assert isinstance(renderable, gen_proto.Renderable_pb2.Renderable)
pointCloud = renderable.pointCloud
assert isinstance(
pointCloud,
gen_proto.Renderable_pb2.Renderable.PointCloudXYZRGB)
points = pointCloud.points
assert isinstance(points,
gen_proto.Renderable_pb2.Renderable.PointXYZRGB)
pointCloud.points[ind].point.x = point_location[0, 0]
pointCloud.points[ind].point.y = point_location[1, 0]
pointCloud.points[ind].point.z = point_location[2, 0]
pointCloud.points[ind].color.red = color[0]
pointCloud.points[ind].color.green = color[1]
pointCloud.points[ind].color.blue = color[2]
renderable.pointCloud.units = 1.0
rospy.loginfo(
str(self.__class__) +
"::GraspitProtobufSocket:: Finished building protobuf" +
"pointcloud message")
return gm
def top_cam_tag_callback(self, tag_detections):
#rospy.loginfo("Received top cam tag!")
for detection in tag_detections.detections:
self.top_cam_to_table_stamp = tag_detections.header.stamp
self.top_cam_tag_found = True
tag_id = detection.id[0]
self.top_cam_to_table[tag_id] = tfc.toMatrix(tfc.fromMsg(detection.pose.pose.pose))
def try_get_world_transform(self):
try:
self.transform_listener.waitForTransform("/camera_rgb_optical_frame", "/world", rospy.Time(0),rospy.Duration(.1))
except:
return False
self.world_transform = tf_conversions.toMatrix(tf_conversions.fromTf(
self.transform_listener.lookupTransform(
"/camera_rgb_optical_frame",'/world', rospy.Time(0))))
return True
def update_table(self):
try:
trans = tf_conversions.toMatrix(tf_conversions.fromTf(self.transform_listener.lookupTransform('world','object', rospy.Time(0))))
trans[0:3,3] *= 1000
trans[2,3] -= 70
self.graspit_commander.set_body_trans('experiment_table', trans)
except:
pdb.post_mortem()
def transform_pose(grasp_pose_in_world, world_frame_to_object_frame_transform):
# Convert ROSmsg to 4x4 numpy arrays
grasp_pose_in_world_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(grasp_pose_in_world))
object_pose_in_world_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(world_frame_to_object_frame_transform))
# Transform the grasp pose into the object reference frame
grasp_pose_in_object_tran_matrix = np.dot(
np.linalg.inv(object_pose_in_world_tran_matrix),
grasp_pose_in_world_tran_matrix)
# Convert pose back into ROS message
grasp_pose_in_object = tf_conversions.toMsg(
tf_conversions.fromMatrix(grasp_pose_in_object_tran_matrix))
return grasp_pose_in_object
def xyzrpy_to_xform(xyzrpy):
r = tf_conversions.Rotation.RPY(*xyzrpy[3:])
pose = Pose(Point(*xyzrpy[:3]), Quaternion(*r.GetQuaternion()))
frame = tf_conversions.fromMsg(pose)
transform = tf_conversions.toMatrix(frame)
rotMatrix = transform[0:3, 0:3].flatten().tolist()
tran = [pose.position.x, pose.position.y, pose.position.z]
return rotMatrix, tran
def get_original_grasp_tran(moveit_grasp_msg):
listener = tf.listener.TransformListener()
print '1'
listener.waitForTransform("approach_tran", 'staubli_rx60l_link7', rospy.Time(), timeout=rospy.Duration(1.0))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform("approach_tran", 'staubli_rx60l_link7',rospy.Time())
print '2'
moveit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(tf_conversions.fromMsg(moveit_grasp_msg.grasp_pose.pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
original_ee_pose_matrix = np.dot( moveit_grasp_msg_final_grasp_tran_matrix, numpy.linalg.inv(approach_tran_to_end_effector_tran_matrix))
return original_ee_pose_matrix
def _get_world_transform(self):
try:
self._transform_listener.waitForTransform("/camera_rgb_optical_frame", "/world", rospy.Time(0), rospy.Duration(10))
except:
rospy.logwarn("Failed to get world transform for: " + self.__class__.__name())
return None
world_transform = tf_conversions.toMatrix(tf_conversions.fromTf(
self._transform_listener.lookupTransform(
"/camera_rgb_optical_frame", '/world', rospy.Time(0))))
return world_transform
def get_world_transform():
transform_listener = tf.TransformListener()
try:
transform_listener.waitForTransform("/camera_rgb_optical_frame", "/world", rospy.Time(0), rospy.Duration(10))
except:
rospy.logwarn("Failed to get world transform for: ")
return None
world_transform = tf_conversions.toMatrix(tf_conversions.fromTf(
transform_listener.lookupTransform(
"/camera_rgb_optical_frame", '/world', rospy.Time(0))))
return world_transform
def get_most_recent_transform(self):
left_pose, left_time = self.left_arm_tf_updater.current_pose_and_time
right_pose, right_time = self.left_arm_tf_updater.current_pose_and_time
if(right_time > left_time):
pose = right_pose
camera_transform = self.right_arm_tf_updater.camera_transform
else:
pose = left_pose
camera_transform = self.left_arm_tf_updater.camera_transform
transform = None
if(pose):
checkerboard_in_camera = tf_conversions.toMatrix((tf_conversions.fromMsg(pose)))
checkerboard_in_body = camera_transform * checkerboard_in_camera
checkerboard_in_body_tf = tf_conversions.toTf(tf_conversions.toMatrix(checkerboard_in_body))
checkerboard_rpy = tf.transformations.quaternion_to_euler(*checkerboard_in_body_tf[1])
transform = checkboard_body_in_tf[0] + checkerboard_rpy
return transform
def _get_world_transform(self):
try:
self._transform_listener.waitForTransform(
"/camera_rgb_optical_frame", "/world", rospy.Time(0),
rospy.Duration(10))
except:
rospy.logwarn("Failed to get world transform for: " +
self.__class__.__name())
return None
world_transform = tf_conversions.toMatrix(
tf_conversions.fromTf(
self._transform_listener.lookupTransform(
"/camera_rgb_optical_frame", '/world', rospy.Time(0))))
return world_transform
def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander,
listener,
graspit_grasp_msg,
grasp_frame='/approach_tran'):
"""
:param move_group_commander: A move_group command from which to get the end effector link.
:type move_group_commander: moveit_commander.MoveGroupCommander
:param listener: A transformer for looking up the transformation
:type tf.TransformListener
:param graspit_grasp_msg: A graspit grasp message
:type graspit_grasp_msg: graspit_msgs.msg.Grasp
"""
try:
listener.waitForTransform(grasp_frame,
move_group_commander.get_end_effector_link(),
rospy.Time(0),
timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame, move_group_commander.get_end_effector_link(),
rospy.Time())
except:
rospy.logerr(
"graspit_grasp_pose_to_moveit_grasp_pose::\n " +
"Failed to find transform from %s to %s" %
(grasp_frame, move_group_commander.get_end_effector_link()))
ipdb.set_trace()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link':
rospy.logerr('This is a PR2\'s left arm so we have to rotate things.')
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation(
[0, 0, 0],
tf.transformations.quaternion_from_euler(0, math.pi / 2, 0))
else:
pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation(
[0, 0, 0], [0, 0, 0])
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix,
approach_tran_to_end_effector_tran_matrix)
actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat)
actual_ee_pose = tf_conversions.toMsg(
tf_conversions.fromMatrix(actual_ee_pose_matrix))
rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose))
return actual_ee_pose
def make_pointcloud_proto(self, pointcloud_msg):
"""
:param pointcloud_msg: input sensor_msgs.msg.PointCloud2
:type pointcloud_msg: sensor_msgs.msg.PointCloud2
:rtype: gen_proto.GraspitMessage_pb2.GraspitProtobufMessage
"""
pointcloud_transform = tf_conversions.toMatrix(tf_conversions.fromTf(self.listener.lookupTransform(
self.graspit_frame, pointcloud_msg.header.frame_id, rospy.Time(0))))
rospy.loginfo(self.__class__.__name__ + " is relaying message")
if self.downsample_factor != 1:
self.create_uvs(pointcloud_msg)
points = sensor_msgs.point_cloud2.read_points(pointcloud_msg, None, False, self.uvs)
#points = sensor_msgs.point_cloud2.read_points(pointcloud_msg)
gm = gen_proto.GraspitMessage_pb2.GraspitProtobufMessage()
#gm.renderable.pointCloud.points.add()
gm.renderable.pointCloud.points.extend(
[gen_proto.Renderable_pb2.Renderable.PointXYZRGB()] *
(pointcloud_msg.width*pointcloud_msg.height/(self.downsample_factor**2)))
#renderable = gen_proto.Renderable_pb2.Renderable()
#pointcloud = gen_proto.Renderable_pb2.Renderable.PointCloudXYZRGB()
for ind,point in enumerate(points):
#pointXYZRGB = gen_proto.Renderable_pb2.Renderable.PointXYZRGB()
color = self.get_color_converted_point(point)
point_location = numpy.dot(pointcloud_transform, numpy.array([point[:3] + (1,)]).transpose())
renderable = gm.renderable
assert isinstance(renderable, gen_proto.Renderable_pb2.Renderable)
pointCloud = renderable.pointCloud
assert isinstance(pointCloud, gen_proto.Renderable_pb2.Renderable.PointCloudXYZRGB)
points = pointCloud.points
assert isinstance(points, gen_proto.Renderable_pb2.Renderable.PointXYZRGB)
pointCloud.points[ind].point.x = point_location[0, 0]
pointCloud.points[ind].point.y = point_location[1, 0]
pointCloud.points[ind].point.z = point_location[2, 0]
pointCloud.points[ind].color.red = color[0]
pointCloud.points[ind].color.green = color[1]
pointCloud.points[ind].color.blue = color[2]
renderable.pointCloud.units = 1.0
rospy.loginfo(str(self.__class__) + "::GraspitProtobufSocket:: Finished building protobuf"
+ "pointcloud message")
return gm
def servo_to_pose_call(self,req):
if self.driver_status == 'SERVO':
rospy.logwarn(req)
self.F_command = tf_c.fromMsg(req.target)
M_command = tf_c.toMatrix(self.F_command)
# Calculate IK
joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics
if joint_positions is not None:
pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
self.q = joint_positions
else:
rospy.logwarn('no solution found')
# self.send_command(F_command)
return 'SUCCESS - moved to pose'
else:
rospy.logerr('SIMPLE UR -- Not in servo mode')
return 'FAILED - not in servo mode'
def get_original_grasp_tran(moveit_grasp_msg):
listener = tf.listener.TransformListener()
print '1'
listener.waitForTransform("approach_tran",
'staubli_rx60l_link7',
rospy.Time(),
timeout=rospy.Duration(1.0))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
"approach_tran", 'staubli_rx60l_link7', rospy.Time())
print '2'
moveit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(moveit_grasp_msg.grasp_pose.pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
original_ee_pose_matrix = np.dot(
moveit_grasp_msg_final_grasp_tran_matrix,
numpy.linalg.inv(approach_tran_to_end_effector_tran_matrix))
return original_ee_pose_matrix
def graspit_grasp_pose_to_moveit_grasp_pose(
listener, # type: tf.TransformListener
graspit_grasp_msg, # type: graspit_msgs.msg.Grasp
end_effector_link, # type: str
grasp_frame # type: str
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param listener: A transformer for looking up the transformation
:param graspit_grasp_msg: A graspit grasp message
"""
try:
listener.waitForTransform(
grasp_frame,
end_effector_link,
rospy.Time(0),
timeout=rospy.Duration(1)
)
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame,
end_effector_link,
rospy.Time()
)
except Exception as e:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n "
"Failed to find transform from {} to {}"
.format(grasp_frame, end_effector_link)
)
ipdb.set_trace()
return geometry_msgs.msg.Pose()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose)
)
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS().fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(tf_conversions.fromMatrix(actual_ee_pose_matrix))
return actual_ee_pose
def update_from_marker(self):
try:
F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/endpoint_interact',rospy.Time(0)))
F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/endpoint_interact',rospy.Time(0)))
F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0)))
self.F_command = F_base_world.Inverse()*F_target_world
M_command = tf_c.toMatrix(self.F_command)
joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics
if joint_positions is not None:
pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
self.q = joint_positions
else:
rospy.logwarn('no solution found')
# self.send_command()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn(str(e))
def graspit_grasp_pose_to_moveit_grasp_pose(
listener, # type: tf.TransformListener
graspit_grasp_msg, # type: graspit_interface.msg.Grasp
end_effector_link, # type: str
grasp_frame # type: str
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param listener: A transformer for looking up the transformation
:param graspit_grasp_msg: A graspit grasp message
"""
try:
listener.waitForTransform(grasp_frame,
end_effector_link,
rospy.Time(0),
timeout=rospy.Duration(1))
at_to_ee_tran, at_to_ee_rot = listener.lookupTransform(
grasp_frame, end_effector_link, rospy.Time())
except Exception as e:
rospy.logerr("graspit_grasp_pose_to_moveit_grasp_pose::\n "
"Failed to find transform from {} to {}".format(
grasp_frame, end_effector_link))
ipdb.set_trace()
return geometry_msgs.msg.Pose()
graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg.pose))
approach_tran_to_end_effector_tran_matrix = tf.TransformerROS(
).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot)
actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix,
approach_tran_to_end_effector_tran_matrix)
actual_ee_pose = tf_conversions.toMsg(
tf_conversions.fromMatrix(actual_ee_pose_matrix))
return actual_ee_pose
def callback(self, camera_info_msg, image_msg, camera_pose_msg):
# wait ufor plane parameters
if self.plane is None:
return
# camera parameters and image
camera_matrix = numpy.float32(camera_info_msg.K).reshape(3, 3)
distortion = numpy.float32(camera_info_msg.D).flatten()
frame = self.cv_bridge.imgmsg_to_cv2(image_msg, 'bgr8')
# camera pose
cam2world = tf_conversions.toMatrix(
tf_conversions.fromMsg(camera_pose_msg.pose))
world2cam = numpy.linalg.inv(cam2world)
tvec = world2cam[:3, 3]
rvec, jacob = cv2.Rodrigues(world2cam[:3, :3])
self.plane.draw(frame, camera_matrix, distortion, rvec, tvec)
for cube in self.cubes:
cube.draw(frame, camera_matrix, distortion, rvec, tvec, self.plane)
self.frame = frame
def get_graspit_grasp_pose_in_new_reference_frame(
graspit_grasp_msg_pose, # type: geometry_msgs.msg.Pose
target_to_source_translation_rotation, # type: tuple of tuples
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param target_to_source_translation_rotation: result of listener.lookupTransform((target_frame, grasp_frame, rospy.Time(0), timeout=rospy.Duration(1))
:param graspit_grasp_msg_pose: The pose of a graspit grasp message i.e. g.pose
t_T_G = t_T_s * s_T_G
"""
graspit_grasp_pose_in_source_frame_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg_pose)
)
source_in_target_frame_tran_matrix = tf.TransformerROS().fromTranslationRotation(
target_to_source_translation_rotation[0],
target_to_source_translation_rotation[1])
graspit_grasp_pose_in_target_frame_matrix = np.dot(source_in_target_frame_tran_matrix,
graspit_grasp_pose_in_source_frame_matrix) # t_T_G = t_T_s * s_T_G
graspit_grasp_pose_in_target_frame = tf_conversions.toMsg(
tf_conversions.fromMatrix(graspit_grasp_pose_in_target_frame_matrix))
return graspit_grasp_pose_in_target_frame
def change_end_effector_link(
graspit_grasp_msg_pose, # type: geometry_msgs.msg.Pose
old_link_to_new_link_translation_rotation, # type: tuple of tuples
):
# type: (...) -> geometry_msgs.msg.Pose
"""
:param old_link_to_new_link_translation_rotation: result of listener.lookupTransform((old_link, new_link, rospy.Time(0), timeout=rospy.Duration(1))
:param graspit_grasp_msg_pose: The pose of a graspit grasp message i.e. g.pose
ref_T_nl = ref_T_ol * ol_T_nl
"""
graspit_grasp_pose_for_old_link_matrix = tf_conversions.toMatrix(
tf_conversions.fromMsg(graspit_grasp_msg_pose)
)
old_link_to_new_link_tranform_matrix = tf.TransformerROS().fromTranslationRotation(
old_link_to_new_link_translation_rotation[0],
old_link_to_new_link_translation_rotation[1])
graspit_grasp_pose_for_new_link_matrix = np.dot(graspit_grasp_pose_for_old_link_matrix,
old_link_to_new_link_tranform_matrix) # ref_T_nl = ref_T_ol * ol_T_nl
graspit_grasp_pose_for_new_link = tf_conversions.toMsg(
tf_conversions.fromMatrix(graspit_grasp_pose_for_new_link_matrix))
return graspit_grasp_pose_for_new_link
def handle_pose_check(self, msg):
#TODO: fix the msg input and function calls used in this node
#self.MoveTriad(msg)
#self.env.RemoveKinBody(self.object)
#rospy.loginfo("handle_pose function called")
#rospy.logdebug(msg)
myrot = PyKDL.Rotation.Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)
r,p,y = PyKDL.Rotation.GetRPY(myrot)
myrot = PyKDL.Rotation.RPY(r, -p, -y)
MyFrame = PyKDL.Frame( myrot , PyKDL.Vector(msg.position.x, msg.position.y, msg.position.z) )
FrameMsg1 = tf_conversions.toMsg(MyFrame)
MyMatrix = tf_conversions.toMatrix(MyFrame)
# Current location of RAVE model hand
tmp = self.manip.GetTransform()
#rospy.logdebug("\n\t\t====Current Matrix====\n" + str(tmp))
# Convert the message to a matrix, MyMatrix will be used to find an IK solution
f1 = tf_conversions.fromMsg(msg)
MyMatrix = tf_conversions.toMatrix(f1)
# rospy.logdebug("\n\t\t====Matrix For Soln====\n" + str(MyMatrix))
# Spread values unused while working with trigrip
#if msg.spread or msg.spread == 0:
# self.robot.SetDOFValues([msg.spread],[10])
with self.env: # Check to see if hand position is in collision. If so, reset hand position.
oldJoints = self.robot.GetDOFValues()
#rospy.logdebug("\n\t====Old joints====\n" + str(oldJoints))
try:
IKtype = IkParameterizationType.Transform6D
#rospy.logdebug(IkParameterization(MyMatrix, IKtype))
# Finds all solutions for a given point and orientation in space
solns = self.manip.FindIKSolutions(IkParameterization(MyMatrix, IKtype), filteroptions=IkFilterOptions.CheckEnvCollisions)
#rospy.loginfo("\n\t===solns===\n" + str(solns))
# Holder for a better solution
soln = None
if solns != []:
# Try to find the solution with the least change in joints 1 and 2, tries to minimize jumping
soln = self.find_soln(solns, oldJoints)
# Could be used in lieu of FindIKSolutions, finds just one solution, not necessarily the best
#soln = self.manip.FindIKSolution(IkParameterization(MyMatrix, IKtype), filteroptions=IkFilterOptions.CheckEnvCollisions)
# We have a solution to apply to the robot
if(soln != None):
#rospy.loginfo("\n\t===soln===\n" + str(soln))
self.robot.SetDOFValues(soln, self.manip.GetArmIndices())
#Send Command to the physical robot
if(msg.move):
# Displays solutions used when A button is pressed
rospy.loginfo("\n\t===Joint Solution===\n" + str(soln))
self.send_cmd(soln)
else:
rospy.loginfo("No IK solution found")
#self.env.AddKinBody(self.object)
return osu_ros_adept.srv.PoseCheckResponse(False,[0,0,0])
except e:
rospy.loginfo("IK not available")
#rospy.logdebug(e)
report=CollisionReport()
for link in self.robot.GetLinks():
linkCollision = self.env.CheckCollision(link,report)
if linkCollision:
rospy.logdebug("Link collision " + str(linkCollision) + "Link: " + str(link))
# Wam code, not used with Adept/Trigrip
if "wam0" in str(link): # If looking at the base of the WAM, skip and continue with next link
continue
incollision=self.env.CheckCollision(link,report)
if incollision:# and msg.j_position[1] == 0 and msg.j_position[2] == 0 or incollision and msg.j_position[3] != 0:
rospy.logdebug("Incollision")
# Detects collision, reset joints
self.robot.SetDOFValues(oldJoints)
rospy.loginfo("Collision detected")
#self.env.AddKinBody(self.object)
return PoseCheckResponse(False,[0,0,0])
#self.MoveTriad(msg)
#if not msg.move: # if not moving, reset robot to old Joint values
# self.robot.SetDOFValues(oldJoints)
if msg.get_norm: # Validated: Return norm is useful when using the Interactive markers to control the WAM
vals = MyMatrix[0:3,2]
#self.env.AddKinBody(self.object)
return PoseCheckResponse(True, vals)
#self.env.AddKinBody(self.object)
return PoseCheckResponse(True,[0,0,0])
def pose_to_transform_array(pose):
# TODO: add to Curvox
frame = tf_conversions.fromMsg(pose)
transform = tf_conversions.toMatrix(frame)
return transform
def get_transform_lists(bag_file_name):
init_node()
broadcaster = tf.TransformBroadcaster()
listener = tf.TransformListener()
listener.setUsingDedicatedThread(True)
checkerboard_in_camera_trans = []
wrist_in_body_trans = []
#pdb.set_trace()
camera_in_body_estimate = None
checkerboard_to_wrist_estimate = None
bag = rosbag.Bag(bag_file_name)
tf_header_ids = set()
tf_child_ids = set()
for topic, message, time in bag.read_messages(topics=['tf', '/tf']):
for tf_message_stamped in message.transforms:
tf_message = tf_message_stamped.transform
translation = [
tf_message.translation.x, tf_message.translation.y,
tf_message.translation.z
]
rotation = [
tf_message.rotation.x, tf_message.rotation.y,
tf_message.rotation.z, tf_message.rotation.w
]
broadcaster.sendTransform(translation, rotation, rospy.Time.now(),
tf_message_stamped.child_frame_id,
tf_message_stamped.header.frame_id)
tf_message_stamped.header.stamp = rospy.Time.now()
listener.setTransform(tf_message_stamped, "user")
for tf_message in message.transforms:
if tf_message.header.frame_id not in tf_header_ids:
tf_header_ids.add(tf_message.header.frame_id)
print 'found new frame %s' % tf_message.header.frame_id
if tf_message.child_frame_id not in tf_child_ids:
tf_child_ids.add(tf_message.child_frame_id)
print 'found new child frame %s' % tf_message.child_frame_id
if 'wrist_board' in tf_message.header.frame_id or 'wrist_board_corner' in tf_message.child_frame_id:
print 'found keyframe'
if camera_in_body_estimate is None:
try:
listener.waitForTransform('/camera_link', '/world',
rospy.Time(0),
rospy.Duration(.01))
camera_in_body_tf = listener.lookupTransform(
'/camera_link', '/world', rospy.Time(0))
print "got camera to world transform"
camera_in_body_estimate = tf_conversions.toMatrix(
tf_conversions.fromTf(camera_in_body_tf))
except:
print 'could not get camera to world transform, skipping. Are you sure you ran tf between camera_link and world?'
continue
print "got camera to world estimate"
if checkerboard_to_wrist_estimate is None:
try:
listener.waitForTransform('/wrist_board',
'/wrist_board_corner',
rospy.Time(0),
rospy.Duration(.01))
#(trans, rot) = listener.lookupTransform('/wrist_board','/wrist_board_corner',rospy.Time(0))
#print trans
#print rot
checkerboard_to_wrist_tf = listener.lookupTransform(
'/wrist_board', '/wrist_board_corner',
rospy.Time(0))
#print checkerboard_to_wrist_tf
#raw_input("press a key")
#checkerboard_to_wrist_tf = ((1.75, -0.75, -0.121),(-0.09, -0.04, 0.73, 0.66))
#print 'yinxiao test'
print "got wristboad in wrist"
checkerboard_to_wrist_estimate = tf_conversions.toMatrix(
tf_conversions.fromTf(checkerboard_to_wrist_tf))
except:
print 'could not get wristboard to wrist_board_corner, skipping'
continue
print "got wristboard in wrist estimate"
try:
listener.waitForTransform('/wrist_board', '/camera_link',
rospy.Time(0),
rospy.Duration(.01))
listener.waitForTransform('/wrist_board_corner', '/world',
rospy.Time(0),
rospy.Duration(.1))
checkerboard_tf = listener.lookupTransform(
'/wrist_board', '/camera_link', rospy.Time(0))
#print "got wristboard in camera"
checkerboard_in_camera_trans.append(
tf_conversions.toMatrix(
tf_conversions.fromTf(checkerboard_tf)))
#print "got left wrist in world"
wrist_in_body_tf = listener.lookupTransform(
'/wrist_board_corner', '/world', rospy.Time(0))
wrist_in_body_trans.append(
tf_conversions.toMatrix(
tf_conversions.fromTf(wrist_in_body_tf)))
except:
continue
#print "finished loop"
return checkerboard_in_camera_trans, wrist_in_body_trans, camera_in_body_estimate, checkerboard_to_wrist_estimate
def UpdateRobotJoints(self, msg):
msg2 = None
#rospy.loginfo('command received')
MyTrans = self.manip.GetTransform() # Get the hand transform
#ftest = tf_conversions.fromMsg(MyTrans)
#ftest = tf_conversions.toMatrix(ftest)
f1 = tf_conversions.fromMsg(msg)
MyMatrix = tf_conversions.toMatrix(f1)
# Create message to set robot commands
move_msg = PoseCheck()
# Copy Postition/Orientation from RobotPose msg to PoseCheck srv
move_msg.position = msg.position
move_msg.orientation = msg.orientation
move_msg.spread = 0.0
# Check if A button was pressed, if so initiate robot movement
if(msg.j_position[0]):
move_msg.move = True
else:
move_msg.move = False
# not currently used so set to false by default
move_msg.get_norm = False
# PRE OSU_ROS_ADEPT CODE, NOT USED IN ADEPT/TRIGRIP PROGRAM
# Function to run if using Interactive Markers
if msg.j_position[3] != 0:
msg2 = PoseCheck()
mycog = self.object.GetTransform()
#Add offset of object and subtract a small amount to fix minor Z offset error.
MyMatrix[0:3,3] = MyMatrix[0:3,3] + mycog[0:3,3] - MyMatrix[0:3,2]*.0175
f2 = tf_conversions.fromMatrix(MyMatrix) # Convert the transform to a ROS frame
msg2.pose = tf_conversions.toMsg(f2)
msg2.move = True
#rospy.logdebug("before receiving error")
if msg.j_position[1] == 0 and msg.j_position[2] == 1:
grasp_msg=GraspObject()
grasp_msg.grasp_direction="open"
grasp_msg.spread=False
grasp_msg.get_joint_vals=False
self.Grasp_Object(grasp_msg) # Open the hand
#self.Grasp_Object(grasp_direction="open") # Open the hand
# END NON OSU_ROS_ADEPT CODE
if msg2 is not None: # Move arm to new position
self.handle_pose_check(msg2)
else:
# Sends osu_ros_adept message
self.handle_pose_check(move_msg)
#TODO: Fix this or remove it for the WAM operation. Cannot use robot.SetTransform.
# For Palm grasp option
if msg.j_position[3] == 2:
HandDirection = MyMatrix[0:3,2]
for i in range(0,1000): # loop until the object is touched or limit is reached
MyMatrix[0:3,3] += .0001 * HandDirection
with env:
self.robot.SetTransform(MyMatrix) # Move hand forward
for link in self.robot.GetLinks(): # Check for collisions
incollision=self.env.CheckCollision(link,report)
if incollision:
i = 1000
if i == 1000: # If hand is in collision, break from the loop
break
# Check to see if any of the manipulator buttons have been pressed. Otherwise, move the hand.
if msg.j_position[1] == 1 and msg.j_position[2] == 0:
grasp_msg=GraspObject()
grasp_msg.grasp_direction="close"
grasp_msg.spread=False
grasp_msg.get_joint_vals=False
self.Grasp_Object(grasp_msg) # Open the hand
#self.Grasp_Object(grasp_direction="close") # Open the hand
return
def joints_callback(self, pos):
#rospy.logdebug(pos)
# perform calculations to find new location
newpos = tf_conversions.fromMsg(pos)
newpos = tf_conversions.toMatrix(newpos)
#rospy.logdebug(newpos)
# Create two matrices to modify the RPY matrix. This is due to the fact that
# the BarrettWAM hand is rotated 90deg with respect to the global frame.
# The M.GetRPY function and Rotation.RPY use the X-axis as the roll axis
# whereas the WAM wants to roll about the Z-axis. Must rotate the RPY values
# 90deg, then calculate the new RPY coordinates, then rotate back to OpenRAVE
# coordinates again.
#modMatrix = PyKDL.Rotation.RPY(0, -3.14159/2, 0)
#modMatrix2 = PyKDL.Rotation.RPY(0, 3.14159/2, 0)
oldrot = PyKDL.Rotation(newpos[0,0],newpos[0,1],newpos[0,2],newpos[1,0],newpos[1,1],newpos[1,2],newpos[2,0],newpos[2,1],newpos[2,2])
#oldrot = oldrot * modMatrix # rotating matrix
# Get the RPY values from the new rotated matrix
ftest = PyKDL.Frame(oldrot, PyKDL.Vector(0,0,0))
testrot = ftest.M.GetRPY()
# Calculate the new positions and roll, pitch, yaw
roll = testrot[0] + .025 * (self.buttons[RB_IDX] - self.buttons[LB_IDX])
pitch = testrot[1]
if testrot[1] < -1.5 and self.axes[RIGHT_VERT] > 0 or testrot[1] > 1.5 and self.axes[RIGHT_VERT] < 0:
pitch = testrot[1]
else:
pitch = testrot[1] - .025 * self.axes[RIGHT_VERT]
yaw = testrot[2] + .025 * self.axes[RIGHT_HOR]
z = ((self.axes[LEFT_TRIG]-3) - (self.axes[RIGHT_TRIG]-3))
#
#
# Two different styles of control that move the hand in different ways
#
#
# Old move controls
#newpos[0,3] = newpos[0,3] + .01 * self.axes[LEFT_VERT]
#newpos[1,3] = newpos[1,3] + .01 * self.axes[LEFT_HOR]
#newpos[2,3] = newpos[2,3] + .01 * z
#Weight to influence the speed of simulated robot motion
weight = .01
# "Cockpit" style control method
newpos[0,3] = newpos[0,3] - weight * self.axes[LEFT_HOR] * math.sin(yaw) + weight * self.axes[1] * math.cos(yaw) * math.cos(pitch) + weight * z * math.sin(pitch) * math.cos(yaw)
newpos[1,3] = newpos[1,3] + weight * self.axes[LEFT_VERT] * math.sin(yaw) * math.cos(pitch) + weight * self.axes[LEFT_HOR] * math.cos(yaw) + weight * z * math.sin(pitch) * math.sin(yaw)
newpos[2,3] = newpos[2,3] + weight * z * math.cos(pitch) - weight * self.axes[LEFT_VERT] * math.sin(pitch)
#rospy.logdebug('cos, sin, yaw: {0} ,{1} ,{2} ' .format(math.cos(yaw), math.sin(yaw), yaw) )
# Create a frame for publishing. Make sure to rotate back before creating the frame.
v = PyKDL.Vector(newpos[0,3],newpos[1,3],newpos[2,3])
r1 = PyKDL.Rotation.RPY(roll, pitch, yaw)
#r1 = r1 * modMatrix2
f1 = PyKDL.Frame(r1, v)
#the hand is non-existent. left over code from the wam. May be useful if someone attached a hand
# Get the hand values and calculate the new positions
#spread = pos.j_position[0]
movement = self.buttons[A_IDX]
"""if self.buttons[2] and spread < 3.1:
spread = spread + .01
if self.buttons[0] and spread > 0:
spread = spread - .01
"""
# publish the new position
FrameMsg = tf_conversions.toMsg(f1)
j_pos = array([movement,self.buttons[B_IDX],self.buttons[Y_IDX],0])
#rospy.logdebug(self.buttons)
j_vel = array([0,0,0,0])
MyMsg = RobotPose()
MyMsg.position = FrameMsg.position
MyMsg.orientation = FrameMsg.orientation
MyMsg.j_position = j_pos
MyMsg.j_velocity = j_vel
self.jointpub.publish(MyMsg)
def transform2matrix(transform):
T = tf_conversions.fromTf(transform)
T = tf_conversions.toMatrix(T)
return T
def callback_c2x_tf(data):
"""
Stores the last c2x message, but transforms it beforehand.
Transformation is only for position and velocity data, since this is the only data that is actively used by the
program. The only exception is the plotting of bounding boxes. These will therefore not be rotated correctly, but
their size will be correct anyway.
"""
global history, steps, constant_velo, c2x_offset_x, c2x_offset_y
tracks = data.tracks
head = SimulatedVehicle.create_def_header(frame_id=src_id)
# transformer: tf.TransformerROS
for track in tracks:
time_obj = rospy.Time(0)
x_pos = track.box.center_x
y_pos = track.box.center_y
x_vel = track.box.velocity_x
y_vel = track.box.velocity_y
try:
# Try to transform the point
# Create a point with z=0 for the source frame (out of c2x tracks x/y) coordinate
point = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
# Don't use the current timestamp, use 0 to use the latest available tf data
point.header.stamp = rospy.Time(0)
# Now transform the point using the data
tf_point = transformer.transformPoint(target_frame=dest_id,
ps=point)
# Acquire the transformation matrix for this
tf_mat = tf_c.toMatrix(
tf_c.fromTf(
transformer.lookupTransform(target_frame=dest_id,
source_frame=src_id,
time=rospy.Time(0))))
# print(tf_mat)
# tf_vel stores the transformed velocity
tf_vel = np.dot(tf_mat[0:2, 0:2], [x_vel, y_vel])
# Update the track with the transformed data
track.box.center_x = tf_point.point.x + c2x_offset_x
track.box.center_y = tf_point.point.y + c2x_offset_y
track.box.velocity_x = tf_vel[0]
track.box.velocity_y = tf_vel[1]
# DEBUG
# print(str(track.box.center_x)+"\t"+str(track.box.center_y))
# print("steps: "+str(steps)+"\tvelx: "+str(point_vel.point.x)+" vely: "+str(point_vel.point.y))
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
print(e)
pass
# Now change the frame_id to dest_id, since the tf was already performed
data.header.frame_id = dest_id
c2x.append(data)
history.add(
"c2x_0",
data.tracks) # Add the data to the history object under the name c2x_0
# The following block adds several fake measurements to the c2x tracking that are time delayed to the original
# measurement from the current timestep
# They also have changed x/y data based on velocity, so that the track doesn't need to be reused later on with the
# outdated coordinates and instead can work with "updated" coordinates for time steps that are not included in the
# real c2x messages.
add_points = 4 # how many "fake" points should be added between this and the next measuremen
# Currently the number and timing of "fake" points are fixed. They are selected to fit the data sets 1&2, but
# in general it can be better to use previous data to estimate the time difference and number of points
if len(data.tracks
) > 0: # only do the following if the track contained something
for i in range(add_points):
# add 4 more measurements, each based on the data, but with manipulated time and position based on velocity
fake_data = copy.deepcopy(data) # Create a copy of the data
# now change the timestamp of this new data
# c2x data arrives every 0.2 seconds, so to fit an additional 4 measurements
time_shift = rospy.Duration(0,
40000000) # increase by 1/20 of a sec
# this would need to change if it should automatically fit to data of other data sets.
fake_data.header.stamp = fake_data.header.stamp + time_shift
for track in fake_data.tracks:
track.box.header.stamp = track.box.header.stamp + time_shift
track.box.center_x += constant_velo * track.box.velocity_x * (
i + 1)
track.box.center_y += constant_velo * track.box.velocity_y * (
i + 1)
c2x.append(fake_data)
history.add("c2x_0", fake_data.tracks)
steps = 0
def get_transform_lists(bag_file_name):
init_node()
broadcaster = tf.TransformBroadcaster()
listener = tf.TransformListener()
listener.setUsingDedicatedThread(True)
checkerboard_in_camera_trans = []
wrist_in_body_trans = []
#pdb.set_trace()
camera_in_body_estimate = None
checkerboard_to_wrist_estimate = None
bag = rosbag.Bag(bag_file_name)
tf_header_ids = set()
tf_child_ids = set()
for topic, message, time in bag.read_messages(topics=['tf', '/tf']):
for tf_message_stamped in message.transforms:
tf_message = tf_message_stamped.transform
translation = [tf_message.translation.x, tf_message.translation.y, tf_message.translation.z]
rotation = [tf_message.rotation.x, tf_message.rotation.y, tf_message.rotation.z, tf_message.rotation.w]
broadcaster.sendTransform( translation, rotation, rospy.Time.now(), tf_message_stamped.child_frame_id, tf_message_stamped.header.frame_id)
tf_message_stamped.header.stamp = rospy.Time.now()
listener.setTransform(tf_message_stamped,"user")
for tf_message in message.transforms:
if tf_message.header.frame_id not in tf_header_ids:
tf_header_ids.add(tf_message.header.frame_id)
print 'found new frame %s' % tf_message.header.frame_id
if tf_message.child_frame_id not in tf_child_ids:
tf_child_ids.add(tf_message.child_frame_id)
print 'found new child frame %s' % tf_message.child_frame_id
if 'wrist_board' in tf_message.header.frame_id or 'wrist_board_corner' in tf_message.child_frame_id:
print 'found keyframe'
if camera_in_body_estimate is None:
try:
listener.waitForTransform('/camera_link','/world',rospy.Time(0), rospy.Duration(.01))
camera_in_body_tf = listener.lookupTransform('/camera_link','/world',rospy.Time(0))
print "got camera to world transform"
camera_in_body_estimate = tf_conversions.toMatrix(tf_conversions.fromTf(camera_in_body_tf))
except:
print 'could not get camera to world transform, skipping. Are you sure you ran tf between camera_link and world?'
continue
print "got camera to world estimate"
if checkerboard_to_wrist_estimate is None:
try:
listener.waitForTransform('/wrist_board','/wrist_board_corner',rospy.Time(0), rospy.Duration(.01))
#(trans, rot) = listener.lookupTransform('/wrist_board','/wrist_board_corner',rospy.Time(0))
#print trans
#print rot
checkerboard_to_wrist_tf = listener.lookupTransform('/wrist_board','/wrist_board_corner',rospy.Time(0))
#print checkerboard_to_wrist_tf
#raw_input("press a key")
#checkerboard_to_wrist_tf = ((1.75, -0.75, -0.121),(-0.09, -0.04, 0.73, 0.66))
#print 'yinxiao test'
print "got wristboad in wrist"
checkerboard_to_wrist_estimate = tf_conversions.toMatrix(tf_conversions.fromTf(checkerboard_to_wrist_tf))
except:
print 'could not get wristboard to wrist_board_corner, skipping'
continue
print "got wristboard in wrist estimate"
try:
listener.waitForTransform('/wrist_board','/camera_link',rospy.Time(0), rospy.Duration(.01))
listener.waitForTransform('/wrist_board_corner','/world',rospy.Time(0), rospy.Duration(.1))
checkerboard_tf = listener.lookupTransform('/wrist_board','/camera_link',rospy.Time(0))
#print "got wristboard in camera"
checkerboard_in_camera_trans.append(tf_conversions.toMatrix(tf_conversions.fromTf(checkerboard_tf)))
#print "got left wrist in world"
wrist_in_body_tf = listener.lookupTransform('/wrist_board_corner','/world',rospy.Time(0))
wrist_in_body_trans.append(tf_conversions.toMatrix(tf_conversions.fromTf(wrist_in_body_tf)))
except:
continue
#print "finished loop"
return checkerboard_in_camera_trans, wrist_in_body_trans, camera_in_body_estimate, checkerboard_to_wrist_estimate
def get_camera_tf(self):
camera_frame_id = '/%s_camera_frame_id'%(self.arm)
tf_listener.waitForTransform('/base',camera_frame_id, rospy.Time(0), rospy.Duration(2))
image_tf = tf_listener.lookupTransform('/base',image.header.frame_id, rospy.Time(0))
self.camera_transform = tf_conversions.toMatrix((tf_conversions.fromTf(image_tf)))
