def parseCameraInfo(self, cameraInfoYamlDict):
self.cameraInfo = dict()
for cameraType in self.cameraTypes:
d = cameraInfoYamlDict[cameraType]
data = dict()
data['raw_data'] = d
# the optical frame is defined as here http://www.ros.org/reps/rep-0103.html#id21 and follows
# the opencv convention https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
# z forward, x right, y down
opticalToLinkVtk = spartanUtils.transformFromPose(
d['extrinsics']['transform_to_reference_link'])
# body is the body frame of the camera as defined here http://www.ros.org/reps/rep-0103.html#id21
# x forward, y left, z up
bodyToLinkVtk = CameraTransformPublisher.transformOpticalFrameToBodyFrame(
opticalToLinkVtk)
bodyToLinkPoseDict = spartanUtils.poseFromTransform(bodyToLinkVtk)
bodyToLink = rosUtils.ROSTransformMsgFromPose(bodyToLinkPoseDict)
cameraToLinkStamped = geometry_msgs.msg.TransformStamped()
cameraToLinkStamped.transform = bodyToLink
cameraToLinkStamped.child_frame_id = self.cameraName + '_' + cameraType + "_frame"
cameraToLinkStamped.header.frame_id = d['extrinsics'][
'reference_link_name']
data['camera_to_link_transform_stamped'] = cameraToLinkStamped
if self.debug:
opticalToLinkPoseDict = spartanUtils.poseFromTransform(
opticalToLinkVtk)
opticalToLink = rosUtils.ROSTransformMsgFromPose(
opticalToLinkPoseDict)
opticalToLinkStamped = geometry_msgs.msg.TransformStamped()
opticalToLinkStamped.transform = opticalToLink
opticalToLinkStamped.child_frame_id = self.cameraName + '_' + cameraType + "_frame_debug"
opticalToLinkStamped.header.frame_id = d['extrinsics'][
'reference_link_name']
data[
'optical_to_link_transform_stamped_debug'] = opticalToLinkStamped
self.cameraInfo[cameraType] = data
rospy.loginfo('finished parsing data for camera %s',
cameraToLinkStamped.header.frame_id)
def vtkFrameToPoseMsg(self, vtkFrame):
poseDict = spartanUtils.poseFromTransform(vtkFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def makePoseStampedFromGraspFrame(self, graspFrame):
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def captureDataAtPose(self, poseName=""):
data = dict()
data['pose_name'] = poseName
data['pose'] = self.robotService.getPose().tolist()
data['hand_frame_name'] = self.handFrame
handTransform = self.robotSystem.robotStateModel.getLinkFrame(self.handFrame)
data['hand_frame'] = spartanUtils.poseFromTransform(handTransform)
data['utime'] = self.robotSystem.robotStateJointController.lastRobotStateMessage.utime
return data
def moveToFrame(self, graspFrame):
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
self.poseStamped = poseStamped
self.robotService.moveToCartesianPosition(poseStamped,
self.config['grasp_speed'])
def testMoveToFrame(self):
pos = [0.51148583, 0.0152224, 0.50182436]
quat = [0.68751512, 0.15384615, 0.69882778, -0.12366916]
targetFrame = transformUtils.transformFromPose(pos, quat)
poseDict = spartanUtils.poseFromTransform(targetFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
self.poseStamped = poseStamped
self.robotService.moveToCartesianPosition(poseStamped, 30)
def captureCurrentRobotAndImageData(self, captureRGB=False, captureIR=False, prefix=None):
assert prefix is not None
data = dict()
data['joint_positions'] = self.robotService.getPose().tolist()
data['hand_frame_name'] = self.handFrame
handTransform = self.robotSystem.robotStateModel.getLinkFrame(self.handFrame)
data['hand_frame'] = spartanUtils.poseFromTransform(handTransform)
rosTime = rospy.Time.now()
data['ros_timestamp'] = rosTime.to_sec()
data['images'] = dict()
imgTopics = dict()
if captureRGB:
imgTopics['rgb'] = self.config['rgb_raw_topic']
if captureIR:
imgTopics['ir'] = self.config['ir_raw_topic']
for key, topic in imgTopics.iteritems():
imageFilename = str(prefix) + "_" + key + "." + self.config['filename_extension']
fullImageFilename = os.path.join(self.calibrationFolderName, imageFilename)
encoding = None
if key == "rgb":
encoding = self.config['encoding'][key]
elif key == "ir":
encoding = self.config['encoding'][key]
else:
raise ValueError("I only know how to handle ir and rgb")
self.saveSingleImage(topic, fullImageFilename, encoding)
# todo: sync this timeout with some variable
self.displayChessboardDetection(fullImageFilename, duration=1.5)
singleImgData = dict()
singleImgData['filename'] = imageFilename
data['images'][key] = singleImgData
return data
def makePoseStampedFromGraspFrame(self, graspFrame):
"""
Make PoseStamped message for the end effector frame from a given grasp frame
:param graspFrame: vtkTransform of the gripper frame
:return : pose of the end-effector for that grasp frame location
:rtype : geometry_msgs/PoseStamped
"""
iiwaLinkEEFrame = self.getIiwaLinkEEFrameFromGraspFrame(graspFrame)
poseDict = spartanUtils.poseFromTransform(iiwaLinkEEFrame)
poseMsg = rosUtils.ROSPoseMsgFromPose(poseDict)
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose = poseMsg
poseStamped.header.frame_id = "base"
return poseStamped
def captureCurrentRobotAndImageData(self,
captureRGB=False,
captureIR=False):
data = dict()
data['joint_positions'] = self.robotService.getPose().tolist()
data['hand_frame_name'] = self.handFrame
handTransform = self.robotSystem.robotStateModel.getLinkFrame(
self.handFrame)
data['hand_frame'] = spartanUtils.poseFromTransform(handTransform)
rosTime = rospy.Time.now()
data['ros_timestamp'] = rosTime.to_sec()
data['images'] = dict()
imgTopics = dict()
if captureRGB:
imgTopics['rgb'] = self.config['rgb_raw_topic']
if captureIR:
imgTopics['ir'] = self.config['ir_raw_topic']
for key, topic in imgTopics.iteritems():
imageFilename = str(data['ros_timestamp']) + "_" + key + ".jpeg"
fullImageFilename = os.path.join(self.calibrationFolderName,
imageFilename)
encoding = None
if key == "rgb":
encoding = 'bgr8'
elif key == "ir":
encoding = 'mono16'
else:
raise ValueError("I only know how to handle ir and rgb")
self.saveSingleImage(topic, fullImageFilename, encoding)
singleImgData = dict()
singleImgData['filename'] = imageFilename
data['images'][key] = singleImgData
return data
