class AnimateRobot(object):
def __init__(self, obj, view):
self.obj = obj
self.view = view
self.timer = TimerCallback(targetFps=60)
self.timer.callback = self.tick
def start(self):
self.startTime = time.time()
self.timer.start()
def stop(self):
self.timer.stop()
def tick(self):
tNow = time.time()
elapsed = tNow - self.startTime
x, y = np.sin(elapsed), np.cos(elapsed)
angle = -elapsed
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(np.degrees(angle))
t.Translate(x, y, 0)
self.obj.getChildFrame().copyFrame(t)
self.view.render()
class RosSubscriberManager(object):
"""A helper class for managing rospy subscribers and dispatching callbacks."""
def __init__(self):
self.msgs = {}
self.pending_msgs = {}
self.counters = {}
self.subs = OrderedDict()
self.callbacks = OrderedDict()
def init_node(self):
rospy.init_node('director_rospy_node', anonymous=True)
def on_idle():
time.sleep(0.0001)
self.idle_timer = TimerCallback(callback=on_idle, targetFps=100)
self.idle_timer.start()
self.dispatch_timer = TimerCallback(targetFps=30)
self.dispatch_timer.callback = self.on_dispatch_timer
self.dispatch_timer.start()
def get_estimated_topic_hz(self, topic_name):
assert topic_name in self.counters
return self.counters[topic_name].getAverageFPS()
def get_latest_msg(self, topic_name):
return self.msgs.get(topic_name)
def subscribe(self,
topic_name,
message_type,
message_function=None,
call_on_thread=False):
def on_message(msg):
self.msgs[topic_name] = msg
self.counters[topic_name].tick()
if call_on_thread and message_function:
message_function(topic_name, msg)
else:
self.pending_msgs[topic_name] = msg
self.counters[topic_name] = FPSCounter()
self.callbacks[topic_name] = message_function
self.subs[topic_name] = rospy.Subscriber(topic_name, message_type,
on_message)
def handle_pending_message(self, topic_name):
msg = self.pending_msgs.pop(topic_name, None)
if msg is not None:
callback = self.callbacks.get(topic_name)
if callback:
callback(topic_name, msg)
def handle_pending_messages(self):
for topic_name in list(self.pending_msgs.keys()):
self.handle_pending_message(topic_name)
def on_dispatch_timer(self):
self.handle_pending_messages()
class LCMForceDisplay(object):
'''
Displays foot force sensor signals in a status bar widget or label widget
'''
def onRobotState(self,msg):
self.l_foot_force_z = msg.force_torque.l_foot_force_z
self.r_foot_force_z = msg.force_torque.r_foot_force_z
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmbotcore.robot_state_t, self.onRobotState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def __del__(self):
lcmUtils.removeSubscriber(self.sub)
def showRate(self):
global leftInContact, rightInContact
self.label.text = '%.2f | %.2f' % (self.l_foot_force_z,self.r_foot_force_z)
class AnimateRobot(object):
def __init__(self, obj, view):
self.obj = obj
self.view = view
self.timer = TimerCallback(targetFps=60)
self.timer.callback = self.tick
def start(self):
self.startTime = time.time()
self.timer.start()
def stop(self):
self.timer.stop()
def tick(self):
tNow = time.time()
elapsed = tNow - self.startTime
x, y = np.sin(elapsed), np.cos(elapsed)
angle = -elapsed
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(np.degrees(angle))
t.Translate(x, y, 0)
self.obj.getChildFrame().copyFrame(t)
self.view.render()
class LCMForceDisplay(object):
'''
Displays foot force sensor signals in a status bar widget or label widget
'''
def onRobotState(self, msg):
self.l_foot_force_z = msg.force_torque.l_foot_force_z
self.r_foot_force_z = msg.force_torque.r_foot_force_z
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel,
lcmbotcore.robot_state_t,
self.onRobotState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def __del__(self):
lcmUtils.removeSubscriber(self.sub)
def showRate(self):
global leftInContact, rightInContact
self.label.text = '%.2f | %.2f' % (self.l_foot_force_z,
self.r_foot_force_z)
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection('MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection('AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection(
'MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection(
'AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.Start()
TimerCallback.start(self)
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.Start()
TimerCallback.start(self)
def start(self):
if self.reader is None:
self.reader = drc.vtkLidarSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 80.0)
self.reader.SetHeightRange(-80.0, 80.0)
self.reader.Start()
TimerCallback.start(self)
class BlackoutMonitor(object):
UPDATE_RATE = 5
AVERAGE_N = 5
def __init__(self, robotStateJointController, view, cameraview, mapServerSource):
self.robotStateJointController = robotStateJointController
self.view = view
self.cameraview = cameraview
self.mapServerSource = mapServerSource
self.lastCameraMessageTime = 0
self.lastScanBundleMessageTime = 0
self.lastBlackoutLengths = []
self.lastBlackoutLength = 0
self.inBlackout = False
self.averageBlackoutLength = 0.0
self.txt = vis.updateText("DATA AGE: 0 sec", "Data Age Text", view=self.view)
self.txt.addProperty('Show Avg Duration', False)
self.txt.setProperty('Visible', False)
self.updateTimer = TimerCallback(self.UPDATE_RATE)
self.updateTimer.callback = self.update
self.updateTimer.start()
def update(self):
self.lastCameraMessageTime = self.cameraview.imageManager.queue.getCurrentImageTime('CAMERA_LEFT')
self.lastScanBundleMessageTime = self.mapServerSource.reader.GetLastScanBundleUTime()
if self.robotStateJointController.lastRobotStateMessage:
elapsedCam = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastCameraMessageTime) / (1000*1000), 0.0)
elapsedScan = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastScanBundleMessageTime) / (1000*1000), 0.0)
# can't be deleted, only hidden, so this is ok
if (self.txt.getProperty('Visible')):
if (self.txt.getProperty('Show Avg Duration')):
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec AVG: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan), math.floor(self.averageBlackoutLength))
else:
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan))
ssize = self.view.size
self.txt.setProperty('Text', textstr)
self.txt.setProperty('Position', [10, 10])
# count out blackouts
if elapsedCam > 1.0:
self.inBlackout = True
self.lastBlackoutLength = elapsedCam
else:
if (self.inBlackout):
# Don't count huge time jumps due to init
if (self.lastBlackoutLength < 100000):
self.lastBlackoutLengths.append(self.lastBlackoutLength)
if len(self.lastBlackoutLengths) > self.AVERAGE_N:
self.lastBlackoutLengths.pop(0)
if len(self.lastBlackoutLengths) > 0:
self.averageBlackoutLength = sum(self.lastBlackoutLengths) / float(len(self.lastBlackoutLengths))
self.inBlackout = False
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source',
shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime,
cameraToLocalFused)
p = filterUtils.transformPolyData(p, cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.SetHeightRange(-80.0, 80.0)
self.reader.Start()
self.setIntensityRange(400, 4000)
TimerCallback.start(self)
def start(self):
if self.reader is None:
self.reader = drc.vtkLidarSource()
self.reader.subscribe(self.channelName)
self.reader.setCoordinateFrame(self.coordinateFrame)
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 80.0)
self.reader.SetHeightRange(-80.0, 80.0)
self.reader.Start()
TimerCallback.start(self)
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p,cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.SetHeightRange(-80.0, 80.0)
self.reader.Start()
self.setIntensityRange(400, 4000)
TimerCallback.start(self)
class RosSubscriberManager(object):
"""A helper class for managing rospy subscribers and dispatching callbacks."""
def __init__(self):
self.msgs = {}
self.pending_msgs = {}
self.counters = {}
self.subs = OrderedDict()
self.callbacks = OrderedDict()
def init_node(self):
rospy.init_node('director_rospy_node', anonymous=True)
def on_idle():
time.sleep(0.0001)
self.idle_timer = TimerCallback(callback=on_idle, targetFps=100)
self.idle_timer.start()
self.dispatch_timer = TimerCallback(targetFps=30)
self.dispatch_timer.callback = self.on_dispatch_timer
self.dispatch_timer.start()
def get_estimated_topic_hz(self, topic_name):
assert topic_name in self.counters
return self.counters[topic_name].getAverageFPS()
def get_latest_msg(self, topic_name):
return self.msgs.get(topic_name)
def subscribe(self, topic_name, message_type, message_function=None, call_on_thread=False):
def on_message(msg):
self.msgs[topic_name] = msg
self.counters[topic_name].tick()
if call_on_thread and message_function:
message_function(topic_name, msg)
else:
self.pending_msgs[topic_name] = msg
self.counters[topic_name] = FPSCounter()
self.callbacks[topic_name] = message_function
self.subs[topic_name] = rospy.Subscriber(topic_name, message_type, on_message)
def handle_pending_message(self, topic_name):
msg = self.pending_msgs.pop(topic_name, None)
if msg is not None:
callback = self.callbacks.get(topic_name)
if callback:
callback(topic_name, msg)
def handle_pending_messages(self):
for topic_name in list(self.pending_msgs.keys()):
self.handle_pending_message(topic_name)
def on_dispatch_timer(self):
self.handle_pending_messages()
def start(self):
if self.reader is None:
self.reader = drc.vtkLidarSource()
self.reader.subscribe(self.channelName)
self.reader.setCoordinateFrame(self.coordinateFrame)
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 80.0)
self.reader.SetHeightRange(-80.0, 80.0)
self.reader.Start()
TimerCallback.start(self)
class CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN',
lcmdrc.robot_plan_t,
self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t,
self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] -
poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
class TaskRunner(object):
def __init__(self):
self.interval = 1 / 60.0
sys.setcheckinterval(1000)
#sys.setswitchinterval(self.interval) # sys.setswitchinterval is only Python 3
self.taskQueue = asynctaskqueue.AsyncTaskQueue()
self.pendingTasks = []
self.threads = []
self.timer = TimerCallback(callback=self._onTimer,
targetFps=1 / self.interval)
def _onTimer(self):
# Give up control to another python thread in self.threads
# that might be running
time.sleep(self.interval)
# add all tasks in self.pendingTasks to the AsyncTaskQueue
if self.pendingTasks:
while True:
try:
self.taskQueue.addTask(self.pendingTasks.pop(0))
except IndexError:
break
# start the AsyncTaskQueue if it's not already running
if self.taskQueue.tasks and not self.taskQueue.isRunning:
self.taskQueue.start()
# check which threads are live
liveThreads = []
for t in self.threads:
if t.is_alive():
liveThreads.append(t)
# only retain the live threads
self.threads = liveThreads
# if no liveThreads then stop the timer
if len(self.threads) == 0:
self.timer.stop()
def callOnMain(self, func, *args, **kwargs):
self.pendingTasks.append(lambda: func(*args, **kwargs))
self.timer.start()
def callOnThread(self, func, *args, **kwargs):
t = Thread(target=lambda: func(*args, **kwargs))
self.threads.append(t)
t.start()
self.timer.start()
class TriggerFingerPublisher(): def __init__(self, lcmChannel): self.lcmChannel = lcmChannel self.reader = midi.MidiReader() self.timer = TimerCallback() self.timer.callback = self.tick self.msg = lcmdrc.trigger_finger_t() def startPublishing(self): print 'Publishing on ' + self.lcmChannel self.timer.start() def publish(self): messages = self.reader.getMessages() for message in messages: channel = message[2] val = message[3] / 127.0 if channel is 102: self.msg.slider1 = val elif channel is 103: self.msg.slider2 = val elif channel is 104: self.msg.slider3 = val elif channel is 105: self.msg.slider4 = val elif channel is 106: self.msg.knob1 = val elif channel is 107: self.msg.knob2 = val elif channel is 108: self.msg.knob3 = val elif channel is 109: self.msg.knob4 = val elif channel is 110: self.msg.knob5 = val elif channel is 111: self.msg.knob6 = val elif channel is 112: self.msg.knob7 = val elif channel is 113: self.msg.knob8 = val if len(messages) is not 0: self.msg.utime = getUtime() lcmUtils.publish(self.lcmChannel, self.msg) def tick(self): self.publish()
class TriggerFingerPublisher():
def __init__(self, lcmChannel):
self.lcmChannel = lcmChannel
self.reader = midi.MidiReader()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.msg = lcmdrc.trigger_finger_t()
def startPublishing(self):
print('Publishing on ' + self.lcmChannel)
self.timer.start()
def publish(self):
messages = self.reader.getMessages()
for message in messages:
channel = message[2]
val = message[3] / 127.0
if channel is 102:
self.msg.slider1 = val
elif channel is 103:
self.msg.slider2 = val
elif channel is 104:
self.msg.slider3 = val
elif channel is 105:
self.msg.slider4 = val
elif channel is 106:
self.msg.knob1 = val
elif channel is 107:
self.msg.knob2 = val
elif channel is 108:
self.msg.knob3 = val
elif channel is 109:
self.msg.knob4 = val
elif channel is 110:
self.msg.knob5 = val
elif channel is 111:
self.msg.knob6 = val
elif channel is 112:
self.msg.knob7 = val
elif channel is 113:
self.msg.knob8 = val
if len(messages) is not 0:
self.msg.utime = getUtime()
lcmUtils.publish(self.lcmChannel, self.msg)
def tick(self):
self.publish()
class CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] - poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
class TaskRunner(object):
def __init__(self):
self.interval = 1/60.0
sys.setcheckinterval(1000)
try:
sys.setswitchinterval(self.interval)
except AttributeError:
# sys.setswitchinterval is only python3
pass
self.taskQueue = asynctaskqueue.AsyncTaskQueue()
self.pendingTasks = []
self.threads = []
self.timer = TimerCallback(callback=self._onTimer, targetFps=1/self.interval)
# call timer.start here to initialize the QTimer now on the main thread
self.timer.start()
def _onTimer(self):
# add all tasks in self.pendingTasks to the AsyncTaskQueue
if self.pendingTasks:
while True:
try:
self.taskQueue.addTask(self.pendingTasks.pop(0))
except IndexError:
break
# start the AsyncTaskQueue if it's not already running
if self.taskQueue.tasks and not self.taskQueue.isRunning:
self.taskQueue.start()
# only retain the live threads
self.threads = [t for t in self.threads if t.is_alive()]
if self.threads:
# Give up control to other python threads that are running
time.sleep(self.interval)
def callOnMain(self, func, *args, **kwargs):
self.pendingTasks.append(lambda: func(*args, **kwargs))
def callOnThread(self, func, *args, **kwargs):
t = Thread(target=lambda: func(*args, **kwargs))
self.threads.append(t)
t.start()
return t
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
class ImageWidget(object):
def __init__(self, imageManager, imageName, view):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
rep = self.imageWidget.GetRepresentation()
rep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
rep.SetImage(self.flip.GetOutput())
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def hide(self):
self.imageWidget.Off()
def show(self):
self.imageWidget.On()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
if not self.initialized:
self.show()
self.initialized = True
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
class EstRobotStatePublisher(object):
def __init__(self, robotSystem):
self.robotSystem = robotSystem
self.timer = TimerCallback(targetFps=25)
self.timer.callback = self.publishEstRobotState
def publishEstRobotState(self):
q = self.robotSystem.robotStateJointController.q
stateMsg = robotstate.drakePoseToRobotState(q)
stateMsg.utime = utimeUtil.getUtime()
lcmUtils.publish("EST_ROBOT_STATE", stateMsg)
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class AffordanceTextureUpdater(object):
def __init__(self, affordanceManager):
self.affordanceManager = affordanceManager
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.updateTextures
self.timer.start()
def updateTexture(self, obj):
if obj.getProperty('Camera Texture Enabled'):
cameraview.applyCameraTexture(obj, cameraview.imageManager)
else:
cameraview.disableCameraTexture(obj)
obj._renderAllViews()
def updateTextures(self):
for aff in affordanceManager.getAffordances():
self.updateTexture(aff)
class FrameVisualizationPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel,
self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width(
) + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updateFrames(self):
self.botFrameUpdater.updateFrames()
def getNameFilter(self):
return str(self.ui.botFramesFilterEdit.text)
def onNameFilterChanged(self):
filter = self.getNameFilter()
class FrameVisualizationPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width() + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updateFrames(self):
self.botFrameUpdater.updateFrames()
def getNameFilter(self):
return str(self.ui.botFramesFilterEdit.text)
def onNameFilterChanged(self):
filter = self.getNameFilter()
class AffordanceTextureUpdater(object):
def __init__(self, affordanceManager):
self.affordanceManager = affordanceManager
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.updateTextures
self.timer.start()
def updateTexture(self, obj):
if obj.getProperty('Camera Texture Enabled'):
cameraview.applyCameraTexture(obj, cameraview.imageManager)
else:
cameraview.disableCameraTexture(obj)
obj._renderAllViews()
def updateTextures(self):
for aff in affordanceManager.getAffordances():
self.updateTexture(aff)
class SpindleAxisDebug(vis.PolyDataItem):
def __init__(self, frameProvider):
vis.PolyDataItem.__init__(self,
"spindle axis",
vtk.vtkPolyData(),
view=None)
self.frameProvider = frameProvider
self.timer = TimerCallback()
self.timer.callback = self.update
self.setProperty("Color", QtGui.QColor(0, 255, 0))
self.setProperty("Visible", False)
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible":
if self.getProperty(propertyName):
self.timer.start()
else:
self.timer.stop()
def onRemoveFromObjectModel(self):
vis.PolyDataItem.onRemoveFromObjectModel(self)
self.timer.stop()
def update(self):
t = self.frameProvider.getFrame("MULTISENSE_SCAN")
p1 = [0.0, 0.0, 0.0]
p2 = [2.0, 0.0, 0.0]
p1 = t.TransformPoint(p1)
p2 = t.TransformPoint(p2)
d = DebugData()
d.addSphere(p1, radius=0.01, color=[0, 1, 0])
d.addLine(p1, p2, color=[0, 1, 0])
self.setPolyData(d.getPolyData())
class PointerTracker(object):
"""
See segmentation.estimatePointerTip() documentation.
"""
def __init__(self, robotModel, stereoPointCloudItem):
self.robotModel = robotModel
self.stereoPointCloudItem = stereoPointCloudItem
self.timer = TimerCallback(targetFps=5)
self.timer.callback = self.updateFit
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
def cleanup(self):
om.removeFromObjectModel(om.findObjectByName("segmentation"))
def updateFit(self, polyData=None):
# if not self.stereoPointCloudItem.getProperty('Visible'):
# return
if not polyData:
self.stereoPointCloudItem.update()
polyData = self.stereoPointCloudItem.polyData
if not polyData or not polyData.GetNumberOfPoints():
self.cleanup()
return
self.tipPosition = segmentation.estimatePointerTip(self.robotModel, polyData)
if self.tipPosition is None:
self.cleanup()
def getPointerTip(self):
return self.tipPosition
class AnimatePropertyValue(object):
'''
This class is used to ramp a scalar or vector property from its current value to a
target value using linear inteprolation. For example:
obj = getSomeObject()
# fade the Alpha property to 0.0
AnimatePropertyValue(obj, 'Alpha', 0.0).start()
'''
def __init__(self, obj, propertyName, targetValue, animationTime=1.0):
self.obj = obj
self.propertyName = propertyName
self.animationTime = animationTime
self.targetValue = targetValue
self.timer = TimerCallback()
def start(self):
self.startTime = time.time()
self.startValue = np.array(self.obj.getProperty(self.propertyName))
self.targetValue = np.array(self.targetValue)
self.timer.callback = self.tick
self.timer.start()
def tick(self):
elapsed = time.time() - self.startTime
p = elapsed/self.animationTime
if p > 1.0:
p = 1.0
newValue = self.startValue + (self.targetValue - self.startValue)*p
self.obj.setProperty(self.propertyName, newValue)
if p == 1.0:
self.timer.callback = None
return False
class PointerTracker(object):
'''
See segmentation.estimatePointerTip() documentation.
'''
def __init__(self, robotModel, stereoPointCloudItem):
self.robotModel = robotModel
self.stereoPointCloudItem = stereoPointCloudItem
self.timer = TimerCallback(targetFps=5)
self.timer.callback = self.updateFit
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
def cleanup(self):
om.removeFromObjectModel(om.findObjectByName('segmentation'))
def updateFit(self, polyData=None):
#if not self.stereoPointCloudItem.getProperty('Visible'):
# return
if not polyData:
self.stereoPointCloudItem.update()
polyData = self.stereoPointCloudItem.polyData
if not polyData or not polyData.GetNumberOfPoints():
self.cleanup()
return
self.tipPosition = segmentation.estimatePointerTip(self.robotModel, polyData)
if self.tipPosition is None:
self.cleanup()
def getPointerTip(self):
return self.tipPosition
class SpindleAxisDebug(vis.PolyDataItem):
def __init__(self, frameProvider):
vis.PolyDataItem.__init__(self, 'spindle axis', vtk.vtkPolyData(), view=None)
self.frameProvider = frameProvider
self.timer = TimerCallback()
self.timer.callback = self.update
self.setProperty('Color', QtGui.QColor(0, 255, 0))
self.setProperty('Visible', False)
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Visible':
if self.getProperty(propertyName):
self.timer.start()
else:
self.timer.stop()
def onRemoveFromObjectModel(self):
vis.PolyDataItem.onRemoveFromObjectModel(self)
self.timer.stop()
def update(self):
t = self.frameProvider.getFrame('SCAN')
p1 = [0.0, 0.0, 0.0]
p2 = [2.0, 0.0, 0.0]
p1 = t.TransformPoint(p1)
p2 = t.TransformPoint(p2)
d = DebugData()
d.addSphere(p1, radius=0.01, color=[0,1,0])
d.addLine(p1, p2, color=[0,1,0])
self.setPolyData(d.getPolyData())
ql.setOrientation(QtCore.Qt.Vertical)
return ql
statusLabel = spawnBasicLabel()
l.addWidget(wrapInVTitledItem("Behavior", [statusLabel]))
def statusUpdate():
behavior = atlasDriver.getCurrentBehaviorName()
statusLabel.setText(behavior)
if (behavior != "user"):
statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white")
else:
statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:white; color:black")
statusTimer = TimerCallback(targetFps=FPS)
statusTimer.callback = statusUpdate
statusTimer.start()
controllerLabel = spawnBasicLabel()
l.addWidget(wrapInVTitledItem("Controller", [controllerLabel]))
def controllerUpdate():
status = atlasDriver.getControllerStatus()
if not status:
status = "Unknown"
if len(status) > 6:
status = status[0:6]
rate = atlasDriver.getControllerRate()
if not rate:
rate = 0.0
controllerLabel.setText("%s\n%06.1fhz" % (status, rate))
if (rate < 500):
controllerLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white")
class Simulator(object):
def __init__(
self,
percentObsDensity=20,
endTime=40,
nonRandomWorld=False,
circleRadius=0.7,
worldScale=1.0,
autoInitialize=True,
verbose=True,
):
self.verbose = verbose
self.startSimTime = time.time()
self.collisionThreshold = 0.4
self.randomSeed = 5
self.Sensor_rayLength = 8
self.percentObsDensity = percentObsDensity
self.defaultControllerTime = 1000
self.nonRandomWorld = nonRandomWorld
self.circleRadius = circleRadius
self.worldScale = worldScale
# create the visualizer object
self.app = ConsoleApp()
self.view = self.app.createView(useGrid=False)
self.initializeOptions()
self.initializeColorMap()
if autoInitialize:
self.initialize()
self.XVelocity_drawing = 0.0
self.YVelocity_drawing = 0.0
def initializeOptions(self):
self.options = dict()
self.options["World"] = dict()
self.options["World"]["obstaclesInnerFraction"] = 0.98
self.options["World"]["randomSeed"] = 40
self.options["World"]["percentObsDensity"] = 0.0
self.options["World"]["nonRandomWorld"] = True
self.options["World"]["circleRadius"] = 1.0
self.options["World"]["scale"] = 1
self.options["Sensor"] = dict()
self.options["Sensor"]["rayLength"] = 10
self.options["Sensor"]["numRays"] = 50
self.options["Car"] = dict()
self.options["Car"]["velocity"] = 4.0
self.options["dt"] = 0.05
self.options["runTime"] = dict()
self.options["runTime"]["defaultControllerTime"] = 100
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions["World"] = dict()
defaultOptions["World"]["obstaclesInnerFraction"] = 0.98
defaultOptions["World"]["randomSeed"] = 40
defaultOptions["World"]["percentObsDensity"] = 30
defaultOptions["World"]["nonRandomWorld"] = True
defaultOptions["World"]["circleRadius"] = 1.75
defaultOptions["World"]["scale"] = 2.5
defaultOptions["Sensor"] = dict()
defaultOptions["Sensor"]["rayLength"] = 10
defaultOptions["Sensor"]["numRays"] = 51
defaultOptions["Car"] = dict()
defaultOptions["Car"]["velocity"] = 20
defaultOptions["dt"] = 0.05
defaultOptions["runTime"] = dict()
defaultOptions["runTime"]["defaultControllerTime"] = 100
for k in defaultOptions:
self.options.setdefault(k, defaultOptions[k])
for k in defaultOptions:
if not isinstance(defaultOptions[k], dict):
continue
for j in defaultOptions[k]:
self.options[k].setdefault(j, defaultOptions[k][j])
def initializeColorMap(self):
self.colorMap = dict()
self.colorMap["default"] = [0, 1, 0]
def initialize(self):
self.dt = self.options["dt"]
self.controllerTypeOrder = ["default"]
self.setDefaultOptions()
self.Sensor = SensorObj(
rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
)
self.SensorApproximator = SensorApproximatorObj(
numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"]
)
self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)
self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName("world"))
self.world = World.buildLineSegmentTestWorld(
percentObsDensity=self.options["World"]["percentObsDensity"],
circleRadius=self.options["World"]["circleRadius"],
nonRandom=self.options["World"]["nonRandomWorld"],
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
om.removeFromObjectModel(om.findObjectByName("robot"))
self.robot, self.frame = World.buildRobot()
self.frame = self.robot.getChildFrame()
self.frame.setProperty("Scale", 3)
# self.frame.setProperty('Visible', False)
# self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]
self.Car.setFrame(self.frame)
print "Finished initialization"
def runSingleSimulation(self, controllerType="default", simulationCutoff=None):
# self.setRandomCollisionFreeInitialState()
self.setInitialStateAtZero()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
firstRaycast = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)
# self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
# self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
# self.Sensor.setLocator(self.LineSegmentLocator)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
thisRunIndex = 0
NMaxSteps = 100
while self.counter < self.numTimesteps - 1:
thisRunIndex += 1
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx, :] = currentCarState
x = self.stateOverTime[idx, 0]
y = self.stateOverTime[idx, 1]
self.setRobotFrameState(x, y, 0.0)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
self.raycastData[idx, :] = currentRaycast
S_current = (currentCarState, currentRaycast)
if controllerType not in self.colorMap.keys():
print
raise ValueError("controller of type " + controllerType + " not supported")
if controllerType in ["default", "defaultRandom"]:
controlInput, controlInputIdx = self.Controller.computeControlInput(
currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False
)
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
print "NEXTCARSTATE is ", nextCarState
x = nextCarState[0]
y = nextCarState[1]
self.setRobotFrameState(x, y, 0.0)
nextRaycast = self.Sensor.raycastAll(self.frame)
# Compute the next control input
S_next = (nextCarState, nextRaycast)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False
)
# bookkeeping
currentCarState = nextCarState
currentRaycast = nextRaycast
self.counter += 1
# break if we are in collision
if self.checkInCollision(nextRaycast):
if self.verbose:
print "Had a collision, terminating simulation"
break
if thisRunIndex > NMaxSteps:
print "was safe during N steps"
break
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter, :] = currentCarState
self.raycastData[self.counter, :] = currentRaycast
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
def setNumpyRandomSeed(self, seed=1):
np.random.seed(seed)
def runBatchSimulation(self, endTime=None, dt=0.05):
self.controllerTypeOrder = ["default"]
self.counter = 0
self.simulationData = []
self.initializeStatusBar()
self.idxDict = dict()
numRunsCounter = 0
self.idxDict["default"] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps)
while (self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1:
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(controllerType="default", simulationCutoff=simCutoff)
runData["startIdx"] = startIdx
runData["controllerType"] = "default"
runData["duration"] = self.counter - runData["startIdx"]
runData["endIdx"] = self.counter
runData["runNumber"] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
# BOOKKEEPING
# truncate stateOverTime, raycastData, controlInputs to be the correct size
self.numTimesteps = self.counter + 1
self.stateOverTime = self.stateOverTime[0 : self.counter + 1, :]
self.raycastData = self.raycastData[0 : self.counter + 1, :]
self.controlInputData = self.controlInputData[0 : self.counter + 1]
self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime
def initializeStatusBar(self):
self.numTicks = 10
self.nextTickComplete = 1.0 / float(self.numTicks)
self.nextTickIdx = 1
print "Simulation percentage complete: (", self.numTicks, " # is complete)"
def printStatusBar(self):
fractionDone = float(self.counter) / float(self.numTimesteps)
if fractionDone > self.nextTickComplete:
self.nextTickIdx += 1
self.nextTickComplete += 1.0 / self.numTicks
timeSoFar = time.time() - self.startSimTime
estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0
print "#" * self.nextTickIdx, "-" * (
self.numTicks - self.nextTickIdx
), "estimated", estimatedTimeLeft_minutes, "minutes left"
def setCollisionFreeInitialState(self):
tol = 5
while True:
x = 0.0
y = -5.0
theta = 0 # + np.random.uniform(0,2*np.pi,1)[0] * 0.01
self.Car.setCarState(x, y, 0.0, 0.0)
self.setRobotFrameState(x, y, theta)
print "In loop"
if not self.checkInCollision():
break
return x, y, theta
def setInitialStateAtZero(self):
x = 0.0
y = 0.0
theta = 0.0
self.Car.setCarState(x, y, 0.0, 0.0)
self.setRobotFrameState(x, y, theta)
return x, y, theta
def setRandomCollisionFreeInitialState(self):
tol = 5
while True:
x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0]
y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0]
theta = np.random.uniform(0, 2 * np.pi, 1)[0]
self.Car.setCarState(x, y, theta)
self.setRobotFrameState(x, y, theta)
if not self.checkInCollision():
break
return x, y, theta
def setupPlayback(self):
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.tick
playButtonFps = 1.0 / self.dt
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
self.sliderMovedByPlayTimer = False
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
self.max_velocity = 20.0
sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged)
sliderXVelocity.setMaximum(self.max_velocity)
sliderXVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderXVelocity)
sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged)
sliderYVelocity.setMaximum(self.max_velocity)
sliderYVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderYVelocity)
firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
self.drawFirstIntersections(self.frame, firstRaycast)
randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal")
randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton)
l.addWidget(randomGlobalGoalButton)
randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles")
randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton)
l.addWidget(randomObstaclesButton)
buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World")
buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles)
l.addWidget(buildWorldFromRandomObstaclesButton)
findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)")
findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton)
l.addWidget(findLocalGoalButton)
drawActionSetButton = QtGui.QPushButton("Draw Action Set")
drawActionSetButton.connect("clicked()", self.onDrawActionSetButton)
l.addWidget(drawActionSetButton)
runSimButton = QtGui.QPushButton("Simulate")
runSimButton.connect("clicked()", self.onRunSimButton)
l.addWidget(runSimButton)
playButton = QtGui.QPushButton("Play/Pause")
playButton.connect("clicked()", self.onPlayButton)
slider = QtGui.QSlider(QtCore.Qt.Horizontal)
slider.connect("valueChanged(int)", self.onSliderChanged)
self.sliderMax = self.numTimesteps
slider.setMaximum(self.sliderMax)
self.slider = slider
# slider4 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider4.setMaximum(self.sliderMax)
# l.addWidget(slider4)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider6 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider6.setMaximum(self.sliderMax)
# l.addWidget(slider6)
# slider7 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider7.setMaximum(self.sliderMax)
# l.addWidget(slider7)
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
self.view.orientationMarkerWidget().Off()
l.addWidget(panel)
w.showMaximized()
self.frame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.frame)
applogic.resetCamera(viewDirection=[0.2, 0, -1])
self.view.showMaximized()
self.view.raise_()
panel = screengrabberpanel.ScreenGrabberPanel(self.view)
panel.widget.show()
cameracontrolpanel.CameraControlPanel(self.view).widget.show()
elapsed = time.time() - self.startSimTime
simRate = self.counter / elapsed
print "Total run time", elapsed
print "Ticks (Hz)", simRate
print "Number of steps taken", self.counter
self.onRandomObstaclesButton()
self.app.start()
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i, :]
if firstRaycast[i] >= self.Sensor.rayLength - 0.1:
d.addLine(origin, endpoint, color=[0, 1, 0])
else:
d.addLine(origin, endpoint, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.locator = self.LineSegmentLocator
self.Sensor.setLocator(self.LineSegmentLocator)
def updateDrawIntersection(self, frame, locator="None"):
if locator == "None":
locator = self.locator
origin = np.array(frame.transform.GetPosition())
# print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
# print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
# camera = self.view.camera()
# camera.SetFocalPoint(frame.transform.GetPosition())
# camera.SetPosition(frame.transform.TransformPoint((-30,0,10)))
def getControllerTypeFromCounter(self, counter):
name = self.controllerTypeOrder[0]
for controllerType in self.controllerTypeOrder[1:]:
if counter >= self.idxDict[controllerType]:
name = controllerType
return name
def setRobotFrameState(self, x, y, theta):
t = vtk.vtkTransform()
t.Translate(x, y, 0.0)
t.RotateZ(np.degrees(theta))
self.robot.getChildFrame().copyFrame(t)
# returns true if we are in collision
def checkInCollision(self, raycastDistance=None):
if raycastDistance is None:
self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2])
raycastDistance = self.Sensor.raycastAll(self.frame)
if np.min(raycastDistance) < self.collisionThreshold:
return True
else:
return False
# if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold:
# return True
# else:
# return False
def tick(self):
# print timer.elapsed
# simulate(t.elapsed)
x = np.sin(time.time())
y = np.cos(time.time())
self.setRobotFrameState(x, y, 0.0)
if (time.time() - self.playTime) > self.endTime:
self.playTimer.stop()
def tick2(self):
newtime = time.time() - self.playTime
print time.time() - self.playTime
x = np.sin(newtime)
y = np.cos(newtime)
self.setRobotFrameState(x, y, 0.0)
# just increment the slider, stop the timer if we get to the end
def playTimerCallback(self):
self.sliderMovedByPlayTimer = True
currentIdx = self.slider.value
nextIdx = currentIdx + 1
self.slider.setSliderPosition(nextIdx)
if currentIdx >= self.sliderMax:
print "reached end of tape, stopping playTime"
self.playTimer.stop()
def onSliderChanged(self, value):
if not self.sliderMovedByPlayTimer:
self.playTimer.stop()
numSteps = len(self.stateOverTime)
idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax)))
idx = min(idx, numSteps - 1)
x, y, xdot, ydot = self.stateOverTime[idx]
self.setRobotFrameState(x, y, 0)
self.sliderMovedByPlayTimer = False
def onXVelocityChanged(self, value):
self.XVelocity_drawing = value
self.onDrawActionSetButton()
print "x velocity changed to ", value
def onYVelocityChanged(self, value):
print value
self.YVelocity_drawing = -value
self.onDrawActionSetButton()
print "y velocity changed to ", -value
def onShowSensorsButton(self):
print "I pressed the show sensors button"
self.setInitialStateAtZero()
firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
self.drawFirstIntersections(self.frame, firstRaycast)
def onRandomObstaclesButton(self):
print "random obstacles button pressed"
self.setInitialStateAtZero()
self.world = World.buildLineSegmentTestWorld(
percentObsDensity=8.0,
circleRadius=self.options["World"]["circleRadius"],
nonRandom=False,
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.updateDrawIntersection(self.frame, locator=self.locator)
def onRandomGlobalGoalButton(self):
print "random global goal button pressed"
self.globalGoal = World.buildGlobalGoal()
def onBuildWorldFromRandomObstacles(self):
distances = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances)
self.polygon_initial_distances = distances
self.polygon_initial_raycastLocations = firstRaycastLocations
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.locator = self.LineSegmentLocator
self.Sensor.setLocator(self.LineSegmentLocator)
self.updateDrawIntersection(self.frame)
def onFindLocalGoalButton(self):
print "find local goal button pressed"
local_goal = self.findLocalGoal()
print local_goal, "is my local goal"
self.localGoal = World.placeLocalGoal(local_goal)
def findLocalGoal(self):
biggest_gap_width = 0
biggest_gap_start_index = 0
current_gap_width = 0
current_gap_start_index = 0
for index, value in enumerate(self.polygon_initial_distances):
# if still in a gap
if value == self.Sensor.rayLength:
current_gap_width += 1
# else terminate counting for this gap
else:
if current_gap_width > biggest_gap_width:
biggest_gap_width = current_gap_width
biggest_gap_start_index = current_gap_start_index
current_gap_width = 0
current_gap_start_index = index + 1
if current_gap_width > biggest_gap_width:
biggest_gap_width = current_gap_width
biggest_gap_start_index = current_gap_start_index
middle_index_of_gap = biggest_gap_start_index + biggest_gap_width / 2
return self.polygon_initial_raycastLocations[middle_index_of_gap, :]
def onDrawActionSetButton(self):
print "drawing action set"
# self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
self.ActionSet.computeAllPositions(self.XVelocity_drawing, self.YVelocity_drawing)
# self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
def onRunSimButton(self):
self.runBatchSimulation()
self.saveToFile("latest")
def onPlayButton(self):
if self.playTimer.isActive():
self.onPauseButton()
return
print "play"
self.playTimer.start()
self.playTime = time.time()
def onPauseButton(self):
print "pause"
self.playTimer.stop()
def saveToFile(self, filename):
# should also save the run data if it is available, i.e. stateOverTime, rewardOverTime
filename = "data/" + filename + ".out"
my_shelf = shelve.open(filename, "n")
my_shelf["options"] = self.options
my_shelf["simulationData"] = self.simulationData
my_shelf["stateOverTime"] = self.stateOverTime
my_shelf["raycastData"] = self.raycastData
my_shelf["controlInputData"] = self.controlInputData
my_shelf["numTimesteps"] = self.numTimesteps
my_shelf["idxDict"] = self.idxDict
my_shelf["counter"] = self.counter
my_shelf.close()
def run(self, launchApp=True):
self.counter = 1
# for use in playback
self.dt = self.options["dt"]
self.endTime = self.defaultControllerTime # used to be the sum of the other times as well
self.t = np.arange(0.0, self.endTime, self.dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps + 1, 4))
self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
self.controlInputData = np.zeros((maxNumTimesteps + 1, 2))
self.numTimesteps = maxNumTimesteps
# self.runBatchSimulation()
if launchApp:
self.setupPlayback()
@staticmethod
def loadFromFile(filename):
filename = "data/" + filename + ".out"
sim = Simulator(autoInitialize=False, verbose=False)
my_shelf = shelve.open(filename)
sim.options = my_shelf["options"]
sim.initialize()
sim.simulationData = my_shelf["simulationData"]
sim.stateOverTime = np.array(my_shelf["stateOverTime"])
sim.raycastData = np.array(my_shelf["raycastData"])
sim.controlInputData = np.array(my_shelf["controlInputData"])
sim.numTimesteps = my_shelf["numTimesteps"]
sim.idxDict = my_shelf["idxDict"]
sim.counter = my_shelf["counter"]
my_shelf.close()
return sim
class PlaybackPanel(object):
def __init__(self, planPlayback, playbackRobotModel,
playbackJointController, robotStateModel,
robotStateJointController, manipPlanner):
self.planPlayback = planPlayback
self.playbackRobotModel = playbackRobotModel
self.playbackJointController = playbackJointController
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.manipPlanner = manipPlanner
manipPlanner.connectPlanCommitted(self.onPlanCommitted)
manipPlanner.connectUseSupports(self.updateButtonColor)
self.autoPlay = True
#self.useOperationColors()
self.useDevelopmentColors()
self.planFramesObj = None
self.plan = None
self.poseInterpolator = None
self.startTime = 0.0
self.endTime = 1.0
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = self.updateAnimation
self.animationClock = SimpleTimer()
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
self.ui = WidgetDict(self.widget.children())
self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)',
self.viewModeChanged)
self.ui.playbackSpeedCombo.connect(
'currentIndexChanged(const QString&)', self.playbackSpeedChanged)
self.ui.interpolationCombo.connect(
'currentIndexChanged(const QString&)', self.interpolationChanged)
self.ui.samplesSpinBox.connect('valueChanged(int)',
self.numberOfSamplesChanged)
self.ui.playbackSlider.connect('valueChanged(int)',
self.playbackSliderValueChanged)
self.ui.animateButton.connect('clicked()', self.animateClicked)
self.ui.hideButton.connect('clicked()', self.hideClicked)
self.ui.executeButton.connect('clicked()', self.executeClicked)
self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
self.ui.stopButton.connect('clicked()', self.stopClicked)
self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.ui.executeButton.connect(
'customContextMenuRequested(const QPoint&)',
self.showExecuteContextMenu)
self.setPlan(None)
self.hideClicked()
def useDevelopmentColors(self):
self.robotStateModelDisplayAlpha = 0.1
self.playbackRobotModelUseTextures = False
self.playbackRobotModelDisplayAlpha = 1
def useOperationColors(self):
self.robotStateModelDisplayAlpha = 1
self.playbackRobotModelUseTextures = False
self.playbackRobotModelDisplayAlpha = 0.5
def showExecuteContextMenu(self, clickPosition):
globalPos = self.ui.executeButton.mapToGlobal(clickPosition)
menu = QtGui.QMenu()
menu.addAction('Visualization Only')
if not self.isPlanFeasible():
menu.addSeparator()
if self.isPlanAPlanWithSupports():
menu.addAction('Execute infeasible plan with supports')
else:
menu.addAction('Execute infeasible plan')
elif self.isPlanAPlanWithSupports():
menu.addSeparator()
menu.addAction('Execute plan with supports')
selectedAction = menu.exec_(globalPos)
if not selectedAction:
return
if selectedAction.text == 'Visualization Only':
self.executePlan(visOnly=True)
elif selectedAction.text == 'Execute infeasible plan':
self.executePlan(overrideInfeasibleCheck=True)
elif selectedAction.text == 'Execute plan with supports':
self.executePlan(overrideSupportsCheck=True)
elif selectedAction.text == 'Execute infeasible plan with supports':
self.executePlan(overrideInfeasibleCheck=True,
overrideSupportsCheck=True)
def getViewMode(self):
return str(self.ui.viewModeCombo.currentText)
def setViewMode(self, mode):
'''
Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
e.g. can hide all plan playback with 'hidden'
'''
self.ui.viewModeCombo.setCurrentIndex(
self.ui.viewModeCombo.findText(mode))
def getPlaybackSpeed(self):
s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
if '/' in s:
n, d = s.split('/')
return float(n) / float(d)
return float(s)
def getInterpolationMethod(self):
return str(self.ui.interpolationCombo.currentText)
def getNumberOfSamples(self):
return self.ui.samplesSpinBox.value
def viewModeChanged(self):
viewMode = self.getViewMode()
if viewMode == 'continuous':
playbackVisible = True
samplesVisible = False
interpolationVisible = True
elif viewMode == 'frames':
playbackVisible = False
samplesVisible = True
interpolationVisible = True
elif viewMode == 'hidden':
playbackVisible = False
samplesVisible = False
interpolationVisible = False
else:
raise Exception('Unexpected view mode')
self.ui.samplesLabel.setVisible(samplesVisible)
self.ui.samplesSpinBox.setVisible(samplesVisible)
self.ui.interpolationLabel.setVisible(interpolationVisible)
self.ui.interpolationCombo.setVisible(interpolationVisible)
self.ui.playbackSpeedLabel.setVisible(playbackVisible)
self.ui.playbackSpeedCombo.setVisible(playbackVisible)
self.ui.playbackSlider.setEnabled(playbackVisible)
self.ui.animateButton.setVisible(playbackVisible)
self.ui.timeLabel.setVisible(playbackVisible)
self.hidePlan()
if self.plan:
if viewMode == 'continuous' and self.autoPlay:
self.startAnimation()
elif viewMode == 'frames':
self.updatePlanFrames()
def playbackSpeedChanged(self):
self.planPlayback.playbackSpeed = self.getPlaybackSpeed()
def getPlaybackTime(self):
sliderValue = self.ui.playbackSlider.value
return (sliderValue / 1000.0) * self.endTime
def updateTimeLabel(self):
playbackTime = self.getPlaybackTime()
self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime
def playbackSliderValueChanged(self):
self.updateTimeLabel()
self.showPoseAtTime(self.getPlaybackTime())
def interpolationChanged(self):
methods = {
'linear': 'slinear',
'cubic spline': 'cubic',
'pchip': 'pchip'
}
self.planPlayback.interpolationMethod = methods[
self.getInterpolationMethod()]
self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(
self.plan)
self.updatePlanFrames()
def numberOfSamplesChanged(self):
self.updatePlanFrames()
def animateClicked(self):
self.startAnimation()
def hideClicked(self):
if self.ui.hideButton.text == 'hide':
self.ui.playbackFrame.setEnabled(False)
self.hidePlan()
self.ui.hideButton.text = 'show'
self.ui.executeButton.setEnabled(False)
if not self.plan:
self.ui.hideButton.setEnabled(False)
else:
self.ui.playbackFrame.setEnabled(True)
self.ui.hideButton.text = 'hide'
self.ui.hideButton.setEnabled(True)
self.ui.executeButton.setEnabled(True)
self.viewModeChanged()
self.updateButtonColor()
def executeClicked(self):
self.executePlan()
def executePlan(self,
visOnly=False,
overrideInfeasibleCheck=False,
overrideSupportsCheck=False):
if visOnly:
_, poses = self.planPlayback.getPlanPoses(self.plan)
self.onPlanCommitted(self.plan)
self.robotStateJointController.setPose('EST_ROBOT_STATE',
poses[-1])
else:
if (self.isPlanFeasible() or overrideInfeasibleCheck) and (
not self.isPlanAPlanWithSupports()
or overrideSupportsCheck):
self.manipPlanner.commitManipPlan(self.plan)
def onPlanCommitted(self, plan):
self.setPlan(None)
self.hideClicked()
def stopClicked(self):
self.stopAnimation()
self.manipPlanner.sendPlanPause()
def isPlanFeasible(self):
plan = robotstate.asRobotPlan(self.plan)
return plan is not None and (max(plan.plan_info) < 10
and min(plan.plan_info) >= 0)
def getPlanInfo(self, plan):
plan = robotstate.asRobotPlan(self.plan)
return max(plan.plan_info)
def isPlanAPlanWithSupports(self):
return hasattr(
self.plan,
'support_sequence') or self.manipPlanner.publishPlansWithSupports
def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(
self.plan, self.playbackJointController, self.playbackRobotModel,
numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
[startColor, endColor],
axis=0,
kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(),
'robot plan',
alpha=1.0,
visible=False,
colorByName='RGB255',
parent='planning')
self.showPlanFrames()
def showPlanFrames(self):
self.planFramesObj.setProperty('Visible', True)
self.robotStateModel.setProperty('Visible', False)
self.playbackRobotModel.setProperty('Visible', False)
def startAnimation(self):
self.showPlaybackModel()
self.stopAnimation()
self.ui.playbackSlider.value = 0
self.animationClock.reset()
self.animationTimer.start()
self.updateAnimation()
def stopAnimation(self):
self.animationTimer.stop()
def showPlaybackModel(self):
self.robotStateModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty(
'Color Mode', 1 if self.playbackRobotModelUseTextures else 0)
self.robotStateModel.setProperty('Alpha',
self.robotStateModelDisplayAlpha)
self.playbackRobotModel.setProperty(
'Alpha', self.playbackRobotModelDisplayAlpha)
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
def hidePlan(self):
self.stopAnimation()
self.ui.playbackSlider.value = 0
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
if self.playbackRobotModel:
self.playbackRobotModel.setProperty('Visible', False)
self.robotStateModel.setProperty('Visible', True)
self.robotStateModel.setProperty('Alpha', 1.0)
def showPoseAtTime(self, time):
pose = self.poseInterpolator(time)
self.playbackJointController.setPose('plan_playback', pose)
def updateAnimation(self):
tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
if tNow > self.endTime:
tNow = self.endTime
sliderValue = int(1000.0 * tNow / self.endTime)
self.ui.playbackSlider.blockSignals(True)
self.ui.playbackSlider.value = sliderValue
self.ui.playbackSlider.blockSignals(False)
self.updateTimeLabel()
self.showPoseAtTime(tNow)
return tNow < self.endTime
def updateButtonColor(self):
if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible(
):
styleSheet = 'background-color:red'
elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports(
):
styleSheet = 'background-color:orange'
else:
styleSheet = ''
self.ui.executeButton.setStyleSheet(styleSheet)
def setPlan(self, plan):
self.ui.playbackSlider.value = 0
self.ui.timeLabel.text = 'Time: 0.00 s'
self.ui.planNameLabel.text = ''
self.plan = plan
self.endTime = 1.0
self.updateButtonColor()
if not self.plan:
return
planText = 'Plan: %d. %.2f seconds' % (
plan.utime, self.planPlayback.getPlanElapsedTime(plan))
self.ui.planNameLabel.text = planText
self.startTime = 0.0
self.endTime = self.planPlayback.getPlanElapsedTime(plan)
self.interpolationChanged()
info = self.getPlanInfo(plan)
app.displaySnoptInfo(info)
if self.ui.hideButton.text == 'show':
self.hideClicked()
else:
self.viewModeChanged()
self.updateButtonColor()
if self.autoPlay and self.widget.parent() is not None:
self.widget.parent().show()
class ImageWidget(object):
def __init__(self, imageManager, imageName, view, visible=True):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.visible = visible
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
imageRep = self.imageWidget.GetRepresentation()
self.imageWidget.ResizableOff()
self.imageWidget.SelectableOn()
imageRep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
imageRep.SetImage(self.flip.GetOutput())
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.view.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.onResizeEvent)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def setWidgetSize(self, desiredWidth=400):
image = self.imageManager.getImage(self.imageName)
dims = image.GetDimensions()
if 0.0 in dims:
return
aspectRatio = float(dims[0]) / dims[1]
imageWidth, imageHeight = desiredWidth, desiredWidth / aspectRatio
viewWidth, viewHeight = self.view.width, self.view.height
rep = self.imageWidget.GetBorderRepresentation()
rep.SetShowBorderToOff()
coord = rep.GetPositionCoordinate()
coord2 = rep.GetPosition2Coordinate()
coord.SetCoordinateSystemToDisplay()
coord2.SetCoordinateSystemToDisplay()
coord.SetValue(0, viewHeight - imageHeight)
coord2.SetValue(imageWidth, imageHeight)
self.view.render()
def onResizeEvent(self):
self.setWidgetSize(400)
def setImageName(self, imageName):
self.imageName = imageName
self.flip.SetInput(imageManager.getImage(imageName))
def setOpacity(self, opacity=1.0):
self.imageWidget.GetRepresentation().GetImageProperty().SetOpacity(opacity)
def hide(self):
self.visible = False
self.imageWidget.Off()
self.view.render()
def show(self):
self.visible = True
if self.haveImage():
self.imageWidget.On()
self.view.render()
def haveImage(self):
image = self.imageManager.getImage(self.imageName)
dims = image.GetDimensions()
return 0.0 not in dims
def updateView(self):
if not self.visible or not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
if not self.initialized and self.visible and self.haveImage():
self.show()
self.setWidgetSize(400)
self.initialized = True
class CameraImageView(object):
def __init__(self, imageManager, imageName, viewName=None, view=None):
imageManager.addImage(imageName)
self.cameraRoll = None
self.imageManager = imageManager
self.viewName = viewName or imageName
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.initView(view)
self.initEventFilter()
def getImagePixel(self, displayPoint, restrictToImageDimensions=True):
worldPoint = [0.0, 0.0, 0.0, 0.0]
vtk.vtkInteractorObserver.ComputeDisplayToWorld(
self.view.renderer(), displayPoint[0], displayPoint[1], 0, worldPoint
)
imageDimensions = self.getImage().GetDimensions()
if (
0.0 <= worldPoint[0] <= imageDimensions[0]
and 0.0 <= worldPoint[1] <= imageDimensions[1]
or not restrictToImageDimensions
):
return [worldPoint[0], worldPoint[1], 0.0]
else:
return None
def getWorldPositionAndRay(self, imagePixel, imageUtime=None):
"""
Given an XY image pixel, computes an equivalent ray in the world
coordinate system using the camera to local transform at the given
imageUtime. If imageUtime is None, then the utime of the most recent
image is used.
Returns the camera xyz position in world, and a ray unit vector.
"""
if imageUtime is None:
imageUtime = self.imageManager.getUtime(self.imageName)
# input is pixel u,v, output is unit x,y,z in camera coordinates
cameraPoint = self.imageManager.queue.unprojectPixel(self.imageName, imagePixel[0], imagePixel[1])
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.imageName, "local", imageUtime, cameraToLocal)
p = np.array(cameraToLocal.TransformPoint(cameraPoint))
cameraPosition = np.array(cameraToLocal.GetPosition())
ray = p - cameraPosition
ray /= np.linalg.norm(ray)
return cameraPosition, ray
def filterEvent(self, obj, event):
if self.eventFilterEnabled and event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == "p":
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == "r":
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def onRubberBandPick(self, obj, event):
displayPoints = self.interactorStyle.GetStartPosition(), self.interactorStyle.GetEndPosition()
imagePoints = [vis.pickImage(point, self.view)[1] for point in displayPoints]
sendFOVRequest(self.imageName, imagePoints)
def getImage(self):
return self.imageManager.getImage(self.imageName)
def initView(self, view):
self.view = view or app.getViewManager().createView(self.viewName, "VTK View")
self.view.installImageInteractor()
self.interactorStyle = self.view.renderWindow().GetInteractor().GetInteractorStyle()
self.interactorStyle.AddObserver("SelectionChangedEvent", self.onRubberBandPick)
self.imageActor = vtk.vtkImageActor()
self.imageActor.SetInput(self.getImage())
self.imageActor.SetVisibility(False)
self.view.renderer().AddActor(self.imageActor)
self.view.orientationMarkerWidget().Off()
self.view.backgroundRenderer().SetBackground(0, 0, 0)
self.view.backgroundRenderer().SetBackground2(0, 0, 0)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.filterEvent)
self.eventFilterEnabled = True
def setCameraRoll(self, roll):
self.cameraRoll = roll
self.resetCamera()
def resetCamera(self):
camera = self.view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0, 0, 0)
camera.SetPosition(0, 0, -1)
camera.SetViewUp(0, -1, 0)
if self.cameraRoll is not None:
camera.SetRoll(self.cameraRoll)
self.view.resetCamera()
self.fitImageToView()
self.view.render()
def fitImageToView(self):
camera = self.view.camera()
image = self.getImage()
imageWidth, imageHeight, _ = image.GetDimensions()
viewWidth, viewHeight = self.view.renderWindow().GetSize()
aspectRatio = float(viewWidth) / viewHeight
parallelScale = max(imageWidth / aspectRatio, imageHeight) / 2.0
camera.SetParallelScale(parallelScale)
def setImageName(self, imageName):
if imageName == self.imageName:
return
assert self.imageManager.hasImage(imageName)
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.imageActor.SetInput(self.imageManager.getImage(self.imageName))
self.imageActor.SetVisibility(False)
self.view.render()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.view.render()
if not self.imageInitialized and self.imageActor.GetInput().GetDimensions()[0]:
self.imageActor.SetVisibility(True)
self.resetCamera()
self.imageInitialized = True
class PlanPlayback(object):
def __init__(self):
self.animationCallback = None
self.animationTimer = None
self.interpolationMethod = "slinear"
self.playbackSpeed = 1.0
self.jointNameRegex = ""
@staticmethod
def getPlanPoses(msgOrList):
if isinstance(msgOrList, list):
messages = msgOrList
allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0])
for msg in messages[1:]:
poseTimes, poses = PlanPlayback.getPlanPoses(msg)
poseTimes += allPoseTimes[-1]
allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:]))
allPoses += poses[1:]
return allPoseTimes, allPoses
else:
msg = asRobotPlan(msgOrList)
poses = []
poseTimes = []
for plan in msg.plan:
pose = robotstate.convertStateMessageToDrakePose(plan)
poseTimes.append(plan.utime / 1e6)
poses.append(pose)
return np.array(poseTimes), poses
@staticmethod
def getPlanElapsedTime(msg):
msg = asRobotPlan(msg)
startTime = msg.plan[0].utime
endTime = msg.plan[-1].utime
return (endTime - startTime) / 1e6
def stopAnimation(self):
if self.animationTimer:
self.animationTimer.stop()
def setInterpolationMethod(method):
self.interpolationMethod = method
def playPlan(self, msg, jointController):
self.playPlans([msg], jointController)
def playPlans(self, messages, jointController):
assert len(messages)
poseTimes, poses = self.getPlanPoses(messages)
self.playPoses(poseTimes, poses, jointController)
def getPoseInterpolatorFromPlan(self, message):
poseTimes, poses = self.getPlanPoses(message)
return self.getPoseInterpolator(poseTimes, poses)
def getPoseInterpolator(self, poseTimes, poses, unwrap_rpy=True):
if unwrap_rpy:
poses = np.array(poses, copy=True)
poses[:, 3:6] = np.unwrap(poses[:, 3:6], axis=0)
if self.interpolationMethod in ["slinear", "quadratic", "cubic"]:
f = scipy.interpolate.interp1d(poseTimes,
poses,
axis=0,
kind=self.interpolationMethod)
elif self.interpolationMethod == "pchip":
f = scipy.interpolate.PchipInterpolator(poseTimes, poses, axis=0)
return f
def getPlanPoseMeshes(self, messages, jointController, robotModel,
numberOfSamples):
poseTimes, poses = self.getPlanPoses(messages)
f = self.getPoseInterpolator(poseTimes, poses)
sampleTimes = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
meshes = []
for sampleTime in sampleTimes:
pose = f(sampleTime)
jointController.setPose("plan_playback", pose)
polyData = vtk.vtkPolyData()
robotModel.model.getModelMesh(polyData)
meshes.append(polyData)
return meshes
def showPoseAtTime(self, time, jointController, poseInterpolator):
pose = poseInterpolator(time)
jointController.setPose("plan_playback", pose)
def playPoses(self, poseTimes, poses, jointController):
f = self.getPoseInterpolator(poseTimes, poses)
timer = SimpleTimer()
def updateAnimation():
tNow = timer.elapsed() * self.playbackSpeed
if tNow > poseTimes[-1]:
pose = poses[-1]
jointController.setPose("plan_playback", pose)
if self.animationCallback:
self.animationCallback()
return False
pose = f(tNow)
jointController.setPose("plan_playback", pose)
if self.animationCallback:
self.animationCallback()
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
updateAnimation()
def picklePlan(self, filename, msg):
poseTimes, poses = self.getPlanPoses(msg)
pickle.dump((poseTimes, poses), open(filename, "w"))
def getMovingJointNames(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [
robotstate.getDrakePoseJointNames()[jointId]
for jointId in jointIds
]
return jointNames
def plotPlan(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
self.plotPoses(poseTimes, poses)
def plotPoses(self, poseTimes, poses):
import matplotlib.pyplot as plt
poses = np.array(poses)
if self.jointNameRegex:
jointIds = range(poses.shape[1])
else:
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [
robotstate.getDrakePoseJointNames()[jointId]
for jointId in jointIds
]
jointTrajectories = [poses[:, jointId] for jointId in jointIds]
seriesNames = []
sampleResolutionInSeconds = 0.01
numberOfSamples = (poseTimes[-1] -
poseTimes[0]) / sampleResolutionInSeconds
xnew = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
fig = plt.figure()
ax = fig.add_subplot(111)
for jointId, jointName, jointTrajectory in zip(jointIds, jointNames,
jointTrajectories):
if self.jointNameRegex and not re.match(self.jointNameRegex,
jointName):
continue
x = poseTimes
y = jointTrajectory
y = np.rad2deg(y)
if self.interpolationMethod in ["slinear", "quadratic", "cubic"]:
f = scipy.interpolate.interp1d(x,
y,
kind=self.interpolationMethod)
elif self.interpolationMethod == "pchip":
f = scipy.interpolate.PchipInterpolator(x, y)
ax.plot(x, y, "ko")
seriesNames.append(jointName + " points")
ax.plot(xnew, f(xnew), "-")
seriesNames.append(jointName + " " + self.interpolationMethod)
ax.legend(seriesNames, loc="upper right").draggable()
ax.set_xlabel("time (s)")
ax.set_ylabel("joint angle (deg)")
ax.set_title("joint trajectories")
plt.show()
print reader
import time
app = ConsoleApp()
view = app.createView()
def spin():
polyData = vtk.vtkPolyData()
reader.GetMesh(polyData)
vis.updatePolyData(polyData, 'mesh')
print "Number of points (a)", polyData.GetNumberOfPoints()
if (polyData.GetNumberOfPoints() == 0):
return
polyDataPC = vtk.vtkPolyData()
reader.GetPointCloud(polyDataPC)
vis.updatePolyData(polyDataPC, 'output')
print "Number of points (b)", polyDataPC.GetNumberOfPoints()
quitTimer = TimerCallback(targetFps=5.0)
quitTimer.callback = spin
quitTimer.start()
if app.getTestingInteractiveEnabled():
view.show()
app.showObjectModel()
app.start()
class ImageWidget(object):
"""
Wrapper for displaying vtkImageData on a director-style view.
@note This is more like director's `CameraImageView` than its
`ImageWidget`.
"""
def __init__(self, image_handler):
self._name = 'Image View'
self._view = PythonQt.dd.ddQVTKWidgetView()
self._image_handler = image_handler
self._image = vtk.vtkImageData()
self._prev_attrib = None
# Initialize the view.
self._view.installImageInteractor()
# Add actor.
self._image_actor = vtk.vtkImageActor()
vtk_SetInputData(self._image_actor, self._image)
self._image_actor.SetVisibility(False)
self._view.renderer().AddActor(self._image_actor)
self._view.orientationMarkerWidget().Off()
self._view.backgroundRenderer().SetBackground(0, 0, 0)
self._view.backgroundRenderer().SetBackground2(0, 0, 0)
self._depth_mapper = None
# Add timer.
self._render_timer = TimerCallback(targetFps=60, callback=self.render)
self._render_timer.start()
def get_widget(self):
return self._view
def render(self):
if not self._view.isVisible():
return
has_new = self._image_handler.update_image(self._image)
assert isinstance(has_new, bool)
if not has_new:
return
cur_attrib = get_vtk_image_attrib(self._image)
if self._prev_attrib != cur_attrib:
if self._prev_attrib is None:
# Initialization. Ensure it is visible.
self._image_actor.SetVisibility(True)
# Fit image to view.
self._on_new_image_attrib(cur_attrib)
# Update.
self._prev_attrib = cur_attrib
if self._depth_mapper is not None:
depth_range = self._get_depth_range()
for i in xrange(2):
value = [0.] * 6
coloring = self._depth_mapper.GetLookupTable()
coloring.GetNodeValue(i, value)
value[0] = depth_range[i]
coloring.SetNodeValue(i, value)
self._view.render()
def _get_depth_range(self):
lower_depth = 0
upper_depth = _max_depth
if upper_depth == -1:
# @note `GetScalarRange` permits non-finite values, such as `inf`.
# Use a custom mechanism to get min/max.
data = vtk_image_to_numpy(self._image)
if data.dtype == np.float32:
good = np.isfinite(data[:])
elif data.dtype == np.uint16:
maxarray = np.full(data.shape, 65535)
good = np.less(data[:], maxarray)
else:
raise RuntimeError("Unsupported depth format: {}".format(
data.dtype))
if np.any(good):
upper_depth = np.max(data[good])
return (lower_depth, upper_depth)
def _on_new_image_attrib(self, attrib):
((w, h, num_channels), dtype) = attrib
if self._image_handler.is_depth_image():
assert num_channels == 1, num_channels
assert dtype in (np.uint16, np.float32), dtype
# TODO(eric.cousineau): Delegate to outside of `ImageWidget`?
# This is depth-image specific.
# For now, just set arbitrary values.
depth_range = self._get_depth_range()
lower_color = (1, 1, 1) # White
upper_color = (0, 0, 0) # Black
nan_color = (0.5, 0.5, 1) # Light blue - No return.
inf_color = (0.5, 0, 0.) # Dark red - Too far / too close.
# Use `vtkColorTransferFunction` as it provides a more intuitive
# interpolating interface for me (Eric) than `vtkLookupTable`,
# since it permits direct specification of RGB values.
coloring = vtk.vtkColorTransferFunction()
coloring.AddRGBPoint(depth_range[0], *lower_color)
coloring.AddRGBPoint(depth_range[1], *upper_color)
coloring.SetNanColor(*nan_color)
# @note `coloring.SetAboveRangeColor` doesn't seem to work?
coloring.AddRGBPoint(depth_range[1] + 10000, *inf_color)
coloring.SetClamping(True)
coloring.SetScaleToLinear()
self._depth_mapper = vtk.vtkImageMapToColors()
self._depth_mapper.SetLookupTable(coloring)
vtk_SetInputData(self._depth_mapper, self._image)
vtk_SetInputData(self._image_actor, self._depth_mapper.GetOutput())
self._image_actor.GetMapper().SetInputConnection(
self._depth_mapper.GetOutputPort())
else:
# Direct connection.
self._depth_mapper = None
vtk_SetInputData(self._image_actor, self._image)
# Must render first.
self._view.render()
# Fit image to view.
# TODO(eric.cousineau): No idea why this is needed; it worked for
# VTK 5, but no longer for VTK 6+?
camera = self._view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0, 0, 0)
camera.SetPosition(0, 0, -1)
camera.SetViewUp(0, -1, 0)
self._view.resetCamera()
image_height, image_width = get_vtk_image_shape(self._image)[:2]
view_width, view_height = self._view.renderWindow().GetSize()
aspect_ratio = float(view_width) / view_height
parallel_scale = max(image_width / aspect_ratio, image_height) / 2.0
camera.SetParallelScale(parallel_scale)
class AsyncTaskQueue(object):
QUEUE_STARTED_SIGNAL = 'QUEUE_STARTED_SIGNAL'
QUEUE_STOPPED_SIGNAL = 'QUEUE_STOPPED_SIGNAL'
TASK_STARTED_SIGNAL = 'TASK_STARTED_SIGNAL'
TASK_ENDED_SIGNAL = 'TASK_ENDED_SIGNAL'
TASK_PAUSED_SIGNAL = 'TASK_PAUSED_SIGNAL'
TASK_FAILED_SIGNAL = 'TASK_FAILED_SIGNAL'
TASK_EXCEPTION_SIGNAL = 'TASK_EXCEPTION_SIGNAL'
class PauseException(Exception):
pass
class FailException(Exception):
pass
def __init__(self):
self.tasks = []
self.generators = []
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.callbackLoop
self.callbacks = callbacks.CallbackRegistry([self.QUEUE_STARTED_SIGNAL,
self.QUEUE_STOPPED_SIGNAL,
self.TASK_STARTED_SIGNAL,
self.TASK_ENDED_SIGNAL,
self.TASK_PAUSED_SIGNAL,
self.TASK_FAILED_SIGNAL,
self.TASK_EXCEPTION_SIGNAL])
self.currentTask = None
self.isRunning = False
def reset(self):
assert not self.isRunning
assert not self.generators
self.tasks = []
def start(self):
self.isRunning = True
self.callbacks.process(self.QUEUE_STARTED_SIGNAL, self)
self.timer.start()
def stop(self):
self.isRunning = False
self.currentTask = None
self.generators = []
self.timer.stop()
self.callbacks.process(self.QUEUE_STOPPED_SIGNAL, self)
def wrapGenerator(self, generator):
def generatorWrapper():
return generator
return generatorWrapper
def addTask(self, task):
if isinstance(task, types.GeneratorType):
task = self.wrapGenerator(task)
assert hasattr(task, '__call__')
self.tasks.append(task)
def callbackLoop(self):
try:
for i in xrange(10):
self.doWork()
if not self.tasks:
self.stop()
except AsyncTaskQueue.PauseException:
assert self.currentTask
self.callbacks.process(self.TASK_PAUSED_SIGNAL, self, self.currentTask)
self.stop()
except AsyncTaskQueue.FailException:
assert self.currentTask
self.callbacks.process(self.TASK_FAILED_SIGNAL, self, self.currentTask)
self.stop()
except:
assert self.currentTask
self.callbacks.process(self.TASK_EXCEPTION_SIGNAL, self, self.currentTask)
self.stop()
raise
return self.isRunning
def popTask(self):
assert not self.isRunning
assert not self.currentTask
if self.tasks:
self.tasks.pop(0)
def completePreviousTask(self):
assert self.currentTask
self.tasks.remove(self.currentTask)
self.callbacks.process(self.TASK_ENDED_SIGNAL, self, self.currentTask)
self.currentTask = None
def startNextTask(self):
self.currentTask = self.tasks[0]
self.callbacks.process(self.TASK_STARTED_SIGNAL, self, self.currentTask)
result = self.currentTask()
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def doWork(self):
if self.generators:
self.handleGenerator(self.generators[0])
else:
if self.currentTask:
self.completePreviousTask()
if self.tasks:
self.startNextTask()
def handleGenerator(self, generator):
try:
result = generator.next()
except StopIteration:
self.generators.remove(generator)
else:
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def connectQueueStarted(self, func):
return self.callbacks.connect(self.QUEUE_STARTED_SIGNAL, func)
def disconnectQueueStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectQueueStopped(self, func):
return self.callbacks.connect(self.QUEUE_STOPPED_SIGNAL, func)
def disconnectQueueStopped(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskStarted(self, func):
return self.callbacks.connect(self.TASK_STARTED_SIGNAL, func)
def disconnectTaskStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskEnded(self, func):
return self.callbacks.connect(self.TASK_ENDED_SIGNAL, func)
def disconnectTaskEnded(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskPaused(self, func):
return self.callbacks.connect(self.TASK_PAUSED_SIGNAL, func)
def disconnectTaskPaused(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskFailed(self, func):
return self.callbacks.connect(self.TASK_FAILED_SIGNAL, func)
def disconnectTaskFailed(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskException(self, func):
return self.callbacks.connect(self.TASK_EXCEPTION_SIGNAL, func)
def disconnectTaskException(self, callbackId):
self.callbacks.disconnect(callbackId)
class ImageWidget(object):
def __init__(self, imageManager, imageName, view, visible=True):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.visible = visible
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
imageRep = self.imageWidget.GetRepresentation()
self.imageWidget.ResizableOff()
self.imageWidget.SelectableOn()
imageRep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(
self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
imageRep.SetImage(self.flip.GetOutput())
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.view.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.onResizeEvent)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def setWidgetSize(self, desiredWidth=400):
image = self.imageManager.getImage(self.imageName)
dims = image.GetDimensions()
if 0.0 in dims:
return
aspectRatio = float(dims[0]) / dims[1]
imageWidth, imageHeight = desiredWidth, desiredWidth / aspectRatio
viewWidth, viewHeight = self.view.width, self.view.height
rep = self.imageWidget.GetBorderRepresentation()
rep.SetShowBorderToOff()
coord = rep.GetPositionCoordinate()
coord2 = rep.GetPosition2Coordinate()
coord.SetCoordinateSystemToDisplay()
coord2.SetCoordinateSystemToDisplay()
coord.SetValue(0, viewHeight - imageHeight)
coord2.SetValue(imageWidth, imageHeight)
self.view.render()
def onResizeEvent(self):
self.setWidgetSize(400)
def setImageName(self, imageName):
self.imageName = imageName
self.flip.SetInput(imageManager.getImage(imageName))
def setOpacity(self, opacity=1.0):
self.imageWidget.GetRepresentation().GetImageProperty().SetOpacity(
opacity)
def hide(self):
self.visible = False
self.imageWidget.Off()
self.view.render()
def show(self):
self.visible = True
if self.haveImage():
self.imageWidget.On()
self.view.render()
def haveImage(self):
image = self.imageManager.getImage(self.imageName)
dims = image.GetDimensions()
return 0.0 not in dims
def updateView(self):
if not self.visible or not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
if not self.initialized and self.visible and self.haveImage():
self.show()
self.setWidgetSize(400)
self.initialized = True
class KorgNanoKontrol(object):
def __init__(self):
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.tick
self.stop = self.timer.stop
self.reader = None
self.initReader()
self.inputs = {
'slider': [0, 8, True],
'dial': [16, 8, True],
'r_button': [64, 8, False],
'm_button': [48, 8, False],
's_button': [32, 8, False],
'track_left': [58, 1, False],
'track_right': [59, 1, False],
'cycle': [46, 1, False],
'marker_set': [60, 1, False],
'marker_left': [61, 1, False],
'marker_right': [62, 1, False],
'rewind': [43, 1, False],
'fastforward': [44, 1, False],
'stop': [42, 1, False],
'play': [41, 1, False],
'record': [45, 1, False],
}
signalNames = []
for inputName, inputDescription in self.inputs.items():
channelStart, numChannels, isContinuous = inputDescription
for i in range(numChannels):
channelId = '' if numChannels == 1 else '_%d' % i
if isContinuous:
signalNames.append('%s%s_value_changed' %
(inputName, channelId))
else:
signalNames.append('%s%s_pressed' % (inputName, channelId))
signalNames.append('%s%s_released' %
(inputName, channelId))
self.callbacks = callbacks.CallbackRegistry(signalNames)
def start(self):
self.initReader()
if self.reader is not None:
self.timer.start()
def initReader(self):
if self.reader:
return
try:
self.reader = midi.MidiReader(midi.findKorgNanoKontrol2())
except:
print('midi controller not found.')
self.reader = None
def onMidiCommand(self, channel, value):
#print channel, '%.2f' % value
inputs = self.inputs
for inputName, inputDescription in inputs.items():
channelStart, numChannels, isContinuous = inputDescription
if channelStart <= channel < (channelStart + numChannels):
if numChannels > 1:
inputName = '%s_%d' % (inputName, channel - channelStart)
if isContinuous:
self.onContinuousInput(inputName, value)
elif value == 1:
self.onButtonDown(inputName)
elif value == 0:
self.onButtonUp(inputName)
def onContinuousInput(self, name, value):
#print name, '%.2f' % value
self.callbacks.process(name + '_value_changed', value)
def onButtonDown(self, name):
#print name, 'down'
self.callbacks.process(name + '_pressed')
def onButtonUp(self, name):
#print name, 'up'
self.callbacks.process(name + '_released')
def tick(self):
try:
messages = self.reader.getMessages()
except:
messages = []
if not messages:
return
targets = {}
for message in messages:
channel = message[2]
value = message[3]
targets[channel] = value
for channel, value in targets.items():
position = value / 127.0
self.onMidiCommand(channel, position)
class CameraImageView(object):
def __init__(self, imageManager, imageName, viewName=None, view=None):
imageManager.addImage(imageName)
self.cameraRoll = None
self.imageManager = imageManager
self.viewName = viewName or imageName
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.initView(view)
self.initEventFilter()
def getImagePixel(self, displayPoint, restrictToImageDimensions=True):
worldPoint = [0.0, 0.0, 0.0, 0.0]
vtk.vtkInteractorObserver.ComputeDisplayToWorld(
self.view.renderer(), displayPoint[0], displayPoint[1], 0,
worldPoint)
imageDimensions = self.getImage().GetDimensions()
if 0.0 <= worldPoint[0] <= imageDimensions[0] and 0.0 <= worldPoint[
1] <= imageDimensions[1] or not restrictToImageDimensions:
return [worldPoint[0], worldPoint[1], 0.0]
else:
return None
def getWorldPositionAndRay(self, imagePixel, imageUtime=None):
'''
Given an XY image pixel, computes an equivalent ray in the world
coordinate system using the camera to local transform at the given
imageUtime. If imageUtime is None, then the utime of the most recent
image is used.
Returns the camera xyz position in world, and a ray unit vector.
'''
if imageUtime is None:
imageUtime = self.imageManager.getUtime(self.imageName)
# input is pixel u,v, output is unit x,y,z in camera coordinates
cameraPoint = self.imageManager.queue.unprojectPixel(
self.imageName, imagePixel[0], imagePixel[1])
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.imageName, 'local',
imageUtime, cameraToLocal)
p = np.array(cameraToLocal.TransformPoint(cameraPoint))
cameraPosition = np.array(cameraToLocal.GetPosition())
ray = p - cameraPosition
ray /= np.linalg.norm(ray)
return cameraPosition, ray
def filterEvent(self, obj, event):
if self.eventFilterEnabled and event.type(
) == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def onRubberBandPick(self, obj, event):
displayPoints = self.interactorStyle.GetStartPosition(
), self.interactorStyle.GetEndPosition()
imagePoints = [
vis.pickImage(point, self.view)[1] for point in displayPoints
]
sendFOVRequest(self.imageName, imagePoints)
def getImage(self):
return self.imageManager.getImage(self.imageName)
def initView(self, view):
self.view = view or app.getViewManager().createView(
self.viewName, 'VTK View')
self.view.installImageInteractor()
self.interactorStyle = self.view.renderWindow().GetInteractor(
).GetInteractorStyle()
self.interactorStyle.AddObserver('SelectionChangedEvent',
self.onRubberBandPick)
self.imageActor = vtk.vtkImageActor()
self.imageActor.SetInput(self.getImage())
self.imageActor.SetVisibility(False)
self.view.renderer().AddActor(self.imageActor)
self.view.orientationMarkerWidget().Off()
self.view.backgroundRenderer().SetBackground(0, 0, 0)
self.view.backgroundRenderer().SetBackground2(0, 0, 0)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(
QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.filterEvent)
self.eventFilterEnabled = True
def setCameraRoll(self, roll):
self.cameraRoll = roll
self.resetCamera()
def resetCamera(self):
camera = self.view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0, 0, 0)
camera.SetPosition(0, 0, -1)
camera.SetViewUp(0, -1, 0)
if self.cameraRoll is not None:
camera.SetRoll(self.cameraRoll)
self.view.resetCamera()
self.fitImageToView()
self.view.render()
def fitImageToView(self):
camera = self.view.camera()
image = self.getImage()
imageWidth, imageHeight, _ = image.GetDimensions()
viewWidth, viewHeight = self.view.renderWindow().GetSize()
aspectRatio = float(viewWidth) / viewHeight
parallelScale = max(imageWidth / aspectRatio, imageHeight) / 2.0
camera.SetParallelScale(parallelScale)
def setImageName(self, imageName):
if imageName == self.imageName:
return
assert self.imageManager.hasImage(imageName)
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.imageActor.SetInput(self.imageManager.getImage(self.imageName))
self.imageActor.SetVisibility(False)
self.view.render()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.view.render()
if not self.imageInitialized and self.imageActor.GetInput(
).GetDimensions()[0]:
self.imageActor.SetVisibility(True)
self.resetCamera()
self.imageInitialized = True
class PlaybackPanel(object):
def __init__(self, planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner):
self.planPlayback = planPlayback
self.playbackRobotModel = playbackRobotModel
self.playbackJointController = playbackJointController
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.manipPlanner = manipPlanner
manipPlanner.connectPlanCommitted(self.onPlanCommitted)
manipPlanner.connectUseSupports(self.updateButtonColor)
self.autoPlay = True
self.useOperationColors()
self.planFramesObj = None
self.plan = None
self.poseInterpolator = None
self.startTime = 0.0
self.endTime = 1.0
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = self.updateAnimation
self.animationClock = SimpleTimer()
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
self.ui = WidgetDict(self.widget.children())
self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)', self.viewModeChanged)
self.ui.playbackSpeedCombo.connect('currentIndexChanged(const QString&)', self.playbackSpeedChanged)
self.ui.interpolationCombo.connect('currentIndexChanged(const QString&)', self.interpolationChanged)
self.ui.samplesSpinBox.connect('valueChanged(int)', self.numberOfSamplesChanged)
self.ui.playbackSlider.connect('valueChanged(int)', self.playbackSliderValueChanged)
self.ui.animateButton.connect('clicked()', self.animateClicked)
self.ui.hideButton.connect('clicked()', self.hideClicked)
self.ui.executeButton.connect('clicked()', self.executeClicked)
self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
self.ui.stopButton.connect('clicked()', self.stopClicked)
self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.ui.executeButton.connect('customContextMenuRequested(const QPoint&)', self.showExecuteContextMenu)
self.setPlan(None)
self.hideClicked()
def useDevelopmentColors(self):
self.robotStateModelDisplayAlpha = 0.1
self.playbackRobotModelUseTextures = True
self.playbackRobotModelDisplayAlpha = 1
def useOperationColors(self):
self.robotStateModelDisplayAlpha = 1
self.playbackRobotModelUseTextures = False
self.playbackRobotModelDisplayAlpha = 0.5
def showExecuteContextMenu(self, clickPosition):
globalPos = self.ui.executeButton.mapToGlobal(clickPosition)
menu = QtGui.QMenu()
menu.addAction('Visualization Only')
if not self.isPlanFeasible():
menu.addSeparator()
if self.isPlanAPlanWithSupports():
menu.addAction('Execute infeasible plan with supports')
else:
menu.addAction('Execute infeasible plan')
elif self.isPlanAPlanWithSupports():
menu.addSeparator()
menu.addAction('Execute plan with supports')
selectedAction = menu.exec_(globalPos)
if not selectedAction:
return
if selectedAction.text == 'Visualization Only':
self.executePlan(visOnly=True)
elif selectedAction.text == 'Execute infeasible plan':
self.executePlan(overrideInfeasibleCheck=True)
elif selectedAction.text == 'Execute plan with supports':
self.executePlan(overrideSupportsCheck=True)
elif selectedAction.text == 'Execute infeasible plan with supports':
self.executePlan(overrideInfeasibleCheck=True, overrideSupportsCheck=True)
def getViewMode(self):
return str(self.ui.viewModeCombo.currentText)
def setViewMode(self, mode):
'''
Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
e.g. can hide all plan playback with 'hidden'
'''
self.ui.viewModeCombo.setCurrentIndex(self.ui.viewModeCombo.findText(mode))
def getPlaybackSpeed(self):
s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
if '/' in s:
n, d = s.split('/')
return float(n)/float(d)
return float(s)
def getInterpolationMethod(self):
return str(self.ui.interpolationCombo.currentText)
def getNumberOfSamples(self):
return self.ui.samplesSpinBox.value
def viewModeChanged(self):
viewMode = self.getViewMode()
if viewMode == 'continuous':
playbackVisible = True
samplesVisible = False
else:
playbackVisible = False
samplesVisible = True
self.ui.samplesLabel.setEnabled(samplesVisible)
self.ui.samplesSpinBox.setEnabled(samplesVisible)
self.ui.playbackSpeedLabel.setVisible(playbackVisible)
self.ui.playbackSpeedCombo.setVisible(playbackVisible)
self.ui.playbackSlider.setEnabled(playbackVisible)
self.ui.animateButton.setVisible(playbackVisible)
self.ui.timeLabel.setVisible(playbackVisible)
if self.plan:
if self.getViewMode() == 'continuous' and self.autoPlay:
self.startAnimation()
else:
self.ui.playbackSlider.value = 0
self.stopAnimation()
self.updatePlanFrames()
def playbackSpeedChanged(self):
self.planPlayback.playbackSpeed = self.getPlaybackSpeed()
def getPlaybackTime(self):
sliderValue = self.ui.playbackSlider.value
return (sliderValue / 1000.0) * self.endTime
def updateTimeLabel(self):
playbackTime = self.getPlaybackTime()
self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime
def playbackSliderValueChanged(self):
self.updateTimeLabel()
self.showPoseAtTime(self.getPlaybackTime())
def interpolationChanged(self):
methods = {'linear' : 'slinear',
'cubic spline' : 'cubic',
'pchip' : 'pchip' }
self.planPlayback.interpolationMethod = methods[self.getInterpolationMethod()]
self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(self.plan)
self.updatePlanFrames()
def numberOfSamplesChanged(self):
self.updatePlanFrames()
def animateClicked(self):
self.startAnimation()
def hideClicked(self):
if self.ui.hideButton.text == 'hide':
self.ui.playbackFrame.setEnabled(False)
self.hidePlan()
self.ui.hideButton.text = 'show'
self.ui.executeButton.setEnabled(False)
if not self.plan:
self.ui.hideButton.setEnabled(False)
else:
self.ui.playbackFrame.setEnabled(True)
self.ui.hideButton.text = 'hide'
self.ui.hideButton.setEnabled(True)
self.ui.executeButton.setEnabled(True)
self.viewModeChanged()
self.updateButtonColor()
def executeClicked(self):
self.executePlan()
def executePlan(self, visOnly=False, overrideInfeasibleCheck=False, overrideSupportsCheck=False):
if visOnly:
_, poses = self.planPlayback.getPlanPoses(self.plan)
self.onPlanCommitted(self.plan)
self.robotStateJointController.setPose('EST_ROBOT_STATE', poses[-1])
else:
if (self.isPlanFeasible() or overrideInfeasibleCheck) and (not self.isPlanAPlanWithSupports() or overrideSupportsCheck):
self.manipPlanner.commitManipPlan(self.plan)
def onPlanCommitted(self, plan):
self.setPlan(None)
self.hideClicked()
def stopClicked(self):
self.stopAnimation()
self.manipPlanner.sendPlanPause()
def isPlanFeasible(self):
plan = robotstate.asRobotPlan(self.plan)
return plan is not None and (max(plan.plan_info) < 10 and min(plan.plan_info) >= 0)
def getPlanInfo(self, plan):
plan = robotstate.asRobotPlan(self.plan)
return max(plan.plan_info)
def isPlanAPlanWithSupports(self):
return hasattr(self.plan, 'support_sequence') or self.manipPlanner.publishPlansWithSupports
def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85/255.0, 255/255.0, 255/255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning')
self.showPlanFrames()
def showPlanFrames(self):
self.planFramesObj.setProperty('Visible', True)
self.robotStateModel.setProperty('Visible', False)
self.playbackRobotModel.setProperty('Visible', False)
def startAnimation(self):
self.showPlaybackModel()
self.stopAnimation()
self.ui.playbackSlider.value = 0
self.animationClock.reset()
self.animationTimer.start()
self.updateAnimation()
def stopAnimation(self):
self.animationTimer.stop()
def showPlaybackModel(self):
self.robotStateModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Color Mode', 1 if self.playbackRobotModelUseTextures else 0)
self.robotStateModel.setProperty('Alpha', self.robotStateModelDisplayAlpha)
self.playbackRobotModel.setProperty('Alpha', self.playbackRobotModelDisplayAlpha)
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
def hidePlan(self):
self.stopAnimation()
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
if self.playbackRobotModel:
self.playbackRobotModel.setProperty('Visible', False)
self.robotStateModel.setProperty('Visible', True)
self.robotStateModel.setProperty('Alpha', 1.0)
def showPoseAtTime(self, time):
pose = self.poseInterpolator(time)
self.playbackJointController.setPose('plan_playback', pose)
def updateAnimation(self):
tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
if tNow > self.endTime:
tNow = self.endTime
sliderValue = int(1000.0 * tNow / self.endTime)
self.ui.playbackSlider.blockSignals(True)
self.ui.playbackSlider.value = sliderValue
self.ui.playbackSlider.blockSignals(False)
self.updateTimeLabel()
self.showPoseAtTime(tNow)
return tNow < self.endTime
def updateButtonColor(self):
if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible():
styleSheet = 'background-color:red'
elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports():
styleSheet = 'background-color:orange'
else:
styleSheet = ''
self.ui.executeButton.setStyleSheet(styleSheet)
def setPlan(self, plan):
self.ui.playbackSlider.value = 0
self.ui.timeLabel.text = 'Time: 0.00 s'
self.ui.planNameLabel.text = ''
self.plan = plan
self.endTime = 1.0
self.updateButtonColor()
if not self.plan:
return
planText = 'Plan: %d. %.2f seconds' % (plan.utime, self.planPlayback.getPlanElapsedTime(plan))
self.ui.planNameLabel.text = planText
self.startTime = 0.0
self.endTime = self.planPlayback.getPlanElapsedTime(plan)
self.interpolationChanged()
info = self.getPlanInfo(plan)
app.displaySnoptInfo(info)
if self.ui.hideButton.text == 'show':
self.hideClicked()
else:
self.viewModeChanged()
self.updateButtonColor()
if self.autoPlay and self.widget.parent() is not None:
self.widget.parent().show()
class TaskUserPanel(object):
def __init__(self, windowTitle='Task Panel'):
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddTaskUserPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.widget.setWindowTitle(windowTitle)
self.manualButtons = {}
self.imageViewLayout = QtGui.QHBoxLayout(self.ui.imageFrame)
self._setupParams()
self._setupPropertiesPanel()
self._initTaskPanel()
def addManualButton(self, text, callback):
b = QtGui.QPushButton(text)
b.connect('clicked()', callback)
self.manualButtons[text] = b
self.addManualWidget(b)
return b
def addManualSpacer(self):
line = QtGui.QFrame()
line.setFrameShape(QtGui.QFrame.HLine)
line.setFrameShadow(QtGui.QFrame.Sunken)
self.addManualWidget(line)
def addManualWidget(self, widget):
self.ui.manualButtonsLayout.insertWidget(self.ui.manualButtonsLayout.count()-1, widget)
def initImageView(self, imageView, activateAffordanceUpdater=True):
if activateAffordanceUpdater:
self.affordanceUpdater = affordanceupdater.AffordanceInCameraUpdater(segmentation.affordanceManager, imageView)
self.affordanceUpdater.timer.start()
self.imageViewLayout.addWidget(imageView.view)
def _setupParams(self):
self.params = om.ObjectModelItem('Task Params')
self.params.properties.connectPropertyChanged(self.onPropertyChanged)
def _setupPropertiesPanel(self):
l = QtGui.QVBoxLayout(self.ui.propertyFrame)
l.setMargin(0)
self.propertiesPanel = PythonQt.dd.ddPropertiesPanel()
self.propertiesPanel.setBrowserModeToWidget()
l.addWidget(self.propertiesPanel)
self.panelConnector = propertyset.PropertyPanelConnector(self.params.properties, self.propertiesPanel)
def onPropertyChanged(self, propertySet, propertyName):
pass
def getNextTasks(self):
return self.taskTree.getTasks(fromSelected=True)
def onContinue(self):
self._activatePrompts()
self.completedTasks = []
self.taskQueue.reset()
for obj in self.getNextTasks():
self.taskQueue.addTask(obj.task)
self.taskQueue.start()
def _activatePrompts(self):
rt.UserPromptTask.promptFunction = self.onTaskPrompt
rt.PrintTask.printFunction = self.appendMessage
def onStep(self):
assert not self.taskQueue.isRunning
self._activatePrompts()
tasks = self.getNextTasks()
if not tasks:
return
task = tasks[0].task
self.nextStepTask = tasks[1].task if len(tasks) > 1 else None
self.completedTasks = []
self.taskQueue.reset()
self.taskQueue.addTask(task)
self.taskQueue.start()
def updateTaskButtons(self):
self.ui.taskStepButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskContinueButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskPauseButton.setEnabled(self.taskQueue.isRunning)
def onPause(self):
if not self.taskQueue.isRunning:
return
self.nextStepTask = None
currentTask = self.taskQueue.currentTask
self.taskQueue.stop()
if currentTask:
currentTask.stop()
self.appendMessage('<font color="red">paused</font>')
def onQueueStarted(self, taskQueue):
self.updateTaskButtons()
def onQueueStopped(self, taskQueue):
self.clearPrompt()
self.updateTaskButtons()
def onTaskStarted(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="green">start</font>'
self.appendMessage(msg)
self.taskTree.selectTask(task)
item = self.taskTree.findTaskItem(task)
if len(self.completedTasks) and item.getProperty('Visible'):
self.appendMessage('<font color="red">paused</font>')
raise atq.AsyncTaskQueue.PauseException()
def onTaskEnded(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="green">end</font>'
self.appendMessage(msg)
self.completedTasks.append(task)
if self.taskQueue.tasks:
self.taskTree.selectTask(self.taskQueue.tasks[0])
elif self.nextStepTask:
self.taskTree.selectTask(self.nextStepTask)
#else:
# self.taskTree.selectTask(self.completedTasks[0])
def onTaskFailed(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">failed: %s</font>' % task.failReason
self.appendMessage(msg)
def onTaskPaused(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">paused</font>'
self.appendMessage(msg)
def onTaskException(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">exception:\n\n%s</font>' % traceback.format_exc()
self.appendMessage(msg)
def appendMessage(self, msg):
if msg == self.lastStatusMessage:
return
self.lastStatusMessage = msg
self.ui.outputConsole.append(msg.replace('\n', '<br/>'))
def updateTaskStatus(self):
currentTask = self.taskQueue.currentTask
if not currentTask or not currentTask.statusMessage:
return
name = currentTask.properties.getProperty('Name')
status = currentTask.statusMessage
msg = name + ': ' + status
self.appendMessage(msg)
def clearPrompt(self):
self.promptTask = None
self.ui.promptLabel.text = ''
self.ui.promptAcceptButton.enabled = False
self.ui.promptRejectButton.enabled = False
def onAcceptPrompt(self):
self.promptTask.accept()
self.clearPrompt()
def onRejectPrompt(self):
self.promptTask.reject()
self.clearPrompt()
def onTaskPrompt(self, task, message):
self.promptTask = task
self.ui.promptLabel.text = message
self.ui.promptAcceptButton.enabled = True
self.ui.promptRejectButton.enabled = True
def _initTaskPanel(self):
self.lastStatusMessage = ''
self.nextStepTask = None
self.completedTasks = []
self.taskQueue = atq.AsyncTaskQueue()
self.taskQueue.connectQueueStarted(self.onQueueStarted)
self.taskQueue.connectQueueStopped(self.onQueueStopped)
self.taskQueue.connectTaskStarted(self.onTaskStarted)
self.taskQueue.connectTaskEnded(self.onTaskEnded)
self.taskQueue.connectTaskPaused(self.onTaskPaused)
self.taskQueue.connectTaskFailed(self.onTaskFailed)
self.taskQueue.connectTaskException(self.onTaskException)
self.timer = TimerCallback(targetFps=2)
self.timer.callback = self.updateTaskStatus
self.timer.start()
self.taskTree = tmw.TaskTree()
self.ui.taskFrame.layout().insertWidget(0, self.taskTree.treeWidget)
l = QtGui.QVBoxLayout(self.ui.taskPropertiesGroupBox)
l.addWidget(self.taskTree.propertiesPanel)
PythonQt.dd.ddGroupBoxHider(self.ui.taskPropertiesGroupBox)
self.ui.taskStepButton.connect('clicked()', self.onStep)
self.ui.taskContinueButton.connect('clicked()', self.onContinue)
self.ui.taskPauseButton.connect('clicked()', self.onPause)
self.ui.promptAcceptButton.connect('clicked()', self.onAcceptPrompt)
self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt)
self.clearPrompt()
self.updateTaskButtons()
def start(self):
if self.reader is None:
self.reader = drc.vtkMapServerSource()
self.reader.Start()
TimerCallback.start(self)
class CameraView(object):
def __init__(self, imageManager, view=None):
self.imageManager = imageManager
self.updateUtimes = {}
self.robotModel = None
self.sphereObjects = {}
self.sphereImages = [
'CAMERA_LEFT', 'CAMERACHEST_RIGHT', 'CAMERACHEST_LEFT'
]
for name in self.sphereImages:
imageManager.addImage(name)
self.updateUtimes[name] = 0
self.initView(view)
self.initEventFilter()
self.rayCallback = rayDebug
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def onViewDoubleClicked(self, displayPoint):
obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)
if pickedPoint is None or not obj:
return
imageName = obj.getProperty('Name')
imageUtime = self.imageManager.getUtime(imageName)
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(imageName, 'local', imageUtime,
cameraToLocal)
utorsoToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform('utorso', 'local', imageUtime,
utorsoToLocal)
p = range(3)
utorsoToLocal.TransformPoint(pickedPoint, p)
ray = np.array(p) - np.array(cameraToLocal.GetPosition())
ray /= np.linalg.norm(ray)
if self.rayCallback:
self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
def filterEvent(self, obj, event):
if event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
self.onViewDoubleClicked(vis.mapMousePosition(obj, event))
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(
QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.filterEvent)
def initImageRotations(self, robotModel):
self.robotModel = robotModel
# Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie)
if robotModel.getHeadLink():
tf = robotModel.getLinkFrame(robotModel.getHeadLink())
roll = transformUtils.rollPitchYawFromTransform(tf)[0]
if np.isclose(np.abs(roll), np.pi, atol=1e-1):
self.imageManager.setImageRotation180('CAMERA_LEFT')
def initView(self, view):
self.view = view or app.getViewManager().createView(
'Camera View', 'VTK View')
self.renderers = [self.view.renderer()]
renWin = self.view.renderWindow()
renWin.SetNumberOfLayers(3)
for i in [1, 2]:
ren = vtk.vtkRenderer()
ren.SetLayer(2)
ren.SetActiveCamera(self.view.camera())
renWin.AddRenderer(ren)
self.renderers.append(ren)
def applyCustomBounds():
self.view.addCustomBounds([-100, 100, -100, 100, -100, 100])
self.view.connect('computeBoundsRequest(ddQVTKWidgetView*)',
applyCustomBounds)
app.setCameraTerrainModeEnabled(self.view, True)
self.resetCamera()
def resetCamera(self):
self.view.camera().SetViewAngle(90)
self.view.camera().SetPosition(-7.5, 0.0, 5.0)
self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
self.view.camera().SetViewUp(0.0, 0.0, 1.0)
self.view.render()
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {
'CAMERA_LEFT': 20,
'CAMERACHEST_LEFT': 20,
'CAMERACHEST_RIGHT': 20
}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = 'tcoords_%s' % imageName
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(),
'tcoords_U')
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(),
'tcoords_V')
geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
geometry.GetPointData().SetTCoords(
geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry,
imageName,
view=self.view,
parent='cameras')
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def updateSphereGeometry(self):
for imageName in self.sphereImages:
sphereObj = self.getSphereGeometry(imageName)
if not sphereObj:
continue
transform = vtk.vtkTransform()
self.imageManager.queue.getBodyToCameraTransform(
imageName, transform)
sphereObj.actor.SetUserTransform(transform.GetLinearInverse())
def updateImages(self):
updated = False
for imageName, lastUtime in self.updateUtimes.iteritems():
currentUtime = self.imageManager.updateImage(imageName)
if currentUtime != lastUtime:
self.updateUtimes[imageName] = currentUtime
updated = True
return updated
def updateView(self):
if not self.view.isVisible():
return
if not self.updateImages():
return
self.updateSphereGeometry()
self.view.render()
class ImageWidget(object):
"""
Wrapper for displaying vtkImageData on a director-style view.
@note This is more like director's `CameraImageView` than its
`ImageWidget`.
"""
def __init__(self, image_handler):
self._name = 'Image View'
self._view = PythonQt.dd.ddQVTKWidgetView()
self._image_handler = image_handler
self._image = vtk.vtkImageData()
self._prev_attrib = None
# Initialize the view.
self._view.installImageInteractor()
# Add actor.
self._image_actor = vtk.vtkImageActor()
vtk_SetInputData(self._image_actor, self._image)
self._image_actor.SetVisibility(False)
self._view.renderer().AddActor(self._image_actor)
self._view.orientationMarkerWidget().Off()
self._view.backgroundRenderer().SetBackground(0, 0, 0)
self._view.backgroundRenderer().SetBackground2(0, 0, 0)
self._depth_mapper = None
# Add timer.
self._render_timer = TimerCallback(
targetFps=60,
callback=self.render)
self._render_timer.start()
def get_widget(self):
return self._view
def render(self):
if not self._view.isVisible():
return
has_new = self._image_handler.update_image(self._image)
assert isinstance(has_new, bool)
if not has_new:
return
cur_attrib = get_vtk_image_attrib(self._image)
if self._prev_attrib != cur_attrib:
if self._prev_attrib is None:
# Initialization. Ensure it is visible.
self._image_actor.SetVisibility(True)
# Fit image to view.
self._on_new_image_attrib(cur_attrib)
# Update.
self._prev_attrib = cur_attrib
if self._depth_mapper is not None:
depth_range = self._get_depth_range()
for i in xrange(2):
value = [0.] * 6
coloring = self._depth_mapper.GetLookupTable()
coloring.GetNodeValue(i, value)
value[0] = depth_range[i]
coloring.SetNodeValue(i, value)
self._view.render()
def _get_depth_range(self):
lower_depth = 0
upper_depth = _max_depth
if upper_depth == -1:
# @note `GetScalarRange` permits non-finite values, such as `inf`.
# Use a custom mechanism to get min/max.
data = vtk_image_to_numpy(self._image)
if data.dtype == np.float32:
good = np.isfinite(data[:])
elif data.dtype == np.uint16:
maxarray = np.full(data.shape, 65535)
good = np.less(data[:], maxarray)
else:
raise RuntimeError(
"Unsupported depth format: {}".format(data.dtype))
if np.any(good):
upper_depth = np.max(data[good])
return (lower_depth, upper_depth)
def _on_new_image_attrib(self, attrib):
((w, h, num_channels), dtype) = attrib
if self._image_handler.is_depth_image():
assert num_channels == 1, num_channels
assert dtype in (np.uint16, np.float32), dtype
# TODO(eric.cousineau): Delegate to outside of `ImageWidget`?
# This is depth-image specific.
# For now, just set arbitrary values.
depth_range = self._get_depth_range()
lower_color = (1, 1, 1) # White
upper_color = (0, 0, 0) # Black
nan_color = (0.5, 0.5, 1) # Light blue - No return.
inf_color = (0.5, 0, 0.) # Dark red - Too far / too close.
# Use `vtkColorTransferFunction` as it provides a more intuitive
# interpolating interface for me (Eric) than `vtkLookupTable`,
# since it permits direct specification of RGB values.
coloring = vtk.vtkColorTransferFunction()
coloring.AddRGBPoint(depth_range[0], *lower_color)
coloring.AddRGBPoint(depth_range[1], *upper_color)
coloring.SetNanColor(*nan_color)
# @note `coloring.SetAboveRangeColor` doesn't seem to work?
coloring.AddRGBPoint(depth_range[1] + 10000, *inf_color)
coloring.SetClamping(True)
coloring.SetScaleToLinear()
self._depth_mapper = vtk.vtkImageMapToColors()
self._depth_mapper.SetLookupTable(coloring)
vtk_SetInputData(self._depth_mapper, self._image)
vtk_SetInputData(self._image_actor, self._depth_mapper.GetOutput())
self._image_actor.GetMapper().SetInputConnection(
self._depth_mapper.GetOutputPort())
else:
# Direct connection.
self._depth_mapper = None
vtk_SetInputData(self._image_actor, self._image)
# Must render first.
self._view.render()
# Fit image to view.
# TODO(eric.cousineau): No idea why this is needed; it worked for
# VTK 5, but no longer for VTK 6+?
camera = self._view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0, 0, 0)
camera.SetPosition(0, 0, -1)
camera.SetViewUp(0, -1, 0)
self._view.resetCamera()
image_height, image_width = get_vtk_image_shape(self._image)[:2]
view_width, view_height = self._view.renderWindow().GetSize()
aspect_ratio = float(view_width) / view_height
parallel_scale = max(image_width / aspect_ratio, image_height) / 2.0
camera.SetParallelScale(parallel_scale)
class CameraView(object):
def __init__(self, imageManager, view=None):
self.imageManager = imageManager
self.updateUtimes = {}
self.robotModel = None
self.sphereObjects = {}
self.sphereImages = ["CAMERA_LEFT", "CAMERACHEST_RIGHT", "CAMERACHEST_LEFT"]
for name in self.sphereImages:
imageManager.addImage(name)
self.updateUtimes[name] = 0
self.initView(view)
self.initEventFilter()
self.rayCallback = rayDebug
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def onViewDoubleClicked(self, displayPoint):
obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)
if pickedPoint is None or not obj:
return
imageName = obj.getProperty("Name")
imageUtime = self.imageManager.getUtime(imageName)
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(imageName, "local", imageUtime, cameraToLocal)
utorsoToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform("utorso", "local", imageUtime, utorsoToLocal)
p = range(3)
utorsoToLocal.TransformPoint(pickedPoint, p)
ray = np.array(p) - np.array(cameraToLocal.GetPosition())
ray /= np.linalg.norm(ray)
if self.rayCallback:
self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
def filterEvent(self, obj, event):
if event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
self.onViewDoubleClicked(vis.mapMousePosition(obj, event))
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == "p":
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == "r":
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.filterEvent)
def initImageRotations(self, robotModel):
self.robotModel = robotModel
# Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie)
if robotModel.getHeadLink():
tf = robotModel.getLinkFrame(robotModel.getHeadLink())
roll = transformUtils.rollPitchYawFromTransform(tf)[0]
if np.isclose(np.abs(roll), np.pi, atol=1e-1):
self.imageManager.setImageRotation180("CAMERA_LEFT")
def initView(self, view):
self.view = view or app.getViewManager().createView("Camera View", "VTK View")
self.renderers = [self.view.renderer()]
renWin = self.view.renderWindow()
renWin.SetNumberOfLayers(3)
for i in [1, 2]:
ren = vtk.vtkRenderer()
ren.SetLayer(2)
ren.SetActiveCamera(self.view.camera())
renWin.AddRenderer(ren)
self.renderers.append(ren)
def applyCustomBounds():
self.view.addCustomBounds([-100, 100, -100, 100, -100, 100])
self.view.connect("computeBoundsRequest(ddQVTKWidgetView*)", applyCustomBounds)
app.setCameraTerrainModeEnabled(self.view, True)
self.resetCamera()
def resetCamera(self):
self.view.camera().SetViewAngle(90)
self.view.camera().SetPosition(-7.5, 0.0, 5.0)
self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
self.view.camera().SetViewUp(0.0, 0.0, 1.0)
self.view.render()
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {"CAMERA_LEFT": 20, "CAMERACHEST_LEFT": 20, "CAMERACHEST_RIGHT": 20}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = "tcoords_%s" % imageName
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(), "tcoords_U")
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(), "tcoords_V")
geometry = clipRange(geometry, "tcoords_U", [0.0, 1.0])
geometry = clipRange(geometry, "tcoords_V", [0.0, 1.0])
geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent="cameras")
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def updateSphereGeometry(self):
for imageName in self.sphereImages:
sphereObj = self.getSphereGeometry(imageName)
if not sphereObj:
continue
transform = vtk.vtkTransform()
self.imageManager.queue.getBodyToCameraTransform(imageName, transform)
sphereObj.actor.SetUserTransform(transform.GetLinearInverse())
def updateImages(self):
updated = False
for imageName, lastUtime in self.updateUtimes.iteritems():
currentUtime = self.imageManager.updateImage(imageName)
if currentUtime != lastUtime:
self.updateUtimes[imageName] = currentUtime
updated = True
return updated
def updateView(self):
if not self.view.isVisible():
return
if not self.updateImages():
return
self.updateSphereGeometry()
self.view.render()
class CameraTrackerManager(object):
def __init__(self):
self.target = None
self.targetFrame = None
self.trackerClass = None
self.camera = None
self.view = None
self.timer = TimerCallback()
self.timer.callback = self.updateTimer
self.addTrackers()
self.initTracker()
def updateTimer(self):
tNow = time.time()
dt = tNow - self.tLast
if dt < self.timer.elapsed / 2.0:
return
self.update()
def setView(self, view):
self.view = view
self.camera = view.camera()
def setTarget(self, target):
"""
target should be an instance of TargetFrameConverter or
any object that provides a method getTargetFrame().
"""
if target == self.target:
return
self.disableActiveTracker()
if not target:
return
self.target = target
self.targetFrame = target.getTargetFrame()
self.callbackId = self.targetFrame.connectFrameModified(self.onTargetFrameModified)
self.initTracker()
def disableActiveTracker(self):
if self.targetFrame:
self.targetFrame.disconnectFrameModified(self.callbackId)
self.target = None
self.targetFrame = None
self.initTracker()
def update(self):
tNow = time.time()
dt = tNow - self.tLast
self.tLast = tNow
if self.activeTracker:
self.activeTracker.dt = dt
self.activeTracker.update()
def reset(self):
self.tLast = time.time()
if self.activeTracker:
self.activeTracker.reset()
def getModeActions(self):
if self.activeTracker:
return self.activeTracker.actions
return []
def onModeAction(self, actionName):
if self.activeTracker:
self.activeTracker.onAction(actionName)
def getModeProperties(self):
if self.activeTracker:
return self.activeTracker.properties
return None
def onTargetFrameModified(self, frame):
self.update()
def initTracker(self):
self.timer.stop()
self.activeTracker = (
self.trackerClass(self.view, self.targetFrame) if (self.trackerClass and self.targetFrame) else None
)
self.reset()
self.update()
if self.activeTracker:
minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
if minimumUpdateRate > 0:
self.timer.targetFps = minimumUpdateRate
self.timer.start()
def addTrackers(self):
self.trackers = OrderedDict(
[
["Off", None],
["Position", PositionTracker],
["Position & Orientation", PositionOrientationTracker],
["Smooth Follow", SmoothFollowTracker],
["Look At", LookAtTracker],
["Orbit", OrbitTracker],
]
)
def setTrackerMode(self, modeName):
assert modeName in self.trackers
self.trackerClass = self.trackers[modeName]
self.initTracker()
def start(self):
if self.reader is None:
self.reader = drc.vtkMapServerSource()
self.reader.Start()
TimerCallback.start(self)
