本文整理汇总了Python中Transform.rospose2tr方法的典型用法代码示例。如果您正苦于以下问题：Python Transform.rospose2tr方法的具体用法？Python Transform.rospose2tr怎么用？Python Transform.rospose2tr使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform的用法示例。
在下文中一共展示了Transform.rospose2tr方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: get_bbox
# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import rospose2tr [as 别名]
def get_bbox(pc, pose=None):
'''
get the bounding box for the point cloud
if a pose is provided then it will find the bbox
for the data aligned to that pose
return: bbox array
|xmin, xmax|
|ymin, ymax|
|zmin, zmax|
'''
pts = ru.to_array(pc);
if pose != None:
ps = ru.to_PoseStamped(pose);
ps_wrtPC = ru.transformPose(ru.get_frame_id(pc), ps);
T = tr.rospose2tr(ps_wrtPC.pose);
pts = np.dot(np.linalg.inv(T), np.vstack((pts.T, np.ones(pts.shape[0])))).T;
pmin = np.min(pts[:,:3], axis=0);
pmax = np.max(pts[:,:3], axis=0);
bbox = np.array([pmin, pmax]).T;
return bbox;示例2: to_array
# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import rospose2tr [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]);