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