def run(self, dummy=False):
try:
self.ergo = PoppyErgoJr(use_http=True,
simulator='poppy-simu' if dummy else None,
camera='dummy')
except IOError as e:
rospy.logerr("Ergo hardware failed to init: {}".format(e))
return None
self.ergo.compliant = False
self.go_to_start()
self.last_activity = rospy.Time.now()
self.srv_reset = rospy.Service('/apex_playground/ergo/reset', Reset,
self._cb_reset)
rospy.loginfo('Ergo is ready and starts joystick servoing...')
while not rospy.is_shutdown():
self.go_or_resume_standby()
pygame.event.get()
x = self.joystick.get_axis(0)
y = self.joystick.get_axis(1)
self.servo_robot(y, x)
self.publish_eef()
self.publish_state()
self.publish_button()
# Publishers
self.publish_joy(x, y, self.joy_pub)
x = self.joystick2.get_axis(0)
y = self.joystick2.get_axis(1)
self.publish_joy(x, y, self.joy_pub2)
self.rate.sleep()
self.ergo.compliant = True
self.ergo.close()
def make_jr_or_die_trying():
while True:
try:
jr = PoppyErgoJr()
break
except (IndexError, ValueError, OSError):
print('Tries to connect to the ErgoJr failed!'
' Retry in {}s...'.format(RETRY))
time.sleep(RETRY)
# except (EnvironmentError):
# print('You most likely have to unplug/replug your ErgoJr!')
# sys.exit(1)
print('Sucessfully connected to the ErgoJr!')
return jr
from poppy.creatures import PoppyErgoJr jr = PoppyErgoJr(simulator='threejs', use_http=True) # Magie Python pour lancer le server web en background from threading import Thread t = Thread(target=jr.http.run) t.daemon = True t.start() jr.dance.start()
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
from contextlib import closing
from numpy import mean, std
from pypot.primitive.utils import PositionWatcher
from poppy.creatures import PoppyErgoJr
from hampy import detect_markers
D = 10
cpu = []
if __name__ == '__main__':
psutil.cpu_percent()
time.sleep(.5)
with closing(PoppyErgoJr()) as jr:
jr.rest_posture.start()
jr.rest_posture.wait_to_stop()
traj_rec = PositionWatcher(jr, 25., jr.motors)
jr.dance.start()
traj_rec.start()
t0 = time.time()
while time.time() - t0 < D:
cpu.append(psutil.cpu_percent())
img = jr.camera.frame
markers = detect_markers(img)
for m in markers:
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu', use_snap=True)
self.base_url = 'http://127.0.0.1:6969'
import time
from poppy.creatures import PoppyErgoJr
# INIT JOYSTICK
os.environ["SDL_VIDEODRIVER"] = "dummy"
pygame.display.init()
screen = pygame.display.set_mode((1,1))
pygame.joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
print 'Initialized Joystick : %s' % j.get_name()
# INIT POPPY
poppy = PoppyErgoJr(camera="dummy")
poppy.compliant = False
# goto start position
for m,p in zip(poppy.motors, [0.0, -15.4, 35.34, -8.06, -15.69, 71.99]):
m.goto_position(p, 1.)
time.sleep(1.)
print "Poppy started"
def servo_axis0(x, id):
if x <= 1 and x >= -1:
p = poppy.motors[id].goal_position
if -180 < p+x < 180 :
poppy.motors[id].goto_position(p + 2*x, 0.1)
import cv2
from poppy.creatures import PoppyErgoJr
if __name__ == '__main__':
jr = PoppyErgoJr()
while True:
img = jr.camera.frame
for m in jr.marker_detector.markers:
m.draw_contour(img)
cv2.imshow('live', img)
cv2.waitKey(100)
def test_lowerlimit_correctly_setup(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')