def make_traj_multi_stage(req, demo_name, stage_num, tool_stage_info, prev_exp_cloud_pc2, verb_data_accessor, transform_type): assert len(req.object_clouds) == 1 exp_tool_cloud = None if stage_num == 0 else ru.pc2xyzrgb(prev_exp_cloud_pc2)[0] exp_target_cloud = ru.pc2xyzrgb(req.object_clouds[0])[0] clouds_frame_id = req.object_clouds[0].header.frame_id world_to_grip_transform_func = make_world_to_grip_transform_tf("%s_gripper_tool_frame" % current_stage_info.arms_used) return make_traj_multi_stage_do_work(demo_name, exp_target_cloud, clouds_frame_id, stage_num, tool_stage_info, exp_tool_cloud, verb_data_accessor, world_to_grip_transform_func, transform_type)
def load_cloud_from_sensor(input_topic, frame): import sensor_msgs.msg msg = rospy.wait_for_message(input_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1,3) xyz = ros_utils.transform_points(xyz, Globals.tf_listener, frame, msg.header.frame_id) return xyz
def get_transformed_clouds(bag,times): """ get transformed point clouds at times """ listener = mytf.TransformListener() all_clouds = [] all_cloud_times = [] all_hmats = [] for (i,(topic, msg, t)) in enumerate(bag.read_messages()): if topic=="/tf": listener.callback(msg) elif topic.endswith("points"): xyz, bgr = ros_utils.pc2xyzrgb(msg) all_clouds.append((xyz, bgr)) try: all_hmats.append(listener.transformer.lookup_transform_mat("/base_footprint",msg.header.frame_id)) except Exception: all_hmats.append(np.eye(4)) all_cloud_times.append(t.to_sec()) cloud_inds = np.searchsorted(all_cloud_times, times) clouds = [] for i in cloud_inds: xyz,rgb = all_clouds[i] hmat = all_hmats[i] xyz1 = np.c_[xyz.reshape(-1,3), np.ones((xyz.size/3,1))] xyz1_tf = np.dot(xyz1, hmat.T) xyz_tf = xyz1_tf[:,:3].reshape(xyz.shape) clouds.append((xyz_tf, rgb)) return clouds
def execute(self,userdata): """ - move head to the right place - get a point cloud returns: success, failure """ global HANDLES HANDLES=[] # Globals.pr2.head.set_pan_tilt(0, HEAD_TILT) Globals.pr2.larm.goto_posture('side') Globals.pr2.rarm.goto_posture('side') Globals.pr2.join_all() if HUMAN_GET_ROPE: xyz,frame = human_get_rope() xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", frame) pose_array = conversions.array_to_pose_array(xyz,"base_footprint") HANDLES.append(Globals.rviz.draw_curve(pose_array,id=3250864,rgba=(0,0,1,1))) xyz = curves.unif_resample(xyz, 100,tol=.01) else: msg = rospy.wait_for_message("/preprocessor/points", sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1,3) xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", msg.header.frame_id) userdata.points = xyz return "success"
def get_transformed_clouds(bag, times): """ get transformed point clouds at times """ listener = mytf.TransformListener() all_clouds = [] all_cloud_times = [] all_hmats = [] for (i, (topic, msg, t)) in enumerate(bag.read_messages()): if topic == "/tf": listener.callback(msg) elif topic.endswith("points"): xyz, bgr = ros_utils.pc2xyzrgb(msg) all_clouds.append((xyz, bgr)) try: all_hmats.append( listener.transformer.lookup_transform_mat( "/base_footprint", msg.header.frame_id)) except Exception: all_hmats.append(np.eye(4)) all_cloud_times.append(t.to_sec()) cloud_inds = np.searchsorted(all_cloud_times, times) clouds = [] for i in cloud_inds: xyz, rgb = all_clouds[i] hmat = all_hmats[i] xyz1 = np.c_[xyz.reshape(-1, 3), np.ones((xyz.size / 3, 1))] xyz1_tf = np.dot(xyz1, hmat.T) xyz_tf = xyz1_tf[:, :3].reshape(xyz.shape) clouds.append((xyz_tf, rgb)) return clouds
def load_cloud_from_sensor(input_topic, frame): import sensor_msgs.msg msg = rospy.wait_for_message(input_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) xyz = ros_utils.transform_points(xyz, Globals.tf_listener, frame, msg.header.frame_id) return xyz
def filter_pc2(cloud_pc2): cloud_xyz = (pc2xyzrgb(cloud_pc2)[0]).reshape(-1,3) cloud_xyz_down = voxel_downsample(cloud_xyz, .02) graph = ri.points_to_graph(cloud_xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) pose_array = juc.array_to_pose_array(good_xyzs, "base_footprint") Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")) raw_input("press enter when done looking") del Globals.handles[:] return xyz2pc(good_xyzs, cloud_pc2.header.frame_id)
def human_get_rope(): point_cloud = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xys = roi.get_polyline(bgr,'draw curve') uvs = np.int32(xys)[:,::-1] us,vs = uvs.T xyzs = xyz[us,vs] xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)] print "%i out of %i labeled points are good"%(len(xyzs_good), len(xyzs)) xyzs_unif = curves.unif_resample(xyzs_good,100,tol=.002) return xyzs_unif
def read_from_ros(cloud_topic, frame='base_footprint'): '''format: ros:/my/topic/points(:frame)''' import rospy from brett2 import ros_utils import sensor_msgs import time if rospy.get_name() == '/unnamed': rospy.init_node('cloud_reader', disable_signals=True) msg = rospy.wait_for_message(cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) tf_listener = ros_utils.get_tf_listener() return ros_utils.transform_points(xyz, tf_listener, frame, msg.header.frame_id)
def execute(self, userdata): """ - move head to the right place - get a point cloud returns: success, failure """ Globals.handles = [] draw_table() Globals.pr2.rgrip.set_angle(.08) Globals.pr2.lgrip.set_angle(.08) Globals.pr2.join_all() if not args.use_tracking: Globals.pr2.larm.goto_posture('side') Globals.pr2.rarm.goto_posture('side') else: try: increment_pose(Globals.pr2.rarm, [0, 0, .02]) except PR2.IKFail: print "couldn't raise right arm" try: increment_pose(Globals.pr2.larm, [0, 0, .02]) except PR2.IKFail: print "couldn't raise left arm" Globals.pr2.rgrip.set_angle(.08) Globals.pr2.lgrip.set_angle(.08) Globals.pr2.join_all() if args.delay_before_look > 0: rospy.loginfo('sleeping for %f secs before looking', args.delay_before_look) rospy.sleep(args.delay_before_look) if args.test: xyz = np.squeeze( np.asarray(demos[select_from_list(demos.keys())]["cloud_xyz"])) elif args.use_tracking: msg = rospy.wait_for_message("/tracker/object", TrackedObject) xyz = [(pt.x, pt.y, pt.z) for pt in msg.rope.nodes] else: msg = rospy.wait_for_message(args.cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", msg.header.frame_id) userdata.points = xyz return "success"
def main(): import argparse parser = argparse.ArgumentParser() parser.add_argument('--dataset', default='overhand_knot', help='name of dataset') parser.add_argument('--method', choices=['geodesic_dist', 'shape_context', 'geodesic_dist+shape_context'], default='geodesic_dist', help='matching algorithm') parser.add_argument('--input_mode', choices=['from_dataset', 'kinect', 'file'], default='kinect', help='input cloud acquisition method') parser.add_argument('--cloud_topic', default='/preprocessor/kinect1/points', help='ros topic for kinect input mode') parser.add_argument('--input_file', help='input file for --input_mode=file') parser.add_argument('--show_shape_context', action='store_true') args = parser.parse_args() dataset = recognition.DataSet.LoadFromTaskDemos(args.dataset) # read input to match input_xyz = None if args.input_mode == 'from_dataset': key = select_from_list(sorted(dataset.keys())) input_xyz = np.squeeze(np.asarray(dataset[key]['cloud_xyz'])) elif args.input_mode == 'kinect': import rospy from brett2 import ros_utils import sensor_msgs import time if rospy.get_name() == '/unnamed': rospy.init_node('matcher', disable_signals=True) msg = rospy.wait_for_message(args.cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) tf_listener = ros_utils.get_tf_listener() time.sleep(1) # so tf works input_xyz = ros_utils.transform_points(xyz, tf_listener, "ground", msg.header.frame_id) elif args.input_mode == 'file': input_xyz = np.load(args.input_file)['cloud_xyz'] else: raise NotImplementedError # create the matcher matcher = None if args.method == 'geodesic_dist': matcher = recognition.GeodesicDistMatcher(dataset) elif args.method == 'shape_context': matcher = recognition.ShapeContextMatcher(dataset) elif args.method == 'geodesic_dist+shape_context': matcher = recognition.CombinedNNMatcher(dataset, [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1]) else: raise NotImplementedError # do the matching best_match_name, best_match_cost = matcher.match(input_xyz) print 'Best match name:', best_match_name, 'with cost:', best_match_cost draw_comparison(left_cloud=input_xyz, right_cloud=dataset[best_match_name], show_shape_context=args.show_shape_context)
def read_from_ros(cloud_topic, frame='ground'): '''format: ros:/my/topic/points(:frame)''' import rospy from brett2 import ros_utils import sensor_msgs import time if rospy.get_name() == '/unnamed': rospy.init_node('cloud_reader', disable_signals=True) msg = rospy.wait_for_message(cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) tf_listener = ros_utils.get_tf_listener() return ros_utils.transform_points(xyz, tf_listener, frame, msg.header.frame_id)
def human_get_rope(): point_cloud = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xys = roi.get_polyline(bgr, 'draw curve') uvs = np.int32(xys)[:, ::-1] us, vs = uvs.T xyzs = xyz[us, vs] xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)] print "%i out of %i labeled points are good" % (len(xyzs_good), len(xyzs)) xyzs_unif = curves.unif_resample(xyzs_good, 100, tol=.002) return xyzs_unif
def create_obstacle_env(env): clone=env.CloneSelf(1) point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", point_cloud.header.frame_id) #xyz = xyz[250:, 250:] clusters = tabletop.segment_tabletop_scene(xyz,'base_footprint') bounds = tabletop.get_table_dimensions(xyz) kinbodies.create_box_from_bounds(clone,bounds,"table") print len(clusters),"clusters" for (i_clu,clu) in enumerate(clusters): x,y,z,r,h = tabletop.get_cylinder(clu) #print kinbodies.create_cylinder(clone, (x,y,z),r,h, name="cylinder%i"%i_clu) return clone
def handle_initialization_request(req): "rope initialization service" xyz, _ = pc2xyzrgb(req.cloud) xyz = np.squeeze(xyz) obj_type = determine_object_type(xyz, plotting=False) print "object type:", obj_type if obj_type == "towel": xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :] center, (height, width), angle = cv2.minAreaRect( xyz[:, :2].astype('float32').reshape(1, -1, 2)) angle *= np.pi / 180 corners = np.array(center)[None, :] + np.dot( np.array([[-height / 2, -width / 2], [-height / 2, width / 2], [height / 2, width / 2], [height / 2, -width / 2]]), np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]])) resp = bs.InitializationResponse() resp.objectInit.type = "towel_corners" resp.objectInit.towel_corners.polygon.points = [ gm.Point(corner[0], corner[1], np.median(xyz[:, 2])) for corner in corners ] resp.objectInit.towel_corners.header = req.cloud.header print req.cloud.header poly_pub.publish(resp.objectInit.towel_corners) if obj_type == "rope": total_path_3d = find_path_through_point_cloud(xyz, plotting=False) resp = bs.InitializationResponse() resp.objectInit.type = "rope" rope = resp.objectInit.rope = bm.Rope() rope.header = req.cloud.header rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d] rope.radius = .006 rospy.logwarn( "TODO: actually figure out rope radius from data. setting to .4cm") pose_array = gm.PoseArray() pose_array.header = req.cloud.header pose_array.poses = [ gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1)) for point in rope.nodes ] marker_handles[0] = rviz.draw_curve(pose_array, 0) return resp
def handle_initialization_request(req): "rope initialization service" xyz, _ = pc2xyzrgb(req.cloud) xyz = np.squeeze(xyz) obj_type = determine_object_type(xyz) print "object type:", obj_type if obj_type == "towel": xyz = xyz[(xyz[:,2]-np.median(xyz[:,2]))<.05,:] center, (height, width), angle = cv2.minAreaRect(xyz[:,:2].astype('float32').reshape(1,-1,2)) angle *= np.pi/180 corners = np.array(center)[None,:] + np.dot( np.array([[-height/2,-width/2], [-height/2,width/2], [height/2, width/2], [height/2, -width/2]]), np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle),np.cos(angle)]])) resp = bs.InitializationResponse() resp.objectInit.type = "towel_corners" resp.objectInit.towel_corners.polygon.points = [gm.Point(corner[0], corner[1], np.median(xyz[:,2])) for corner in corners] resp.objectInit.towel_corners.header = req.cloud.header print req.cloud.header poly_pub.publish(resp.objectInit.towel_corners) if obj_type == "rope": total_path_3d = find_path_through_point_cloud(xyz, plotting=False) resp = bs.InitializationResponse() resp.objectInit.type = "rope" rope = resp.objectInit.rope = bm.Rope() rope.header = req.cloud.header rope.nodes = [gm.Point(x,y,z) for (x,y,z) in total_path_3d] rope.radius = .005 rospy.logwarn("TODO: actually figure out rope radius from data. setting to .5cm") pose_array = gm.PoseArray() pose_array.header = req.cloud.header pose_array.poses = [gm.Pose(position=point, orientation=gm.Quaternion(0,0,0,1)) for point in rope.nodes] marker_handles[0] = rviz.draw_curve(pose_array,0) return resp
def execute(self,userdata): """ - move head to the right place - get a point cloud returns: success, failure """ Globals.handles = [] draw_table() Globals.pr2.rgrip.set_angle(.08) Globals.pr2.lgrip.set_angle(.08) Globals.pr2.join_all() if not args.use_tracking: Globals.pr2.larm.goto_posture('side') Globals.pr2.rarm.goto_posture('side') else: try: increment_pose(Globals.pr2.rarm, [0,0,.02]) except PR2.IKFail: print "couldn't raise right arm" try: increment_pose(Globals.pr2.larm, [0,0,.02]) except PR2.IKFail: print "couldn't raise left arm" Globals.pr2.rgrip.set_angle(.08) Globals.pr2.lgrip.set_angle(.08) Globals.pr2.join_all() if args.delay_before_look > 0: rospy.loginfo('sleeping for %f secs before looking', args.delay_before_look) rospy.sleep(args.delay_before_look) if args.test: xyz = np.squeeze(np.asarray(demos[select_from_list(demos.keys())]["cloud_xyz"])) elif args.use_tracking: msg = rospy.wait_for_message("/tracker/object", TrackedObject) xyz = [(pt.x, pt.y, pt.z) for pt in msg.rope.nodes] else: msg = rospy.wait_for_message(args.cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1,3) xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", msg.header.frame_id) ELOG.log('LookAtObject', 'xyz', xyz) userdata.points = xyz return "success"
def get_center(): pc = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2) xyz, bgr = pc2xyzrgb(pc) bgr = bgr.copy() hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV) h = hsv[:, :, 0] s = hsv[:, :, 1] v = hsv[:, :, 2] mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100) labels, max_label = ndi.label(mask) centers = [] for label in xrange(1, max_label + 1): us, vs = np.nonzero(labels == label) if len(us) >= 8: pts = xyz[us, vs, :] centers.append(pts.mean(axis=0)) centers = np.array(centers) if len(centers) > 0: listener.waitForTransform(pc.header.frame_id, "l_gripper_tool_frame", rospy.Time(0), rospy.Duration(1)) gripper_trans, _ = listener.lookupTransform(pc.header.frame_id, "l_gripper_tool_frame", rospy.Time(0)) gripper_trans = np.array(gripper_trans) dists = ssd.cdist(centers, gripper_trans[None, :]) closest_center = centers[dists.argmin()] if dists.min() < .025: print "successfully got point" return gripper_trans, closest_center else: print "fail" return None contours = cv2.findContours(mask.astype('uint8'), cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(bgr, contours, -1, (0, 255, 0)) cv2.imshow("bgr", bgr) cv2.waitKey(10)
def get_center(): pc = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2) xyz, bgr = pc2xyzrgb(pc) bgr = bgr.copy() hsv = cv2.cvtColor(bgr,cv2.COLOR_BGR2HSV) h=hsv[:,:,0] s=hsv[:,:,1] v=hsv[:,:,2] mask = ((h<10) | (h>150)) & (s > 100) & (v > 100) labels,max_label = ndi.label(mask) centers = [] for label in xrange(1, max_label+1): us, vs = np.nonzero(labels == label) if len(us) >= 8: pts = xyz[us, vs, :] centers.append(pts.mean(axis=0)) centers= np.array(centers) if len(centers) > 0: listener.waitForTransform(pc.header.frame_id, "l_gripper_tool_frame", rospy.Time(0), rospy.Duration(1)) gripper_trans,_ = listener.lookupTransform(pc.header.frame_id, "l_gripper_tool_frame", rospy.Time(0)) gripper_trans = np.array(gripper_trans) dists = ssd.cdist(centers, gripper_trans[None,:]) closest_center = centers[dists.argmin()] if dists.min() < .025: print "successfully got point" return gripper_trans, closest_center else: print "fail" return None contours = cv2.findContours(mask.astype('uint8'),cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(bgr,contours,-1,(0,255,0)) cv2.imshow("bgr",bgr) cv2.waitKey(10)
def callback(request): global HANDLES HANDLES = [] xyzs, _ = pc2xyzrgb(request.point_cloud) new_mins = xyzs.reshape(-1, 3).min(axis=0) new_maxes = xyzs.reshape(-1, 3).max(axis=0) new_obj_xyz = (new_mins + new_maxes) / 2 new_obj_dim = new_maxes - new_mins f.fit(np.atleast_2d(obj_xyz), np.atleast_2d(new_obj_xyz), 0.001, 0.001) new_xyzs, new_mats = f.transform_frames(old_xyzs, conversions.quats2mats(old_quats)) new_quats = conversions.mats2quats(new_mats) show_object_and_trajectory(new_xyzs, new_obj_xyz, obj_quat, new_obj_dim, NEW_ID) show_object_and_trajectory(xyzquat[:, :3], obj_xyz, obj_quat, obj_dim, OLD_ID) Globals.pr2.update_rave() joint_positions, inds = trajectories.make_joint_traj( new_xyzs, new_quats, Globals.pr2.rarm.manip, "base_link", "r_wrist_roll_link", filter_options=18 ) response = PushResponse() if joint_positions is not None: Globals.pr2.rarm.goto_joint_positions(joint_positions[0]) # current_joints = Globals.pr2.rarm.get_joint_positions()[None,:] joint_positions, joint_velocities, times = retiming.make_traj_with_limits( joint_positions, Globals.pr2.rarm.vel_limits, Globals.pr2.rarm.acc_limits, smooth=True ) # joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001) Globals.pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times) response.success = True else: response.success = False return response
parser = argparse.ArgumentParser() parser.add_argument("out") parser.add_argument("--cloud_in", default="/drop/points", nargs="?") parser.add_argument("--dont_transform", action="store_true") args = parser.parse_args() from brett2 import ros_utils from jds_utils import conversions import roslib roslib.load_manifest('tf') import tf import rospy import numpy as np import sensor_msgs.msg as sm from brett2.ros_utils import transformPointCloud2 rospy.init_node("get_point_cloud") listener = tf.TransformListener() rospy.sleep(.3) pc = rospy.wait_for_message(args.cloud_in, sm.PointCloud2) if args.dont_transform: pc_tf = pc else: pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id) xyz, bgr = ros_utils.pc2xyzrgb(pc_tf) np.savez(args.out, xyz=xyz, bgr=bgr)
def main(): import argparse parser = argparse.ArgumentParser() parser.add_argument('--dataset', default='overhand_knot', help='name of dataset') parser.add_argument('--method', choices=[ 'geodesic_dist', 'shape_context', 'geodesic_dist+shape_context' ], default='geodesic_dist', help='matching algorithm') parser.add_argument('--input_mode', choices=['from_dataset', 'kinect', 'file'], default='kinect', help='input cloud acquisition method') parser.add_argument('--cloud_topic', default='/preprocessor/kinect1/points', help='ros topic for kinect input mode') parser.add_argument('--input_file', help='input file for --input_mode=file') parser.add_argument('--show_shape_context', action='store_true') args = parser.parse_args() dataset = recognition.DataSet.LoadFromTaskDemos(args.dataset) # read input to match input_xyz = None if args.input_mode == 'from_dataset': key = select_from_list(sorted(dataset.keys())) input_xyz = np.squeeze(np.asarray(dataset[key]['cloud_xyz'])) elif args.input_mode == 'kinect': import rospy from brett2 import ros_utils import sensor_msgs import time if rospy.get_name() == '/unnamed': rospy.init_node('matcher', disable_signals=True) msg = rospy.wait_for_message(args.cloud_topic, sensor_msgs.msg.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(msg) xyz = xyz.reshape(-1, 3) tf_listener = ros_utils.get_tf_listener() time.sleep(1) # so tf works input_xyz = ros_utils.transform_points(xyz, tf_listener, "ground", msg.header.frame_id) elif args.input_mode == 'file': input_xyz = np.load(args.input_file)['cloud_xyz'] else: raise NotImplementedError # create the matcher matcher = None if args.method == 'geodesic_dist': matcher = recognition.GeodesicDistMatcher(dataset) elif args.method == 'shape_context': matcher = recognition.ShapeContextMatcher(dataset) elif args.method == 'geodesic_dist+shape_context': matcher = recognition.CombinedNNMatcher( dataset, [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1]) else: raise NotImplementedError # do the matching best_match_name, best_match_cost = matcher.match(input_xyz) print 'Best match name:', best_match_name, 'with cost:', best_match_cost draw_comparison(left_cloud=input_xyz, right_cloud=dataset[best_match_name], show_shape_context=args.show_shape_context)
from image_proc.pcd_io import load_xyzrgb import rope_vision.rope_initialization as ri import numpy as np import sys import networkx as nx import itertools from mayavi import mlab from brett2 import ros_utils import sensor_msgs.msg as sm import rospy rospy.init_node("test_rope_init") cloud = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2) xyz,rgb = ros_utils.pc2xyzrgb(cloud) # xyz, rgb = load_xyzrgb("data000000000004.pcd") # xyz = xyz.reshape(-1,3) #xyz = np.squeeze(np.loadtxt("/tmp/rope.txt")) total_path_3d = ri.find_path_through_point_cloud(xyz, plotting=True)
import roslib roslib.load_manifest("verb_msgs") from verb_msgs.srv import * from brett2.ros_utils import xyzrgb2pc import geometry_msgs.msg as gm import sensor_msgs.msg as sm import numpy as np if __name__ == "__main__": if rospy.get_name() == "/unnamed": rospy.init_node("test_tabletop", disable_signals=True) pr2 = PR2.create() rviz = ros_utils.RvizWrapper.create() rospy.sleep(1) point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xyz = ros_utils.transform_points(xyz, pr2.tf_listener, "base_footprint", point_cloud.header.frame_id) clusters = tabletop.segment_tabletop_scene(xyz, "base_footprint", plotting2d=True, plotting3d=True) if False: push_svc = rospy.ServiceProxy("push", Push) req = PushRequest() xyz0 = clusters[np.argmax(map(len, clusters))].reshape(1, -1, 3) rgb0 = np.zeros(xyz0.shape, 'uint8') point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.point_cloud = point_cloud push_svc.call(req)
def exec_traj(req, traj_ik_func=ik_functions.do_traj_ik_graph_search, obj_pc=None, obj_name="", can_move_lower=True): assert isinstance(req, ExecTrajectoryRequest) traj = req.traj return exec_traj_do_work(traj.l_gripper_poses.poses, traj.l_gripper_angles, traj.r_gripper_poses.poses, traj.r_gripper_angles, traj_ik_func, ros_utils.pc2xyzrgb(obj_pc)[0], obj_name, can_move_lower)
import roslib; roslib.load_manifest("verb_msgs") from verb_msgs.srv import * from brett2.ros_utils import xyzrgb2pc import geometry_msgs.msg as gm import sensor_msgs.msg as sm import numpy as np if __name__ == "__main__": if rospy.get_name() == "/unnamed": rospy.init_node("test_tabletop", disable_signals=True) pr2 = PR2.create() rviz = ros_utils.RvizWrapper.create() rospy.sleep(1) point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xyz = ros_utils.transform_points(xyz, pr2.tf_listener, "base_footprint", point_cloud.header.frame_id) clusters = tabletop.segment_tabletop_scene(xyz,"base_footprint",plotting2d=True, plotting3d=True) if False: push_svc = rospy.ServiceProxy("push",Push) req = PushRequest() xyz0 = clusters[np.argmax(map(len,clusters))].reshape(1,-1,3) rgb0 = np.zeros(xyz0.shape,'uint8') point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.point_cloud = point_cloud push_svc.call(req) if False: pour_srv = rospy.ServiceProxy("pour", Pour) req = PourRequest()
from jds_image_proc.pcd_io import load_xyzrgb import rope_vision.rope_initialization as ri import numpy as np import sys import networkx as nx import itertools from mayavi import mlab from brett2 import ros_utils import sensor_msgs.msg as sm import rospy rospy.init_node("test_rope_init") cloud = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2) xyz, rgb = ros_utils.pc2xyzrgb(cloud) # xyz, rgb = load_xyzrgb("data000000000004.pcd") # xyz = xyz.reshape(-1,3) #xyz = np.squeeze(np.loadtxt("/tmp/rope.txt")) total_path_3d = ri.find_path_through_point_cloud(xyz, plotting=True)
bagfile = traj["bag_file"].value bag = rosbag.Bag(bagfile) seg_dicts = yamlfile["segments"] listener = mytf.TransformListener() t_start_bag = bag.read_messages().next()[2].to_sec() cloud_times, cloud_xyzs, cloud_bgrs = [],[],[] for (topic, msg, t) in bag.read_messages(): if topic=="/tf": listener.callback(msg) elif hasattr(msg, "row_step"): xyz, bgr = pc2xyzrgb(msg) try: xyz_tf = transform_points(xyz, listener, traj["ref_frame"].value, args.kinect_frame) cloud_xyzs.append(xyz_tf) cloud_bgrs.append(bgr) cloud_times.append(t.to_sec()) except Exception: print "failed to transform points" traceback.print_exc() cloud_times = np.array(cloud_times) cloud_times -= t_start_bag traj_times = np.asarray(traj["times"])-t_start_bag for (i_seg,seg_dict) in enumerate(seg_dicts): i_start, i_stop = np.searchsorted(traj_times, (seg_dict["start"],seg_dict["stop"])) seg_name = "%s.%.2i"%(traj.name,i_seg)
import roslib; roslib.load_manifest("rospy") import rospy rospy.init_node("write clouds",disable_signals = True) from brett2.ros_utils import pc2xyzrgb import sensor_msgs.msg as sm import numpy as np while not rospy.is_shutdown(): fname = raw_input("filename?: ") print "waiting for point cloud on input topic" msg = rospy.wait_for_message("cloud_in", sm.PointCloud2) print "ok" xyz,_ = pc2xyzrgb(msg) np.savetxt(fname, np.squeeze(xyz))
def callback(req): xyz, rgb = pc2xyzrgb(req.cloud_in) rgb = rgb.copy() gc = GetClick() cv2.setMouseCallback("rgb", gc.callback) while not gc.done: cv2.imshow("rgb", rgb) cv2.waitKey(10) gm = GetMouse() xy_corner1 = np.array(gc.xy) cv2.setMouseCallback("rgb", gm.callback) while not gm.done: img = rgb.copy() if gm.xy is not None: xy_corner2 = np.array(gm.xy) cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0)) cv2.imshow("rgb",img) cv2.waitKey(10) xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0) xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0) #mask = np.zeros(xyz.shape[:2],dtype='uint8') #rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy])) #tmp1 = np.zeros((1, 13 * 5)) #tmp2 = np.zeros((1, 13 * 5)) #result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT) #from cv2 import cv #mask[mask == cv.GC_BGD] = 0 #mask[mask == cv.GC_PR_BGD] = 63 #mask[mask == cv.GC_FGD] = 255 #mask[mask == cv.GC_PR_FGD] = 192 #cv2.imshow('mask',mask) #cv2.waitKey(10) #table_height = get_table_height(xyz) if args.grabcut: rospy.loginfo("using grabcut") xl, yl = xy_tl w, h = xy_br - xy_tl mask = np.zeros((h,w),dtype='uint8') mask.fill(cv2.GC_PR_BGD) if args.init == "height": initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50) mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD elif args.init == "middle": mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD tmp1 = np.zeros((1, 13 * 5)) tmp2 = np.zeros((1, 13 * 5)) cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK) else: rospy.loginfo("using table height") table_height = rospy.get_param("table_height") xl, yl = xy_tl w, h = xy_br - xy_tl mask = np.zeros((h,w),dtype='uint8') mask[np.isfinite(xyz[yl:yl+h, xl:xl+w,2]) & (xyz[yl:yl+h, xl:xl+w,2] > table_height+.02)] = 1 mask = mask % 2 if (args.erode > 0): mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8') contours = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0),thickness=2) cv2.imshow('rgb', rgb) cv2.waitKey(10) zsel = xyz[yl:yl+h, xl:xl+w, 2] mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1) resp = ProcessCloudResponse() xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')] rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')] resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id) if (args.plotting): pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id) Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")] return resp
arg_names = F[verb]["arg_names"] print "arguments: %s" % " ".join(arg_names) pr2.join_all() pc = rospy.wait_for_message("/drop/points", sm.PointCloud2) pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id) n_sel = F[verb]["nargs"].value arg_clouds = [] for i_sel in xrange(n_sel): pc_sel = seg_svc.call( ProcessCloudRequest(cloud_in=pc_tf)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) arg_clouds.append(xyz.reshape(-1, 3)) make_and_execute_verb_traj(verb, arg_clouds) raw_input("press enter to continue") elif verb in ["swap-tool"]: l_switch_posture = np.array( [0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864]) lr = select_from_list(["left", "right"]) if lr == 'l': arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture else: arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(
if os.path.exists(outfilename): os.remove(outfilename) outfile = h5py.File(outfilename, "w") def array_contains(arr, pt): return np.any(np.all((pt[None,:] == arr), axis=1)) def get_good_inds(orig_xyz, good_xyz): good_inds = [] for i, xyz in enumerate(orig_xyz): if array_contains(good_xyzs, xyz): good_inds.append(i) return good_inds for object_name in object_names: pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) xyz = xyz.reshape(-1,3) rgb = rgb.reshape(-1,3) if args.do_filtering: xyz_down = voxel_downsample(xyz, .02) graph = ri.points_to_graph(xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) good_inds = get_good_inds(xyz, good_xyzs) good_rgbs = rgb[good_inds] else: good_xyzs, good_rgbs = xyz, rgb outfile.create_group(object_name) outfile[object_name]["xyz"] = good_xyzs #outfile[object_name]["rgb"] = good_rgbs
def callback(req): assert isinstance(req, MakeTrajectoryRequest) if req.verb not in h5file: rospy.logerr("verb %s was not found in library",req.verb) demo_group = h5file[req.verb]["demos"] new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] rospy.logwarn("using first demo") best_demo_name = demo_group.keys()[0] best_demo = demo_group[best_demo_name] assert "transform" in verb_info[req.verb] transform_type = verb_info[req.verb]["transform"] x_nd = voxel_downsample(asarray(best_demo["object_clouds"].values()[0]),.02) y_md = voxel_downsample(new_object_clouds[0],.02) if transform_type == "tps": warp = registration.tps_rpm(x_nd, y_md, plotting = 4, reg_init = 2, reg_final = .1, n_iter = 39) elif transform_type == "translation2d": warp = registration.Translation2d() warp.fit(x_nd, y_md) elif transform_type == "rigid2d": warp = registration.Rigid2d() warp.fit(x_nd, y_md) else: raise Exception("transform type %s is not yet implemented"%transform_type) this_verb_info = verb_info[req.verb] l_offset,r_offset = np.zeros(3), np.zeros(3) if "r_tool" in this_verb_info: r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) if "l_tool" in this_verb_info: l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) if "r_offset" in this_verb_info: r_offset += asarray(this_verb_info["r_offset"]) if "l_offset" in this_verb_info: l_offset += asarray(this_verb_info["l_offset"]) arms_used = best_demo["arms_used"].value warped_demo = warping.transform_demo(warp, best_demo,arms_used in 'lb', arms_used in 'rb',object_clouds=True, l_offset = l_offset, r_offset = r_offset) resp = MakeTrajectoryResponse() traj = resp.traj if arms_used == "l": traj.arms_used = "l" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"]) traj.gripper0_angles = warped_demo["l_gripper_angles"] if "l_tool" in this_verb_info: traj.gripper0_angles *= 0 elif arms_used == "r": traj.arms_used = "r" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"]) traj.gripper0_angles = warped_demo["r_gripper_angles"] if "r_tool" in this_verb_info: traj.gripper0_angles *= 0 elif arms_used == "b": traj.arms_used = "b" traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"]) traj.gripper1_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"]) traj.gripper0_angles = warped_demo["l_gripper_angles"] traj.gripper1_angles = warped_demo["r_gripper_angles"] if "l_tool" in this_verb_info: traj.gripper0_angles *= 0 if "r_tool" in this_verb_info: traj.gripper1_angles *= 0 traj.gripper0_poses.header.frame_id = req.object_clouds[0].header.frame_id traj.gripper1_poses.header.frame_id = req.object_clouds[0].header.frame_id Globals.handles = [] plot_original_and_warped_demo(best_demo, warped_demo, traj) return resp
def save_pcs(clouds_pc2, save_file): os.chdir(osp.join(osp.dirname(__file__), "exp_pcs")) for i, cloud_pc2 in enumerate(clouds_pc2): np.savetxt("%s.pc%i" % (save_file, i), voxel_downsample(pc2xyzrgb(cloud_pc2)[0], 0.02))
def handle_initialization_request(req): "rope initialization service" if args.save_requests: fname = "/tmp/init_req_%i.pkl"%time.time() with open(fname,"w") as fh: cPickle.dump(req, fh) print "saved", fname xyz, _ = pc2xyzrgb(req.cloud) xyz = np.squeeze(xyz) obj_type = determine_object_type(xyz, plotting=args.plotting) print "object type:", obj_type if obj_type == "towel": xyz = xyz[(xyz[:,2]-np.median(xyz[:,2]))<.05,:] center, (height, width), angle = cv2.minAreaRect(xyz[:,:2].astype('float32').reshape(1,-1,2)) angle *= np.pi/180 corners = np.array(center)[None,:] + np.dot( np.array([[-height/2,-width/2], [-height/2,width/2], [height/2, width/2], [height/2, -width/2]]), np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle),np.cos(angle)]])) resp = bs.InitializationResponse() resp.objectInit.type = "towel_corners" resp.objectInit.towel_corners.polygon.points = [gm.Point(corner[0], corner[1], np.median(xyz[:,2])) for corner in corners] resp.objectInit.towel_corners.header = req.cloud.header print req.cloud.header poly_pub.publish(resp.objectInit.towel_corners) elif obj_type == "rope": if args.simple_rope_init: total_path_3d = ri.find_path_through_point_cloud_simple(xyz, plotting=args.plotting) else: total_path_3d = ri.find_path_through_point_cloud(xyz, plotting=args.plotting) resp = bs.InitializationResponse() resp.objectInit.type = "rope" rope = resp.objectInit.rope = bm.Rope() rope.header = req.cloud.header rope.nodes = [gm.Point(x,y,z) for (x,y,z) in total_path_3d] rope.radius = .006 print "lengths:", [np.linalg.norm(total_path_3d[i+1] - total_path_3d[i]) for i in xrange(len(total_path_3d)-1)] rospy.loginfo("created a rope with %i nodes, each has length %.2f, radius %.2f", len(rope.nodes), np.linalg.norm(total_path_3d[i+1] - total_path_3d[i]), rope.radius) pose_array = gm.PoseArray() pose_array.header = req.cloud.header pose_array.poses = [gm.Pose(position=point, orientation=gm.Quaternion(0,0,0,1)) for point in rope.nodes] # marker_handles[0] = rviz.draw_curve(pose_array,0) elif obj_type == "box": xyz = xyz[abs(xyz[:,2]-np.median(xyz[:,2]))<.05,:] center, (height, width), angle = cv2.minAreaRect(xyz[:,:2].astype('float32').reshape(1,-1,2)) angle *= np.pi/180 print 'width', width, 'height', height, 'angle', 'median', np.median(xyz[:,2]),angle*180/np.pi resp = bs.InitializationResponse() resp.objectInit.type = "box" rope = resp.objectInit.box = bm.Box() #rope.header = req.cloud.header rope.center.x = center[0] rope.center.y = center[1] rope.center.z = np.median(xyz[:,2])/2.0 rope.extents.x = height rope.extents.y = width rope.extents.z = np.median(xyz[:,2]) rope.angle = angle return resp
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')] resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id) if (args.plotting): pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id) Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")] return resp if __name__ == "__main__": if args.test: if rospy.get_name() == "/unnamed": rospy.init_node("test_interactive_segmentation_service",disable_signals=True) listener = tf.TransformListener() pc = rospy.wait_for_message("/drop/points", sm.PointCloud2) pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id) Globals.setup() req = ProcessCloudRequest() req.cloud_in = pc_tf resp = callback(req) xyz, rgb = pc2xyzrgb(resp.cloud_out) pose_array = conversions.array_to_pose_array(xyz.reshape(-1,3), 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST,width=.002, ns = 'segmentation')) else: rospy.init_node("test_interactive_segmentation_service",disable_signals=True) Globals.setup() service = rospy.Service("interactive_segmentation", ProcessCloud, callback) time.sleep(1000000)
def make_traj_multi_stage(req, current_stage_info, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None): assert isinstance(req, MakeTrajectoryRequest) prev_exp_clouds = None if stage_num == 0 else [pc2xyzrgb(cloud)[0] for cloud in prev_exp_clouds] cur_exp_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] clouds_frame_id = req.object_clouds[0].header.frame_id return make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func)
def make_traj(req): """ Generate a trajectory using warping See MakeTrajectory service description (TODO) should be able to specify a specific demo """ assert isinstance(req, MakeTrajectoryRequest) new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] scene_info = "PLACEHOLDER" best_demo_name, best_demo_info = verbs.get_closest_demo( req.verb, scene_info) best_demo_data = verbs.get_demo_data(best_demo_name) transform_type = "tps" old_object_clouds = [ best_demo_data["object_clouds"][obj_name]["xyz"] for obj_name in best_demo_data["object_clouds"].keys() ] if len(old_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(old_object_clouds[0], .02) y_md = voxel_downsample(new_object_clouds[0], .02) if transform_type == "tps": #warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) warp = registration.tps_rpm(x_nd, y_md, plotting=2, reg_init=2, reg_final=.05, n_iter=10, verbose=False) elif transform_type == "translation2d": warp = registration.Translation2d() warp.fit(x_nd, y_md) elif transform_type == "rigid2d": warp = registration.Rigid2d() warp.fit(x_nd, y_md) else: raise Exception("transform type %s is not yet implemented" % transform_type) l_offset, r_offset = np.zeros(3), np.zeros(3) #if "r_tool" in best_demo_info: #r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) #if "l_tool" in best_demo_info: #l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) arms_used = best_demo_info["arms_used"] warped_demo_data = warping.transform_verb_demo(warp, best_demo_data) resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used if arms_used in "lb": traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses( warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"]) traj.l_gripper_angles = warped_demo_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = req.object_clouds[ 0].header.frame_id if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0 if arms_used in "rb": traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses( warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"]) traj.r_gripper_angles = warped_demo_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = req.object_clouds[ 0].header.frame_id if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0 Globals.handles = [] plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append( Globals.rviz.draw_curve(pose_array, rgba=(0, 0, 1, 1), width=.01, type=Marker.CUBE_LIST)) return resp
def make_traj(req): """ Generate a trajectory using warping See MakeTrajectory service description (TODO) should be able to specify a specific demo """ assert isinstance(req, MakeTrajectoryRequest) new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] scene_info = "PLACEHOLDER" best_demo_name, best_demo_info = verbs.get_closest_demo(req.verb, scene_info) best_demo_data = verbs.get_demo_data(best_demo_name) transform_type = "tps" old_object_clouds = [best_demo_data["object_clouds"][obj_name]["xyz"] for obj_name in best_demo_data["object_clouds"].keys()] if len(old_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(old_object_clouds[0],.02) y_md = voxel_downsample(new_object_clouds[0],.02) if transform_type == "tps": #warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) warp = registration.tps_rpm(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) elif transform_type == "translation2d": warp = registration.Translation2d() warp.fit(x_nd, y_md) elif transform_type == "rigid2d": warp = registration.Rigid2d() warp.fit(x_nd, y_md) else: raise Exception("transform type %s is not yet implemented"%transform_type) l_offset,r_offset = np.zeros(3), np.zeros(3) #if "r_tool" in best_demo_info: #r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) #if "l_tool" in best_demo_info: #l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) arms_used = best_demo_info["arms_used"] warped_demo_data = warping.transform_verb_demo(warp, best_demo_data) resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used if arms_used in "lb": traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"]) traj.l_gripper_angles = warped_demo_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0 if arms_used in "rb": traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"]) traj.r_gripper_angles = warped_demo_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0 Globals.handles = [] plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST)) return resp
zsel = xyz[yl:yl+h, xl:xl+w, 2] mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1) resp = ProcessCloudResponse() xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')] rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')] resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id) return resp if __name__ == "__main__": if args.test: if rospy.get_name() == "/unnamed": rospy.init_node("test_interactive_segmentation_service",disable_signals=True) listener = tf.TransformListener() pc = rospy.wait_for_message("/drop/points", sm.PointCloud2) pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id) Globals.setup() req = ProcessCloudRequest() req.cloud_in = pc_tf resp = callback(req) xyz, rgb = pc2xyzrgb(resp.cloud_out) pose_array = conversions.array_to_pose_array(xyz.reshape(-1,3), 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array,rgba=(1,0,0,1),type=Marker.CUBE_LIST,width=.002, ns = 'segmentation')) else: rospy.init_node("test_interactive_segmentation_service",disable_signals=True) Globals.setup() service = rospy.Service("interactive_segmentation", ProcessCloud, callback) rospy.spin()
def callback(req): xyz, rgb = pc2xyzrgb(req.cloud_in) rgb = rgb.copy() gc = GetClick() cv2.setMouseCallback("rgb", gc.callback) while not gc.done: cv2.imshow("rgb", rgb) cv2.waitKey(10) gm = GetMouse() xy_corner1 = np.array(gc.xy) cv2.setMouseCallback("rgb", gm.callback) while not gm.done: img = rgb.copy() if gm.xy is not None: xy_corner2 = np.array(gm.xy) cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0)) cv2.imshow("rgb",img) cv2.waitKey(10) xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0) xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0) #mask = np.zeros(xyz.shape[:2],dtype='uint8') #rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy])) #tmp1 = np.zeros((1, 13 * 5)) #tmp2 = np.zeros((1, 13 * 5)) #result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT) #from cv2 import cv #mask[mask == cv.GC_BGD] = 0 #mask[mask == cv.GC_PR_BGD] = 63 #mask[mask == cv.GC_FGD] = 255 #mask[mask == cv.GC_PR_FGD] = 192 #cv2.imshow('mask',mask) #cv2.waitKey(10) #table_height = get_table_height(xyz) xl, yl = xy_tl w, h = xy_br - xy_tl mask = np.zeros((h,w),dtype='uint8') mask.fill(cv2.GC_PR_BGD) if args.init == "height": initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50) mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD elif args.init == "middle": print int(yl+h/4),int(yl+3*h/4),int(xl+w/4),int(xl+3*w/4) mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD print mask.mean() tmp1 = np.zeros((1, 13 * 5)) tmp2 = np.zeros((1, 13 * 5)) cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK) contours = cv2.findContours(mask.astype('uint8')%2,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0] cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0)) cv2.imshow('rgb', rgb) cv2.waitKey(10) zsel = xyz[yl:yl+h, xl:xl+w, 2] mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1) resp = ProcessCloudResponse() xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')] rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')] resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id) return resp
import argparse parser = argparse.ArgumentParser() parser.add_argument("out") parser.add_argument("--cloud_in",default="/drop/points",nargs = "?") parser.add_argument("--dont_transform", action="store_true") args = parser.parse_args() from brett2 import ros_utils from jds_utils import conversions import roslib roslib.load_manifest('tf') import tf import rospy import numpy as np import sensor_msgs.msg as sm from brett2.ros_utils import transformPointCloud2 rospy.init_node("get_point_cloud") listener = tf.TransformListener() rospy.sleep(.3) pc = rospy.wait_for_message(args.cloud_in, sm.PointCloud2) if args.dont_transform: pc_tf = pc else: pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id) xyz, bgr = ros_utils.pc2xyzrgb(pc_tf) np.savez(args.out, xyz=xyz, bgr=bgr)
def find_closest_demo(verb, exp_clouds_pc2): exp_clouds = [pc2xyzrgb(cloud)[0] for cloud in exp_clouds_pc2] closest_demo_name = scene_diff.get_closest_demo(verb, exp_clouds) return closest_demo_name
def handle_initialization_request(req): "rope initialization service" if args.save_requests: fname = "/tmp/init_req_%i.pkl" % time.time() with open(fname, "w") as fh: cPickle.dump(req, fh) print "saved", fname xyz, _ = pc2xyzrgb(req.cloud) xyz = np.squeeze(xyz) obj_type = determine_object_type(xyz, plotting=args.plotting) print "object type:", obj_type if obj_type == "towel": xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :] center, (height, width), angle = cv2.minAreaRect( xyz[:, :2].astype('float32').reshape(1, -1, 2)) angle *= np.pi / 180 corners = np.array(center)[None, :] + np.dot( np.array([[-height / 2, -width / 2], [-height / 2, width / 2], [height / 2, width / 2], [height / 2, -width / 2]]), np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]])) resp = bs.InitializationResponse() resp.objectInit.type = "towel_corners" resp.objectInit.towel_corners.polygon.points = [ gm.Point(corner[0], corner[1], np.median(xyz[:, 2])) for corner in corners ] resp.objectInit.towel_corners.header = req.cloud.header print req.cloud.header poly_pub.publish(resp.objectInit.towel_corners) if obj_type == "rope": total_path_3d = find_path_through_point_cloud(xyz, plotting=args.plotting) resp = bs.InitializationResponse() resp.objectInit.type = "rope" rope = resp.objectInit.rope = bm.Rope() rope.header = req.cloud.header rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d] rope.radius = .006 print "lengths:", [ np.linalg.norm(total_path_3d[i + 1] - total_path_3d[i]) for i in xrange(len(total_path_3d) - 1) ] rospy.loginfo( "created a rope with %i nodes, each has length %.2f, radius %.2f", len(rope.nodes), np.linalg.norm(total_path_3d[i + 1] - total_path_3d[i]), rope.radius) pose_array = gm.PoseArray() pose_array.header = req.cloud.header pose_array.poses = [ gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1)) for point in rope.nodes ] # marker_handles[0] = rviz.draw_curve(pose_array,0) return resp
def callback(request): Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') #Globals.rviz.remove_all_markers() #draw_table() new_cloud1, _ = pc2xyzrgb(request.object_clouds[0]) new_cloud2, _ = pc2xyzrgb(request.object_clouds[1]) new_cloud1, new_cloud2 = sorted([new_cloud1, new_cloud2], key = lambda x: np.squeeze(x).ptp(axis=0).prod()) new_cloud1 = new_cloud1.reshape(-1,3) new_cloud2 = new_cloud2.reshape(-1,3) new_xyz1 = (new_cloud1.max(axis=0) + new_cloud1.min(axis=0))/2 new_xyz2 = (new_cloud2.max(axis=0) + new_cloud2.min(axis=0))/2 f.fit(np.array([xyz1, xyz2]), np.array([new_xyz1, new_xyz2]), 1e6, 1e-3) new_gripper_xyzs, new_gripper_mats = f.transform_frames(old_gripper_xyzs, conversions.quats2mats(old_gripper_quats)) new_gripper_quats = conversions.mats2quats(new_gripper_mats) #print "warning: using old oreitnations" show_objects_and_trajectory(new_gripper_xyzs, np.array([new_xyz1, new_xyz2]), np.array([quat1, quat2]), obj_dims, (0,1,0,1)) show_objects_and_trajectory(old_gripper_xyzs, np.array([xyz1, xyz2]), np.array([quat1, quat2]), obj_dims, (0,0,1,1)) grid_handle = draw_grid(Globals.rviz, f.transform_points, old_gripper_xyzs.min(axis=0), old_gripper_xyzs.max(axis=0), "base_footprint") HANDLES.append(grid_handle) Globals.pr2.join_all() Globals.pr2.update_rave() best_traj_info, best_feasible_frac = None, 0 env = Globals.pr2.robot.GetEnv() Globals.pr2.update_rave() collision_env = create_obstacle_env(env) basemanip = openravepy.interfaces.BaseManipulation(collision_env.GetRobot("pr2"),plannername=None) rospy.sleep(.1) #collision_env.SetViewer("qtcoin") #raw_input("press enter to continue") for (lr, arm) in zip("lr",[Globals.pr2.larm,Globals.pr2.rarm]): name = arm.manip.GetName() manip = collision_env.GetRobot('pr2').GetManipulator(name) rospy.loginfo("trying to plan to initial position with %s",name) first_mat1 = np.eye(4) first_mat1[:3,:3] = new_gripper_mats[0] first_mat1[:3,3] = new_gripper_xyzs[0] first_mat = transform_relative_pose_for_ik(manip, first_mat1, "world", "%s_gripper_tool_frame"%lr) collision_env.GetRobot("pr2").SetActiveManipulator(name) trajobj = None try: trajobj = basemanip.MoveToHandPosition([first_mat],seedik=16,outputtrajobj=True,execute=0) rospy.loginfo("planning succeeded") except Exception: rospy.loginfo("planning failed") traceback.print_exc() print "initial ik result", manip.FindIKSolutions(first_mat,0) continue rospy.loginfo("trying ik") Globals.pr2.update_rave() joint_positions, inds = trajectories.make_joint_traj(new_gripper_xyzs, new_gripper_quats, manip, 'base_footprint', '%s_gripper_tool_frame'%lr, filter_options = 1+16) feasible_frac = len(inds)/len(new_gripper_xyzs) print inds if feasible_frac > best_feasible_frac: best_feasible_frac = feasible_frac best_traj_info = dict( feasible_frac = feasible_frac, lr = 'l' if name == 'leftarm' else 'r', manip = manip, arm = arm, initial_traj = trajobj, joint_positions = joint_positions) collision_env.Destroy() response = PourResponse() if best_feasible_frac > .75: if best_traj_info["initial_traj"] is not None: follow_rave_traj(best_traj_info["arm"], best_traj_info["initial_traj"]) else: print "no initial traj" #print "feasible inds", best_traj_info["inds"] body_traj = np.zeros(len(best_traj_info["joint_positions"]),dtype=trajectories.BodyState) lr = best_traj_info["lr"] body_traj["%s_arm"%lr] = best_traj_info["joint_positions"] body_traj["%s_gripper"%lr] = gripper_angles trajectories.follow_body_traj(Globals.pr2, body_traj, times, r_arm = (lr=='r'), r_gripper = (lr=='r'), l_arm = (lr=='l'), l_gripper= (lr=='l')) Globals.pr2.join_all() response.success = True else: rospy.logerr("could not execute trajectory because not enough points are reachable") response.success = False return response