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