本文整理汇总了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 '';