コード例 #1
0
ファイル: wam2seds.py プロジェクト: LASA-ros-pkg/SEDSros
def process_bags(outfilename, inbags):

    outbag = rosbag.Bag(outfilename, "w")
    pm = SedsMessage() # store previous message for dx computation
    cm = SedsMessage() # current seds message
    zero = rospy.rostime.Duration(0)

    # open all the trajectory bags
    for (i,bagname) in enumerate(inbags):
        rospy.loginfo("Processing bag %d: %s" %  (i,bagname))

        # need a new listener for each bag -- otherwise TF_OLD_DATA warnings
        bag = rosbag.Bag(bagname)
        first = True # set pm on first step -- compute pm.dx on every other step

        # read all the bag messages and process /tf messages
        for topic, msg, t in bag.read_messages():

            if topic == "/wam/cartesian_coordinates":

                cm.x = npa(msg.position)
                cm.t = t
                cm.index = i
                if first:
                    pm.x = cm.x
                    pm.index = cm.index
                    pm.t = cm.t
                    first = False

                # compute dx
                pm.dx = cm.x - pm.x

                # compute dt
                pm.dt = cm.t - pm.t;

                # Write out the seds bag if time has changed.
                # Some tf frames change at a much faster rate and
                # so listener reprocesses the same transform
                # lookup even though latest common time remains
                # the same. If we did not check pm.dt != zero,
                # we'd end up with a lot of duplicate entries.

                # We also check whether pm.dx is zero. This choice
                # is the result of a fair amount of
                # experimentation to try to extract the actual
                # motions from a bagfile that probably contains a
                # lot of the robot just sitting there (e.g. before
                # the teaching trajectory actually starts). I
                # experimented a fair bit with the best way to
                # extract the useful subtrajectory out of the bag
                # and this criterion is the best in terms of
                # simplicity and quality.

                if (pm.dt != zero and not iszero(pm.dx)):
                    rospy.logdebug("Message time: %s" % str(pm.t))
                    outbag.write('seds/trajectories', pm)

                # alt. set t parameter to orginal bag
                # time? would need to process bags in
                # order to avoid time travel

                pm.x = cm.x
                pm.index = cm.index
                pm.t = cm.t

        # end of current bag -- write out last entry with dx = 0
        cm.dx = cm.x - pm.x
        outbag.write('seds/trajectories', cm)

        bag.close()

    rospy.loginfo("Writing bag: %s" % outfilename)
    outbag.close() # very important to close a rosbag after writing
コード例 #2
0
ファイル: tf2seds.py プロジェクト: LASA-ros-pkg/SEDSros
def process_bags(outfilename, inbags, source_fid, target_fid):

    outbag = rosbag.Bag(outfilename, "w")
    pm = SedsMessage() # store previous message for dx computation
    cm = SedsMessage() # current seds message
    zero = rostime.Duration(0)


    s1 = String()
    s1.data = source_fid
    outbag.write('seds/source_fid', s1)
    rospy.loginfo("Writing seds/source_fid " + s1.data)

    s2 = String()
    s2.data = target_fid
    outbag.write('seds/target_fid', s2)
    rospy.loginfo("Writing seds/target_fid " + s2.data)

    # open all the trajectory bags
    for (i,bagname) in enumerate(inbags):
        rospy.loginfo("Processing bag %d: %s" %  (i,bagname))

        # need a new listener for each bag -- otherwise TF_OLD_DATA warnings
        listener = BagListener()
        bag = rosbag.Bag(bagname)
        first = True # set pm on first step -- compute pm.dx on every other step

        tf_cnt=0
        tf_match_cnt=0
        nonzero_cnt=0

        # read all the bag messages and process /tf messages
        for topic, msg, t in bag.read_messages():

            #only process /tf messages
            if topic[-2:] == "tf":# and (cfid == target_fid or cfid == source_fid):
                # loads the transforms
                listener.load(msg)
                tf_cnt+=1
                try:
                    #Can we transform between these?
                    #et = listener.lookupTransform(source_fid, target_fid, rostime.Time(0))
                    #since source and target may be at very different rates, we need to time travel
                    #so get times with respect to a constant (the torso)
                    unmoving_fid="torso_lift_link"
                    source_time=listener.getLatestCommonTime(source_fid,unmoving_fid)
                    target_time=listener.getLatestCommonTime(target_fid,unmoving_fid)
                    et = listener.lookupTransformFull(source_fid,source_time,target_fid,target_time,unmoving_fid)
                    #et = listener.lookupTransformFull(target_fid,target_time,source_fid,source_time,unmoving_fid) 
                    tf_match_cnt+=1
                    # get the current x
                    cm.x = et[0][:] + euler_from_quaternion(et[1][:])
                    cm.index = i
                    cm.t = max(source_time,target_time)

                    if first:
                        pm.x = cm.x
                        pm.index = cm.index
                        pm.t = cm.t
                        first = False
                        
                    # compute dx
                    pm.dx = npa(cm.x) - npa(pm.x)

                    # compute dt
                    pm.dt = cm.t - pm.t;
                    
                    # Write out the seds bag if time has changed.
                    # Some tf frames change at a much faster rate and
                    # so listener reprocesses the same transform
                    # lookup even though latest common time remains
                    # the same. If we did not check pm.dt != zero,
                    # we'd end up with a lot of duplicate entries.
                    
                    # We also check whether pm.dx is zero. This choice
                    # is the result of a fair amount of
                    # experimentation to try to extract the actual
                    # motions from a bagfile that probably contains a
                    # lot of the robot just sitting there (e.g. before
                    # the teaching trajectory actually starts). I
                    # experimented a fair bit with the best way to
                    # extract the useful subtrajectory out of the bag
                    # and this criterion is the best in terms of
                    # simplicity and quality.
                                  
                    if (pm.dt != zero) and not iszero(pm.dx):
                        #rospy.loginfo("Message time: %s" % str(pm.t))
                        #print "At time " + str(min(source_time,target_time))
                        #print "-Translation " + str(pm.x[0:3])
                        
                        outbag.write('seds/trajectories', pm)
                        nonzero_cnt+=1
                        
                        # alt. set t parameter to orginal bag
                        # time? would need to process bags in
                        # order to avoid time travel
                        
                        pm.x = cm.x
                        #pm.index = cm.index
                        pm.t = cm.t
                    

                except tf.Exception, error:
                    rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))
                    #rospy.loginfo("TF Exception %s" % error)

        # end of current bag -- write out last entry with dx = 0
        cm.dx = npa(cm.x) - npa(pm.x)
        outbag.write('seds/trajectories', cm)
        #print "Found " + str(tf_cnt) + "tfs, " + str(tf_match_cnt) + " match, and " + str(nonzero_cnt) + " are nonzero."
        rospy.loginfo("Processed %s, found transforms for %s and %s are nonzero" % (str(tf_cnt), str(tf_match_cnt), str(nonzero_cnt)))
        bag.close()
