def __init__(self, context):
super(StepPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StepPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'Step.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StepPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.tg = TrajectoryGenerator()
self._widget.bUp.clicked.connect(self.Up)
self._widget.bDown.clicked.connect(self.Down)
self._widget.bLeft.clicked.connect(self.Left)
self._widget.bRight.clicked.connect(self.Right)
self._widget.bFront.clicked.connect(self.Front)
self._widget.bBack.clicked.connect(self.Back)
self._widget.bStart.clicked.connect(self.Start)
self._widget.IrisInputBox.insertItems(
0, ['iris1', 'iris2', 'iris3', 'iris4'])
self.point = []
def __init__(self, my_id, bodies):
self.gain = 2. #x,y direction
self.gain_z = 0. #z direction
self.tg = TrajectoryGenerator()
self.my_id = my_id
self.bodies = bodies
self.states = []
for i in range(0, len(self.bodies)):
self.states.append(QuadPositionDerived())
rospy.Subscriber("/body_data/id_" + str(self.bodies[i]),
QuadPositionDerived, self.__set_states)
def __init__(self, trajectory_node, start, end):
Trajectory.__init__(self, trajectory_node)
self.start_point = start
self.end_point = end
self.tg = TrajectoryGenerator()
self.dist = self.tg.get_distance(self.start_point, self.end_point)
n = [0., 0., 0.]
for i in range(0, 3):
n[i] = self.end_point[i] - self.start_point[i]
self.e_t = self.tg.get_direction(n)
self.t_f = math.sqrt(6 * self.dist / 0.9 * self.a_max)
self.constant = -2.0 / self.t_f**3.0 * self.dist
def __init__(self,trajectory_node,mid,start,velo,psi): Trajectory.__init__(self,trajectory_node) self.tg = TrajectoryGenerator() self.midpoint = mid self.start = start n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]] self.radius = self.tg.get_distance(self.start,self.midpoint) self.initial_velo = velo self.velo = self.tg.get_norm(self.initial_velo) #define new coordinates self.e_n = self.tg.get_direction(n) self.yp = self.tg.get_direction(self.initial_velo) self.zp = numpy.cross(self.e_n,self.yp) self.psi = psi #angle of rotation about initial e_n direction self.w = self.radius*self.velo self.theta_z = self.tg.get_projection([0,0,1],self.e_n)
