def __init__(self): # Getting parameters self.offset = rospy.get_param("/trajectory_generator/offset", [0.0, 3.0, 0]) self.id = rospy.get_param('/trajectory_generator/follower_id', 16) self.leader_id = rospy.get_param("/trajectory_generator/leader_id", 8) # change? if type(self.offset) is str: self.offset = ast.literal_eval(start_point) self.pub = rospy.Publisher('trajectory_gen/target', QuadPositionDerived, queue_size=10) self.pub_done = rospy.Publisher('trajectory_gen/done', Permission, queue_size=10) rospy.Subscriber("/body_data/id_" + str(self.leader_id), QuadPositionDerived, self.setLeaderState) rospy.Subscriber("/body_data/id_" + str(self.id), QuadPositionDerived, self.setMyState) r = 15 # Change rate to desired value self.rate = rospy.Rate(r) self.calculated_state = QuadPositionDerived() self.real_state = QuadPositionDerived() #Wait for leader leaderNotHere = True while leaderNotHere: leaderNotHere = not self.leader_state.found_body rospy.logwarn('Leader found')
def Insert_Current_Data(current_data, time): result = QuadPositionDerived() result.found_body = current_data.found_body result.x = current_data.x result.y = current_data.y result.z = current_data.z result.pitch = current_data.pitch result.roll = current_data.roll result.yaw = current_data.yaw result.time_diff = time return (result)
def Insert_Current_Data(current_data,time): result=QuadPositionDerived() result.found_body=current_data.found_body result.x=current_data.x result.y=current_data.y result.z=current_data.z result.pitch=current_data.pitch result.roll=current_data.roll result.yaw=current_data.yaw result.time_diff=time return(result)
def __init__(self): rospy.Timer(rospy.Duration(0.01), self.mocap_get_data_callback) rate = rospy.Rate(30) self.timer = Time() #Get parameters (all the body ID's that are requested) self.body_array = sml_setup.Get_Parameter(NODE_NAME, 'body_array', [8, 12]) if type(self.body_array) is str: self.body_array = ast.literal_eval(self.body_array) #Get topic names for each requested body body_topic_array = Get_Topic_Names(self.body_array) #Establish one publisher topic for each requested body topics_publisher = Get_Publishers(body_topic_array) #Initialize empty past data list self.mocap_past_data = [] empty_data = QuadPositionDerived() for i in range(0, len(self.body_array)): self.mocap_past_data.append(empty_data) #Initialize buffer self.input_signal = [] for i in range(0, len(self.body_array)): self.input_signal.append([]) while not rospy.is_shutdown(): for i in range(0, len(self.body_array)): topics_publisher[i].publish(self.mocap_past_data[i]) rate.sleep()
def __init__(self, trajectory_node, my_id, leader_id): # Getting parameters super(Follower, self).__init__(trajectory_node) self.id = my_id self.leader_id = leader_id rospy.Subscriber("/body_data/id_" + str(self.leader_id), QuadPositionDerived, self.setLeaderState) rospy.Subscriber("/body_data/id_" + str(self.id), QuadPositionDerived, self.setMyState) self.calculated_state = QuadPositionDerived() self.real_state = QuadPositionDerived() #Wait for leader leaderNotHere = True while leaderNotHere: leaderNotHere = not self.leader_state.found_body rospy.logwarn('Leader found')
def Callback(input_data): body_id=input_data.id delta_t=input_data.time_diff pos_previous=input_data.past_positions vel_previous=input_data.past_velocities try: body=body_info(body_id) except: return False, QuadPositionDerived() pos_now=QuadPositionDerived(body.pos.x,body.pos.y,body.pos.z,body.pos.yaw) vel_now=Derive(pos_now,pos_previous,delta_t) acc_now=Derive(vel_now,vel_previous,delta_t) State=MakeQuadState(pos_now,vel_now,acc_now,delta_t) return body.pos.found_body,State
def start_publishing(): rate = rospy.Rate(30) timer = Time() #Get parameters (all the body ID's that are requested) body_array = sml_setup.Get_Parameter(NODE_NAME, 'body_array', [8, 16, 17]) if type(body_array) is str: body_array = ast.literal_eval(body_array) #Get topic names for each requested body body_topic_array = Get_Topic_Names(body_array) #Establish one publisher topic for each requested body topics_publisher = Get_Publishers(body_topic_array) #Initialize empty past data list mocap_past_data = [] empty_data = QuadPositionDerived() for i in range(0, len(body_array)): mocap_past_data.append(empty_data) # Initialize error numbers error = [0] * len(body_array) while not rospy.is_shutdown(): delta_time = timer.get_time_diff() for i in range(0, len(body_array)): mocap_data = Get_Body_Data(body_array[i]) if mocap_data.found_body: mocap_data_derived = Get_Derived_Data(mocap_data, mocap_past_data[i], delta_time) #update past mocap data mocap_past_data[i] = mocap_data_derived #Publish data on topic topics_publisher[i].publish(mocap_data_derived) error[i] = 0 else: error[i] += 1 if error[i] < 30: if error[i] > 5: mocap_past_data[i].found_body = False utils.logwarn("Send body %i not found" % (body_array[i])) utils.logwarn("Body %i: %i errors" % (body_array[i], error[i])) elif error[i] == 30: utils.logwarn("Body %i: %i errors. Stop printing Errors!" % (body_array[i], error[i])) topics_publisher[i].publish(mocap_past_data[i]) rate.sleep()
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): # Getting parameters self.offset = rospy.get_param("trajectory_generator/offset",[0.0,2.0,0]) #self.leader_id = sml_setup.Get_Parameter('FC','trajectory_generator/leader_id',8) self.leader_id = rospy.get_param("trajectory_generator/leader_id",12) # change? if type(self.offset) is str: self.offset = ast.literal_eval(start_point) self.pub = rospy.Publisher('trajectory_gen/target',QuadPositionDerived, queue_size=10) self.pub_done = rospy.Publisher('trajectory_gen/done', Permission, queue_size=10) self.subscriber = rospy.Subscriber("body_data/id_"+str(self.leader_id),QuadPositionDerived, self.setLeaderState) r = 15 # Change rate to desired value self.rate = rospy.Rate(r) self.state = QuadPositionDerived()
def start_publishing(self): utils.loginfo('start publishing') rate = rospy.Rate(30) timer = Time() #Get parameters (all the body ID's that are requested) self.body_names = sml_setup.Get_Parameter(NODE_NAME, 'body_names', ['iris1', 'iris2']) self.body_array = sml_setup.Get_Parameter(NODE_NAME, 'body_array', [1, 2]) if type(self.body_array) is str: self.body_array = ast.literal_eval(self.body_array) #Get topic names for each requested body body_topic_array = self.Get_Topic_Names(self.body_array) #Establish one publisher topic for each requested body topics_publisher = self.Get_Publishers(body_topic_array) #Initialize empty past data list mocap_past_data = [] empty_data = QuadPositionDerived() for i in range(0, len(self.body_array)): mocap_past_data.append(empty_data) while not rospy.is_shutdown(): delta_time = timer.get_time_diff() if self.bodies: for i in range(0, len(self.body_array)): utils.loginfo('body ' + str(i)) if self.body_names[i] in self.bodies.name: indice = self.bodies.name.index(self.body_names[i]) mocap_data = self.get_data(indice) mocap_data_derived = self.Get_Derived_Data( mocap_data, mocap_past_data[i], delta_time) #update past mocap data mocap_past_data[i] = mocap_data_derived #Publish data on topic topics_publisher[i].publish(mocap_data_derived) rate.sleep() utils.logwarn('Mocap stop publishing')
class Follower: leader_state = QuadPositionDerived() def __init__(self): # Getting parameters self.offset = rospy.get_param("trajectory_generator/offset",[0.0,2.0,0]) #self.leader_id = sml_setup.Get_Parameter('FC','trajectory_generator/leader_id',8) self.leader_id = rospy.get_param("trajectory_generator/leader_id",12) # change? if type(self.offset) is str: self.offset = ast.literal_eval(start_point) self.pub = rospy.Publisher('trajectory_gen/target',QuadPositionDerived, queue_size=10) self.pub_done = rospy.Publisher('trajectory_gen/done', Permission, queue_size=10) self.subscriber = rospy.Subscriber("body_data/id_"+str(self.leader_id),QuadPositionDerived, self.setLeaderState) r = 15 # Change rate to desired value self.rate = rospy.Rate(r) self.state = QuadPositionDerived() def follow(self): leader_init_state = self.leader_state while not rospy.is_shutdown(): self.calculateState() self.pub.publish(self.state) self.pub_done.publish(False) # if self.leader_state.z <= leader_init_state: # self.pub_done.publish(True) self.rate.sleep() def setLeaderState(self, data): self.leader_state = data # Calculates the state of the follower from the leader state def calculateState(self): self.state.x = self.leader_state.x + self.offset[0] self.state.y = self.leader_state.y + self.offset[1] self.state.z = 0.8 self.state.yaw = self.leader_state.yaw self.state.x_vel = self.leader_state.x_vel self.state.y_vel = self.leader_state.y_vel self.state.z_vel = self.leader_state.z_vel self.state.yaw_vel = self.leader_state.yaw_vel self.state.x_acc = self.leader_state.x_acc self.state.y_acc = self.leader_state.y_acc self.state.z_acc = self.leader_state.z_acc self.state.yaw_acc = self.leader_state.yaw_acc
def Apply_Filter(input_signal): result = QuadPositionDerived() keys_pos = ['x', 'y', 'z', 'pitch', 'roll', 'yaw'] keys_vel = ['x_vel', 'y_vel', 'z_vel', 'pitch_vel', 'roll_vel', 'yaw_vel'] keys_acc = ['x_acc', 'y_acc', 'z_acc', 'pitch_acc', 'roll_acc', 'yaw_acc'] deltaT = Get_Signal_From_Dict(input_signal, 'time_diff') for k in keys_pos: s = Get_Signal_From_Dict(input_signal, k) r = Apply_Filter_sig(s, deltaT, T_pos) result.__setattr__(k, r) for k in keys_vel: s = Get_Signal_From_Dict(input_signal, k) r = Apply_Filter_sig(s, deltaT, T_vel) result.__setattr__(k, r) for k in keys_acc: s = Get_Signal_From_Dict(input_signal, k) r = Apply_Filter_sig(s, deltaT, T_vel) result.__setattr__(k, r) return result
def start_publishing(): rate = rospy.Rate(30) timer = Time() #Get parameters (all the body ID's that are requested) body_array = sml_setup.Get_Parameter(NODE_NAME, 'body_array', [8, 12]) if type(body_array) is str: body_array = ast.literal_eval(body_array) #Get topic names for each requested body body_topic_array = Get_Topic_Names(body_array) #Establish one publisher topic for each requested body topics_publisher = Get_Publishers(body_topic_array) #Initialize empty past data list mocap_past_data = [] empty_data = QuadPositionDerived() for i in range(0, len(body_array)): mocap_past_data.append(empty_data) while not rospy.is_shutdown(): delta_time = timer.get_time_diff() for i in range(0, len(body_array)): mocap_data = Get_Body_Data(body_array[i]) mocap_data_derived = Get_Derived_Data(mocap_data, mocap_past_data[i], delta_time) #update past mocap data mocap_past_data[i] = mocap_data_derived #Publish data on topic topics_publisher[i].publish(mocap_data_derived) rate.sleep()
def Apply_Filter(input_signal): result = QuadPositionDerived() keys_pos = ['x','y','z','pitch','roll','yaw'] keys_vel = ['x_vel','y_vel','z_vel','pitch_vel','roll_vel','yaw_vel'] keys_acc = ['x_acc','y_acc','z_acc','pitch_acc','roll_acc','yaw_acc'] deltaT = Get_Signal_From_Dict(input_signal,'time_diff') for k in keys_pos: s = Get_Signal_From_Dict(input_signal,k) r = Apply_Filter_sig(s,deltaT,T_pos) result.__setattr__(k,r) for k in keys_vel: s = Get_Signal_From_Dict(input_signal,k) r = Apply_Filter_sig(s,deltaT,T_vel) result.__setattr__(k,r) for k in keys_acc: s = Get_Signal_From_Dict(input_signal,k) r = Apply_Filter_sig(s,deltaT,T_vel) result.__setattr__(k,r) return result
def accelerate(self, pub, a_max, r, tilted_midpoint): t_f = self.velo / math.sqrt(a_max ** 2.0 - self.velo ** 4.0 / self.radius ** 2.0) rate = rospy.Rate(r) time = 0.0 done = False while not rospy.is_shutdown() and not done: outpos = self.transform_coordinates( self.get_outpos_accphase(self.radius, self.velo, tilted_midpoint, time, t_f), self.theta ) outvelo = self.transform_coordinates( self.get_outvelo_accphase(self.radius, self.velo, time, t_f), self.theta ) outacc = self.transform_coordinates(self.get_outacc_accphase(self.radius, self.velo, time, t_f), self.theta) out_msg = QuadPositionDerived() out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = 0 out_msg.x_vel = outvelo[0] out_msg.y_vel = outvelo[1] out_msg.z_vel = outvelo[2] out_msg.yaw_vel = 0 out_msg.x_acc = outacc[0] out_msg.y_acc = outacc[1] out_msg.z_acc = outacc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() time += 1 / r if time > t_f: done = True return time
def go_to_start(self, midpoint, pub): r = 10.0 rate = rospy.Rate(r) for i in range(0, 15 * int(r)): out_msg = QuadPositionDerived() out_msg.x = 0.0 out_msg.y = 0.0 out_msg.z = 0.6 out_msg.yaw = 0 out_msg.x_vel = 0.0 out_msg.y_vel = 0.0 out_msg.z_vel = 0.0 out_msg.yaw_vel = 0 out_msg.x_acc = 0.0 out_msg.y_acc = 0.0 out_msg.z_acc = 0.0 out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() target_point = [0.0, 0.0, 0.0] target_point[0] = midpoint[0] + self.radius target_point[1] = midpoint[1] target_point[2] = midpoint[2] outpos = self.transform_coordinates(target_point, self.theta) rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6]) rospy.set_param("trajectory_generator/end_point", outpos) sl_gen = StraightLineGen() sl_gen.generate() return outpos
def turn(self, pub, pos, yaw): out_msg = QuadPositionDerived() out_msg.x = pos[0] out_msg.y = pos[1] out_msg.z = pos[2] out_msg.yaw = yaw out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) rospy.sleep(1)
def get_tilted_circle(self): tilted_midpoint = self.inverse_transform(self.midpoint) a_max = 0.6 ** 2.0 / 0.8 v_max = (self.radius * a_max) ** (0.5) if self.velo > v_max: self.velo = v_max pub = rospy.Publisher("trajectory_gen/target", QuadPositionDerived, queue_size=10) rospy.init_node("TG") r = 25 rate = rospy.Rate(r) period = self.psi * self.radius / self.velo points_in_period = int(period * r) time = 0.0 counter = 0 while not rospy.is_shutdown() and not self.done: out_pos = self.transform_coordinates(self.get_outpos(self.radius, self.velo, tilted_midpoint, time)) out_velo = self.transform_coordinates(self.get_outvelo(self.radius, self.velo, time)) out_acc = self.transform_coordinates(self.get_outacc(self.radius, self.velo, time)) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = self.get_yaw(self.velo, time, self.radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) time += 1.0 / r counter += 1 rate.sleep() if counter == points_in_period and time == period: self.done = True elif counter == points_in_period: out_pos = self.transform_coordinates(self.get_outpos(self.radius, self.velo, tilted_midpoint, period)) out_velo = self.transform_coordinates(self.get_outvelo(self.radius, self.velo, period)) out_acc = self.transform_coordinates(self.get_outacc(self.radius, self.velo, period)) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = self.get_yaw(self.velo, time, self.radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) self.done = True rate.sleep()
def __init__(self, context): super(utilityPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('utilityPlugin') # 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__)), 'utility.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('utilityUi') # 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.ID = 0 self.State = QuadPositionDerived() self.Target = QuadPositionDerived() self.sub = '' self.name = '' self.pwd = os.environ['PWD'] self._widget.IrisInputBox.insertItems( 0, ['iris1', 'iris2', 'iris3', 'iris4', "iris5"]) self._widget.bStart.clicked.connect(self.Start) self._widget.GravityCancelButton.clicked.connect( self.adjust_gravity_cancel) self._widget.XBox.setMinimum(-10.0) self._widget.XBox.setMaximum(10.0) self._widget.XBox.setSingleStep(0.1) self._widget.YBox.setMinimum(-10.0) self._widget.YBox.setMaximum(10.0) self._widget.YBox.setSingleStep(0.1) self._widget.ZBox.setMinimum(-10.0) self._widget.ZBox.setMaximum(10.0) self._widget.ZBox.setSingleStep(0.1) self._widget.GravitySpinBox.setMaximum(1800) self._widget.GravitySpinBox.setMinimum(1200) self._widget.GravitySpinBox.setSingleStep(10) self._widget.GravitySpinBox.setValue(1500)
def get_tilted_circle(): midpoint = rospy.get_param("trajectory_generator/midpoint",[0.0,0.0,0.6]) if type(midpoint) is str: midpoint = ast.literal_eval(midpoint) radius = rospy.get_param("trajectory_generator/radius",0.8) velo = rospy.get_param("trajectory_generator/velo",0.4) theta = rospy.get_param("trajectory_generator/theta",math.radians(20)) tilted_midpoint = inverse_transform(midpoint,theta) v_max = 12.0 a_max = 9.81/3.0 v_max_from_a_max = (radius*a_max)**(0.5) test = v_max_from_a_max < v_max if test and velo > v_max_from_a_max: velo = v_max_from_a_max elif not test and velo > v_max: velo = v_max pub = rospy.Publisher('trajectory_gen/target',QuadPositionDerived, queue_size=10) rospy.init_node('circle_gen_tilt') r = 25 rate = rospy.Rate(r) time = 0.0 #gets the quad to the starting-point for j in range(0,15*r): out_msg = QuadPositionDerived() out_msg.x = 0.0 out_msg.y = 0.0 out_msg.z = 0.6 out_msg.yaw = 0 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() for i in range(0,5*r): out_msg = QuadPositionDerived() tilted_midpoint = inverse_transform(midpoint, theta) target_point = [0.0,0.0,0.0] target_point[0] = tilted_midpoint[0] + radius target_point[1] = tilted_midpoint[1] target_point[2] = tilted_midpoint[2] outpos = transform_coordinates(target_point,theta) out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = math.pi/2 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() while not rospy.is_shutdown(): #publish "starting point", might be nice to use ptp_gen or some kind of mo_cap feedback out_pos = transform_coordinates(get_outpos(radius, velo, tilted_midpoint, time),theta) out_velo = transform_coordinates(get_outvelo(radius, velo, time),theta) out_acc = transform_coordinates(get_outacc(radius, velo, time),theta) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = get_yaw(velo,time,radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) time += 1.0/r rate.sleep()
def __init__(self, context): super(pointInputPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('pointInputPlugin') # 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__)), 'pointInput.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('pointInputUi') # 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) # Setting default values of the class variables self.ID = 0 self.index = 0 self.State = QuadPositionDerived() self.Target = QuadPositionDerived() self.sub = '' self.targetsub = '' self.name = '' self.pwd = os.environ['PWD'] self.pointlist = [] self.filelist = os.listdir(self.pwd + '/src/kampala/gui/src/gui/DXFFiles') self.filelist2 = os.listdir(self.pwd + '/src/kampala/gui/src/gui/ActionFiles') self._widget.IrisInputBox.insertItems( 0, ['iris1', 'iris2', 'iris3', 'iris4', "iris5"]) self._widget.DXFInputBox.insertItems(0, self.filelist) self._widget.ActionInputBox.insertItems(0, self.filelist2) # Connecting slots to signals. If a QWidget A has a signal X, then self._widget.A.X.connect(fun) # means that the function fun will be called with any output of the signal as its argument each time the signal # X is emitted. self._widget.bStart.clicked.connect(self.Start) self._widget.AddPointButton.clicked.connect(self.AddPoint) self._widget.RemovePointButton.clicked.connect(self.RemovePoint) self._widget.LoadButton.clicked.connect(self.LoadDXF) self._widget.SaveButton.clicked.connect(self.SaveDXF) self._widget.AddWaitButton.clicked.connect(self.AddWait) self._widget.SaveActionsButton.clicked.connect(self.SaveActions) self._widget.LoadActionsButton.clicked.connect(self.LoadActions) self.launch.connect(self.manage_task) self._widget.XBox.setMinimum(-10.0) self._widget.XBox.setMaximum(10.0) self._widget.XBox.setSingleStep(0.1) self._widget.YBox.setMinimum(-10.0) self._widget.YBox.setMaximum(10.0) self._widget.YBox.setSingleStep(0.1) self._widget.ZBox.setMinimum(-10.0) self._widget.ZBox.setMaximum(10.0) self._widget.ZBox.setSingleStep(0.1)
def get_message(self): result = QuadPositionDerived() result.found_body = self.found_body result.x = self.x.p result.y = self.y.p result.z = self.z.p result.pitch = self.pitch.p result.roll = self.roll.p result.yaw = self.yaw.p result.x_vel = self.x.v result.y_vel = self.y.v result.z_vel = self.z.v result.pitch_vel = self.pitch.v result.roll_vel = self.roll.v result.yaw_vel = self.yaw.v result.x_acc = self.x.a result.y_acc = self.y.a result.z_acc = self.z.a result.pitch_acc = self.pitch.a result.roll_acc = self.roll.a result.yaw_acc = self.yaw.a # The time_diff is added to make the position plot on the # GUI work result.time_diff = 0.05 return copy.deepcopy(result)
def decallerate(self, pub, a_max, R, v, tilted_midpoint, time): t_f = self.velo / math.sqrt(a_max**2.0 - self.velo**4.0 / self.radius**2.0) w_0 = v / R theta_0 = v / R * time r = 10.0 rate = rospy.Rate(r) time = 1 / r outpos = [0.0, 0.0, 0.0] while time <= t_f: outpos = self.transform_coordinates( self.get_outpos_deaccphase(self.radius, self.velo, tilted_midpoint, time, t_f, theta_0, w_0), self.theta) outvelo = self.transform_coordinates( self.get_outvelo_deaccphase(self.radius, self.velo, time, t_f, theta_0, w_0), self.theta) outacc = self.transform_coordinates( self.get_outacc_deaccphase(self.radius, self.velo, time, t_f, theta_0, w_0), self.theta) out_msg = QuadPositionDerived() out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = 0 out_msg.x_vel = outvelo[0] out_msg.y_vel = outvelo[1] out_msg.z_vel = outvelo[2] out_msg.yaw_vel = 0 out_msg.x_acc = outacc[0] out_msg.y_acc = outacc[1] out_msg.z_acc = outacc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() time += 1 / r return outpos
def get_tilted_circle(self): tilted_midpoint = self.inverse_transform(self.midpoint, self.theta) a_max = 0.6**2.0 / 0.8 #v_max = (self.radius*a_max)**(0.5) #if self.velo >= v_max: # self.velo = 0.9*v_max rospy.init_node('TG', anonymous=True) pub = rospy.Publisher('trajectory_gen/target', QuadPositionDerived, queue_size=10) start_point = self.go_to_start(tilted_midpoint, pub) sec_pub = rospy.Publisher('trajectory_gen/done', Permission, queue_size=10) rospy.sleep(4.) r = 10.0 rate = rospy.Rate(r) t_f = self.velo / math.sqrt(a_max**2.0 - self.velo**4.0 / self.radius**2.0) time = self.accelerate(pub, a_max, r, tilted_midpoint) / 2.0 - 1 / r period = 2 * math.pi * self.radius / self.velo points_in_period = int(period * r) counter = 0 while not rospy.is_shutdown() and not self.done: out_pos = self.transform_coordinates( self.get_outpos(self.radius, self.velo, tilted_midpoint, time), self.theta) out_velo = self.transform_coordinates( self.get_outvelo(self.radius, self.velo, time), self.theta) out_acc = self.transform_coordinates( self.get_outacc(self.radius, self.velo, time), self.theta) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = 0 out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) sec_pub.publish(Permission(False)) time += 1.0 / r counter += 1 rate.sleep() if time >= 3 * period - t_f / 2: end_pos = self.decallerate(pub, a_max, self.radius, self.velo, tilted_midpoint, time) rospy.set_param("trajectory_generator/start_point", end_pos) rospy.set_param("trajectory_generator/end_point", [0.0, 0.0, 0.6]) sl_gen = StraightLineGen() sl_gen.generate() rospy.sleep(4.) #self.turn(pub, [0.0,0.0,0.6],0) self.done = True sec_pub.publish(Permission(True))
def MakeQuadState(pos,vel,acc,time): result=QuadPositionDerived() result.x=pos.x result.y=pos.y result.z=pos.z result.yaw=pos.yaw result.x_vel=vel.x result.y_vel=vel.y result.z_vel=vel.z result.yaw_vel=vel.yaw result.x_acc=acc.x result.y_acc=acc.y result.z_acc=acc.z result.yaw_acc=acc.yaw result.time_diff=time return result
def Get_Augmented_Data(quad_state): data = QuadPositionDerived() data.x = quad_state.x data.y = quad_state.y data.z = quad_state.z data.yaw = quad_state.yaw data.x_vel = quad_state.x_vel data.y_vel = quad_state.y_vel data.z_vel = quad_state.z_vel data.yaw_vel = quad_state.yaw_vel data.x_acc = quad_state.x_acc data.y_acc = quad_state.y_acc data.z_acc = quad_state.z_acc data.yaw_acc = quad_state.yaw_acc data.time_diff = quad_state.time_diff return data
def get_message(self): result = QuadPositionDerived() result.found_body=self.found_body result.x=self.x.p result.y=self.y.p result.z=self.z.p result.pitch=self.pitch.p result.roll=self.roll.p result.yaw=self.yaw.p result.x_vel=self.x.v result.y_vel=self.y.v result.z_vel=self.z.v result.pitch_vel=self.pitch.v result.roll_vel=self.roll.v result.yaw_vel=self.yaw.v result.x_acc=self.x.a result.y_acc=self.y.a result.z_acc=self.z.a result.pitch_acc=self.pitch.a result.roll_acc=self.roll.a result.yaw_acc=self.yaw.a # The time_diff is added to make the position plot on the # GUI work result.time_diff = 0.05 return copy.deepcopy(result)
def decallerate(self, pub, a_max, R, v, tilted_midpoint, time): t_f = self.velo / math.sqrt(a_max ** 2.0 - self.velo ** 4.0 / self.radius ** 2.0) w_0 = v / R theta_0 = v / R * time r = 10.0 rate = rospy.Rate(r) time = 1 / r outpos = [0.0, 0.0, 0.0] while time <= t_f: outpos = self.transform_coordinates( self.get_outpos_deaccphase(self.radius, self.velo, tilted_midpoint, time, t_f, theta_0, w_0), self.theta ) outvelo = self.transform_coordinates( self.get_outvelo_deaccphase(self.radius, self.velo, time, t_f, theta_0, w_0), self.theta ) outacc = self.transform_coordinates( self.get_outacc_deaccphase(self.radius, self.velo, time, t_f, theta_0, w_0), self.theta ) out_msg = QuadPositionDerived() out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = 0 out_msg.x_vel = outvelo[0] out_msg.y_vel = outvelo[1] out_msg.z_vel = outvelo[2] out_msg.yaw_vel = 0 out_msg.x_acc = outacc[0] out_msg.y_acc = outacc[1] out_msg.z_acc = outacc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() time += 1 / r return outpos
def Get_Quad_State(obj): result = QuadPositionDerived() result.found_body = obj.found_body result.x = obj.x result.y = obj.y result.z = obj.z result.yaw = obj.yaw result.pitch = obj.pitch result.roll = obj.roll result.x_vel = obj.x_vel result.y_vel = obj.y_vel result.z_vel = obj.z_vel result.yaw_vel = obj.yaw_vel result.pitch_vel = obj.pitch_vel result.roll_vel = obj.roll_vel result.x_acc = obj.x_acc result.y_acc = obj.y_acc result.z_acc = obj.z_acc result.yaw_acc = obj.yaw_acc result.pitch_acc = obj.pitch_acc result.roll_acc = obj.roll_acc result.time_diff = obj.time_secs return (result)
def get_tilted_circle(self): tilted_midpoint = self.inverse_transform(self.midpoint, self.theta) a_max = 0.6 ** 2.0 / 0.8 # v_max = (self.radius*a_max)**(0.5) # if self.velo >= v_max: # self.velo = 0.9*v_max rospy.init_node("TG", anonymous=True) pub = rospy.Publisher("trajectory_gen/target", QuadPositionDerived, queue_size=10) start_point = self.go_to_start(tilted_midpoint, pub) sec_pub = rospy.Publisher("trajectory_gen/done", Permission, queue_size=10) rospy.sleep(4.0) r = 10.0 rate = rospy.Rate(r) t_f = self.velo / math.sqrt(a_max ** 2.0 - self.velo ** 4.0 / self.radius ** 2.0) time = self.accelerate(pub, a_max, r, tilted_midpoint) / 2.0 - 1 / r period = 2 * math.pi * self.radius / self.velo points_in_period = int(period * r) counter = 0 while not rospy.is_shutdown() and not self.done: out_pos = self.transform_coordinates( self.get_outpos(self.radius, self.velo, tilted_midpoint, time), self.theta ) out_velo = self.transform_coordinates(self.get_outvelo(self.radius, self.velo, time), self.theta) out_acc = self.transform_coordinates(self.get_outacc(self.radius, self.velo, time), self.theta) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = 0 out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) sec_pub.publish(Permission(False)) time += 1.0 / r counter += 1 rate.sleep() if time >= 3 * period - t_f / 2: end_pos = self.decallerate(pub, a_max, self.radius, self.velo, tilted_midpoint, time) rospy.set_param("trajectory_generator/start_point", end_pos) rospy.set_param("trajectory_generator/end_point", [0.0, 0.0, 0.6]) sl_gen = StraightLineGen() sl_gen.generate() rospy.sleep(4.0) # self.turn(pub, [0.0,0.0,0.6],0) self.done = True sec_pub.publish(Permission(True))
def Get_Augmented_Data(quad_state): data=QuadPositionDerived() data.x=quad_state.x data.y=quad_state.y data.z=quad_state.z data.yaw=quad_state.yaw data.x_vel=quad_state.x_vel data.y_vel=quad_state.y_vel data.z_vel=quad_state.z_vel data.yaw_vel=quad_state.yaw_vel data.x_acc=quad_state.x_acc data.y_acc=quad_state.y_acc data.z_acc=quad_state.z_acc data.yaw_acc=quad_state.yaw_acc data.time_diff=quad_state.time_diff return data
def get_message(self,pos,velo,acc): msg = QuadPositionDerived() msg.x = pos[0] msg.y = pos[1] msg.z = pos[2] msg.yaw = pos[3] msg.x_vel = velo[0] msg.y_vel = velo[1] msg.z_vel = velo[2] msg.yaw_vel = velo[3] msg.x_acc = velo[0] msg.y_acc = velo[1] msg.z_acc = velo[2] msg.yaw_acc = velo[3] return msg
def get_tilted_circle(self): tilted_midpoint = self.inverse_transform(self.midpoint) a_max = 0.6**2.0 / 0.8 v_max = (self.radius * a_max)**(0.5) if self.velo > v_max: self.velo = v_max pub = rospy.Publisher('trajectory_gen/target', QuadPositionDerived, queue_size=10) rospy.init_node('TG') r = 25 rate = rospy.Rate(r) period = self.psi * self.radius / self.velo points_in_period = int(period * r) time = 0.0 counter = 0 while not rospy.is_shutdown() and not self.done: out_pos = self.transform_coordinates( self.get_outpos(self.radius, self.velo, tilted_midpoint, time)) out_velo = self.transform_coordinates( self.get_outvelo(self.radius, self.velo, time)) out_acc = self.transform_coordinates( self.get_outacc(self.radius, self.velo, time)) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = self.get_yaw(self.velo, time, self.radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) time += 1.0 / r counter += 1 rate.sleep() if counter == points_in_period and time == period: self.done = True elif counter == points_in_period: out_pos = self.transform_coordinates( self.get_outpos(self.radius, self.velo, tilted_midpoint, period)) out_velo = self.transform_coordinates( self.get_outvelo(self.radius, self.velo, period)) out_acc = self.transform_coordinates( self.get_outacc(self.radius, self.velo, period)) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = self.get_yaw(self.velo, time, self.radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) self.done = True rate.sleep()
def MakeQuadState(pos, vel, acc, time): result = QuadPositionDerived() result.x = pos.x result.y = pos.y result.z = pos.z result.yaw = pos.yaw result.x_vel = vel.x result.y_vel = vel.y result.z_vel = vel.z result.yaw_vel = vel.yaw result.x_acc = acc.x result.y_acc = acc.y result.z_acc = acc.z result.yaw_acc = acc.yaw result.time_diff = time return result
def accelerate(self, pub, a_max, r, tilted_midpoint): t_f = self.velo / math.sqrt(a_max**2.0 - self.velo**4.0 / self.radius**2.0) rate = rospy.Rate(r) time = 0.0 done = False while not rospy.is_shutdown() and not done: outpos = self.transform_coordinates( self.get_outpos_accphase(self.radius, self.velo, tilted_midpoint, time, t_f), self.theta) outvelo = self.transform_coordinates( self.get_outvelo_accphase(self.radius, self.velo, time, t_f), self.theta) outacc = self.transform_coordinates( self.get_outacc_accphase(self.radius, self.velo, time, t_f), self.theta) out_msg = QuadPositionDerived() out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = 0 out_msg.x_vel = outvelo[0] out_msg.y_vel = outvelo[1] out_msg.z_vel = outvelo[2] out_msg.yaw_vel = 0 out_msg.x_acc = outacc[0] out_msg.y_acc = outacc[1] out_msg.z_acc = outacc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() time += 1 / r if time > t_f: done = True return time
def Get_Quad_State(obj): result=QuadPositionDerived() result.found_body=obj.found_body result.x=obj.x result.y=obj.y result.z=obj.z result.yaw=obj.yaw result.pitch=obj.pitch result.roll=obj.roll result.x_vel=obj.x_vel result.y_vel=obj.y_vel result.z_vel=obj.z_vel result.yaw_vel=obj.yaw_vel result.pitch_vel=obj.pitch_vel result.roll_vel=obj.roll_vel result.x_acc=obj.x_acc result.y_acc=obj.y_acc result.z_acc=obj.z_acc result.yaw_acc=obj.yaw_acc result.pitch_acc=obj.pitch_acc result.roll_acc=obj.roll_acc result.time_diff=obj.time_secs return(result)
def get_circle(): midpoint = rospy.get_param("trajectory_generator/midpoint",[0.0,0.0,0.6]) radius = rospy.get_param("trajectory_generator/radius",0.8) velo = rospy.get_param("trajectory_generator/velo",0.4) # if type(midpoint is str): # midpoint = ast.literal_eval(midpoint) v_max = 10.0 a_max = 10.0 v_max_from_a_max = (radius*a_max)**(0.5) test = v_max_from_a_max < v_max if test and velo > v_max_from_a_max: velo = v_max_from_a_max elif not test and velo > v_max: velo = v_max pub = rospy.Publisher('trajectory_gen/target',QuadPositionDerived, queue_size=10) rospy.init_node('circle_gen') r = 15 rate = rospy.Rate(r) time = 0.0 #gets the quad to the starting-point for j in range(0,10*r): out_msg = QuadPositionDerived() out_msg.x = 0.0 out_msg.y = 0.0 out_msg.z = 0.6 out_msg.yaw = 0 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) got_to_start = True rate.sleep() for i in range(0,5*r): out_msg = QuadPositionDerived() out_msg.x = midpoint[0] + radius out_msg.y = midpoint[1] out_msg.z = midpoint[2] out_msg.yaw = math.pi/2 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) got_to_start = True rate.sleep() while not rospy.is_shutdown(): out_pos = get_outpos(radius, velo, midpoint, time) out_velo = get_outvelo(radius, velo, time) out_acc = get_outacc(radius, velo, time) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] # out_msg.yaw = get_yaw(velo,time,radius) out_msg.yaw=0 out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) time += 1.0/r rate.sleep()
class Follower(Trajectory): __metaclass__ = ABCMeta leader_state = QuadPositionDerived() done = False def __init__(self, trajectory_node, my_id, leader_id): # Getting parameters super(Follower, self).__init__(trajectory_node) self.id = my_id self.leader_id = leader_id rospy.Subscriber("/body_data/id_" + str(self.leader_id), QuadPositionDerived, self.setLeaderState) rospy.Subscriber("/body_data/id_" + str(self.id), QuadPositionDerived, self.setMyState) self.calculated_state = QuadPositionDerived() self.real_state = QuadPositionDerived() #Wait for leader leaderNotHere = True while leaderNotHere: leaderNotHere = not self.leader_state.found_body rospy.logwarn('Leader found') def begin(): self.__set_done(False) def loop(self): rate = rospy.Rate(15.) leader_init_state = self.leader_state rospy.sleep(5) while not rospy.is_shutdown() and not self.is_done(): self.calculateState() self.trajectory_node.send_msg(self.calculated_state) self.trajectory_node.send_permission(False) distance = self.__getDistance() rate.sleep() if self.leader_state.found_body == False or distance < 0.9: utils.logwarn('Leader: ' + str(self.leader_state.found_body) + ', Distance:' + str(distance)) self.__set_done(True) self.trajectory_node.send_permission(True) def setLeaderState(self, data): self.leader_state = data def setMyState(self, data): self.real_state = data def __getDistance(self): my_pos = [self.real_state.x, self.real_state.y, self.real_state.z] leader_pos = [ self.leader_state.x, self.leader_state.y, self.leader_state.z ] temp = [0.0, 0.0, 0.0] for i in range(0, 2): temp[i] = my_pos[i] - leader_pos[i] return lg.norm(temp) # Calculates the state of the follower from the leader state @abstractmethod def calculateState(self): pass def __set_done(self, boolean): self.done = boolean def is_done(self): return self.done
class Follower: leader_state = QuadPositionDerived() def __init__(self): # Getting parameters self.offset = rospy.get_param("/trajectory_generator/offset", [0.0, 3.0, 0]) self.id = rospy.get_param('/trajectory_generator/follower_id', 16) self.leader_id = rospy.get_param("/trajectory_generator/leader_id", 8) # change? if type(self.offset) is str: self.offset = ast.literal_eval(start_point) self.pub = rospy.Publisher('trajectory_gen/target', QuadPositionDerived, queue_size=10) self.pub_done = rospy.Publisher('trajectory_gen/done', Permission, queue_size=10) rospy.Subscriber("/body_data/id_" + str(self.leader_id), QuadPositionDerived, self.setLeaderState) rospy.Subscriber("/body_data/id_" + str(self.id), QuadPositionDerived, self.setMyState) r = 15 # Change rate to desired value self.rate = rospy.Rate(r) self.calculated_state = QuadPositionDerived() self.real_state = QuadPositionDerived() #Wait for leader leaderNotHere = True while leaderNotHere: leaderNotHere = not self.leader_state.found_body rospy.logwarn('Leader found') def follow(self): leader_init_state = self.leader_state rospy.sleep(5) while not rospy.is_shutdown(): self.calculateState() self.pub.publish(self.calculated_state) distance = self.__getDistance() if self.leader_state.found_body == False or distance < 0.9: rospy.logwarn('Leader: ' + str(self.leader_state.found_body) + ', Distance:' + str(distance)) self.pub_done.publish(True) else: self.pub_done.publish(False) self.rate.sleep() def setLeaderState(self, data): self.leader_state = data def setMyState(self, data): self.real_state = data def __getDistance(self): my_pos = [self.real_state.x, self.real_state.y, self.real_state.z] leader_pos = [ self.leader_state.x, self.leader_state.y, self.leader_state.z ] temp = [0.0, 0.0, 0.0] for i in range(0, 2): temp[i] = my_pos[i] - leader_pos[i] return lg.norm(temp) # Calculates the state of the follower from the leader state def calculateState(self): self.calculated_state.x = self.leader_state.x + self.offset[0] self.calculated_state.y = self.leader_state.y + self.offset[1] self.calculated_state.z = self.leader_state.z + self.offset[2] self.calculated_state.yaw = self.leader_state.yaw self.calculated_state.x_vel = self.leader_state.x_vel self.calculated_state.y_vel = self.leader_state.y_vel self.calculated_state.z_vel = self.leader_state.z_vel self.calculated_state.yaw_vel = self.leader_state.yaw_vel self.calculated_state.x_acc = self.leader_state.x_acc self.calculated_state.y_acc = self.leader_state.y_acc self.calculated_state.z_acc = self.leader_state.z_acc self.calculated_state.yaw_acc = self.leader_state.yaw_acc
def get_tilted_circle(): midpoint = rospy.get_param("trajectory_generator/midpoint", [0.0, 0.0, 0.6]) if type(midpoint) is str: midpoint = ast.literal_eval(midpoint) radius = rospy.get_param("trajectory_generator/radius", 0.8) velo = rospy.get_param("trajectory_generator/velo", 0.4) theta = rospy.get_param("trajectory_generator/theta", math.radians(20)) tilted_midpoint = inverse_transform(midpoint, theta) v_max = 12.0 a_max = 9.81 / 3.0 v_max_from_a_max = (radius * a_max)**(0.5) test = v_max_from_a_max < v_max if test and velo > v_max_from_a_max: velo = v_max_from_a_max elif not test and velo > v_max: velo = v_max pub = rospy.Publisher('trajectory_gen/target', QuadPositionDerived, queue_size=10) rospy.init_node('circle_gen_tilt') r = 25 rate = rospy.Rate(r) time = 0.0 #gets the quad to the starting-point for j in range(0, 15 * r): out_msg = QuadPositionDerived() out_msg.x = 0.0 out_msg.y = 0.0 out_msg.z = 0.6 out_msg.yaw = 0 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() for i in range(0, 5 * r): out_msg = QuadPositionDerived() tilted_midpoint = inverse_transform(midpoint, theta) target_point = [0.0, 0.0, 0.0] target_point[0] = tilted_midpoint[0] + radius target_point[1] = tilted_midpoint[1] target_point[2] = tilted_midpoint[2] outpos = transform_coordinates(target_point, theta) out_msg.x = outpos[0] out_msg.y = outpos[1] out_msg.z = outpos[2] out_msg.yaw = math.pi / 2 out_msg.x_vel = 0 out_msg.y_vel = 0 out_msg.z_vel = 0 out_msg.yaw_vel = 0 out_msg.x_acc = 0 out_msg.y_acc = 0 out_msg.z_acc = 0 out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() while not rospy.is_shutdown(): #publish "starting point", might be nice to use ptp_gen or some kind of mo_cap feedback out_pos = transform_coordinates( get_outpos(radius, velo, tilted_midpoint, time), theta) out_velo = transform_coordinates(get_outvelo(radius, velo, time), theta) out_acc = transform_coordinates(get_outacc(radius, velo, time), theta) out_msg = QuadPositionDerived() out_msg.x = out_pos[0] out_msg.y = out_pos[1] out_msg.z = out_pos[2] out_msg.yaw = get_yaw(velo, time, radius) out_msg.x_vel = out_velo[0] out_msg.y_vel = out_velo[1] out_msg.z_vel = out_velo[2] out_msg.yaw_vel = 0 out_msg.x_acc = out_acc[0] out_msg.y_acc = out_acc[1] out_msg.z_acc = out_acc[2] out_msg.yaw_acc = 0 pub.publish(out_msg) time += 1.0 / r rate.sleep()
def go_to_start(self, midpoint, pub): r = 10.0 rate = rospy.Rate(r) for i in range(0, 15 * int(r)): out_msg = QuadPositionDerived() out_msg.x = 0. out_msg.y = 0. out_msg.z = 0.6 out_msg.yaw = 0 out_msg.x_vel = 0. out_msg.y_vel = 0. out_msg.z_vel = 0. out_msg.yaw_vel = 0 out_msg.x_acc = 0. out_msg.y_acc = 0. out_msg.z_acc = 0. out_msg.yaw_acc = 0 pub.publish(out_msg) rate.sleep() target_point = [0.0, 0.0, 0.0] target_point[0] = midpoint[0] + self.radius target_point[1] = midpoint[1] target_point[2] = midpoint[2] outpos = self.transform_coordinates(target_point, self.theta) rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6]) rospy.set_param("trajectory_generator/end_point", outpos) sl_gen = StraightLineGen() sl_gen.generate() return outpos