def detect_cb_pose(a, m):
global detect
global success_count
# pp.pprint("detect_cb-- {}".format(m))
# print(a)
sleep(0.3)
if len(m['object']['payload']['detections']) > 0 and m['object'][
'payload']['detections'][0]['class']['name'] == "beacon":
print('object seen, closing in...')
sched.stop_action(a)
sleep(1)
action = sched.fwd(.2, maxdist=0.2)
save_image = True
success_count += 1
if success_count < 2:
sleep(3)
pose = orspy.EulerPose(66.3, 97.2, 0, 0, 0, -math.pi / 3)
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)
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 pose_from_info(x, y, angle=0):
return orspy.EulerPose(x, y, 0, 0, 0, angle)
if simulated:
return simulated_rid
return real_rid
# maxspeed==None means "as fast as you can go"
def goto(pose,
maxspeed=None,
allowed_trans_error=None,
allowed_rot_error=None):
action_done_event.clear()
action_num = sched.start_action(rid(),
'goto_pose',
pose=pose,
maxspeed=maxspeed,
allowed_trans_error=allowed_trans_error,
allowed_rot_error=allowed_rot_error)
print("started action {}".format(action_num))
action_done_event.wait()
print("done going to pose")
return action_num
#this is good #pose = orspy.EulerPose(68, 98.2, 0, 0, 0, math.pi) # hmmm, I wonder where this is...?
pose = orspy.EulerPose(68.35, 98.35, 0, 0, 0,
0 * math.pi) # hmmm, I wonder where this is...?
goto(pose, maxspeed=.2, allowed_trans_error=0.15
) # , allowed_trans_error=0.3, allowed_rot_error=0.5)
# sched.stop_action(rid(), 0)
from IPython.display import Image
import matplotlib.pyplot as plt
import urllib.request
import pprint
global success_count
global save_image
save_image = False
success_count = 0
pp = pprint.PrettyPrinter(indent=4)
simulated = False # change this to switch between simulated and real robot
sched = utils.Scheduler()
sched.use_simulated(simulated)
pose = orspy.EulerPose(66.3, 96.6, 0, 0, 0, -math.pi / 3)
sleep(3) # wait for all callbacks
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)
action = sched.rotate(math.pi / 32)
if not wws_lite.test_connection():
print("could not connect to WWS")
def detect_cb_pose(a, m):
global detect
global success_count
#!/bin/python3
import orspy
import math
import utils
import sys
pose = orspy.EulerPose(68.35, 98.35, 0, 0, 0, math.pi)
simulated = False # change this to switch between simulated and real robot
steps = 4
sched = utils.Scheduler()
sched.use_simulated(simulated)
import time
time.sleep(3) # wait for all callbacks
import wws_lite
if not wws_lite.test_connection():
print("could not connect to WWS")
current_x = None
current_y = None
current_angle = None
def wws_pose_cb(m):
if simulated:
current_x = m["payload"]["pose"]["pose"]["position"]["x"]
current_y = m["payload"]["pose"]["pose"]["position"]["y"]
else: