def main(): Globals.handles = [] if rospy.get_name() == '/unnamed': rospy.init_node("publish_pert_rope", disable_signals=True) Globals.setup() rospy.sleep(1) if args.from_log: import logtool cloud_xyz = logtool.get_first_seen_cloud(logtool.read_log(args.from_log)) else: cloud_xyz = read_cloud(read_demos()) rope = ri.find_path_through_point_cloud(cloud_xyz) #prope = cpert.perturb_curve(rope, args.s, args.const_radius) draw_cloud(cloud_xyz, width=0.01, rgba=(1, 0, 1, .5), ns='publish_pert_rope_cloud_orig') draw_rope(rope, width=0.01, rgba=(1, 1, 0, 1), ns='publish_pert_rope_orig') #draw_rope(prope, width=0.01, rgba=(0, 1, 1, 1), ns='publish_pert_rope_perturbed') draw_movement_markers(rope, args.s, rgba=(0, 1, 1, 1), ns='publish_pert_rope_movement') #rospy.loginfo('generated rope has mean distance %f', calc_rope_dist(rope, prope)) rospy.loginfo('Publishing...') while not rospy.is_shutdown(): for h in Globals.handles: h.pub.publish(h.marker) rospy.sleep(1)
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
assert new_curve.shape == curve.shape return new_curve if __name__ == '__main__': import argparse parser = argparse.ArgumentParser() parser.add_argument('--s', action='store', type=float, default=0.001, help='variance of randomness to introduce to the b-spline control points') parser.add_argument('--n', action='store', type=int, default=1, help='num samples to draw') parser.add_argument('--const_radius', action='store_true', help='don\'t use gaussian around each control point with variance s (just use a random angle, with constant radius sqrt(s)') parser.add_argument('input') args = parser.parse_args() import os assert os.path.exists(args.input) import matplotlib.pyplot as plt #curve = np.loadtxt(args.input) import logtool import rope_initialization as ri curve = ri.find_path_through_point_cloud(logtool.get_first_seen_cloud(logtool.read_log(args.input))) plt.plot(curve[:,0], curve[:,1], 'b.-') for _ in range(args.n): new_curve = perturb_curve(curve, args.s, args.const_radius) plt.plot(new_curve[:,0], new_curve[:,1], 'm.-') orig = _eval_spline(_to_spline(curve)) plt.plot(orig[:,0], orig[:,1], 'g.-') plt.axis('equal') plt.show()