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)