本文整理汇总了Python中tf.TransformListener.transformPoint方法的典型用法代码示例。如果您正苦于以下问题：Python TransformListener.transformPoint方法的具体用法？Python TransformListener.transformPoint怎么用？Python TransformListener.transformPoint使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf.TransformListener的用法示例。
在下文中一共展示了TransformListener.transformPoint方法的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class FTPNode:
def __init__(self, *args):
print("init")
self.tf = TransformListener()
self.tt = Transformer()
rospy.Subscriber("/trackedHumans", TrackedHumans, self.pos_callback)
self.publisher = rospy.Publisher("directionmarker_array", MarkerArray)
def pos_callback(self, data):
rospy.loginfo("on updated pos, face robot towards guy...")
print("hi")
if (len(data.trackedHumans) > 0):
print(data.trackedHumans[0].location.point.x)
try:
self.tf.waitForTransform(data.trackedHumans[0].location.header.frame_id, "/base_link", rospy.Time.now(), rospy.Duration(2.0))
pp = self.tf.transformPoint("/base_link", data.trackedHumans[0].location)
print "Original:"
print [data.trackedHumans[0].location.point]
print "Transform:"
print pp.point
phi = math.atan2(pp.point.y, pp.point.x)
sss.move_base_rel("base", [0,0,phi])
print phi*180/math.pi
markerArray = MarkerArray()
marker = Marker()
marker.header.stamp = rospy.Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.header.frame_id = "/base_link"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
p1 = Point()
p1.x = 0
p1.y = 0
p1.z = 0
p2 = Point()
p2.x = pp.point.x
p2.y = pp.point.y
p2.z = 0
marker.points = [p1,p2]
#marker.pose.position.x = 1
#marker.pose.position.y = 0
#marker.pose.position.z = 1
#marker.pose.orientation.w = 1
markerArray.markers.append(marker)
self.publisher.publish(markerArray)
print "try ended"
except Exception as e:
print e示例2: CameraPointer
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class CameraPointer(object):
def __init__(self, side, camera_ik):
self.side = side
self.camera_ik = camera_ik
self.joint_names = self.joint_angles_act = self.joint_angles_des = None
self.tfl = TransformListener()
self.joint_state_sub = rospy.Subscriber('/{0}_arm_controller/state'.format(self.side), JointTrajectoryControllerState, self.joint_state_cb)
self.joint_traj_pub = rospy.Publisher('/{0}_arm_controller/command'.format(self.side), JointTrajectory)
# Wait for joint information to become available
while self.joint_names is None and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("[{0}] Waiting for joint state from arm controller.".format(rospy.get_name()))
#Set rate limits on a per-joint basis
self.max_vel_rot = [np.pi]*len(self.joint_names)
self.target_sub = rospy.Subscriber('{0}/lookat_ik/goal'.format(rospy.get_name()), PointStamped, self.goal_cb)
rospy.loginfo("[{0}] Ready.".format(rospy.get_name()))
def joint_state_cb(self, jtcs):
if self.joint_names is None:
self.joint_names = jtcs.joint_names
self.joint_angles_act = jtcs.actual.positions
self.joint_angles_des = jtcs.desired.positions
def goal_cb(self, pt_msg):
rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
if (pt_msg.header.frame_id != self.camera_ik.base_frame):
try:
now = pt_msg.header.stamp
self.tfl.waitForTransform(pt_msg.header.frame_id,
self.camera_ik.base_frame,
now, rospy.Duration(1.0))
pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
pt_msg.header.frame_id,
self.camera_ik.base_frame))
target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
# Get IK Solution
current_angles = list(copy.copy(self.joint_angles_act))
iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
# Start with current angles, then replace angles in camera IK with IK solution
# Use desired joint angles to avoid sagging over time
jtp = JointTrajectoryPoint()
jtp.positions = list(copy.copy(self.joint_angles_des))
for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
deltas = np.abs(np.subtract(jtp.positions, current_angles))
duration = np.max(np.divide(deltas, self.max_vel_rot))
jtp.time_from_start = rospy.Duration(duration)
jt = JointTrajectory()
jt.joint_names = self.joint_names
jt.points.append(jtp)
rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
self.joint_traj_pub.publish(jt)示例3: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
self.last_invited_at = rospy.Time.now()
self.new_face = False
rospy.loginfo("new person")
self.face_counter = self.face_counter + 1
# cd = getPointDist(self.face)
# TODO decide action based on distance ?
self.go(self._left_arm, self.l_home_pose) # if a person is too close, dont move hand?
self.go(self._right_arm, self.r_advert)
string = self.getRandomFromArray(self.invite_strings)
self.say(string)
# TODO wait some min. time + say something
# after 20 seconds of no detected face, let's continue
if self.face.header.stamp + rospy.Duration(10) < rospy.Time.now():
rospy.loginfo("person left")
string = self.getRandomFromArray(self.goodbye_strings)
self.say(string)
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self.face = None
return
self._head_buff.put((copy.deepcopy(self.face), 0.4, False))
def init_head(self):
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 2.0
p.point.y = 0.0
p.point.z = 1.7
self._head_buff.put((p, 0.1, True))
def face_cb(self, point):
# transform point
try:
self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2))
pt = self._tf.transformPoint(self._robot_frame, point)
except:
rospy.logerr("Transform error")
return
if self.face is not None:
cd = self.getPointDist(pt) # current distance
dd = fabs(self.face_last_dist - cd) # change in distance
if dd < 0.5:
self.face.header = pt.header
# filter x,y,z values a bit
self.face.point = pt.point
#self.face.point.x = (self.face.point.x + pt.point.x) / 2
#self.face.point.y = (self.face.point.y + pt.point.y) / 2
#self.face.point.z = (self.face.point.z + pt.point.z) / 2
else:
if self._person_prob_left < 2:
self._person_prob_left += 1
else:
self._person_prob_left = 0
print "new face ddist: " + str(dd)
self.new_face = True
self.face = pt
self.face_last_dist = cd
else:
self._person_prob_left = 0
self.new_face = True
self.face = pt示例4: ORKTabletop
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
# (would also require shifting the observed centroid down by table_dim.z/2)
table_dim.z = 0.0
table_dim.x = abs(max_x - min_x)
table_dim.y = abs(max_y - min_y)
print "Dimensions: ", table_dim.x, table_dim.y
# Temporary frame used for transformations
table_link = 'table_link'
# centroid of a table in table_link frame
centroid = PoseStamped()
centroid.header.frame_id = table_link
centroid.header.stamp = table_pose.header.stamp
centroid.pose.position.x = (max_x + min_x)/2.
centroid.pose.position.y = (max_y + min_y)/2.
centroid.pose.position.z = 0.0
centroid.pose.orientation.x = 0.0
centroid.pose.orientation.y = 0.0
centroid.pose.orientation.z = 0.0
centroid.pose.orientation.w = 1.0
# generate transform from table_pose to our newly-defined table_link
tt = TransformStamped()
tt.header = table_pose.header
tt.child_frame_id = table_link
tt.transform.translation = table_pose.pose.position
tt.transform.rotation = table_pose.pose.orientation
self.tl.setTransform(tt)
self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0))
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid)
self.pose_pub.publish(centroid_table_pose)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
# transform each object into desired frame; add to list of clusters
cluster_list = []
for i in range (data.objects.__len__()):
rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__())
pc = PointCloud2()
pc = data.objects[i].point_clouds[0]
arr = pointclouds.pointcloud2_to_array(pc, 1)
arr_xyz = pointclouds.get_xyz_points(arr)
arr_xyz_trans = []
for j in range (arr_xyz.__len__()):
ps = PointStamped();
ps.header.frame_id = table_link
ps.header.stamp = table_pose.header.stamp
ps.point.x = arr_xyz[j][0]
ps.point.y = arr_xyz[j][1]
ps.point.z = arr_xyz[j][2]
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z])
pc_trans = PointCloud2()
pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]),
table_pose.header.stamp, table_pose.header.frame_id)
self.pub.publish(pc_trans)
cluster_list.append(pc_trans)
rospy.loginfo("cluster size %d", cluster_list.__len__())
# finally - save all data in the object that'll be sent in response to actionserver requests
with self.result_lock:
self._result.objects = cluster_list
self._result.table_dims = table_dim
self._result.table_pose = centroid_table_pose
self.has_data = True
def execute_cb(self, goal):
rospy.loginfo('Executing ORKTabletop action')
# want to get the NEXT data coming in, rather than the current one.
with self.result_lock:
self.has_data = False
rr = rospy.Rate(1.0)
while not rospy.is_shutdown() and not self._as.is_preempt_requested():
with self.result_lock:
if self.has_data:
break
rr.sleep()
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
elif rospy.is_shutdown():
self._as.set_aborted()
else:
with self.result_lock:
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)示例5: TestMotionExecutionBuffer
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
for z in range(2):
min_dist = 1000
if(z%2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime("/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint("base_link",finger_point)
distance = math.sqrt(math.pow(finger_point_base.point.x-.6,2)+math.pow(finger_point_base.point.y+.6,2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",finger_point_base.point.x,finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
rospy.loginfo("Min dist %d is %g",z,min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd-extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()示例6: Collector
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class Collector():
def __init__(self):
rospy.init_node('collector', anonymous = False)
self.lis=TransformListener();
self.data_out=SBC_Output();
rospy.Subscriber("/joint_states", JointState, self.j_callback)
rospy.Subscriber("/finger1/ContactState", KCL_ContactStateStamped, self.f_callback1)
rospy.Subscriber("/finger2/ContactState", KCL_ContactStateStamped, self.f_callback2)
rospy.Subscriber("/pressure", PressureControl, self.p_callback)
rospy.Subscriber("/prob_fail", Float64, self.prob_callback)
self.publisher=rospy.Publisher("sbc_data", SBC_Output)
self.point1=PointStamped()
self.point2=PointStamped()
self.rate=rospy.Rate(20);
def getParams(self):
self.data_out.D_Gain=rospy.get_param("/bhand_pid/d_gain")
self.data_out.F_ref_pid=rospy.get_param("/bhand_pid/f_ref")
self.data_out.I_Gain=rospy.get_param("/bhand_pid/i_gain")
self.data_out.P_Gain=rospy.get_param("/bhand_pid/p_gain")
self.data_out.freq=rospy.get_param("/pressure_reg/frequency")
self.data_out.Beta=rospy.get_param("/bhand_sbc/beta")
self.data_out.Delta=rospy.get_param("/bhand_sbc/delta")
self.data_out.Eta=rospy.get_param("/bhand_sbc/eta")
self.data_out.F_ref_sbc=rospy.get_param("/bhand_sbc/f_ref")
def j_callback(self,data):
self.joints=data;
self.data_out.effort1=data.effort[1]
self.data_out.effort2=data.effort[0]
def f_callback1(self,data):
self.data_out.Fn1=data.Fnormal;
ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
self.data_out.Ft1=np.sqrt(ft.dot(ft));
self.point1=PointStamped();
self.point1.header=data.header;
self.point1.point=data.contact_position;
def f_callback2(self,data):
self.data_out.Fn2=data.Fnormal;
ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
self.data_out.Ft2=np.sqrt(ft.dot(ft));
self.point2=PointStamped();
self.point2.header=data.header;
self.point2.point=data.contact_position;
def p_callback(self,data):
self.data_out.p_demand=data.p_demand;
self.data_out.p_measure=data.p_measure;
def prob_callback(self,data):
self.data_out.Pfailure=data.data;
def transform_it(self,data):
if(data.header.frame_id):
#data.header.stamp=rospy.Time.now();
if(self.lis.canTransform("base_link",data.header.frame_id,data.header.stamp) or True):
#print(rospy.Time.now())
data.header.stamp=data.header.stamp-rospy.Duration(0.02)
#point=self.lis.transformPoint("base_link", data)
try:
#self.lis.waitForTransform("base_link", data.header.frame_id, data.header.stamp, rospy.Duration(1))
# print(rospy.Time.now())
self.point=self.lis.transformPoint("base_link", data)
return True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF problem 2")
pass
else:
rospy.logwarn("Cannot Transform")
else:
print(data.header.frame_id)
return False
def get_distance(self,point1,point2):
d=np.array([point1.x-point2.x, point1.y-point2.y, point1.z-point2.z])
return np.sqrt(d.dot(d));
def send_it(self):
while not rospy.is_shutdown():
self.data_out.header.stamp=rospy.Time.now();
self.getParams()
got_it=self.transform_it(self.point1);
if(got_it):
self.data_out.contact1=self.point.point
got_it=self.transform_it(self.point2);
if(got_it):
self.data_out.contact2=self.point.point
self.data_out.distance=self.get_distance(self.data_out.contact1,self.data_out.contact2)*100
self.publisher.publish(self.data_out);
self.rate.sleep(); 示例7: ray_to_points
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
x4,y4 = fence_line_msg.polygon.points[1].x,fence_line_msg.polygon.points[1].y
px = ((x1*y2-y1*x2)*(x3-x4) - (x1-x2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4) - (y1-y2)*(x3-x4))
py = ((x1*y2-y1*x2)*(y3-y4) - (y1-y2)*(x3*y4-y3*x4))/((x1-x2)*(y3-y4) - (y1-y2)*(x3-x4))
if (np.abs(px) < np.abs(x2)) and ((y2<py<0) or (0<py<y2)):
return True
else:
return False
def send_marker(self, named_pt):
m=Marker()
m.header = copy.deepcopy(named_pt.header)
m.type=Marker.CYLINDER
m.pose.position = named_pt.point
m.pose.orientation.x=0.707
m.pose.orientation.y=0.0
m.pose.orientation.z=0.0
m.pose.orientation.w=0.707
m.scale.x=0.2
m.scale.y=0.2
m.scale.z=0.2
m.color.r=0.8
m.color.g=0.8
m.color.b=0.8
m.color.a=1.0
m.id = self.count
#m.text=named_pt.name
self.marker_pub.publish(m)
self.count += 1
t=Marker()
t.header = copy.deepcopy(named_pt.header)
m.type = Marker.TEXT_VIEW_FACING
m.pose.position = named_pt.point
m.pose.position.z += 0.1
m.pose.orientation.x=0.707
m.pose.orientation.y=0.0
m.pose.orientation.z=0.0
m.pose.orientation.w=0.707
m.scale.x=0.2
m.scale.y=0.2
m.scale.z=0.2
m.color.r=0.8
m.color.g=0.8
m.color.b=0.8
m.color.a=1.0
m.text = named_pt.name
m.id = self.count
self.marker_pub.publish(m)
self.count += 1
def cast_ray(self, point_in, tf, name):
base_link_point = tf.transformPoint('/base_link', point_in)
t = tf.getLatestCommonTime('/base_link', point_in.header.frame_id)
pos, quat = tf.lookupTransform('/base_link', point_in.header.frame_id, t)
height = pos[2]
x_slope = (base_link_point.point.x - pos[0])/(pos[2]-base_link_point.point.z)
y_slope = (base_link_point.point.y - pos[1])/(pos[2]-base_link_point.point.z)
ground_point = np.array([0.,0.,0.])
ground_point[0] = x_slope*height + pos[0]
ground_point[1] = y_slope*height + pos[1]
ground_named_point = NamedPoint()
ground_named_point.point.x = ground_point[0]
ground_named_point.point.y = ground_point[1]
ground_named_point.point.z = ground_point[2]
ground_named_point.header = point_in.header
ground_named_point.header.frame_id = 'base_link'
ground_named_point.header.stamp = point_in.header.stamp
ground_named_point.name = name
odom_named_point = NamedPoint()
odom_point = self.tf.transformPoint('/odom',ground_named_point)
odom_named_point.point = odom_point.point
odom_named_point.header = point_in.header
odom_named_point.header.frame_id = "/odom"
odom_named_point.header.stamp = point_in.header.stamp
odom_named_point.name = name
return ground_named_point, odom_named_point
def make_point_cloud(Point):
# Take a vector, nominally [x,y,1] and apply some rotation about x (pitch)
# and about y (yaw) in the base_link frame. This will make a frustum that
# can be sampled for a point cloud
p = self.pitch_error
pitch_mat = np.array([[1., 0., 0.],[0., np.cos(p), -np.sin(p)],[0., np.sin(p), np.cos(p)]])
y = self.yaw_error
yaw_mat = np.array([[np.cos(y), 0., np.sin(y)],[0., 1., 0.],[-np.sin(y), 0., np.cos(y)]])
vec = np.array([0,0,0])
vec[0] = Point.x
vec[1] = Point.y
vec[2] = Point.z
down_left = np.dot(pitch_mat,np.dot(yaw_mat,vec))
down_right = np.dot(pitch_mat,np.dot(-yaw_mat,vec))
up_left = np.dot(-pitch_mat,np.dot(yaw_mat,vec))
up_right = np.dot(-pitch_mat,np.dot(-yaw_mat,vec))示例8: TestGetStateValidity
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
self.obj_pub.publish(obj2)
rospy.sleep(5.)
def test_get_state_validity(self):
req = GetRobotStateRequest()
cur_state = self.get_robot_state_server.call(req)
#for i in range(len(cur_state.robot_state.joint_state.name)):
# print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i]
state_req = GetStateValidityRequest()
state_req.robot_state = cur_state.robot_state
group_req = GetGroupInfoRequest()
group_req.group_name = 'right_arm'
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
right_arm_links = group_res.link_names
group_req.group_name = ''
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(group_res.link_names,
right_arm_links)
for i in state_req.ordered_collision_operations.collision_operations:
print 'Disabling ', i.object1
state_req.check_collisions = True
res = self.get_state_validity_server.call(state_req)
#should be in collision
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be some contacts
self.failIf(len(res.contacts) == 0)
for c in res.contacts:
#getting everything in common frame of base_link
contact_point = PointStamped()
contact_point.header = c.header
contact_point.point = c.position
contact_point_base = self.tf.transformPoint("base_link",contact_point)
gripper_point = PointStamped()
gripper_point.header.stamp = c.header.stamp
gripper_point.header.frame_id = "r_gripper_palm_link"
gripper_point.point.x = 0
gripper_point.point.y = 0
gripper_point.point.z = 0
gripper_point_base = self.tf.transformPoint("base_link", gripper_point)
print 'x diff:', abs(gripper_point_base.point.x-contact_point_base.point.x)
print 'y diff:', abs(gripper_point_base.point.y-contact_point_base.point.y)
print 'z diff:', abs(gripper_point_base.point.z-contact_point_base.point.z)
self.failIf(abs(gripper_point_base.point.x-contact_point_base.point.x) > .031)
self.failIf(abs(gripper_point_base.point.y-contact_point_base.point.y) > .031)
self.failIf(abs(gripper_point_base.point.z-contact_point_base.point.z) > .031)
#now we delete obj1
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "test_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
self.obj_pub.publish(obj2)
rospy.sleep(5.)
cur_state = self.get_robot_state_server.call(req)
state_req.robot_state = cur_state.robot_state
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val != res.error_code.SUCCESS)
# now it should collide
state_req.ordered_collision_operations.collision_operations = []
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val == res.error_code.SUCCESS)示例9: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class FakeFaceDetector:
def __init__(self, debug=False, cam_frame_id = "/head_mount_kinect_rgb_optical_frame", robot_frame_id = "/base_link"):
self.tfl = TransformListener()
rospy.sleep(2)
self.debug = debug
self._point_pub = rospy.Publisher('nearest_face', PointStamped)
self.cam_frame_id = cam_frame_id
self.robot_frame_id = robot_frame_id
self.det_duration = rospy.Duration(20)
self.det_last = rospy.Time(0)
self.pt = PointStamped()
self._timer = rospy.Timer(rospy.Duration(0.1), self.timer)
def timer(self,event):
now = rospy.Time.now()
self.pt.header.stamp = now - rospy.Duration(0.5)
if (self.det_last + self.det_duration < now):
self.det_last = now
self.pt.header.frame_id = self.robot_frame_id
self.pt.point.x = random.uniform(1.5, 3.5)
self.pt.point.y = random.uniform(-0.5,0.5)
self.pt.point.z = random.uniform(1.3, 2.0)
if self.debug:
rospy.loginfo("Fake face at: [" + str(self.pt.point.x) + ", " + str(self.pt.point.y) + ", " + str(self.pt.point.z) + "] (" + self.robot_frame_id + ")")
pt = self.pt
pt.point.x = random.uniform(pt.point.x - 0.05, pt.point.x + 0.05)
pt.point.y = random.uniform(pt.point.y - 0.05, pt.point.y + 0.05)
pt.point.z = random.uniform(pt.point.z - 0.05, pt.point.z + 0.05)
# TODO "simulate" movement?
try:
pt = self.tfl.transformPoint(self.cam_frame_id, pt)
except:
rospy.logerr("Transform error")
self.det_last = rospy.Time(0)
return
self._point_pub.publish(pt)示例10: DepthImageCreator
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class DepthImageCreator(object):
def __init__(self, use_depth_only):
self.use_depth_only = use_depth_only
self.depth_image_lock = Lock()
self.image_list_lock = Lock()
self.image_list = []
self.image_list_max_size = 100
self.downsample_factor = 2
self.tf = TransformListener()
rospy.Subscriber("/color_camera/camera_info",
CameraInfo,
self.process_camera_info,
queue_size=10)
rospy.Subscriber("/point_cloud",
PointCloud,
self.process_point_cloud,
queue_size=10)
rospy.Subscriber("/color_camera/image_raw/compressed",
CompressedImage,
self.process_image,
queue_size=10)
self.clicked_point_pub = rospy.Publisher("/clicked_point",PointStamped,queue_size=10)
self.camera_info = None
self.P = None
self.depth_image = None
self.image = None
self.last_image_timestamp = None
self.click_timestamp = None
self.depth_timestamp = None
cv2.namedWindow("depth_feed")
cv2.namedWindow("image_feed")
cv2.namedWindow("combined_feed")
cv2.setMouseCallback('image_feed',self.handle_click)
cv2.setMouseCallback('combined_feed',self.handle_combined_click)
def handle_click(self,event,x,y,flags,param):
if event == cv2.EVENT_LBUTTONDOWN:
self.click_timestamp = self.last_image_timestamp
self.click_coords = (x*self.downsample_factor,y*self.downsample_factor)
def process_image(self,msg):
self.image_list_lock.acquire()
np_arr = np.fromstring(msg.data, np.uint8)
self.last_image_timestamp = msg.header.stamp
self.image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
if len(self.image_list) == self.image_list_max_size:
self.image_list.pop(0)
self.image_list.append((msg.header.stamp,self.image))
self.image_list_lock.release()
def process_camera_info(self, msg):
self.camera_info = msg
self.P = np.array(msg.P).reshape((3,4))
self.K = np.array(msg.K).reshape((3,3))
# TODO: this is necessary due to a mistake in intrinsics_server.py
self.D = np.array([msg.D[0],msg.D[1],0,0,msg.D[2]])
def get_nearest_image_temporally(self,timestamp):
self.image_list_lock.acquire()
diff_list = []
for im_stamp,image in self.image_list:
diff_list.append((abs((im_stamp-timestamp).to_sec()),image))
closest_temporally = min(diff_list,key=lambda t: t[0])
print closest_temporally[0]
self.image_list_lock.release()
return closest_temporally[1]
def handle_combined_click(self,event,x,y,flags,param):
if event == cv2.EVENT_LBUTTONDOWN:
try:
self.depth_image_lock.acquire()
click_coords = (x*self.downsample_factor,y*self.downsample_factor)
distances = []
for i in range(self.projected_points.shape[0]):
dist = (self.projected_points[i,0,0] - click_coords[0])**2 + (self.projected_points[i,0,1] - click_coords[1])**2
distances.append(dist)
three_d_coord = self.points_3d[:,np.argmin(distances)]
# again, we have to reshuffle the coordinates due to differences in ROS Tango coordinate systems
point_msg = PointStamped(header=Header(stamp=self.depth_image_timestamp,
frame_id="depth_camera"),
point=Point(y=three_d_coord[0],
z=three_d_coord[1],
x=three_d_coord[2]))
self.tf.waitForTransform("depth_camera",
"odom",
self.depth_image_timestamp,
rospy.Duration(1.0))
transformed_coord = self.tf.transformPoint('odom', point_msg)
self.clicked_point_pub.publish(transformed_coord)
self.depth_image_lock.release()
except Exception as ex:
print "Encountered an errror! ", ex
self.depth_image_lock.release()
def process_point_cloud(self, msg):
self.depth_image_lock.acquire()
try:
if self.P == None:
#.........这里部分代码省略.........
示例11: transformPoint
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
def transformPoint(self, target_frame, ps):
now = rospy.Time.now()
self.waitForTransform(target_frame, ps.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
ps.header.stamp = now
return TransformListener.transformPoint(self, target_frame, ps)示例12: Person_Follow
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
class Person_Follow(object):
def __init__(self):
rospy.init_node('person_follow')
self.twist=Twist()
self.twist.linear.x = 0; self.twist.linear.y = 0; self.twist.linear.z = 0
self.twist.angular.x = 0; self.twist.angular.y = 0; self.twist.angular.z = 0
self.dist0=0
self.target=1
self.p_angle=.01
self.i_angle=.0005
self.p_d=1
self.i_d=.005
self.error_angle_sum=0
self.error_d_sum=0
self.centroid=Point(x=0,y=0)
self.header_msg=Header(stamp=rospy.Time.now(), frame_id='base_link')
def Find_Pos(self, Data):
d={}
for i in range(len(Data.ranges)):
if(Data.ranges[i]!=0):
d[i]=Data.ranges[i]
d_sum_x=0
d_sum_y=0
d_count=0
for key in d:
if key>345 or key<15:
d_sum_x+=d[key]*math.cos(math.radians(key))
d_sum_y+=d[key]*math.sin(math.radians(key))
d_count+=1
self.dist0=Data.ranges[0]
d_avg_x=d_sum_x/d_count
d_avg_y=d_sum_y/d_count
self.centroid.x = d_avg_x
self.centroid.y = d_avg_y
# print self.centroid.x
def run(self):
rospy.Subscriber("/scan",LaserScan,self.Find_Pos,queue_size=10)
self.pub = rospy.Publisher('cmd_vel',Twist,queue_size=10)
self.t=TransformListener()
self.pub_centroid = rospy.Publisher('/centroid',PointStamped,queue_size=10)
r = rospy.Rate(30)
while not rospy.is_shutdown():
theta=0
if self.centroid.x!=0:
theta = 3*math.atan2(self.centroid.y,self.centroid.x)
print theta
self.twist.angular.z = theta
print self.centroid.x
if self.dist0 == 0:
speed = 1
else:
speed = .1 *(self.dist0-.5)
self.twist.linear.x=speed
self.pub.publish(self.twist)
#print self.t.allFramesAsString()
try:
point_stamped_msg = PointStamped(header=self.header_msg,
point=self.t.waitForTransform('odom','base_link',target.header.stamp, rospy.Duration(0.5))Point(x=-self.centroid.x, y=-self.centroid.y))
self.header_msg.stamp = rospy.Time.now()
#print self.header_msg
self.t.waitForTransform('odom','base_link',self.header_msg.stamp, rospy.Duration(0.5))
point_stamped_msg_odom = self.t.transformPoint('/odom',point_stamped_msg)
#print point_stamped_msg_odom
self.pub_centroid.publish(point_stamped_msg_odom)
except Exception as inst:
print inst示例13: color_name_sample_detection
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
# grab camera matrix and distortion model
self.K = CameraInfo.K
self.D = CameraInfo.D
self.R = CameraInfo.R
self.P = CameraInfo.P
self.h = CameraInfo.height
self.w = CameraInfo.width
self.frame_id = CameraInfo.header.frame_id
self.P = np.asarray(self.P).reshape(3,4)
self.K = np.asarray(self.K).reshape(3,3)
self.inv_K = scipy.linalg.inv(self.K)
def compute_color_name(self,hull,img,mask):
rect = cv2.boundingRect(hull)
cv2.drawContours(mask,[hull],-1,255,-1)
small_img = img[rect[1]:rect[1]+rect[3],rect[0]:rect[0]+rect[2]]
small_mask = mask[rect[1]:rect[1]+rect[3],rect[0]:rect[0]+rect[2]]/255.
index_img = np.floor(small_img[:,:,0]/8) + 32*np.floor(small_img[:,:,1]/8) + 32*32*np.floor(small_img[:,:,2]/8)
out = self.color_mat['w2c'][np.int16(index_img)]
for i in range(out.shape[-1]):
out[:,:,i] *= small_mask
ave_vec= np.sum(np.sum(out,axis=0),axis=0)
top_index = np.argmax(ave_vec)
similarity = ave_vec[top_index]/cv2.countNonZero(small_mask)
#if similarity > 0.1 and (top_index == 3 or top_index == 9):
cv2.putText(self.debug_img,self.color_names[top_index] + str(similarity),(rect[0],rect[1]),cv2.FONT_HERSHEY_PLAIN,2,(255,0,255))
if top_index == 3 or top_index == 9 or top_index == 1:
cv2.polylines(self.debug_img,[hull],1,(255,0,255),3)
cv2.putText(self.debug_img,self.color_names[top_index] + str(similarity),(rect[0],rect[1]),cv2.FONT_HERSHEY_PLAIN,2,(255,0,255))
return top_index,similarity
def cast_ray(self, point_in, tf, name):
#rospy.logdebug("Point In: %s", point_in)
base_link_point = tf.transformPoint('/base_link', point_in)
t = tf.getLatestCommonTime('/base_link', point_in.header.frame_id)
pos, quat = tf.lookupTransform('/base_link', point_in.header.frame_id, t)
height = pos[2]
#rospy.logdebug("Pos: %s", pos)
x_slope = np.abs((pos[0]-base_link_point.point.x)/(pos[2]-base_link_point.point.z))
y_slope = np.abs((pos[1]-base_link_point.point.y)/(pos[2]-base_link_point.point.z))
#rospy.logdebug("X Slope: %s", x_slope)
#rospy.logdebug("Y Slope: %s", y_slope)
ground_point = np.array([0.,0.,0.])
ground_point[0] = x_slope*height
ground_point[1] = y_slope*height
ground_named_point = NamedPoint()
ground_named_point.point.x = ground_point[0]
ground_named_point.point.y = ground_point[1]
ground_named_point.point.z = ground_point[2]
ground_named_point.header = copy.deepcopy(point_in.header)
ground_named_point.header.frame_id = 'base_link'
ground_named_point.name = name
odom_named_point = self.tf.transformPoint('/odom',ground_named_point)
#rospy.logdebug("Ground Point: %s", ground_named_point)
#rospy.logdebug("Odom Point: %s", odom_named_point)
return ground_named_point, odom_named_point
def real_size_check(self,hull,header):
rect = cv2.boundingRect(hull)
#top_left = np.array([rect[0],rect[1]])
bot_left = np.array([rect[0],rect[1]+rect[3]])
bot_right = np.array([rect[0]+rect[2],rect[1]+rect[3]])
#rospy.logdebug("Top Left: %s", top_left)
#rospy.logdebug("Bot Right: %s", bot_right)
bot_left_point = PointStamped()
bot_left_point.header = copy.deepcopy(header)
bot_left_point.point.x = bot_left[0]
bot_left_point.point.y = bot_left[1]
bot_left_point.point.z = 1.0
bot_right_point = PointStamped()
bot_right_point.header = copy.deepcopy(header)
bot_right_point.point.x = bot_right[0]
bot_right_point.point.y = bot_right[1]
bot_right_point.point.z = 1.0
#rospy.logdebug("Top Left Point: %s", top_left_point)
#rospy.logdebug("Bot Right Point: %s", bot_right_point)
bot_left_ground, bot_left_odom = self.cast_ray(bot_left_point,self.tf,'bot_left')
bot_right_ground, bot_right_odom = self.cast_ray(bot_right_point,self.tf,'bot_right')
#rospy.logdebug("Top Left Ground: %s", top_left_ground)
#rospy.logdebug("Bot Right Ground: %s", bot_right_ground)
width = np.array([0.,0.])
width[0] = bot_left_ground.point.x - bot_right_ground.point.x
width[1] = bot_left_ground.point.y - bot_right_ground.point.y
rospy.logdebug("Width: %s", width)
size = scipy.linalg.norm(width)
rospy.logdebug("Size: %s", size)
return size示例14: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
# Find circles given the camera image
dp = 1.1
minDist = 90
circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist)
# If no circles are detected then exit the program
if circles == None:
print "No circles found using these parameters"
sys.exit()
circles = numpy.uint16(numpy.around(circles))
# Draw the center of the circle closest to the top
ycoord = []
for i in circles[0,:]:
ycoord.append(i[1])
# Find the top circle in the frame
top_circ_y_coor = min(ycoord)
x_coor = 0
y_coor = 0
for i in circles[0,:]:
if i[1] == top_circ_y_coor:
# draw the center of the circle
#cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3)
# draw outer circle
#cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2)
x_coor = i[0]
y_coor = i[1]
cv2.circle(self.rgb_image,(327,247),2,(0,0,255),3)
# Set the coordinates for the center of the circle
self.center = (x_coor, y_coor)
#self.center = (328,248)
cv2.imshow('rgb', self.rgb_image)
cv2.waitKey(1)
def on_depth(self, image):
# Use numpy so that cv2 can use image
self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape((480,640))
nmask = numpy.isnan(self.depth_image)
#Find the minimum and the maximum of the depth image
Dmin = self.depth_image[~nmask].min()
Dmax = self.depth_image[~nmask].max()
# If a circle has been found find its depth and apply that to the ray
if self.center!=None:
ray = self.cam_model.projectPixelTo3dRay(self.center)
depth = self.depth_image[self.center[1]][self.center[0]]
# If the depth is not a number exit
if math.isnan(depth):
print "Incomplete information on depth at this point"
return
# Otherwise mulitply the depth by the unit vector of the ray
else:
print "depth", depth
cam_coor = [depth*ray[0],depth*ray[1],depth*ray[2]]
#print "camera frame coordinate:", cam_coor
else:
return
# Rescale to [0,1]
cv2.imshow('depth', self.depth_image / 2.0)
cv2.waitKey(1)
# Only use depth if the RGB image is running
if self.rgb_image_time is None:
rospy.loginfo('not using depth cause no RGB yet')
return
time_since_rgb = image.header.stamp - self.rgb_image_time
# Only use depth if it is close in time to the RGB image
if time_since_rgb > rospy.Duration(0.5):
rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb))
#return
# Find transform from camera frame to world frame
self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link",
image.header.stamp, rospy.Duration(1.0))
# Set up the point to be published
self.return_point.header.stamp = image.header.stamp
self.return_point.header.frame_id = self.cam_model.tfFrame()
self.return_point.point.x = cam_coor[0]
self.return_point.point.y = cam_coor[1]
self.return_point.point.z = cam_coor[2]
# Transform point to the baselink frame
self.return_point = self.tf_listener.transformPoint("base_link", self.return_point)
print "world frame coordinate:", self.return_point.point.x, \
self.return_point.point.y,self.return_point.point.z, "\n"
self.point_pub.publish(self.return_point)示例15: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPoint [as 别名]
#.........这里部分代码省略.........
delay = random.uniform(30, 5)
self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
return
else:
self.no_face_random_delay = None
if self.new_face:
self.face_counter = self.face_counter + 1
self.new_face = False
#cd = getPointDist(self.face)
# TODO decide action based on distance ?
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_advert)
string = self.getRandomFromArray(self.invite_strings)
self.snd_handle.say(string)
# TODO wait some min. time + say something
# after 20 seconds of no detected face, let's continue
if self.face.header.stamp + rospy.Duration(20) < rospy.Time.now():
string = self.getRandomFromArray(self.goodbye_strings)
self.snd_handle.say(string)
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self.face = None
return
self._head_buff.put(self.face)
def init_head(self):
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 2.0
p.point.y = 0.0
p.point.z = 1.7
self._head_buff.put(p)
def face_cb(self,point):
# transform point
try:
self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2))
pt = self._tf.transformPoint(self._robot_frame, point)
except:
rospy.logerr("Transform error")
return
if self.face is not None:
cd = self.getPointDist(pt) # current distance
dd = fabs(self.face_last_dist - cd) # change in distance
if dd < 0.2:
self.face.header = pt.header
# filter x,y,z values a bit
self.face.point.x = (15*self.face.point.x + pt.point.x)/16
self.face.point.y = (15*self.face.point.y + pt.point.y)/16
self.face.point.z = (15*self.face.point.z + pt.point.z)/16
else:
self.new_face = True
self.face = pt
self.face_last_dist = cd
else:
self.new_face = True
self.face = pt
if self.face_from == rospy.Time(0):
#self.snd_handle.say('Hello. Come closer.')
rospy.loginfo("New face.")
self.face_from = self.face.header.stamp