コード例 #3
0
ファイル: test_driver.py プロジェクト: LASA-ros-pkg/SEDSros
    rospy.wait_for_service("/ds_node/load_model")
    lm = rospy.ServiceProxy("/ds_node/load_model", Empty)

    rospy.wait_for_service('/ds_node/ds_server')
    ds = rospy.ServiceProxy('/ds_node/ds_server', DSSrv)

    # arg 1 is the model
    # arg 2 is the outbag filename

    # load the model we want to test
    # lm(sys.argv[1])
    lm()

    outbag = rosbag.Bag(sys.argv[1], 'w')
    sm = SedsMessage()

    # for bagfiles collected on 6/15
    starts = [[0.268864,-0.663690,-0.331069],
              [0.156349,-0.542263,0.355905],
              [0.183113,0.272913,-0.263200],
              [0.061941,-0.539489,-0.842995],
              [-0.006358,-1.179837,0.026429],
              [0.215756,-0.564982,-0.329221]]

    # end positions for reference
    ends = [[0.524915,-0.149382,-0.178386],
            [0.511823,-0.299487,-0.089839],
            [0.478417,-0.222597,-0.099429],
            [0.532435,-0.075026,-0.247842],
            [0.531750,-0.071448,-0.256943],
コード例 #4
0
ファイル: tf2seds.py プロジェクト: stober/SEDSros
def process_bags(outfilename, inbags, source_fid, target_fid):

    outbag = rosbag.Bag(outfilename, "w")
    pm = SedsMessage() # store previous message for dx computation
    cm = SedsMessage() # current seds message
    zero = rostime.Duration(0)


    s1 = String()
    s1.data = source_fid
    outbag.write('seds/source_fid', s1)
    rospy.loginfo("Writing seds/source_fid " + s1.data)

    s2 = String()
    s2.data = target_fid
    outbag.write('seds/target_fid', s2)
    rospy.loginfo("Writing seds/target_fid " + s2.data)

    # open all the trajectory bags
    for (i,bagname) in enumerate(inbags):
        rospy.loginfo("Processing bag %d: %s" %  (i,bagname))

        # need a new listener for each bag -- otherwise TF_OLD_DATA warnings
        listener = BagListener()
        bag = rosbag.Bag(bagname)
        first = True # set pm on first step -- compute pm.dx on every other step

        # read all the bag messages and process /tf messages
        for topic, msg, t in bag.read_messages():

            if topic[-2:] == "tf":
                # loads the transforms
                listener.load(msg)

                # computes the coordinates
                try:
                    et = listener.lookupTransform(source_fid, target_fid, rostime.Time(0))
                    t = listener.getLatestCommonTime(source_fid, target_fid)

                    # fill in the seds message
                    cm.x = et[0][:] # pos (x,y,z)

                    cm.index = i
                    cm.t = t

                    if first:
                        pm.x = cm.x
                        pm.index = cm.index
                        pm.t = cm.t
                        first = False

                    # compute dx
                    pm.dx = npa(cm.x) - npa(pm.x)

                    # compute dt
                    pm.dt = cm.t - pm.t;

                    # Write out the seds bag if time has changed.
                    # Some tf frames change at a much faster rate and
                    # so listener reprocesses the same transform
                    # lookup even though latest common time remains
                    # the same. If we did not check pm.dt != zero,
                    # we'd end up with a lot of duplicate entries.

                    # We also check whether pm.dx is zero. This choice
                    # is the result of a fair amount of
                    # experimentation to try to extract the actual
                    # motions from a bagfile that probably contains a
                    # lot of the robot just sitting there (e.g. before
                    # the teaching trajectory actually starts). I
                    # experimented a fair bit with the best way to
                    # extract the useful subtrajectory out of the bag
                    # and this criterion is the best in terms of
                    # simplicity and quality.

                    if (pm.dt != zero and not iszero(pm.dx)):
                        rospy.logdebug("Message time: %s" % str(pm.t))
                        outbag.write('seds/trajectories', pm)

                        # alt. set t parameter to orginal bag
                        # time? would need to process bags in
                        # order to avoid time travel

                    pm.x = cm.x
                    pm.index = cm.index
                    pm.t = cm.t

                except LookupException, error:
                    # sometimes not enough info is recorded to complete the transform lookup
                    rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))

                except ConnectivityException, error:
                    # sometimes the perceptual information drops out
                    rospy.loginfo("ConnectivityException %s" % error)
                    rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))