def compute_fk_client(self, group, joint_values, links):
rospy.wait_for_service('compute_fk')
try:
compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = group.get_pose_reference_frame()
rs = RobotState()
rs.joint_state.header = header
rs.joint_state.name = group.get_active_joints()
rs.joint_state.position = joint_values
res = compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
评论列表
文章目录