def takeAction(data):
global schedulerTopic
global publishDroneScan
global idNbr
global dronePlannerId
if (data.command != 'idle'):
if data.command == 'scan':
value = Bool()
value.data = True
publishDroneScan.publish(value)
# send idle command to the scheduler after 5 seconds
time.sleep(5)
droneMessage = drone_command()
droneMessage.command = 'idle'
droneMessage.drone_id = dronePlannerId
schedulerTopic.publish(droneMessage)
else:
cdata = data._connection_header
rospy.loginfo('%s now initiates %s', dronePlannerId, data.command)
rate.sleep()
msg = drone_command()
msg.drone_id = dronePlannerId
msg.command = 'idle'
schedulerTopic.publish(msg)
def turtle(args):
global rate
global id
global schedulerTopic, actuatorTopic
#global idRequestTopic
#TODO: data validation of arg
arg = getopt.getopt(args, '')[1][0]
id = 'turtle' + arg
rospy.init_node(id, anonymous=True)
rospy.loginfo('started ' + id)
schedulerTopic = rospy.Publisher(id, drone_command, queue_size=10)
rospy.Subscriber(id, drone_command, takeAction)
#Assign Publisher that publishes the goal to the robot to move
# THIS IS A DUMMY VERSION SO IT HAS NO ACTUATORS
#actuatorTopic = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
#subscribe to goal status from mobile base
# THIS IS A DUMMY VERSION SO THERE IS NO ROBOT TO GET CALLBACKS FROM
#rospy.Subscriber("/move_base/status", GoalStatusArray, goal_status_callback)
rate = rospy.Rate(1)
rate.sleep() #has to sleep after subscribing to a new topic
msg = drone_command()
msg.drone_id = id
msg.command = 'idle'
schedulerTopic.publish(msg)
rospy.spin()
def ListenTo(data):
global idleBefore
currentX = data.pose.pose.position.x
currentY = data.pose.pose.position.y
targetX = targetPoint.pose.pose.position.x
targetY = targetPoint.pose.pose.position.y
distanceToTarget = np.sqrt(np.square(currentX-targetX)+np.square(currentY-targetY))
distanceThreshold = 0.5
droneMessage = drone_command()
print(distanceToTarget,idleBefore)
if ((distanceToTarget < distanceThreshold) and (idleBefore == False)):
droneMessage.command = 'idle'
droneMessage.drone_id = 'drone0'
droneMessage.posX = currentX
droneMessage.posY = currentY
droneMessage.posZ = data.pose.pose.position.z
droneMessage.angle = 0
publishDroneStatus.publish(droneMessage)
print("I'm here")
idleBefore = True
def takeAction(data):
global topic
if (data.command != 'idle'):
cdata = data._connection_header
rospy.loginfo('%s now initiates %s', id, data.command)
rate.sleep()
msg = drone_command()
msg.drone_id = id
msg.command = 'idle'
topic.publish(msg)
def publish(robot, state):
global topic
global idByRobot
rospy.loginfo("state to be published: " + ' '.join( ` state `))
msg = drone_command()
msg.drone_id = idByRobot[robot]
#TODO: split case to handle drones as well; currently +o''.join(nly) turtles
msg.command = state[0]
if msg.command in ['drive', 'fly']:
location = state[1]
position = positions[location]
msg.posX = position[0]
msg.posY = position[1]
msg.angle = 0
topic[robot].publish(msg)
else:
topic[robot].publish(msg)
def publish(robot, state):
global topic
global idByRobot
rospy.loginfo("state to be published: ");
rospy.loginfo(state);
msg = drone_command();
msg.drone_id = idByRobot[robot];
#TODO: split case to handle drones as well; currently only turtles
msg.command = state[0];
if msg.command == 'drive':
location = state[1];
position = positions[location];
msg.posX = position[0];
msg.posY = position[1];
msg.angle = 0;
topic[robot].publish(msg);
else:
topic[robot].publish(msg);
def drone(args):
arg = getopt.getopt(args, '')[1][0]
id = 'drone' + arg
rospy.init_node(id, anonymous=True)
rospy.loginfo('started ' + id)
topic = rospy.Publisher(id, drone_command, queue_size=10)
msg = drone_command()
msg.drone_id = id
msg.command = 'idle'
count = 0
while (count < 10):
raw_input("Press Enter to continue...")
topic.publish(msg)
count += 1
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def drone(args):
global rate
global id
global topic
#global idRequestTopic
#TODO: data validation of arg
arg = getopt.getopt(args, '')[1][0]
id = 'drone' + arg
rospy.init_node(id, anonymous=True)
rospy.loginfo('started ' + id)
topic = rospy.Publisher(id, drone_command, queue_size=10)
rospy.Subscriber(id, drone_command, takeAction)
rate = rospy.Rate(1)
rate.sleep() #has to sleep after subscribing to a new topic
msg = drone_command()
msg.drone_id = id
msg.command = 'idle'
topic.publish(msg)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def drone(args):
global droneId
global dronePlannerId
global idNbr
global rate
global schedulerTopic
global publishDroneScan
#global idRequestTopic
#TODO: data validation of arg
# Assign drone id
droneId = 'drone' + str(sys.argv[1])
dronePlannerId = 'drone' + str(sys.argv[2])
idNbr = str(sys.argv[2])
rospy.init_node(dronePlannerId, anonymous=True)
rospy.loginfo('started ' + droneId + ' as drone no. ' + dronePlannerId)
schedulerTopic = rospy.Publisher(dronePlannerId,
drone_command,
queue_size=10)
rospy.Subscriber(dronePlannerId, drone_command, takeAction)
# Publisher to dronex/scan
droneScanTopic = droneId + '/scan'
publishDroneScan = rospy.Publisher(droneScanTopic, Bool, queue_size=1)
rate = rospy.Rate(1)
rate.sleep() #has to sleep after subscribing to a new topic
msg = drone_command()
msg.drone_id = dronePlannerId
msg.command = 'idle'
schedulerTopic.publish(msg)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
# Subscribe to the odometry position from the SLAM est.
slamTopic = 'drone2/slam/pos'
rospy.Subscriber(slamTopic,Odometry,ListenTo)
# Subscribe to the scheduling topic to know target position
schedulerTopic = 'drone0'
rospy.Subscriber(schedulerTopic,drone_command,SetTarget)
publishDroneStatus = rospy.Publisher(schedulerTopic,drone_command,queue_size=1)
# Publish to drone pid controller the target value
targetDataTopic = 'drone2/planner/targetPosition'
publishTargetData = rospy.Publisher(targetDataTopic, Odometry, queue_size=1)
# Has to sleep after subscribing to a new topic, before publishing
rate = rospy.Rate(1)
rate.sleep()
# Send an 'idle' message to declare that drone is ready
msg = drone_command();
msg.drone_id = 'drone0';
msg.command = 'idle';
publishDroneStatus.publish(msg)
rospy.spin()