def callback(data):
owner = [
R1.location_x, R1.location_y, R1.location_z, R1.velocity,
R1.velocity_angle
]
firefly2 = [
I1.location_x, I1.location_y, I1.location_z, I1.velocity,
I1.velocity_angle
]
firefly3 = [
I2.location_x, I2.location_y, I2.location_z, I2.velocity,
I2.velocity_angle
]
firefly4 = [
I3.location_x, I3.location_y, I2.location_z, I3.velocity,
I3.velocity_angle
]
# start = time.clock()
action = collision_detecter(owner, [firefly2, firefly3, firefly4])
# print("All infos:", owner, [firefly2, firefly3])
if action is None:
steer_angle = steering_angle()
R1.velocity_angle = steer_angle
R1.velocity = 2
if distance2target() > 1:
new_x = R1.location_x + R1.velocity * cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity * sin(R1.velocity_angle)
print("curent target:", new_x, new_y)
print("curent location:", R1.location_x, R1.location_y)
# new_xx = new_x - 1.40
# new_yy = new_y + 1.40
publish_command([new_x, new_y, R1.target_z], [0, 0, 0])
# print("publish info:", [new_x, new_y, R1.target_z],[0, 0, 0])
# print(R1.location_x, R1.location_y)
# used_time = time.clock()-start
# time_UAV1.append(used_time)
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly1 arrived target location!")
# for ii in range(len(time_UAV1)):
# time_UAV1[ii] = str(time_UAV1[ii])
# with open('/home/wenbing/Learning_ROS_ws/src/multi_uav_control/script/time_uav1.csv', 'a') as f:
# f.write(sep.join(time_UAV1))
# rospy.loginfo("Writing")
elif action is not None:
print("Firefly1 Should action:", action)
if action[0] + action[1] > 0:
R1.velocity = R1.velocity - (R1.velocity - 0.3) * float(action[0])
steer_angle = R1.velocity_angle - radians(float(action[1]))
R1.velocity_angle = steer_angle
if distance2target() > 1:
new_x = R1.location_x + R1.velocity * cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity * sin(R1.velocity_angle)
publish_command([new_x, new_y, R1.target_z], [0, 0, 0])
print("Should:", [new_x, new_y, R1.target_z], [0, 0, 0])
# used_time = time.clock()-start
# time_UAV1.append(used_time)
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly1 arrived target location!")
def callback(data):
owner = [R1.location_x, R1.location_y, R1.location_z, R1.velocity, R1.velocity_angle]
firefly1 = [I1.location_x, I1.location_y, I1.location_z, I1.velocity, I1.velocity_angle]
firefly3 = [I2.location_x, I2.location_y, I2.location_z, I2.velocity, I2.velocity_angle]
firefly4 = [I3.location_x, I3.location_y, I2.location_z, I3.velocity, I3.velocity_angle]
action = collision_detecter(owner, [firefly1, firefly3, firefly4])
if action is None:
steer_angle = steering_angle()
R1.velocity_angle = steer_angle
R1.velocity = 2
if distance2target() > 1:
new_x = R1.location_x + R1.velocity*cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity*sin(R1.velocity_angle)
print("curent location:",R1.location_x, R1.location_y)
publish_command([new_x, new_y, R1.target_z],[0, 0, 0])
# print("Should:", [new_x, new_y, R1.target_z],[0, 0, 0])
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly2 arrived target location!")
elif action is not None:
print("Firefly2 Should action:", action)
if action[0] + action[1] > 0:
R1.velocity = R1.velocity - (R1.velocity - 0.1)*float(action[0])
steer_angle = R1.velocity_angle - radians(float(action[1]))
R1.velocity_angle = steer_angle
if distance2target() > 1:
new_x = R1.location_x + R1.velocity*cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity*sin(R1.velocity_angle)
publish_command([new_x, new_y, R1.target_z],[0, 0, 0])
# print("Should:", [new_x, new_y, R1.target_z],[0, 0, 0])
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly2 arrived target location!")
def callback(data):
owner = [R1.location_x, R1.location_y, R1.location_z, R1.velocity, R1.velocity_angle]
global robot_list
all_intruder_info = []
for r in robot_list:
all_intruder_info.append([r.location_x, r.location_y, r.location_z, I1.velocity, r.velocity_angle])
action = collision_detecter(owner, all_intruder_info)
if action is None:
steer_angle = steering_angle()
R1.velocity_angle = steer_angle
R1.velocity = 2
if distance2target() > 1:
new_x = R1.location_x + R1.velocity*cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity*sin(R1.velocity_angle)
publish_command([new_x, new_y, R1.target_z],[0, 0, 0])
# print("Should:", [new_x, new_y, R1.target_z],[0, 0, 0])
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly2 arrived target location!")
elif action is not None:
print("Firefly2 Should action:", action)
if action[0] + action[1] > 0:
R1.velocity = R1.velocity - (R1.velocity - 0.3)*float(action[0])
steer_angle = R1.velocity_angle - radians(float(action[1]))
R1.velocity_angle = steer_angle
if distance2target() > 1:
new_x = R1.location_x + R1.velocity*cos(R1.velocity_angle)
new_y = R1.location_y + R1.velocity*sin(R1.velocity_angle)
publish_command([new_x, new_y, R1.target_z],[0, 0, 0])
# print("Should:", [new_x, new_y, R1.target_z],[0, 0, 0])
else:
# publish_command([R1.location_x, R1.location_x, R1.target_z],[0, 0, 0])
rospy.loginfo("Firefly2 arrived target location!")