def _ravenStateCallback(self,msg): if self.errorCorrectionMode == 'NONE': for arm in self.arms: if self.estimatedPose.has_key(arm): if tfx.stamp(msg.header.stamp) - self.estimatedPose[arm][0].stamp < 0.2: return arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0] joints = dict((j.type,j.position) for j in arm_msg.joints) fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints,stamp=msg.header.stamp) estPose = tfx.pose(fwdArmKinPose,frame='0_link',stamp = msg.header.stamp) estPose.frame = '0_link' # TEMP self.estimatedPose[arm] = (estPose,False) self.est_pose_pub[arm].publish(estPose.msg.PoseStamped()) elif self.errorCorrectionMode == 'SYS': for arm in self.arms: if self.estimatedPose.has_key(arm): if tfx.stamp(msg.header.stamp) - self.estimatedPose[arm][0].stamp < 0.2: return arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0] joints = dict((j.type,j.position) for j in arm_msg.joints) fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints,stamp=msg.header.stamp) estPose = tfx.pose(self.sys_error[arm] * fwdArmKinPose,frame='0_link',stamp = msg.header.stamp) estPose.frame = '0_link' # TEMP self.estimatedPose[arm] = (estPose,False) self.est_pose_pub[arm].publish(estPose.msg.PoseStamped()) elif self.errorCorrectionMode == 'PRE_POST': if self.calcPose: prevTime = self.calcPose.values()[0].stamp if tfx.stamp(msg.header.stamp).seconds - prevTime.seconds < 0.2: return for arm in self.arms: arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0] #self.calcPose[arm] = tfx.pose(arm_msg.tool.pose,header=msg.header) joints = dict((j.type,j.position) for j in arm_msg.joints) fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints) self.calcPose[arm] = (fwdArmKinPose.as_tf() * self.calcPosePostAdjustment[arm]).as_pose(stamp=msg.header.stamp) self._updateEstimatedPose(arm)
def _ravenStateCallback(self,msg): if self.calcPose: prevTime = self.calcPose.values()[0].stamp if tfx.stamp(msg.header.stamp).seconds - prevTime.seconds < 0.2: return for arm in self.arms: arm_msg = [msg_arm for msg_arm in msg.arms if msg_arm.name == arm][0] #self.calcPose[arm] = tfx.pose(arm_msg.tool.pose,header=msg.header) joints = dict((j.type,j.position) for j in arm_msg.joints) fwdArmKinPose, grasp = kinematics.fwdArmKin(arm,joints) self.calcPose[arm] = (fwdArmKinPose.as_tf() * self.calcPosePostAdjustment[arm]).as_pose(stamp=msg.header.stamp) self._updateEstimatedPose(arm)
def tool_poses(self, msg=False, tf=None): if 'tool_pose' not in self.arms.columns.levels[1]: return None poses = {} pose_data = self.arms.xs('tool_pose', axis=1, level='field') arm_names = pose_data.columns.levels[0] if tf: tf = tfx.transform(tf, pose=False) for arm in arm_names: arm_pose_data = pose_data[arm] arm_poses = [] stamps = [] for i in xrange(len(arm_pose_data)): if np.isnan(arm_pose_data.ix[i, 'x']): continue translation = arm_pose_data.ix[i, ['x', 'y', 'z']] rotation = arm_pose_data.ix[i, ['qx', 'qy', 'qz', 'qw']] if msg: pose = PoseStamped() pose.header.frame_id = frame = self.robot.ix[i, 'frame'] pose.header.stamp = tfx.stamp(arm_pose_data.index[i]).ros pose.pose.position.x = translation[0] pose.pose.position.y = translation[1] pose.pose.position.z = translation[2] pose.pose.orientation.x = rotation[0] pose.pose.orientation.y = rotation[1] pose.pose.orientation.z = rotation[2] pose.pose.orientation.w = rotation[3] else: pose = tfx.pose(translation, rotation, stamp=arm_pose_data.index[i], frame=self.robot.ix[i, 'frame'], name=arm) if tf: pose = tf * pose arm_poses.append(pose) stamps.append(arm_pose_data.index[i]) if not msg: arm_poses = pd.Series(arm_poses, index=stamps, name=arm) poses[arm] = arm_poses if not msg: return pd.DataFrame(poses) else: return poses
def tool_poses(self,msg=False,tf=None): if 'tool_pose' not in self.arms.columns.levels[1]: return None poses = {} pose_data = self.arms.xs('tool_pose',axis=1,level='field') arm_names = pose_data.columns.levels[0] if tf: tf = tfx.transform(tf,pose=False) for arm in arm_names: arm_pose_data = pose_data[arm] arm_poses = [] stamps = [] for i in xrange(len(arm_pose_data)): if np.isnan(arm_pose_data.ix[i,'x']): continue translation = arm_pose_data.ix[i,['x','y','z']] rotation = arm_pose_data.ix[i,['qx','qy','qz','qw']] if msg: pose = PoseStamped() pose.header.frame_id = frame=self.robot.ix[i,'frame'] pose.header.stamp = tfx.stamp(arm_pose_data.index[i]).ros pose.pose.position.x = translation[0] pose.pose.position.y = translation[1] pose.pose.position.z = translation[2] pose.pose.orientation.x = rotation[0] pose.pose.orientation.y = rotation[1] pose.pose.orientation.z = rotation[2] pose.pose.orientation.w = rotation[3] else: pose = tfx.pose(translation,rotation,stamp=arm_pose_data.index[i],frame=self.robot.ix[i,'frame'],name=arm) if tf: pose = tf * pose arm_poses.append(pose) stamps.append(arm_pose_data.index[i]) if not msg: arm_poses = pd.Series(arm_poses,index=stamps,name=arm) poses[arm] = arm_poses if not msg: return pd.DataFrame(poses) else: return poses
def _foam_points_cb(self,msg): if self.ignore: return if rospy.is_shutdown(): return with self.lock: self._filterCenterHistory() t = tfx.stamp(msg.header.stamp) all_pts = tuple([tfx.convertToFrame(tfx.point(pt,frame=msg.header.frame_id),raven_constants.Frames.Link0) for pt in msg.points])#,header=msg.header self.centerHistory[t] = all_pts pts = [] for pt in all_pts: for _, estPose in self.estPose.iteritems(): if estPose is not None: tfxPoint = tfx.convertToFrame(tfx.point(pt,frame=msg.header.frame_id),estPose.frame) if estPose.position.distance(tfxPoint) < self.estPoseExclusionRadius: break else: pts.append(pt) self.currentCenters = pts self.newCenters = True