def improviserListener(self, msg):
# print(msg)
if msg.flag and self.state == WAITING:
self.improPos = self.c_to_d(msg.x, msg.y)
self.state = ROTATING
self.des_x, self.des_y = self.planner(self.improPos, self.curPos)
print(self.des_x, self.des_y)
self.adv_pub.publish(FlagPosition(False, None, None))
def __init__(self):
print("Setting up turtle...")
rospy.Subscriber('/vrpn_client_node/adv/pose', PoseStamped, self.adv)
rospy.Subscriber('/improviser', FlagPosition, self.improviserListener)
self.pub = rospy.Publisher('/blue/mobile_base/commands/velocity',
Twist,
queue_size=10)
self.adv_pub = rospy.Publisher('/adversary',
FlagPosition,
queue_size=10)
# self.tfBuffer = tf2_ros.Buffer()
# self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
self.state = WAITING
# self.simulated = True
self.curPos = (5, 5)
self.k = 7
self.r = rospy.Rate(10) # 10hz
# self.K1 = 0.3
# self.K2 = 1
self.delta = 0.3
self.move_tolerance = 0.01
self.rotate_tolerance = 0.01
self.planner = self.loopAdv
self.improPos = None
self.loop_state = "down"
self.des_x = None
self.des_y = None
self.orientation = 0.0
self.move_rotations = {
(0, 1): 0.0,
(0, -1): np.pi,
(1, 0): -np.pi / 2,
(-1, 0): np.pi / 2
}
if self.simulated:
while not rospy.is_shutdown():
if self.state == MOVING:
print("Adversary moving...")
self.curPos = self.planner(self.improPos, self.curPos)
# print("Adversary move: ", advMove)
# = tuple([self.curPos[0] + advMove[0], self.curPos[1] + advMove[1]])
# print("Adversary position: ", self.curPos)
new_x, new_y = self.d_to_c(*self.curPos)
# print("Continuous position", (new_x, new_y))
rospy.sleep(3)
self.adv_pub.publish(FlagPosition(True, new_x, new_y))
# print("Adversary move complete!")
self.state = WAITING
def adv(self, msg):
if self.state == MOVING:
print("Adversary moving...")
if not self.simulated:
advPos = self.c_to_d(msg.x, msg.y)
des_x, des_y = self.d_to_c(self.planner(self.improPos, advPos))
arrived = self.move(msg, des_x, des_y)
if arrived:
self.impro_pub.publish(
FlagPosition(True, advPos[0], advPos[1]))
self.state = WAITING
def fly(self, msg):
if self.isHovering:
# print("Current move: ", self.curr_move)
if self.state == FLYING:
dronePos = msg.pose.position
dronex = dronePos.x
droney = dronePos.y
dronez = dronePos.z
# print("Current position: ", self.curr_pos)
# print("Moving drone to: (" + str(self.next_pos[0]) + ", " + str(self.next_pos[1]) + ")")
arrived = self.flyToPlace(msg, *self.next_pos)
self.f_impro_real.write(str((dronex, droney)))
# print(arrived)
if arrived:
self.curr_pos = self.c_to_d(dronex, droney)
# print("Arrived @ ", self.curr_pos)
self.impro_pub.publish(FlagPosition(True, dronex, droney))
self.state = WAITING
elif self.state == WAITING:
self.flyToPlace(msg, *self.curr_pos)
def adv(self, msg):
adv_pos = self.c_to_d(msg.pose.position.x, msg.pose.position.y)
if self.state == ROTATING:
print("Rotating...")
arrived = self.rotate(msg, self.des_x, self.des_y)
# arrived = False
if arrived:
print("Finished rotating!")
self.state = MOVING
if self.state == MOVING:
print("Moving...")
arrived = self.move(msg, self.des_x, self.des_y)
if arrived:
print("Finished moving!")
self.adv_pub.publish(FlagPosition(True, adv_pos[0],
adv_pos[1]))
self.state = WAITING
self.curPos = adv_pos
def adversaryListener(self, msg):
if msg.flag and self.state == WAITING:
adversaryPosition = self.c_to_d(msg.x, msg.y)
if (adversaryPosition == self.curr_pos):
self.land()
rospy.signal_shutdown("I got caught!")
print("Improviser @: ", self.curr_pos)
print("Adversary @: ", adversaryPosition)
advMove = tuple((adversaryPosition[0] - self.advPos[0],
adversaryPosition[1] - self.advPos[1]))
self.advPos = adversaryPosition
impro_step = self.rci_drone.impro.send(advMove)[0]
print("Algo step: ", impro_step)
self.next_pos = tuple([
self.dclamp(self.curr_pos[i] + impro_step[i]) for i in range(2)
])
print("Next pos: ", self.next_pos)
# print("New improviser position: ", self.curr_pos)
self.f_impro_grid.write(str(self.curr_pos))
self.f_adv_grid.write(str((msg.x, msg.y)))
self.state = FLYING
self.impro_pub.publish(FlagPosition(False, None, None))
def improviserListener(self, msg):
# print(msg)
if msg.flag and self.state == WAITING:
self.improPos = self.c_to_d(msg.x, msg.y)
self.state = MOVING
self.adv_pub.publish(FlagPosition(False, None, None))
import rospy
import numpy as np
from crazyflie_msgs.msg import PositionVelocityState
from crazyflie_msgs.msg import PositionVelocityStateStamped
from geometry_msgs.msg import PoseStamped
from crazyflie_msgs.msg import FlagPosition
rospy.init_node("simpro", anonymous=True)
impro_pub = rospy.Publisher('/improviser', FlagPosition, queue_size=10)
while not rospy.is_shutdown():
x = raw_input()
print(x)
impro_pub.publish(FlagPosition(True, 0, 0))