def ros_loop(self):
r = rospy.Rate(30)
self.keyboard_toggle = 1
while not rospy.is_shutdown():
if self.teach_state == True:
self.teach_loop()
if self.keyboard.key.data == 'space' and self.keyboard_toggle == 1:
self.addWaypoint()
self.text_updater.update("STORED WAYPOINT " +
str(len(self.waypoints) - 1))
self.keyboard_toggle = 0
elif self.keyboard.key.data == 'space_released' and self.keyboard_toggle == 0:
self.keyboard_toggle = 1
elif self.keyboard.key.data == 'enter':
self.text_updater.update("GENERATING TRAJECTORY")
# catch fitpack error
try:
dfitpack.sproot(-1, -1, -1)
except Exception as e:
dfitpack_error = type(e)
try:
self.interpolate()
except dfitpack_error:
print("Not enough waypoints")
try:
rospy.wait_for_service('get_context', timeout=2.0)
get_context = rospy.ServiceProxy(
'get_context', GetContext)
resp = get_context()
self.context = resp.context
# empty waypoints
self.waypoints = []
self.teach_state = False
except (rospy.ServiceException, rospy.ROSException) as e:
print("Service call failed: %s" % e)
r.sleep()
def root_offset(self, root=0):
""" Return the values where the spline equals 'root'
Restriction: only cubic splines are supported by fitpack.
"""
t, c, k = self._eval_args
new_c = np.array(c)
new_c[np.nonzero(new_c)] += -root
if k == 3:
z, m, ier = dfitpack.sproot(t, new_c)
if not ier == 0:
raise ValueError("Error code returned by spalde: %s" % ier)
return z[:m]
raise NotImplementedError('finding roots unsupported for '
'non-cubic splines')
"""
Created on Wed Mar 30 09:51:40 2016
@author: semion
"""
from PyQt5 import QtCore, Qt, QtWidgets
import pyqtgraph as pg
from pyqtgraph import ViewBox
from pyqtgraph import InfiniteLine
from pyqtgraph import PlotWidget
import numpy as np
from scipy.interpolate import UnivariateSpline
from scipy.interpolate import dfitpack
try:
dfitpack.sproot(-1, -1, -1)
except Exception as e:
DFitPackError = type(e)
class myViewBox(ViewBox):
"""
ViewBox, настроенная для удаления точек при щелчках около них и показа
меню в противном случае.
"""
def __init__(self, parent_plot_widget, *args, **kwargs):
self.parent_plot_widget = parent_plot_widget
super(myViewBox, self).__init__(*args, **kwargs)
def raiseContextMenu(self, event):
if self.parent_plot_widget.removePoint(event):
from scipy.interpolate import dfitpack
try:
dfitpack.sproot(-1, -1, -1)
except Exception, e:
dfitpack_error = type(e)
print Exception
print e
print type(e)
try:
dfitpack.sproot(-1, -1, -1)
except dfitpack_error:
print "Got it!"
