本文整理汇总了Python中tf.ConnectivityException方法的典型用法代码示例。如果您正苦于以下问题:Python tf.ConnectivityException方法的具体用法?Python tf.ConnectivityException怎么用?Python tf.ConnectivityException使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf
的用法示例。
在下文中一共展示了tf.ConnectivityException方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: tick
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def tick(self):
try:
(trans,rot) = self.listener.lookupTransform('/map', '/base_link', rospy.Time(0))
msg = self.get_link_state(link_name="base_link")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return False
self.pose = msg.link_state.pose
self.trans, self.rot = trans, rot
#quaternion = (rot[0], rot[1], rot[2], rot[3])
orientation = self.pose.orientation
quaternion = (orientation.x, orientation.y, orientation.z,
orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.roll = euler[0]
self.pitch = euler[1]
self.yaw = euler[2]
return True
示例2: _wait_for_transform_tf2
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def _wait_for_transform_tf2(self,
target_frame, source_frame,
time, timeout):
try:
ret = self.tf_listener.can_transform(
target_frame, source_frame, time, timeout, True)
if ret[0] > 0:
return True
else:
raise Exception(ret[1])
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException,
tf2_ros.TransformException,
rospy.exceptions.ROSTimeMovedBackwardsException):
return False
示例3: _lookup_transform_tf1
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def _lookup_transform_tf1(self, target_frame, source_frame, time, timeout):
self._wait_for_transform_tf1(target_frame, source_frame, time, timeout)
try:
res = self.tf_listener.lookupTransform(
target_frame, source_frame, time)
if time.is_zero():
time = self.tf_listener.getLatestCommonTime(
target_frame, source_frame)
except (tf.LookupException,
tf.ConnectivityException,
tf.ExtrapolationException,
rospy.exceptions.ROSTimeMovedBackwardsException):
return False
ret = geometry_msgs.msg.TransformStamped()
ret.header.frame_id = target_frame
ret.header.stamp = time
ret.child_frame_id = source_frame
ret.transform.translation.x = res[0][0]
ret.transform.translation.y = res[0][1]
ret.transform.translation.z = res[0][2]
ret.transform.rotation.x = res[1][0]
ret.transform.rotation.y = res[1][1]
ret.transform.rotation.z = res[1][2]
ret.transform.rotation.w = res[1][3]
return ret
示例4: get_position
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def get_position(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(*trans)
示例5: get_odom
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
示例6: get_odom_angle
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def get_odom_angle(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
# Convert the rotation from a quaternion to an Euler angle
return quat_to_angle(Quaternion(*rot))
示例7: _image_callback
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def _image_callback(self, image_msg):
try:
self.quad_to_obj_pos, self.quad_to_obj_rot = self.listener.lookupTransform('bebop', 'car', image_msg.header.stamp)
self.quad_pos, self.quad_rot = self.listener.lookupTransform('world', 'bebop', image_msg.header.stamp)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
self.image = self.cv_bridge.imgmsg_to_cv2(image_msg)
示例8: getErrorToGoal
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def getErrorToGoal(self):
# Look up flie position, convert to 2d, publish, return error
if self.wandControl:
#update goal using wand
try:
(t,r) = self.lookupTransformInWorld(frame='/wand', forceRealtime=True)
e = tf.transformations.euler_from_quaternion(r)
self.goal= [t[0], t[1], t[2], degrees(e[2]-e[0])]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo('Could not find wand')
# Publish GOAL from world frame
q = tf.transformations.quaternion_from_euler(0, 0, radians(self.goal[3]))
self.pub_tf.sendTransform((self.goal[0],self.goal[1],self.goal[2]), q, rospy.Time.now(), "/goal", "/world")
# Look up 6D flie pose, publish 3d+yaw pose
(trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', poseNoiseMeters=0.0)
euler = tf.transformations.euler_from_quaternion(rot)
q = tf.transformations.quaternion_from_euler(0, 0, euler[2])
self.pub_tf.sendTransform(trans, q, rospy.Time.now(), "/cf_gt2d", "/world")
# Look up error goal->cf
(trans,rot) = self.sub_tf.lookupTransform('/cf_gt2d', '/goal', rospy.Time(0))
#self.pub_tf.sendTransform(trans, rot, rospy.Time.now(), "/goalCF", "/cf_gt2d")
euler = tf.transformations.euler_from_quaternion(rot)
return trans[0], trans[1], trans[2], degrees(euler[2])
示例9: setGoalFromCurrent
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def setGoalFromCurrent(self,config={}):
try:
(trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', forceRealtime=True)
euler = tf.transformations.euler_from_quaternion(rot)
config['x'],config['y'],config['z'],config['rz']= trans[0], trans[1], trans[2], degrees(euler[2])
rospy.loginfo('Updated goal state: [%.2f %.2f %.2f %.1f]', trans[0], trans[1], trans[2], degrees(euler[2]))
self.goal = [config['x'],config['y'],config['z'],config['rz']]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('Could not look up transform world -> cf_gt')
return config
示例10: getCameraGoal
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def getCameraGoal(self):
""" Set the goal in camera coordinates from world coordinates """
now = rospy.Time.now()
try:
self.sub_tf.waitForTransform("/camera_depth_optical_frame","/goal", now, rospy.Duration(0.1))
(self.goal, rot) = self.sub_tf.lookupTransform("/camera_depth_optical_frame","/goal", now)
self.goal = list(self.goal)
#self.pub_tf.sendTransform(self.goal, rot, now, "/goalCam", "/camera_depth_optical_frame")#TODO: debugging only
return self.goal
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('Could not get Goal position in the camera frame')
pass
示例11: getGTDist
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def getGTDist(self):
now = rospy.Time.now()
try:
self.sub_tf.waitForTransform("/camera_depth_optical_frame","/cf_gt", now, rospy.Duration(0.1))
(t, r) = self.sub_tf.lookupTransform("/camera_depth_optical_frame","/cf_gt", now )
return list(t)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('Could not get ground truth position in the camera frame')
pass
示例12: getKinectPoint
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def getKinectPoint(self):
now = rospy.Time.now()
try:
self.sub_tf.waitForTransform("/world","/camera_depth_optical_frame", now, rospy.Duration(0.1))
(t, r) = self.sub_tf.lookupTransform("/world","/camera_depth_optical_frame", now)
return list(t), list(r)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('Could not get ground kinect position in the world')
pass
示例13: get_robot_pose
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def get_robot_pose(self, event):
try:
(pos, rot) = self.tf_listener.lookupTransform('base_link', 'tool_link', rospy.Time(0))
self.position = pos
self.rpy = get_rpy_from_quaternion(rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("TF fail")
示例14: _get_tf_transform
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def _get_tf_transform(self, tf_listener, tgt_frame, src_frame):
"""
Uses ROS TF to lookup the current transform from tgt_frame to src_frame,
If the returned transform is applied to data, it will transform data in
the src_frame into the tgt_frame
:param tgt_frame: target frame
:param src_frame: source frame
:type tgt_frame: string
:type src_frame: string
:returns: trans, translation (x,y,z)
:rtype: tuple (of floats)
:returns: quat, rotation as a quaternion (x,y,z,w)
:rtype: tuple (of floats)
"""
try:
tf_listener.waitForTransform(
tgt_frame, src_frame, rospy.Time(0), rospy.Duration(3)
)
(trans, quat) = tf_listener.lookupTransform(
tgt_frame, src_frame, rospy.Time(0)
)
except (
tf.LookupException,
tf.ConnectivityException,
tf.ExtrapolationException,
):
raise RuntimeError(
"Cannot fetch the transform from"
" {0:s} to {1:s}".format(tgt_frame, src_frame)
)
return trans, quat
示例15: get_tf_transform
# 需要导入模块: import tf [as 别名]
# 或者: from tf import ConnectivityException [as 别名]
def get_tf_transform(tf_listener, tgt_frame, src_frame):
"""
Uses ROS TF to lookup the current transform from tgt_frame to src_frame,
If the returned transform is applied to data, it will transform data in
the src_frame into the tgt_frame
:param tgt_frame: target frame
:param src_frame: source frame
:type tgt_frame: string
:type src_frame: string
:returns: trans, translation (x,y,z)
:rtype: tuple (of floats)
:returns: quat, rotation as a quaternion (x,y,z,w)
:rtype: tuple (of floats)
"""
try:
tf_listener.waitForTransform(
tgt_frame, src_frame, rospy.Time(0), rospy.Duration(3)
)
(trans, quat) = tf_listener.lookupTransform(tgt_frame, src_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
raise RuntimeError(
"Cannot fetch the transform from"
" {0:s} to {1:s}".format(tgt_frame, src_frame)
)
return trans, quat