Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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)
Exemplo n.º 4
0
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)