def tfCallback(event): # Called at specified rate. gets tf of all agents, concatenating, and publishing to all agents
global tfListener, numAgents
# Initialize message
poseMsg = PoseArray()
poseMsg.poseArray = [PoseStamped() for i in range(numAgents)]
try:
for agentID in range(numAgents):
now = rospy.Time.now()
# Prepare PoseStamped message for this agent
agentPose = PoseStamped()
agentPose.header.stamp = now
agentPose.header.frame_id = ns_prefix+str(agentID)
if mode=='sim':
ext = '/base_footprint'
else:
ext = ''
tfListener.waitForTransform("/world","/"+ns_prefix+str(agentID)+ext,now,rospy.Duration(0.5))
(trans,_) = tfListener.lookupTransform("/world","/"+ns_prefix+str(agentID)+ext,now)
(agentPose.pose.position.x,agentPose.pose.position.y) = np.array(trans[0:2])
# Add this agent's PoseStamped message to array. Also, update header to reflect latest time.
poseMsg.header.stamp = agentPose.header.stamp
poseMsg.poseArray[agentID] = agentPose
# Check for collisions, and publish
publishPose(poseMsg)
except tf.Exception as e:
print e
def tfCallback(
event
): # Called at specified rate. gets tf of all agents, concatenating, and publishing to all agents
global tfListener, numAgents
# Initialize message
poseMsg = PoseArray()
poseMsg.poseArray = [PoseStamped() for i in range(numAgents)]
try:
for agentID in range(numAgents):
now = rospy.Time.now()
# Prepare PoseStamped message for this agent
agentPose = PoseStamped()
agentPose.header.stamp = now
agentPose.header.frame_id = ns_prefix + str(agentID)
if mode == 'sim':
ext = '/base_footprint'
else:
ext = ''
tfListener.waitForTransform("/world",
"/" + ns_prefix + str(agentID) + ext,
now, rospy.Duration(0.5))
(trans, _) = tfListener.lookupTransform(
"/world", "/" + ns_prefix + str(agentID) + ext, now)
(agentPose.pose.position.x,
agentPose.pose.position.y) = np.array(trans[0:2])
# Add this agent's PoseStamped message to array. Also, update header to reflect latest time.
poseMsg.header.stamp = agentPose.header.stamp
poseMsg.poseArray[agentID] = agentPose
# Check for collisions, and publish
publishPose(poseMsg)
except tf.Exception as e:
print e
def groundStation(): # Main node
global c, obstacles, numAgents, sensingPub, sensingDropout, poseMsg
global posePub, abortPub, tfListener, velPubs
global mode, collisionBubble, ns_prefix
rospy.init_node('groundStation')
# Get some parameters
mode = rospy.get_param('~mode','demo')
collisionBubble = rospy.get_param('~collisionBubble',0.1)
ns_prefix = rospy.get_param('~ns_prefix','ugv')
sensingDropout = rospy.get_param('~sensingDropout',False)
formation_file = rospy.get_param('~formation',None)
obstacles_file = rospy.get_param('~obstacles',None)
formationScale = rospy.get_param('~formationScale',1.0)
use_gps = rospy.get_param('~use_gps',False)
# get formation configuration
(numAgents,c,obstacles) = readConfig(formation_file,obstacles_file,formationScale)
rospy.logwarn("GroundStation numAgents: "+str(numAgents))
rospy.logwarn("Collision bubble: "+str(collisionBubble))
# start service handler
s = rospy.Service('initFormation',Graphs,initFormationHandler)
rospy.logwarn("initFormation service started.")
# pose publisher
posePub = rospy.Publisher('/allAgentPose',PoseArray,queue_size=1)
rospy.logwarn("Publishing agent positions on topic /allAgentPose")
# Use simulated sensor fusion or tf
if use_gps:
# Initialize message
poseMsg = PoseArray()
poseMsg.poseArray = [PoseStamped() for i in range(numAgents)]
init_pose = [{'x':rospy.get_param('/'+ns_prefix+str(agentID)+'/initX'),'y':rospy.get_param('/'+ns_prefix+str(agentID)+'/initY')} for agentID in range(numAgents)] # initial pose of agents
pose_subs = [rospy.Subscriber('/'+ns_prefix+str(agentID)+'/pose',PoseStamped,agentPoseCallback,callback_args=(init_pose,agentID),queue_size=1) for agentID in range(numAgents)]
else:
# Send tf as pose msg
if mode=='sim':
rate = 0.2
else:
rate = 0.04
rospy.logwarn("here here here here here here here here")
tfListener = tf.TransformListener()
rospy.Timer(rospy.Duration.from_sec(rate),tfCallback,oneshot=False)
# Generate new sensing graph at specified interval and publish
sensingPub = rospy.Publisher('/sensingGraph',Sensing,latch=True,queue_size=10)
rospy.logwarn("Publishing sensing graph on topic /sensingGraph")
rospy.Timer(rospy.Duration(1), sensingPublisher) # publish sensing graph info at specified rate
# Pause on button press
velPubs = [rospy.Publisher(ns_prefix+str(ii)+'/cmd_vel_mux/input/teleop',Twist,queue_size=1) for ii in range(numAgents)] # turtlebot velocity publisher
joySub = rospy.Subscriber('joy',Joy,joyCallback)
# abort publisher
abortPub = rospy.Publisher('/abortReset',AbortReset,latch=True,queue_size=10)
rospy.logwarn("Monitoring agents for potential collisions...")
rospy.spin() # block until rospy shutdown
posePub.unregister() # End pose publishing (for avoiding display of errors)
def groundStation(): # Main node
global c, obstacles, numAgents, sensingPub, sensingDropout, poseMsg
global posePub, abortPub, tfListener, velPubs
global mode, collisionBubble, ns_prefix
rospy.init_node('groundStation')
# Get some parameters
mode = rospy.get_param('~mode', 'demo')
collisionBubble = rospy.get_param('~collisionBubble', 0.1)
ns_prefix = rospy.get_param('~ns_prefix', 'ugv')
sensingDropout = rospy.get_param('~sensingDropout', False)
formation_file = rospy.get_param('~formation', None)
obstacles_file = rospy.get_param('~obstacles', None)
formationScale = rospy.get_param('~formationScale', 1.0)
use_gps = rospy.get_param('~use_gps', False)
# get formation configuration
(numAgents, c, obstacles) = readConfig(formation_file, obstacles_file,
formationScale)
rospy.logwarn("GroundStation numAgents: " + str(numAgents))
rospy.logwarn("Collision bubble: " + str(collisionBubble))
# start service handler
s = rospy.Service('initFormation', Graphs, initFormationHandler)
rospy.logwarn("initFormation service started.")
# pose publisher
posePub = rospy.Publisher('/allAgentPose', PoseArray, queue_size=1)
rospy.logwarn("Publishing agent positions on topic /allAgentPose")
# Use simulated sensor fusion or tf
if use_gps:
# Initialize message
poseMsg = PoseArray()
poseMsg.poseArray = [PoseStamped() for i in range(numAgents)]
init_pose = [{
'x':
rospy.get_param('/' + ns_prefix + str(agentID) + '/initX'),
'y':
rospy.get_param('/' + ns_prefix + str(agentID) + '/initY')
} for agentID in range(numAgents)] # initial pose of agents
pose_subs = [
rospy.Subscriber('/' + ns_prefix + str(agentID) + '/pose',
PoseStamped,
agentPoseCallback,
callback_args=(init_pose, agentID),
queue_size=1) for agentID in range(numAgents)
]
else:
# Send tf as pose msg
if mode == 'sim':
rate = 0.2
else:
rate = 0.04
rospy.logwarn("here here here here here here here here")
tfListener = tf.TransformListener()
rospy.Timer(rospy.Duration.from_sec(rate), tfCallback, oneshot=False)
# Generate new sensing graph at specified interval and publish
sensingPub = rospy.Publisher('/sensingGraph',
Sensing,
latch=True,
queue_size=10)
rospy.logwarn("Publishing sensing graph on topic /sensingGraph")
rospy.Timer(
rospy.Duration(1),
sensingPublisher) # publish sensing graph info at specified rate
# Pause on button press
velPubs = [
rospy.Publisher(ns_prefix + str(ii) + '/cmd_vel_mux/input/teleop',
Twist,
queue_size=1) for ii in range(numAgents)
] # turtlebot velocity publisher
joySub = rospy.Subscriber('joy', Joy, joyCallback)
# abort publisher
abortPub = rospy.Publisher('/abortReset',
AbortReset,
latch=True,
queue_size=10)
rospy.logwarn("Monitoring agents for potential collisions...")
rospy.spin() # block until rospy shutdown
posePub.unregister(
) # End pose publishing (for avoiding display of errors)
