本文整理匯總了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]);