class Smartie(object):
''' Class to connect to and publish the Korg NanoKontrol midi device
as a ros joystick
'''
def __init__(self):
''' connect to midi device and set up ros
'''
self.sp = SmartPAD('SmartPAD')
self.axis = Axis()
self.axis.init()
self.walker = Walker()
self.walker.init()
self.web = pages.Pages()
rospy.loginfo("Using input device %s" % self.sp.device)
os.environ['PORT'] = '8081'
def finish(self):
self.sp.finish()
self.axis.fini()
self.web.finish()
#del self.sp
#del self.axis
def run(self):
try:
thread.start_new_thread(self.web.run, ())
while not rospy.is_shutdown():
done, data, steps = self.sp.readin()
if done:
rospy.signal_shutdown("Smartie Done")
self.axis.process_keys(data)
self.walker.process_keys(steps)
finally:
rospy.loginfo("Smartie Died")
self.finish()
class Flyer:
def __init__(self,ip,port):
# global flyermmap
print(posix_ipc.MESSAGE_QUEUES_SUPPORTED)
print(posix_ipc.QUEUE_MESSAGES_MAX_DEFAULT)
print(posix_ipc.QUEUE_MESSAGE_SIZE_MAX_DEFAULT)
self.model_pub = rospy.Publisher("/gazebo/set_model_state",ModelState,queue_size=1)
self.bridge = CvBridge()
#self.image_sub = rospy.Subscriber("/mycam/image_raw",Image,self.callback)
self.image_sub = rospy.Subscriber("/multisense/camera/left/image_raw",Image,self.callback)
self.imageflag = 0
self.imagecnt = 0
self.key = 0
firstFoot = Foot([0,-0.1,0],Foot.RIGHT)
standingFoot = Foot([0,0.1,0],Foot.LEFT)
self.list = Steplist(firstFoot,standingFoot)
self.height = 1.6
self.tilt = 0.0
memory = posix_ipc.SharedMemory("flyermmap", posix_ipc.O_CREAT, size=8*Mib)
self.sem = posix_ipc.Semaphore("flyersem", posix_ipc.O_CREAT)
self.memmap = mmap.mmap(memory.fd, memory.size)
# flyermmap = self.memmap
memory.close_fd()
self.queue = posix_ipc.MessageQueue("/flyerqueue", posix_ipc.O_CREAT)
#self.wsproc = prc.Popen('python flyerws.py', shell=True )
self.wsproc = prc.Popen('python -u flyerws.py --ip %s --port %s' % (ip,port), shell=True )
self.writecnt = 0
self.loc = (0,0,0)
self.walker = Walker()
self.walker.init()
def callback(self,msg):
#roi = (90,300,384,384) #valkyrie image is 544x1024
roi = (0,0,544,1024) #valkyrie image is 544x1024
self.imagecnt += 1
if self.imageflag == 0:
try:
im8uc3 = self.bridge.imgmsg_to_cv2(msg, "bgr8")
im32fc3 = im8uc3.astype('float32')
#self.cv_image = (im32fc3/255.0)[roi[0]:roi[0]+roi[2], roi[1]:roi[1]+roi[3]]
#self.cv_image = im8uc3
#self.cv_image = im8uc3[::2,::2,:] #272x512
self.cv_image = im8uc3[::16,::16,:] #34x64
self.imageflag = 1
except CvBridgeError as e:
print(e)
'''
cv2.imshow("Image window", self.cv_image)
key = cv2.waitKey(3) & 0xFF
if key != 255:
self.key = key
if key != 27: print(" key %d %s "%(self.key,chr(self.key)))
'''
def get_image(self):
self.imageflag = 0
timeout = time.time() + 1.0
while self.imageflag == 0 and time.time() < timeout:
time.sleep(0.1)
key = self.key
self.key = 0
return key
def write_image(self,lock=True):
scale = 1.0/2.0
if (True):
if lock:
self.sem.acquire()
self.memmap.seek(28)
#thumb = cv2.resize(self.cv_image,None,0,scale,scale,cv2.INTER_AREA)
#thumb = self.cv_image[::2,::2,:]
#self.memmap.write(thumb.copy())
self.memmap.write(self.cv_image.copy())
l = self.memmap.tell()
self.memmap.seek(0)
self.memmap.write( np.array( [ l, self.writecnt ], 'i' ) )
hd = [ self.loc[0], self.loc[1], self.loc[2],
self.height, self.tilt ]
buf = np.array(hd,'f')
self.memmap.write(buf)
#print("Wrote %d bytes" % self.memmap.tell())
self.sem.release()
self.writecnt += 1
def check_queue(self):
found = False
try:
while self.queue.current_messages > 0:
found = True
print("%s has %d msgs." % (self.queue.name,self.queue.current_messages))
payload,prio = self.queue.receive(0)
#print(payload)
msg = json.loads(payload)
#print(msg)
cmd = str(msg.get('cmd','X'))
r = float(msg.get('r',0.0))
t = float(msg.get('t',0.0))
if cmd == 'A': #Absolute x,y,z, rotation, tilt
x = float(msg.get('x',0.0))
y = float(msg.get('y',0.0))
z = float(msg.get('z',0.0))
self.loc = (x, y, r)
self.height = z
self.tilt = t
self.update_cam()
if cmd == 'S':
self.step_cam(r,t)
if cmd == 'V':
self.lift_cam(r)
if cmd == 'T':
self.tilt_cam(r)
except Exception as e:
print("Flyer Queue Exception %s"%e )
return found
def step_cam(self,r,th):
self.loc = self.list.fstep(r,th)
#self.update_cam()#{key: 'wlk', R: 50, t: }
step = Astep('wlk',64)
step.R = r
step.t = th
print(step)
steps = [step]
self.walker.process_keys(steps)
def tilt_cam(self,th):
tl = self.tilt + th
self.tilt = min(0.52,max(-0.52,tl))
#self.update_cam()
def lift_cam(self,r):
ht = self.height + r
self.height = min(5.0,max(1.2,ht))
#self.update_cam()
'''
def update_cam(self):
loc = self.loc
print("stepped to (%6.3f,%6.3f) heading %6.3f" % loc)
pose = Pose()
pose.position.x = loc[0]
pose.position.y = loc[1]
pose.position.z = self.height
q = quaternion_from_euler( 0, self.tilt, loc[2] )
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
msg = ModelState(model_name='mycam',pose=pose)
self.model_pub.publish(msg)
'''
def fini(self):
try:
self.wsproc.terminate()
print("WebSockets terminated")
self.memmap.close()
self.queue.close()
posix_ipc.unlink_shared_memory("flyermmap")
self.sem.release()
self.sem.unlink()
self.queue.unlink()
except:
print("Error shutting down Flyer")
class Flyer:
def __init__(self, ip, port):
self.bridge = CvBridge()
self.imageflags = [0, 0, 0, 0,
0] #I 1gray 2small 3fc 4seg (zero not used)
self.imagecnt = 0
self.key = 0
self.height = 1.6
self.tilt = 0.0
self.torso = 0.0
self.headtilt = 0.0
self.headrot = 0.0
self.stepspending = 0
self.totalsteps = 0
self.nextposetime = 0
self.nextfsstime = 0
memory = posix_ipc.SharedMemory("flyermmap",
posix_ipc.O_CREAT,
size=8 * IPComm.Mib)
self.sem = posix_ipc.Semaphore("flyersem", posix_ipc.O_CREAT)
self.memmap = mmap.mmap(memory.fd, memory.size)
memory.close_fd()
self.image_sub = rospy.Subscriber("/olrun/image/gray", Image,
self.callback)
self.fcimage_sub = rospy.Subscriber("/olrun/image/fc", Image,
self.callbackfc)
self.segimage_sub = rospy.Subscriber("/olrun/image/seg", Image,
self.callbackseg)
self.smallimage_sub = rospy.Subscriber("/olrun/image/small", Image,
self.callbacksmall)
ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name')
fss_name = "/ihmc_ros/{0}/output/footstep_status".format(ROBOT_NAME)
pose_name = "/ihmc_ros/{0}/output/robot_pose".format(ROBOT_NAME)
self.fssSubscriber = rospy.Subscriber(fss_name,
FootstepStatusRosMessage,
self.rcvdfss)
self.poseSubscriber = rospy.Subscriber(pose_name, Odometry,
self.rcvdpose)
self.queue = posix_ipc.MessageQueue("/flyerqueue", posix_ipc.O_CREAT)
self.wsproc = prc.Popen('python -u flyerws.py --ip %s --port %s' %
(ip, port),
shell=True)
self.writecnt = 0
self.loc = [0.0, 0.0, 0.0]
self.walker = Walker()
self.walker.init()
self.neck_joint = [0.0, 0.0, 0.0]
self.torso_joint = 0.0
def rcvdpose(self, msg):
now = rospy.Time.now().to_sec()
if now > self.nextposetime:
self.nextposetime = now + MSG_INTERVAL
pos = msg.pose.pose.position
quat = msg.pose.pose.orientation
eul = euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
self.loc = [float(pos.x), float(pos.y), float(eul[2])]
def rcvdfss(self, msg):
#now = rospy.Time.now().to_sec()
#if now > self.nextfsstime:
# self.nextfsstime = now + SrcState.INTERVAL
if msg.status == 1:
self.stepspending -= 1
def callbackfc(self, msg): #3fc
num = 3
if self.imageflags[num] == 0:
try:
self.fcimg = self.bridge.imgmsg_to_cv2(msg, "rgb8")
self.imageflags[num] = 1
except CvBridgeError as e:
print(e)
def callbacksmall(self, msg): #2small
num = 2
if self.imageflags[num] == 0:
try:
self.smallimg = self.bridge.imgmsg_to_cv2(msg, "rgb8")
self.imageflags[num] = 1
except CvBridgeError as e:
print(e)
def callbackseg(self, msg): #4seg
num = 4
if self.imageflags[num] == 0:
try:
self.segimg = self.bridge.imgmsg_to_cv2(msg, "mono8")
self.imageflags[num] = 1
except CvBridgeError as e:
print(e)
def callback(self, msg): #1-gray
num = 1
if self.imageflags[num] == 0:
try:
self.grayimg = self.bridge.imgmsg_to_cv2(msg, "mono8")
self.imageflags[num] = 1
except CvBridgeError as e:
print(e)
def get_image(self, num):
self.imageflags[num] = 0 #I 1gray 2small 3fc 4seg
timeout = time.time() + 1.0
while self.imageflags[num] == 0 and time.time() < timeout:
time.sleep(0.1)
key = self.key
self.key = 0
return key
def write_image(self, img, lock=True):
if lock:
self.sem.acquire()
self.memmap.seek(28)
self.memmap.write(img) #copy?
l = self.memmap.tell()
self.memmap.seek(0)
self.memmap.write(np.array([l, self.writecnt], 'i'))
hd = [
self.loc[0], self.loc[1], self.loc[2],
float(self.totalsteps),
float(self.stepspending)
]
buf = np.array(hd, 'f')
self.memmap.write(buf)
#print("Wrote %d bytes" % self.memmap.tell())
self.sem.release()
self.writecnt += 1
def check_queue(self):
#X reset, I 1gray 2small 3fc 4seg, S step R,th, V step u/d, T torso, H head u/d, L hd look l/r
found = False
try:
while self.queue.current_messages > 0:
found = True
#print("%s has %d msgs." % (self.queue.name,self.queue.current_messages))
payload, prio = self.queue.receive(0)
#print(payload)
msg = json.loads(payload)
#print(msg)
cmd = str(msg.get('cmd', ' '))
r = float(msg.get('r', 0.0))
t = float(msg.get('t', 0.0))
#print(cmd,r,t)
if cmd == 'X': #X reset walker
self.reset_walker(r)
if cmd == 'I': #I 1gray 2small 3fc 4seg
self.send_image(r)
if cmd == 'S': #S step R,th
self.step_cam(r, t)
if cmd == 'V': #V step u/d
self.lift_cam(r)
if cmd == 'T': #T torso
self.tilt_body(r)
if cmd == 'H': #H head u/d
self.tilt_cam(r)
if cmd == 'L': #L hd look l/r
self.rotate_cam(r)
if cmd == 'A': #Abort walking
self.abort_walker(r)
except Exception as e:
print("Flyer Queue Exception %s" % e)
return found
def reset_walker(
self, r
): #all walker cmds = rst-reset,wlk,sup-stepup,tlt-torso,nck-[3],abt
self.send_walker('rst', r)
def send_image(self, r): #1-Gray 2-Small 3-FC(save) 4-Seg
num = int(r)
self.get_image(num)
if num == 1:
self.write_image(self.grayimg)
elif num == 2:
self.write_image(self.smallimg)
elif num == 3:
try:
path = "../olr/fc%d.jpg" % self.writecnt
print("saving image to %s" % path)
cv2.imwrite(path, self.fcimg)
self.writecnt += 1
except Exception as e:
print("saving image error %s" % e)
elif num == 4:
self.write_image(self.segimg)
def step_cam(self, r, th):
self.send_walker('wlk', r, th)
self.stepspending += 1
self.totalsteps += 1
def lift_cam(self, r):
self.send_walker('sup', r)
def tilt_body(self, r):
self.torso_joint = r
self.send_walker('tlt', self.torso_joint)
def tilt_cam(self, r):
if r > 0.0: #down
self.neck_joint[0] = r
self.neck_joint[2] = 0.0
if r <= 0.0: #down
self.neck_joint[2] = r
self.neck_joint[0] = 0.0
self.send_walker('nck', self.neck_joint)
def rotate_cam(self, r): #neck joint 1 l:r +0.8:-0.8
self.neck_joint[1] = r
self.send_walker('nck', self.neck_joint)
def abort_walker(self, r):
self.send_walker('abt', 1)
def send_walker(self, cmd, r, th=0.0):
step = Astep(cmd, 64)
step.R = r
step.t = th
print(cmd, r, th)
steps = [step]
ret = self.walker.process_keys(steps)
if ret:
self.write_image(ret)
def fini(self):
try:
self.wsproc.terminate()
print("WebSockets terminated")
self.memmap.close()
self.queue.close()
posix_ipc.unlink_shared_memory("flyermmap")
self.sem.release()
self.sem.unlink()
self.queue.unlink()
except:
print("Error shutting down Flyer")