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
