本文整理匯總了Python中Transform.rosq2rot方法的典型用法代碼示例。如果您正苦於以下問題：Python Transform.rosq2rot方法的具體用法？Python Transform.rosq2rot怎麽用？Python Transform.rosq2rot使用的例子？那麽, 這裏精選的方法代碼示例或許可以為您提供幫助。您也可以進一步了解該方法所在類Transform的用法示例。
在下文中一共展示了Transform.rosq2rot方法的2個代碼示例，這些例子默認根據受歡迎程度排序。您可以為喜歡或者感覺有用的代碼點讚，您的評價將有助於係統推薦出更棒的Python代碼示例。
示例1: to_array
# 需要導入模塊: import Transform [as 別名]
# 或者: from Transform import rosq2rot [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]);示例2: lookupTransform
# 需要導入模塊: import Transform [as 別名]
# 或者: from Transform import rosq2rot [as 別名]
return listener.transformPointCloud(target_frame, point_cloud);
def lookupTransform(dstFrame, srcFrame, stamp=rospy.Time(), waitTimeout=0.1):
''' returns the 4x4 transform for the given frames '''
global listener
try:
listener.waitForTransform(dstFrame, srcFrame, stamp, rospy.Duration(waitTimeout))
(t,q) = listener.lookupTransform(dstFrame, srcFrame, stamp);
except (tf.Exception, tf.ExtrapolationException), ex:
# transform failed, use most current transform
rospy.logwarn(__name__ + ': unable to get transform : %s' % ex);
rospy.logwarn(__name__ + ': proceeding with most recent transform');
(t,q) = listener.lookupTransform(dstFrame, srcFrame, rospy.Time());
return np.dot(tr.trans(t), tr.rosq2rot(q));
def get_frame_id(msg):
'''
returns the frame id for the input
'' if no frame is specified
'''
if hasattr(msg, 'header'):
return msg.header.frame_id;
elif hasattr(msg, 'frame_id'):
return obj.frame_id;
else:
return '';