allowed_trans_error=0.4,
allowed_rot_error=0.5
) # , allowed_trans_error=0.3, allowed_rot_error=0.5)
action = sched.rotate(math.pi / 32)
# a = sched.rotate(math.pi/4)
# action = a
# pp.pprint("detect_cb-- {}".format(m))
print("object type- {}".format(
m['object']['payload']['detections'][0]['class']['name']))
if success_count >= 2:
wws_lite.unsubscribe(topic_pose)
wws_lite.unsubscribe(topic_img)
def wws_detect_img_cb(m):
global save_image
global success_count
detect = m['image']['url']
if save_image:
f = urllib.request.urlopen(detect)
a = plt.imread(f, 0)
plt.imshow(a)
plt.savefig("output{}.png".format(success_count))
topic_img = "{}/object_img".format(sched.wws_topic_prefix())
topic_pose = "{}/object_pose".format(sched.wws_topic_prefix())
wws_lite.subscribe(topic_img, wws_detect_img_cb)
wws_lite.subscribe(topic_pose, lambda x: detect_cb_pose(action, x))
def start(self):
self.topic_img = "{}/object_img".format(self.sched.wws_topic_prefix())
self.topic_pose = "{}/object_pose".format(self.sched.wws_topic_prefix())
wws_lite.subscribe(self.topic_img, lambda x: self.wws_detect_img_cb(x))
wws_lite.subscribe(self.topic_pose, lambda x: self.detect_cb_pose(x))
def wws_pose_cb(m):
if simulated:
current_x = m["payload"]["pose"]["pose"]["position"]["x"]
current_y = m["payload"]["pose"]["pose"]["position"]["y"]
else:
current_x = m["payload"]["pose"]["position"]["x"]
current_y = m["payload"]["pose"]["position"]["y"]
current_angle = m["payload"]["pose"]["orientation"]["z"]
print("x: {}, y: {}, angle: {}".format(current_x, current_y, current_angle))
wws_lite.unsubscribe(topic)
topic = "{}/pose".format(sched.wws_topic_prefix())
sched.goto(pose, maxspeed=.2, allowed_trans_error=0.4, allowed_rot_error=0.5)# , allowed_trans_error=0.3, allowed_rot_error=0.5)
time.sleep(1)
print("start rotate")
for _ in range(steps * 2 - 2):
sched.rotate(speed=math.pi/8, maxrot=math.pi/steps)# - math.pi / (steps * 16))
sched.goto(pose, maxspeed=.2, allowed_trans_error=0.4, allowed_rot_error=0.5)# , allowed_trans_error=0.3, allowed_rot_error=0.5)
time.sleep(1)
wws_lite.subscribe(topic, wws_pose_cb)
sys.exit()