def cv2ros(rvecs,tvecs):
"""Transform pose from openCV solvePNP output format to ROS TransformRTStampedWithHeader format.
Args:
rvecs: in format [rx, ry, rz] units : radians
tvecs: in format [tx, ty, tz] units : meters
(type: list)
Returns:
An HTM (type: TransformRTStampedWithHeader)
"""
# ROS pose
ros_pose = TransformRTStampedWithHeader()
# ROS position
ros_pose.transform.translation.x = tvecs[0]
ros_pose.transform.translation.y = tvecs[1]
ros_pose.transform.translation.z = tvecs[2]
# Ros orientation
angle = sqrt(rvecs[0] ** 2 + rvecs[1] ** 2 + rvecs[2] ** 2)
direction = [i / angle for i in rvecs[0:3]]
np_T = tf.rotation_matrix(angle, direction)
np_q = tf.quaternion_from_matrix(np_T)
ros_pose.transform.rotation.x = np_q[0]
ros_pose.transform.rotation.y = np_q[1]
ros_pose.transform.rotation.z = np_q[2]
ros_pose.transform.rotation.w = np_q[3]
return ros_pose
def __init__(self):
self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
100, 1e-16)
np.set_printoptions(precision=16)
self.robotSpeed = 0.0
self.cam_mtx = np.array(
[[9.68727469e+02, 0.00000000e+00, 1.02687381e+03],
[0.00000000e+00, 9.68978580e+02, 7.74022724e+02], [0, 0, 1]],
np.float32)
self.cam_dist = np.array(
[0.07588704, 0.04285212, -0.00064595, 0.00132813, -0.25794438],
np.float32)
self.objp = self.createObjectpoints(5, 8, 0.04)
self.pose = TransformRTStampedWithHeader()
rospy.init_node('chkbd_pose_estimation', anonymous=True)
self.pub = rospy.Publisher('kin_chkbd_rt_tf',
TransformRTStampedWithHeader,
queue_size=1)
rospy.Subscriber('/rgb/image_raw',
Image,
self.transform_from_image,
queue_size=1)
#rospy.Subscriber('joint_states', JointState, self.update_speed, queue_size = 3)
rospy.spin()
def np2ros_RTStampedWithHeader(np_pose):
"""Transform pose from np.array format to ROS Pose format.
Args:
np_pose: A pose in np.array format (type: np.array)
Returns:
An HTM (type: Pose).
"""
# ROS pose
ros_pose = TransformRTStampedWithHeader()
# ROS position
ros_pose.transform.translation.x = np_pose[0, 3]
ros_pose.transform.translation.y = np_pose[1, 3]
ros_pose.transform.translation.z = np_pose[2, 3]
# ROS orientation
np_q = tf.quaternion_from_matrix(np_pose)
ros_pose.transform.rotation.x = np_q[0]
ros_pose.transform.rotation.y = np_q[1]
ros_pose.transform.rotation.z = np_q[2]
ros_pose.transform.rotation.w = np_q[3]
return ros_pose
def __init__(self):
self.pose = TransformRTStampedWithHeader()
rospy.init_node('chkbd_pose_estimation', anonymous=True)
self.pub = rospy.Publisher('kin_chkbd_rt_tf',
TransformRTStampedWithHeader,
queue_size=1)
rospy.Subscriber('/pose',
PoseStamped,
self.transform_from_image,
queue_size=1)
rospy.spin()