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 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_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
