def test():
rospy.init_node('intersect_plane_test')
marker_pub = rospy.Publisher('table_marker', Marker)
pose_pub = rospy.Publisher('pose', PoseStamped)
int_pub = rospy.Publisher('intersected_points', PointCloud2)
tfl = tf.TransformListener()
# Test table
table = Table()
table.pose.header.frame_id = 'base_link'
table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
table.x_min = -0.5
table.x_max = 0.5
table.y_min = -0.5
table.y_max = 0.5
# A marker for that table
marker = Marker()
marker.header.frame_id = table.pose.header.frame_id
marker.id = 1
marker.type = Marker.LINE_STRIP
marker.action = 0
marker.pose = table.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0)
marker.frame_locked = False
marker.ns = 'table'
marker.points = [
Point(table.x_min,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_min, table.pose.pose.position.z),
]
marker.colors = []
marker.text = ''
# marker.mesh_resource = ''
marker.mesh_use_embedded_materials = False
marker.header.stamp = rospy.Time.now()
# A test pose
pose = PoseStamped()
pose.header = table.pose.header
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
intersection = cast_ray(pose, table, tfl)
cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
cloud.header = pose.header
while not rospy.is_shutdown():
marker_pub.publish(marker)
pose_pub.publish(pose)
int_pub.publish(cloud)
rospy.loginfo('published')
rospy.sleep(0.1)
def click_cb(self, msg):
self.click = rospy.Time.now()
# self.sameObject(pt, self.click_loc)
self.click_stale = False
with self.cursor_lock:
self.click_loc = self.selected_pt
msg = xyz_array_to_pointcloud2(np.array(self.cursor_pts_xyz))
msg.header.frame_id = '/table'
msg.header.stamp = rospy.Time.now()
self.click_stats_pub.publish(msg)
def click_cb(msg): point = msg.ray.direction origin = msg.ray.origin cloud = xyz_array_to_pointcloud2(np.array([[ point.x, point.y, point.z ]])) cloud.header = msg.ray.header int_pub.publish(cloud) print 'published', point
def shutdown(event):
pt = PointStamped()
pt.header.frame_id = '/table'
pt.header.stamp = now()
points = np.array([[100,100,100]])
points_msg = xyz_array_to_pointcloud2(points, now(), '/table')
topics.object_cloud(points_msg)
pt.point.x, pt.point.y, pt.point.z = points[0]
services.hilight_object(pt, ColorRGBA(0,0,0,0))
rospy.signal_shutdown(0)
def handle_get_cursor_stats(self, req):
while not (rospy.Time.now() - self.click) < self.click_duration:
rospy.sleep(0.05)
with self.cursor_lock:
msg = xyz_array_to_pointcloud2(
np.array([list(p) + [0] for p in self.projected_cursor]))
msg.header.frame_id = '/table'
msg.header.stamp = rospy.Time.now()
pt_msg = PointStamped()
pt_msg.header = msg.header
pt_msg.point.x, pt_msg.point.y, pt_msg.point.z = self.click_loc
return projector_interface.srv.GetCursorStatsResponse(msg, pt_msg)
def mouseMoved(self, location):
offset_x = float(self.offset_x.text())
offset_y = float(self.offset_y.text())
res = float(self.wid_resolution.text())
# Qt defines 0,0 in the upper left, so, and down as +y, so flip the y
x, y = location.x()*res+offset_x, -location.y()*res+offset_y
message = '%s x=%0.4fm, y=%0.4f' % (self.draw_mode, x, y)
self.window().statusBar().showMessage(message)
if (self.wid_tabs.currentIndex() == MOUSE_MODE_TEST) and (self.but_send.isEnabled()):
cloud = pointclouds.xyz_array_to_pointcloud2(np.array([[x, y, 0]]), frame_id=self.wid_frame.text())
self.cursor_pub.publish(cloud)
def run(self):
while not rospy.is_shutdown():
if not self.pose: continue
intersection = cast_ray(self.pose, self.table_pose, self.tfl)
if intersection:
cloud = xyz_array_to_pointcloud2(np.array([[
intersection.point.x,
intersection.point.y,
intersection.point.z
]]))
cloud.header.frame_id = self.plane_frame
cloud.header.stamp = self.pose.header.stamp
self.int_pub.publish(cloud)
self.rate.sleep()
def run(self):
while not rospy.is_shutdown():
if not self.pose: continue
intersection = cast_ray(self.pose, self.table_pose, self.tfl)
if intersection:
cloud = xyz_array_to_pointcloud2(
np.array([[
intersection.point.x, intersection.point.y,
intersection.point.z
]]))
cloud.header.frame_id = self.plane_frame
cloud.header.stamp = self.pose.header.stamp
self.int_pub.publish(cloud)
self.rate.sleep()
def detect(detect_srv):
detection = detect_srv.call()
objects = detection.clusters
header = rospy.Header()
header.stamp = rospy.Time.now()
cloud = None
table = None
if objects:
frame_id = objects[0].header.frame_id
cloud = xyz_array_to_pointcloud2(
np.asarray([location_from_cluster(obj) for obj in objects]), stamp=rospy.Time.now(), frame_id="/table"
)
if detection.table.pose.header.frame_id:
table = detection.table
return cloud, table
def detect(detect_srv):
detection = detect_srv.call()
objects = detection.clusters
header = rospy.Header()
header.stamp = rospy.Time.now()
cloud = None
table = None
if objects:
frame_id = objects[0].header.frame_id
cloud = xyz_array_to_pointcloud2(np.asarray(
[location_from_cluster(obj) for obj in objects]),
stamp=rospy.Time.now(),
frame_id='/table')
if detection.table.pose.header.frame_id:
table = detection.table
return cloud, table
from point_tests import sameObject, closestPoint
load('rosh_robot', globals())
xmin = -0.79651666
xmax = 0.06501853
ymin = 0.03261997
ymax = 0.50817382
# xs = np.linspace(xmin,xmax,4)
ys = np.linspace(ymin,ymax,3)
xs = np.arange(xmin+0.05, xmax, ys[1] - ys[0])
xx, yy = np.meshgrid(xs, ys)
points = np.array([(x,y,0) for x,y in zip(xx.ravel(), yy.ravel())])
points_msg = xyz_array_to_pointcloud2(points, now(), '/table')
topics.object_cloud(points_msg)
services.set_selection_method(0)
order = [
[0, 2, 1, 3],
[7, 5, 6, 4],
[8, 10, 9, 11],
[0, 4, 8 ],
[1, 5, 9 ],
[2, 6, 10],
[3, 7, 11]
]
targets = []
stds = []
import scipy.io
import numpy as np
load('rosh_robot', globals())
xmin = -0.79651666
xmax = 0.06501853
ymin = 0.03261997
ymax = 0.50817382
# xs = np.linspace(xmin,xmax,4)
ys = np.linspace(ymin,ymax,3)
xs = np.arange(xmin+0.05, xmax, ys[1] - ys[0])
xx, yy = np.meshgrid(xs, ys)
points = np.array([(x,y,0) for x,y in zip(xx.ravel(), yy.ravel())])
points_msg = xyz_array_to_pointcloud2(points, now(), '/table')
topics.object_cloud(points_msg)
services.set_selection_method(0)
services.clear_hilights()
# points = np.array([[xmin+(xmax-xmin)/2, ymin+(ymax-ymin)/2, 0]])
# points_msg = xyz_array_to_pointcloud2(points, now(), '/table')
# topics.object_cloud(points_msg)
def shutdown(event):
pt = PointStamped()
pt.header.frame_id = '/table'
pt.header.stamp = now()
points = np.array([[100,100,100]])
points_msg = xyz_array_to_pointcloud2(points, now(), '/table')
import roslib; roslib.load_manifest('pr2_python')
import numpy as np
from sensor_msgs.msg import PointCloud2
from pr2_python import pointclouds
points = np.random.random((10, 3))
cloud_msg = pointclouds.xyz_array_to_pointcloud2(points)
print len(cloud_msg.data)
points_new = pointclouds.pointcloud2_to_xyz_array(cloud_msg)
print points - points_new
print points
print points_new
def test():
rospy.init_node('intersect_plane_test')
marker_pub = rospy.Publisher('table_marker', Marker)
pose_pub = rospy.Publisher('pose', PoseStamped)
int_pub = rospy.Publisher('intersected_points', PointCloud2)
tfl = tf.TransformListener()
# Test table
table = Table()
table.pose.header.frame_id = 'base_link'
table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (
0, 0, 0, 1)
table.x_min = -0.5
table.x_max = 0.5
table.y_min = -0.5
table.y_max = 0.5
# A marker for that table
marker = Marker()
marker.header.frame_id = table.pose.header.frame_id
marker.id = 1
marker.type = Marker.LINE_STRIP
marker.action = 0
marker.pose = table.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = (0.005, 0.005, 0.005)
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0, 1.0,
1.0, 1.0)
marker.frame_locked = False
marker.ns = 'table'
marker.points = [
Point(table.x_min, table.y_min, table.pose.pose.position.z),
Point(table.x_min, table.y_max, table.pose.pose.position.z),
Point(table.x_max, table.y_max, table.pose.pose.position.z),
Point(table.x_max, table.y_min, table.pose.pose.position.z),
Point(table.x_min, table.y_min, table.pose.pose.position.z),
]
marker.colors = []
marker.text = ''
# marker.mesh_resource = ''
marker.mesh_use_embedded_materials = False
marker.header.stamp = rospy.Time.now()
# A test pose
pose = PoseStamped()
pose.header = table.pose.header
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0, 0,
0.5)
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(
0, -pi / 5, pi / 5)
intersection = cast_ray(pose, table, tfl)
cloud = xyz_array_to_pointcloud2(
np.array([[
intersection.point.x, intersection.point.y, intersection.point.z
]]))
cloud.header = pose.header
while not rospy.is_shutdown():
marker_pub.publish(marker)
pose_pub.publish(pose)
int_pub.publish(cloud)
rospy.loginfo('published')
rospy.sleep(0.1)
