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)
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()