本文整理汇总了Python中tf.TransformListener.getLatestCommonTime方法的典型用法代码示例。如果您正苦于以下问题:Python TransformListener.getLatestCommonTime方法的具体用法?Python TransformListener.getLatestCommonTime怎么用?Python TransformListener.getLatestCommonTime使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf.TransformListener
的用法示例。
在下文中一共展示了TransformListener.getLatestCommonTime方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: subscriber_callback
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
def subscriber_callback(data):
occupancyMap = transformation(data.data, data.info.width, data.info.height)
way_points = find_goals(occupancyMap, data.info.width, data.info.height)
result = random.choice(way_points)
try:
planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
listener = TransformListener()
listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
t = listener.getLatestCommonTime("base_link", "map")
position, quaternion = listener.lookupTransform("base_link", "map", t)
pos_x = position[0]
pos_y = position[1]
pos_z = position[2]
goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
start_pose = create_message(pos_x,pos_y,pos_z,quaternion)
plan = planMaker(
start_pose,
goal_robot.target_pose,
0.5)
action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
action_server.wait_for_server()
send_goal(goal_robot, action_server)
except rospy.ServiceException, e:
print "plan service call failed: %s"%e
示例2: ForceFromGravity
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class ForceFromGravity(object):
def __init__(self):
self.tf_l = TransformListener()
self.last_wrench = None
self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
WrenchStamped,
self.wrench_cb,
queue_size=1)
def wrench_cb(self, data):
self.last_wrench = data
def compute_forces(self):
qs = self.get_orientation()
o = qs.quaternion
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("wrist_left_ft_link rpy vs world: " + str( (round(r, 3), round(p, 3), round(y, 3)) ))
rospy.loginfo("in degrees: " + str( (round(degrees(r), 3), round(degrees(p), 3), round(degrees(y), 3)) ))
hand_total_force = 10 # 10 Newton, near to a Kg
fx = hand_total_force * cos(r) * -1.0 # hack
fy = hand_total_force * cos(p)
fz = hand_total_force * cos(y)
#total = abs(fx) + abs(fy) + abs(fz)
#rospy.loginfo("fx, fy, fz, total:")
#rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
return fx, fy, fz
def get_last_forces(self):
f = self.last_wrench.wrench.force
return f.x, f.y, f.z
def get_orientation(self, from_frame="wrist_left_ft_link"):
qs = QuaternionStamped()
qs.quaternion.w = 1.0
qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
qs.header.frame_id = "/" + from_frame
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_q = self.tf_l.transformQuaternion("/world", qs)
transform_ok = True
return world_q
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
qs.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
cx, cy, cz = self.compute_forces()
c_total = abs(cx) + abs(cy) + abs(cz)
fx, fy, fz = self.get_last_forces()
f_total = abs(fx) + abs(fy) + abs(fz)
rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" + str( (round(cx, 3), round(cy, 3), round(cz, 3) )))
rospy.loginfo("Real Forces :" + str(round(f_total, 3)) + "\n" + str( (round(fx, 3), round(fy, 3), round(fz, 3) )))
r.sleep()
示例3: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class TfFilter:
def __init__(self, buffer_size):
self.tf = TransformListener(True, rospy.Duration(5))
self.target = ''
self.buffer = np.zeros((buffer_size, 1))
self.buffer_ptr = 0
self.buffer_size = buffer_size
self.tf_sub = rospy.Subscriber('tf', tfMessage, self.tf_cb)
self.tf_pub = rospy.Publisher('tf', tfMessage)
self.srv = rospy.Service(SERVICE, PublishGoal, self.publish_goal_cb)
def tf_cb(self, msg):
for t in msg.transforms:
if t.child_frame_id == self.target:
time = self.tf.getLatestCommonTime(self.source, self.target)
p, q = self.tf.lookupTransform(self.source, self.target, time)
rm = tfs.quaternion_matrix(q)
# assemble a new coordinate frame that has z-axis aligned with
# the vertical direction and x-axis facing the z-axis of the
# original frame
z = rm[:3, 2]
z[2] = 0
axis_x = tfs.unit_vector(z)
axis_z = tfs.unit_vector([0, 0, 1])
axis_y = np.cross(axis_x, axis_z)
rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
# shift the pose along the x-axis
self.position = p + np.dot(rm, self.d)[:3]
self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2]
self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size
self.publish_filtered_tf(t.header)
def publish_filtered_tf(self, header):
yaw = np.median(self.buffer)
q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
ts = TransformStamped()
ts.header = header
ts.header.frame_id = self.source
ts.child_frame_id = self.goal
ts.transform.translation.x = self.position[0]
ts.transform.translation.y = self.position[1]
ts.transform.translation.z = 0
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
msg = tfMessage()
msg.transforms.append(ts)
self.tf_pub.publish(msg)
def publish_goal_cb(self, r):
rospy.loginfo('Received [%s] request. Will start publishing %s frame' %
(SERVICE, r.goal_frame_id))
self.source = r.source_frame_id
self.target = r.target_frame_id
self.goal = r.goal_frame_id
self.d = [r.displacement.x, r.displacement.y, r.displacement.z]
return []
示例4: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class KinectNiteHelp:
def __init__(self, name="kinect_listener", user=1):
# rospy.init_node(name, anonymous=True)
self.tf = TransformListener()
self.user = user
def getLeftHandPos(self):
if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"):
print " Inside TF IF Get LEft HAnd POS "
t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1")
(leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t)
return leftHandPos
def getRightHandPos(self):
if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"):
t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1")
(rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t)
return rightHandPos
示例5: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class TransformNode:
def __init__(self, *args):
self.tf = TransformListener()
rospy.Subscriber("/tf", TFMessage, transform, queue_size=1)
def transform(self, msg):
if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
t = self.tf.getLatestCommonTime("/base_link", "/map")
position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
print position, quaternion
示例6: Transformation
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Transformation():
def __init__(self):
pub = 0
self.tf = TransformListener()
self.tf1 = TransformerROS()
self.fdata = geometry_msgs.msg.TransformStamped()
self.fdata_base = geometry_msgs.msg.TransformStamped()
self.transform = tf.TransformBroadcaster()
self.wrench = WrenchStamped()
self.wrench_bl = WrenchStamped()
def wrench_cb(self,msg):
self.wrench = msg.wrench
self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
self.fdata.transform.translation.x = self.wrench.force.x
self.fdata.transform.translation.y = self.wrench.force.y
self.fdata.transform.translation.z = self.wrench.force.z
try:
if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
#print transform_ee_base_position
#print transform_ee_base_quaternion
#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
except(tf.LookupException,tf.ConnectivityException):
print("TRANSFORMATION ERROR")
sss.say(["error"])
#return 'failed'
self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))
self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))
self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
self.wrench_bl.header.stamp = rospy.Time.now();
self.pub.publish(self.wrench_bl)
示例7: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Demo:
def __init__(self, goals):
rospy.init_node("demo", anonymous=True)
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 0
def run(self):
self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = "world"
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime("/world", self.frame)
if self.listener.canTransform("/world", self.frame, t):
position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if (
math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
and self.goalIndex < len(self.goals) - 1
):
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
示例8: Controller
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
self.pidX = PID(20, 12, 0.0, -30, 30, "x")
self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 0.5
self.lastJoy = Joy()
def _cmdVelTelopChanged(self, data):
self.cmd_vel_telop = data
if self.state == Controller.Manual:
self.pubNav.publish(data)
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _joyChanged(self, data):
if len(data.buttons) == len(self.lastJoy.buttons):
delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
print ("Buton ok")
#Button 1
if delta[0] == 1 and self.state != Controller.Automatic:
print("Automatic!")
thrust = self.cmd_vel_telop.linear.z
print(thrust)
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.state = Controller.TakeOff
print("TakeOff!")
#Button 5
if delta[4] == 1:
self.targetZ += 0.1
print(self.targetZ)
#Button 6
if delta[5] == 1:
self.targetZ -= 0.1
print(self.targetZ)
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark")
if self.listener.canTransform("/mocap", "/Nano_Mark", t):
position, quaternion = self.listener.lookupTransform("/mocap", "/Nano_Mark", t)
if position[2] > 0.1 or thrust > 50000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap")
print(t);
if self.listener.canTransform("/Nano_Mark", "/mocap", t):
targetWorld = PoseStamped()
targetWorld.header.stamp = t
targetWorld.header.frame_id = "mocap"
targetWorld.pose.position.x = 0
targetWorld.pose.position.y = 0
targetWorld.pose.position.z = self.targetZ
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
targetWorld.pose.orientation.x = quaternion[0]
targetWorld.pose.orientation.y = quaternion[1]
targetWorld.pose.orientation.z = quaternion[2]
targetWorld.pose.orientation.w = quaternion[3]
#.........这里部分代码省略.........
示例9: Controller
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
#.........这里部分代码省略.........
#Button 5
if delta[4] == 1:
#self.targetX = -1.0
#self.targetY = -1.0
self.targetZ = 1.0
self.des_angle = -90.0
#if self.des_angle > 179:
# self.des_angle = -179.0
#print(self.targetZ)
#self.power += 100.0
#print(self.power)
self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
#self.targetX = 1.0
#self.targetY = 1.0
self.targetZ = 0.5
self.des_angle = 90.0
#if self.des_angle < -179:
# self.des_angle = 179.0
#print(self.targetZ)
#self.power -= 100.0
#print(self.power)
self.state = Controller.Automatic
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
print(position[0],position[1],position[2])
#if position[2] > 0.1 or thrust > 50000:
if thrust > 51000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 300
self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark_Gon4")
print(t)
if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",t)
#print(position[0],position[1],position[2])
euler = tf.transformations.euler_from_quaternion(quaternion)
print(euler[2]*(180/math.pi))
msg = self.cmd_vel_telop
#print(self.power)
#Descompostion of the x and y contributions following the Z-Rotation
x_prim = self.pidX.update(0.0, self.targetX-position[0])
y_prim = self.pidY.update(0.0,self.targetY-position[1])
示例10: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Bag2UosConverter:
def __init__(self, rootdir, scale_factor, transform_pitch_angle):
rospy.init_node('bad_to_uos_convert', anonymous=False)
rospy.Subscriber('velotime_points', PointCloud2, self.processMsg)
self.counter = 0
self.rootdir = rootdir
self.scale_factor = scale_factor
self.tf = TransformListener()
self.init = True
self.transform_pitch_angle = math.radians(transform_pitch_angle)
pass
def transformPoint(self,x ,y , z):
yy = y
xx = math.cos(self.transform_pitch_angle) * x + math.sin(self.transform_pitch_angle) * z
zz = -math.sin(self.transform_pitch_angle) * x + math.cos(self.transform_pitch_angle) * z
return xx, yy, zz
def run(self):
rospy.spin()
pass
def processMsg(self, msg):
print 'data arrived'
# print list(cloud)
# print self.tf.frameExists("velodyne") , self.tf.frameExists("odom")
if self.tf.frameExists("world") and self.tf.frameExists("odom"):
t = self.tf.getLatestCommonTime("world", "odom")
position, quat= self.tf.lookupTransform("world", "odom", t)
print position, quat
euler = tf.transformations.euler_from_quaternion(quat)
# heading_rad = math.degrees(euler[2])
heading_rad = euler[2]
print heading_rad
cloud = pc2.read_points(msg, skip_nans=True)
self.saveData(list(cloud))
self.savePose(position[0],position[1],0,0,0,heading_rad)
self.counter = self.counter + 1
if self.init:
self.init = False
cloud = pc2.read_points(msg, skip_nans=True)
self.saveData(list(cloud))
self.savePose(0,0,0,0,0,0)
self.counter = self.counter + 1
def saveData(self, cloud):
file_string = 'scan'+format(self.counter,'03d')
fullfilename_data = self.rootdir + file_string + '.3d'
fh_data = open(fullfilename_data, 'w')
for (x,y,z,intensity,ring) in cloud:
# print x,y,z
x, y, z = self.transformPoint(x, y, z)
x = x*self.scale_factor
y = y*self.scale_factor
z = z*self.scale_factor
str_ = format(x, '.1f') + ' ' + format(z, '.1f') + ' ' + format(y, '.1f') + '\n'
fh_data.write(str_)
fh_data.close()
def savePose(self, x, y, z, roll, pitch, yaw):
file_string = 'scan'+format(self.counter,'03d')
fullfilename_pose = self.rootdir + file_string + '.pose'
fh_pose = open(fullfilename_pose, 'w')
x = x*self.scale_factor
y = y*self.scale_factor
z = z*self.scale_factor
roll = roll*180/math.pi
yaw = yaw*180/math.pi
pitch = pitch*180/math.pi
str_first_line = format(x, '.1f') + ' ' + format(-z, '.1f') + ' ' + format(y, '.1f') + '\n'
str_2nd_line = format(roll, '.1f') + ' ' + format(-yaw, '.1f') + ' ' + format(pitch, '.1f') + '\n'
print str_first_line+str_2nd_line
fh_pose.writelines([str_first_line, str_2nd_line])
fh_pose.close()
示例11: ArmByFtWrist
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
#.........这里部分代码省略.........
if sign == '+':
args.append(1.0)
else:
args.append(-1.0)
args.extend([self.axis_force[0], self.axis_force[1], self.axis_force[2],
self.axis_torque[0], self.axis_torque[1], self.axis_torque[2]])
self.frame_fixer = WrenchFixer(*args)
return config
def follow_pose_cb(self, data):
if data.header.frame_id[0] == '/':
frame_id = data.header.frame_id[1:]
else:
frame_id = data.header.frame_id
if frame_id != self.global_frame_id:
rospy.logwarn(
"Poses to follow should be in frame " + self.global_frame_id +
" and is in " + str(frame_id) + ", transforming into " + self.global_frame_id + "...")
world_ps = self.transform_pose_to_world(
data.pose, from_frame=data.header.frame_id)
ps = PoseStamped()
ps.header.stamp = data.header.stamp
ps.header.frame_id = self.global_frame_id
ps.pose = world_ps
self.last_pose_to_follow = ps
else:
self.last_pose_to_follow = data
def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"):
ps = PoseStamped()
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
ps.header.frame_id = "/" + from_frame
# TODO: check pose being PoseStamped or Pose
ps.pose = pose
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_ps = self.tf_l.transformPose(self.global_frame_id, ps)
transform_ok = True
return world_ps
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
def wrench_cb(self, data):
self.last_wrench = data
def get_initial_pose(self):
zero_pose = Pose()
zero_pose.orientation.w = 1.0
current_pose = self.transform_pose_to_world(
zero_pose, from_frame=self.goal_frame_id)
return current_pose
def get_abs_total_force(self, wrench_msg):
f = wrench_msg.wrench.force
return abs(f.x) + abs(f.y) + abs(f.z)
示例12: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
#TODO: Send the arms to ik_solution
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
#.........这里部分代码省略.........
示例13: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except Exception as e:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
import traceback
traceback.print_exc()
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
self.move_to_joints(self.side_prefix, [ik_sol], 1.0)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def marker_cb(self, pose_markers):
rospy.loginfo('AR Marker Pose updating')
transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
offset_array = [0, 0, .03]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
self.update_viz()
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
#.........这里部分代码省略.........
示例14: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if(joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
#.........这里部分代码省略.........
示例15: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import getLatestCommonTime [as 别名]
class Controller:
Idle = 0
Automatic = 1
TakingOff = 2
Landing = 3
def __init__(self, frame):
self.frame = frame
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
self.pidX = PID(35, 10, 0.0, -20, 20, "x")
self.pidY = PID(-35, -10, -0.0, -20, 20, "y")
self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Idle
self.goal = Pose()
rospy.Subscriber("goal", Pose, self._poseChanged)
rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff)
rospy.Service("land", std_srvs.srv.Empty, self._land)
def getTransform(self, source_frame, target_frame):
now = rospy.Time.now()
success = False
if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)):
t = self.listener.getLatestCommonTime(source_frame, target_frame)
if self.listener.canTransform(source_frame, target_frame, t):
position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t)
success = True
delta = (now - t).to_sec() * 1000 #ms
if delta > 25:
rospy.logwarn("Latency: %f ms.", delta)
# self.listener.clear()
# rospy.sleep(0.02)
if success:
return position, quaternion, t
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _poseChanged(self, data):
self.goal = data
def _takeoff(self, req):
rospy.loginfo("Takeoff requested!")
self.state = Controller.TakingOff
return std_srvs.srv.EmptyResponse()
def _land(self, req):
rospy.loginfo("Landing requested!")
self.state = Controller.Landing
return std_srvs.srv.EmptyResponse()
def run(self):
thrust = 0
while not rospy.is_shutdown():
now = rospy.Time.now()
if self.state == Controller.TakingOff:
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] > 0.05 or thrust > 50000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
msg = Twist()
msg.linear.z = thrust
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Landing:
self.goal.position.z = 0.05
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] <= 0.1:
self.state = Controller.Idle
msg = Twist()
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Automatic or self.state == Controller.Landing:
# transform target world coordinates into local coordinates
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
targetWorld = PoseStamped()
targetWorld.header.stamp = t
targetWorld.header.frame_id = "world"
targetWorld.pose = self.goal
#.........这里部分代码省略.........