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