本文整理汇总了Python中Transform.kdlr2tr方法的典型用法代码示例。如果您正苦于以下问题：Python Transform.kdlr2tr方法的具体用法？Python Transform.kdlr2tr怎么用？Python Transform.kdlr2tr使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform的用法示例。
在下文中一共展示了Transform.kdlr2tr方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: to_array
# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import kdlr2tr [as 别名]
def to_array(*args):
'''
converts the input to numpy array format
supported input:
- ROS PointCloud message
- ROS Pose/Stamped message
- ROS Quaternion/Stamped message
- ROS Point/Stamped message
- ROS Vector3/Stamped message
- ROS Image message (returned in RGB format)
- kdl frame
- kdl Rotation
- kdl Vector
- iterable
NOTE: point clouds are returned as Nx3 arrays
NOTE: Poses and Rotations will be returned in 4x4 matrix format
'''
# numpy array
if (len(args) > 0 and type(args[0]) == np.ndarray):
return args[0].copy();
# PointCloud
elif (type(args[0]) == sensor_msgs.PointCloud):
return np.array([(p.x, p.y, p.z) for p in args[0].points]);
# PoseStamped
elif (type(args[0]) == geometry_msgs.PoseStamped):
return tr.rospose2tr(args[0].pose);
# Pose
elif (type(args[0]) == geometry_msgs.Pose):
return tr.rospose2tr(args[0]);
# QuaternionStamped
elif (type(args[0]) == geometry_msgs.QuaternionStamped):
return tr.rosq2rot(args[0].quaternion);
# Quaternion
elif (type(args[0]) == geometry_msgs.Quaternion):
return tr.rosq2rot(args[0]);
# Vector3Stamped
elif (type(args[0]) == geometry_msgs.Vector3Stamped):
return np.array([args[0].vector.x, args[0].vector.y, args[0].vector.z]);
# Vector3
elif (type(args[0]) == geometry_msgs.Vector3):
return np.array([args[0].x, args[0].y, args[0].z]);
# PointStamped
elif (type(args[0]) == geometry_msgs.PointStamped):
return np.array([args[0].point.x, args[0].point.y, args[0].point.z]);
# Point
elif (type(args[0]) == geometry_msgs.Point
or type(args[0]) == geometry_msgs.Point32):
return np.array([args[0].x, args[0].y, args[0].z]);
# ROS Image
elif (type(args[0]) == sensor_msgs.Image):
bridge = cv_bridge.CvBridge();
cvimg = bridge.imgmsg_to_cv(args[0]);
return np.asarray(cvimg);
# kdl Frame
elif (type(args[0]) == kdl.Frame):
return tr.kdlf2tr(args[0]);
# kdl Rotation
elif (type(args[0]) == kdl.Rotation):
return tr.kdlr2tr(args[0]);
# kdl Vector
elif (type(args[0]) == kdl.Vector):
return np.array([args[0].x(), args[0].y(), args[0].z()]);
# iterable
elif hasattr(args[0], '__iter__'):
if (len(args[0]) > 0
and type(args[0][0]) == geometry_msgs.Point or type(args[0][0]) == geometry_msgs.Point32):
return np.array([(p.x, p.y, p.z) for p in args[0]]);
else:
return np.array(args[0]);