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, 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, 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)
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, 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 = []
class AccGen(Trajectory): done = False a_max = 0.6**2.0 / 0.8 t_f = 0 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) def begin(self): v_max = (self.radius * self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): self.__set_t_f( self.velo / math.sqrt(self.a_max**2.0 - self.velo**4.0 / self.radius**2.0)) time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not is_done(): theta = self.velo * time**2.0 / (2 * self.radius * t_f) w = self.velo * time / (self.radius * t_f) alpha = self.velo / (self.radius * t_f) outpos = self.tg.get_circle_point(self.radius, theta) outpos = self.tg.offset(outpos, self.midpoint) outpos.append(self.tg.adjust_yaw([1, 0, 0])) outvelo = self.tg.get_circle_velocity(self.radius, theta, omega) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius, theta, omega, alpha) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1 / r if time >= t_f: self.__set_done(True) def is_done(self): return self.done def __set_done(self, boolean): self.done = boolean def __set_t_f(self, t): self.t_f = t def get_t_f(self): return self.t_f
class StraightLineGen(Trajectory): done = False a_max = 9.81/3. 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 begin(self): self.__set_done(False) def loop(self,start_time): r = 10.0 rate = rospy.Rate(10) time = start_time while not rospy.is_shutdown() and not self.is_done(): outpos = self.get_position(time) outpos.append(0) outvelo = self.get_velocity(time) outvelo.append(0) outacc = self.get_acceleration(time) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if time >= self.t_f: self.__set_done(True) end = self.end_point end.append(0) outmsg = self.tg.get_message(self.end_point, [0.0,0.0,0.0,0.0], [0.0,0.0,0.0,0.0]) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) def __set_done(self,boolean): self.done = boolean def get_position(self,t): outpos = [0.0,0.0,0.0] s = self.get_s(t) for i in range(0,3): outpos[i] = self.start_point[i] + s * self.e_t[i] return outpos def get_s(self,t): return t**2.0 * self.constant * (t-1.5*self.t_f) def get_velocity(self,t): outvelo = [0.0,0.0,0.0] s_d = self.get_s_d(t) for i in range(0,3): outvelo[i] = s_d * self.e_t[i] return outvelo def get_s_d (self,t): return self.constant * (3 * t**2.0 - 3 * self.t_f * t) def get_acceleration(self,t): outacc = [0.0,0.0,0.0] s_dd = self.get_s_dd(t) for i in range(0,3): outacc[i] = s_dd * self.e_t[i] return outacc def get_s_dd (self,t): return self.constant * (6 * t - 3 * self.t_f ) def is_done(self): return self.done
class ArcGen(Trajectory): done = False a_max = 0.6**2.0/0.8 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) def begin(self): v_max = (self.radius*self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not self.is_done(): outpos = self.tg.get_circle_point(self.radius,self.w*time) #outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z]) #outpos = self.tg.vector_to_list(outpos) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,self.w*time,self.w) #outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z]) #outvelo = self.tg.vector_to_list(outvelo) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,self.w*time,self.w,0) #outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z]) #outacc = self.tg.vector_to_list(outacc) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if self.w*time >= self.psi: outpos = self.tg.get_circle_point(self.radius,self.psi) #outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z]) #outpos = self.tg.vector_to_list(outpos) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,self.psi,self.w) #outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z]) #outvelo = self.tg.vector_to_list(outvelo) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,self.psi,self.w,0) #outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z]) #outacc = self.tg.vector_to_list(outacc) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r self.__set_done(True) def is_done(self): return self.done def __set_done(self,boolean): self.done = boolean
class StepPlugin(Plugin): 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 Start(self): abspath = "/" + self._widget.IrisInputBox.currentText() + "/" self.pub = rospy.Publisher(abspath + 'trajectory_gen/target', QuadPositionDerived, queue_size=10) self.security_pub = rospy.Publisher(abspath + 'trajectory_gen/done', Permission, queue_size=10) rospy.Subscriber(abspath + 'security_guard/data_forward', QuadPositionDerived, self.get_position) def get_position(self, data): if self.point == []: self.point = [data.x, data.y, data.z] utils.logwarn("Initial position: %s" % (str(self.point))) def Up(self): self.Goto([0.0, 0.0, 1.0]) def Down(self): self.Goto([0.0, 0.0, 0.5]) def Left(self): self.Goto([-1.0, 0.0, 0.5]) def Right(self): self.Goto([1.0, 0.0, 0.5]) def Back(self): self.Goto([0.0, -1.0, 0.5]) def Front(self): self.Goto([0.0, 1.0, 0.5]) def Goto(self, dest): utils.logwarn(str(dest)) outpos = [sum(x) for x in zip(self.point, dest)] utils.logwarn(str(outpos)) outpos.append(0.0) outvelo = [0.0] * 3 outvelo.append(0.0) outacc = [0.0] * 3 outacc.append(0.0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.pub.publish(outmsg) self.security_pub.publish(False) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) instance_settings.set_value("irisindex", self._widget.IrisInputBox.currentIndex()) def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) index = instance_settings.value("irisindex", 0) self._widget.IrisInputBox.setCurrentIndex(int(index))
class AccGen(Trajectory): done = False a_max = 0.6**2.0/0.8 t_f = 0 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) def begin(self): v_max = (self.radius*self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): self.__set_t_f(self.velo/math.sqrt(self.a_max**2.0 - self.velo**4.0/self.radius**2.0)) time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not is_done(): theta = self.velo*time**2.0/(2*self.radius*t_f) w = self.velo*time/(self.radius*t_f) alpha = self.velo/(self.radius*t_f) outpos = self.tg.get_circle_point(self.radius, theta) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,theta,omega) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,theta,omega,alpha) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if time >= t_f: self.__set_done(True) def is_done(self): return self.done def __set_done(self,boolean): self.done = boolean def __set_t_f(self,t): self.t_f = t def get_t_f(self): return self.t_f
class StraightLineGen(Trajectory): done = False a_max = 9.81 / 3. 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 begin(self): self.__set_done(False) def loop(self, start_time): r = 10.0 rate = rospy.Rate(10) time = start_time while not rospy.is_shutdown() and not self.is_done(): outpos = self.get_position(time) outpos.append(0) outvelo = self.get_velocity(time) outvelo.append(0) outacc = self.get_acceleration(time) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1 / r if time >= self.t_f: self.__set_done(True) end = self.end_point end.append(0) outmsg = self.tg.get_message(self.end_point, [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) def __set_done(self, boolean): self.done = boolean def get_position(self, t): outpos = [0.0, 0.0, 0.0] s = self.get_s(t) for i in range(0, 3): outpos[i] = self.start_point[i] + s * self.e_t[i] return outpos def get_s(self, t): return t**2.0 * self.constant * (t - 1.5 * self.t_f) def get_velocity(self, t): outvelo = [0.0, 0.0, 0.0] s_d = self.get_s_d(t) for i in range(0, 3): outvelo[i] = s_d * self.e_t[i] return outvelo def get_s_d(self, t): return self.constant * (3 * t**2.0 - 3 * self.t_f * t) def get_acceleration(self, t): outacc = [0.0, 0.0, 0.0] s_dd = self.get_s_dd(t) for i in range(0, 3): outacc[i] = s_dd * self.e_t[i] return outacc def get_s_dd(self, t): return self.constant * (6 * t - 3 * self.t_f) def is_done(self): return self.done
class AvoidanceController(): 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 get_potential_output(self): distances = self.__get_distances() directions = self.__get_directions() u = [0., 0., 0.] for i in range(0, len(distances)): if (distances[i] < 0.1): for j in range(0, 2): u[j] += self.gain / 0.1 * directions[i][j] u[2] += self.gain_z / 0.1 * directions[i][2] else: for j in range(0, 2): u[j] += self.gain / distances[i] * directions[i][j] u[2] += self.gain_z / distances[i] * directions[i][2] return u def get_blending_constant(self): alpha_max = 0.8 # <= 1. alpha_min = 0. # >=0. r_min = 1.2 r_max = 2. k = (alpha_min - alpha_max) / (r_max - r_min) m = alpha_max - k * r_min distances = self.__get_distances() if self.gain == 0: return 0 elif min(distances) <= r_min: return alpha_max elif min(distances) > r_max: return alpha_min else: return (k * min(distances) + m) def __set_states(self, data): topic_name = data._connection_header["topic"] for i in range(0, len(self.bodies)): name = "/body_data/id_" + str(self.bodies[i]) if topic_name == name: self.states[i] = data def __get_my_state(self): for i in range(0, len(self.bodies)): if self.bodies[i] == self.my_id: return self.states[i] def __get_distances(self): distances = [] for i in range(0, len(self.states)): if self.bodies[i] != self.my_id: distance = self.tg.get_distance( self.__get_pos_from_state(self.__get_my_state()), self.__get_pos_from_state(self.states[i])) distances.append(distance) return distances def __get_directions(self): directions = [] for i in range(0, len(self.states)): if self.bodies[i] != self.my_id: direction = self.tg.get_direction2( self.__get_pos_from_state(self.__get_my_state()), self.__get_pos_from_state(self.states[i])) directions.append(direction) return directions def __get_pos_from_state(self, state): return [state.x, state.y, state.z]
class StepPlugin(Plugin): 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 Start(self): abspath = "/"+self._widget.IrisInputBox.currentText()+"/" self.pub = rospy.Publisher(abspath+'trajectory_gen/target',QuadPositionDerived, queue_size=10) self.security_pub = rospy.Publisher(abspath+'trajectory_gen/done', Permission, queue_size=10) rospy.Subscriber(abspath+'security_guard/data_forward',QuadPositionDerived,self.get_position) def get_position(self,data): if self.point == []: self.point = [data.x,data.y,data.z] utils.logwarn("Initial position: %s"%(str(self.point))) def Up(self): self.Goto([0.0,0.0,1.0]) def Down(self): self.Goto([0.0,0.0,0.5]) def Left(self): self.Goto([-1.0,0.0,0.5]) def Right(self): self.Goto([1.0,0.0,0.5]) def Back(self): self.Goto([0.0,-1.0,0.5]) def Front(self): self.Goto([0.0,1.0,0.5]) def Goto(self, dest): utils.logwarn(str(dest)) outpos = [sum(x) for x in zip(self.point, dest)] utils.logwarn(str(outpos)) outpos.append(0.0) outvelo = [0.0]*3 outvelo.append(0.0) outacc = [0.0]*3 outacc.append(0.0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.pub.publish(outmsg) self.security_pub.publish(False) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) instance_settings.set_value("irisindex", self._widget.IrisInputBox.currentIndex()) def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) index = instance_settings.value("irisindex",0) self._widget.IrisInputBox.setCurrentIndex(int(index))