def robot_listener(self):
'''
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
'''
robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
robot_vel_pub.publish(cmd)
rate.sleep()
python类LookupException()的实例源码
def markerCB(self, msg):
try:
self.marker_lock.acquire()
if not self.initialize_poses:
return
self.initial_poses = {}
for marker in msg.markers:
if marker.name.startswith("EE:goal_"):
# resolve tf
if marker.header.frame_id != self.frame_id:
ps = PoseStamped(header=marker.header, pose=marker.pose)
try:
transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
self.initial_poses[marker.name[3:]] = transformed_pose.pose
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
rospy.logerr("tf error when resolving tf: %s" % e)
else:
self.initial_poses[marker.name[3:]] = marker.pose #tf should be resolved
finally:
self.marker_lock.release()
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)))
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)))
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)
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))
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)))
def goalCB(self, poseStamped):
# reset timestamp because of bug: https://github.com/ros/geometry/issues/82
poseStamped.header.stamp = rospy.Time(0)
try:
robotToTarget1 = self.listener.transformPose("/base_footprint", poseStamped)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logerr("Error while transforming pose: %s", str(e))
return
quat = robotToTarget1.pose.orientation
(roll,pitch,yaw) = euler_from_quaternion((quat.x, quat.y, quat.z, quat.w))
self.motionProxy.moveTo(robotToTarget1.pose.position.x, robotToTarget1.pose.position.y, yaw)
def transform_pose(self, pose):
try:
new_pose = self.listener.transformPose(self.frame_id, pose)
# now new_pose should be the pose of the tag in the odom_meas frame.
# Let's ignore the height, and rotations not about the z-axis
new_pose.pose.position.z = 0
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return None
return new_pose
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)))
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)))
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)
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))
def get_odom_angle(self):
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 "succeeded"
return self.quat_to_angle(Quaternion(*rot))
def get_odom_angle(self):
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 "succeeded"
return self.quat_to_angle(Quaternion(*rot))
def get_odom_angle(self):
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 "succeeded"
return self.quat_to_angle(Quaternion(*rot))
def get_odom_angle(self):
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 self.quat_to_angle(Quaternion(*rot))
def get_odom_angle(self):
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 self.quat_to_angle(Quaternion(*rot))
def record_tf(self, event):
if self.active:
try:
self.listener.waitForTransform(self.root_frame,
self.measured_frame,
rospy.Time(0),
rospy.Duration.from_sec(1 / (2*self.tf_sampling_freq)))
(trans, rot) = self.listener.lookupTransform(self.root_frame, self.measured_frame, rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException) as e:
rospy.logwarn(e)
pass
else:
if self.first_value:
self.trans_old = trans
self.rot_old = rot
self.first_value = False
return
#print "transformations: \n", "trans[0]", trans[0], "self.trans_old[0]",self.trans_old[0], "trans[1]", trans[1], "self.trans_old[1]",self.trans_old[1], "trans[2]",trans[2], "self.trans_old[2]",self.trans_old[2], "\n ------------------------------------------------ "
path_increment = math.sqrt((trans[0] - self.trans_old[0]) ** 2 + (trans[1] - self.trans_old[1]) ** 2 +
(trans[2] - self.trans_old[2]) ** 2)
if(path_increment < 1):
#rospy.logwarn("Transformation: %s, Path Increment: %s",str(trans), str(path_increment))
self.path_length += path_increment
else:
rospy.logwarn("Transformation Failed! \n Transformation: %s, Path Increment: %s",str(trans), str(path_increment))
self.trans_old = trans
self.rot_old = rot
def go_to_pose(self, name, T, timeout):
msg = convert_to_trans_message(T)
self.update_marker(T)
start_time = time.time()
done = False
while not done and not rospy.is_shutdown():
self.pub_cmd.publish(msg)
try:
(trans,rot) = self.listener.lookupTransform('/world_link','lwr_arm_7_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF Exception!"
continue
TR = numpy.dot(tf.transformations.translation_matrix(trans),
tf.transformations.quaternion_matrix(rot))
if (is_same(T, TR)):
print name + ": Reached desired pose"
done = True
if (time.time() - start_time > timeout) :
print name + ": Robot took too long to reach desired pose"
done = True
else:
rospy.sleep(0.1)
def goto_pose(self, name, T, timeout):
self.server.setPose("move_arm_marker", convert_to_message(T))
self.server.applyChanges()
self.pub_command.publish(convert_to_trans_message(T))
print 'Goal published'
start_time = time.time()
done = False
while not done and not rospy.is_shutdown():
self.mutex.acquire()
last_joint_state = deepcopy(self.joint_state)
self.mutex.release()
if not self.check_validity(last_joint_state):
print 'COLLISION!'
try:
(trans,rot) = self.listener.lookupTransform('/world_link','/lwr_arm_7_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF Exception!"
continue
TR = numpy.dot(tf.transformations.translation_matrix(trans),
tf.transformations.quaternion_matrix(rot))
if (is_same(T, TR)):
print name + ": Reached goal"
done = True
if (time.time() - start_time > timeout) :
done = True
print name + ": Robot took too long to reach goal. Grader timed out"
else:
rospy.sleep(0.05)
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)))
def establish_tfs(self, relations):
"""
Perform a one-time look up of all the requested
*static* relations between frames (available via /tf)
"""
# Init node and tf listener
rospy.init_node(self.__class__.__name__, disable_signals=True)
tf_listener = tf.TransformListener()
# Create tf decoder
tf_dec = TfDecoderAndPublisher(channel='/tf')
# Establish tf relations
print('{} :: Establishing tfs from ROSBag'.format(self.__class__.__name__))
for self.idx, (channel, msg, t) in enumerate(self.log.read_messages(topics='/tf')):
tf_dec.decode(msg)
for (from_tf, to_tf) in relations:
# If the relations have been added already, skip
if (from_tf, to_tf) in self.relations_map_:
continue
# Check if the frames exist yet
if not tf_listener.frameExists(from_tf) or not tf_listener.frameExists(to_tf):
continue
# Retrieve the transform with common time
try:
tcommon = tf_listener.getLatestCommonTime(from_tf, to_tf)
(trans,rot) = tf_listener.lookupTransform(from_tf, to_tf, tcommon)
self.relations_map_[(from_tf,to_tf)] = RigidTransform(tvec=trans, xyzw=rot)
# print('\tSuccessfully received transform: {:} => {:} {:}'
# .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)]))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
# print e
pass
# Finish up once we've established all the requested tfs
if len(self.relations_map_) == len(relations):
break
try:
tfs = [self.relations_map_[(from_tf,to_tf)] for (from_tf, to_tf) in relations]
for (from_tf, to_tf) in relations:
print('\tSuccessfully received transform:\n\t\t {:} => {:} {:}'
.format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)]))
except:
raise RuntimeError('Error concerning tf lookup')
print('{} :: Established {:} relations\n'.format(self.__class__.__name__, len(tfs)))
return tfs
def __init__(self):
# Give the node a name
rospy.init_node('quat_to_angle', anonymous=False)
# Publisher to control the robot's speed
self.turtlebot_angle = rospy.Publisher('/turtlebot_angle', Float64, queue_size=5)
self.turtlebot_posex = rospy.Publisher('/turtlebot_posex', Float64, queue_size=5)
self.turtlebot_posey = rospy.Publisher('/turtlebot_posey', Float64, queue_size=5)
#goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"])))
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
while not rospy.is_shutdown():
(position, rotation) = self.get_odom()
print(position)
self.turtlebot_angle.publish(rotation)
#print(str(position).split('x: ')[1].split('\ny:')[0])
x = float(str(position).split('x: ')[1].split('\ny:')[0])
y = float(str(position).split('y: ')[1].split('\nz:')[0])
self.turtlebot_posex.publish(x)
self.turtlebot_posey.publish(y)
#print(rotation)
rospy.sleep(5)