def test_fromTf(self):
     transformer = Transformer(True, rospy.Duration(10.0))
     m = TransformStamped()
     m.header.frame_id = 'wim'
     m.child_frame_id = 'james'
     m.transform.translation.x = 2.71828183
     m.transform.rotation.w = 1.0
     transformer.setTransform(m)
     b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0)))
Exemple #2
0
 def test_fromTf(self):
     transformer = Transformer(True, rospy.Duration(10.0))
     m = TransformStamped()
     m.header.frame_id = 'wim'
     m.child_frame_id = 'james'
     m.transform.translation.x = 2.71828183
     m.transform.rotation.w = 1.0
     transformer.setTransform(m)
     b = pm.fromTf(
         transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def get_vertices(pose, size, shape_type):
    tf = Transformer(True, rospy.Duration(10.0))
    m = TransformStamped()
    m.header.frame_id = 'object'
    m.transform.rotation.w = 1

    keys = []
    if shape_type=='box':
        for mx in POM:
            for my in POM:
                for mz in POM:
                    key = 'frame%s%s%s'%(F[mx], F[my], F[mz])
                    m.child_frame_id = key
                    keys.append(key)
                    m.transform.translation.x = size[0]/2.0*mx
                    m.transform.translation.y = size[1]/2.0*my
                    m.transform.translation.z = size[2]/2.0*mz  
                    tf.setTransform(m)
    else:
        return None

    m.transform.translation.x = pose.position.x
    m.transform.translation.y = pose.position.y
    m.transform.translation.z = pose.position.z
    m.transform.rotation.x = pose.orientation.x
    m.transform.rotation.y = pose.orientation.y
    m.transform.rotation.z = pose.orientation.z
    m.transform.rotation.w = pose.orientation.w
    m.header.frame_id = 'map'
    m.child_frame_id = 'object'
    tf.setTransform(m)
    
    vertices = []
    for key in keys:
        (off,rpy) = tf.lookupTransform('map', key, rospy.Time(0))
        vertices.append(off)
    return vertices
Exemple #4
0
def get_vertices(pose, size, shape_type):
    tf = Transformer(True, rospy.Duration(10.0))
    m = TransformStamped()
    m.header.frame_id = 'object'
    m.transform.rotation.w = 1

    keys = []
    if shape_type == 'box':
        for mx in POM:
            for my in POM:
                for mz in POM:
                    key = 'frame%s%s%s' % (F[mx], F[my], F[mz])
                    m.child_frame_id = key
                    keys.append(key)
                    m.transform.translation.x = size[0] / 2.0 * mx
                    m.transform.translation.y = size[1] / 2.0 * my
                    m.transform.translation.z = size[2] / 2.0 * mz
                    tf.setTransform(m)
    else:
        return None

    m.transform.translation.x = pose.position.x
    m.transform.translation.y = pose.position.y
    m.transform.translation.z = pose.position.z
    m.transform.rotation.x = pose.orientation.x
    m.transform.rotation.y = pose.orientation.y
    m.transform.rotation.z = pose.orientation.z
    m.transform.rotation.w = pose.orientation.w
    m.header.frame_id = 'map'
    m.child_frame_id = 'object'
    tf.setTransform(m)

    vertices = []
    for key in keys:
        (off, rpy) = tf.lookupTransform('map', key, rospy.Time(0))
        vertices.append(off)
    return vertices
Exemple #5
0
def process(bagfile):

    # Load bag
    print "\nOpening bag file [%s]" % bagfile
    bag = rosbag.Bag(bagfile)


    
    hz = 100

    # Get dictionary of topics
    # topics = list(set([c.topic for c in bag._get_connections() if c.topic.startswith("/cf") or c.topic.]))
    topics = list(set([c.topic for c in bag._get_connections() if c.topic != "/diagnostics" and c.topic.rfind("rosout")+c.topic.rfind("parameter_") == -2]))

    if topics == []:
        rospy.logwarn('[%s] has no topics, skipping')
        return
    start, finish = printInfo(bag, topics=topics)

    dur = finish-start 
    rostf = Transformer(True, dur) #buffer
    duration = dur.to_sec()



    # deal with TF
    # treat each tf frame as a topic
    # tfdict = {}
    # for tfs in [m[1] for m in bag.read_messages("/tf")]:
    #     #from->to
    #     for t in tfs.transforms:
    #         tFrom = t.header.frame_id
    #         tTo   = t.child_frame_id
    #         if not tfdict.has_key(tFrom):
    #             tfdict[tFrom] = {}
    #         if not tfdict[tFrom].has_key(tTo):
    #             tfdict[tFrom][tTo] = []
    #         tfdict[tFrom][tTo].append(t)




    
    for tfs in bag.read_messages("/tf"):
        for t in tfs[1].transforms:
            t.header.stamp -= start

            if (t.header.stamp < rospy.Duration(0)):
                rospy.logwarn('%s - %s [%f]' % (t.header.frame_id, t.child_frame_id, t.header.stamp.to_sec()))
                continue

            rostf.setTransform(t) #cache all tf info into messages to a transformer object

            # tFrom = t.header.frame_id
            # tTo   = t.child_frame_id
            # if not tfdict.has_key(tTo):
            #     tfdict[tTo] = {}
            # if not tfdict[tTo].has_key(tFrom):
            #     tfdict[tTo][tFrom] = []
            # tfdict[tTo][tFrom].append(t)



    # # Check
    # for f in tfdict.keys():
    #     fs = tfdict[f].keys()
    #     if (len(fs)>1):
    #         rospy.logerr("Warning: Frame [%s] has multiple parent frames: %s", f, fs)
    # for f in sorted(tfdict.keys()):
    #     for fs in sorted(tfdict[f].keys()):
    #         print fs, "->", f
    # print "___"


    # Print existing frames
    # get [(child_frame, parent_frame), ...] for all known transforms in bag
    framestr = [(k[k.rfind(' ')+1:],k[0:k.find(' ')]) for k in rostf.allFramesAsString().replace('Frame ','').split('.\n') if k]
    for parent, child in framestr:
        if parent != "NO_PARENT":
            print parent, "->", child


    # List of transforms we are interested in using
    FRAMES = [("/world", "/cf_gt"), ("/world", "/goal"), ("/cf_gt2d", "/goal")]  # from->to pairs

    # For TF we interpolate at equal intervals
    timelist = np.arange(0, duration, 1./hz)

    # container for all transforms
    frames_pq = {"time": timelist}
    for parent, child in FRAMES:
        # Get TF info at given HZ
        pq = []
        for ti in timelist:
            t = rospy.Time(secs=ti)
            if rostf.canTransform(parent, child, t):
                p, q = rostf.lookupTransform(parent, child, t)
                pq.append([p[0],p[1],p[2],q[0],q[1],q[2],q[3]])
            else:
                #rospy.logwarn("Could not get transform %s -> %s at time %fs", parent, child, t.to_sec())
                pq.append([0,0,0, 0,0,0,1])
        frames_pq[parent[1:].replace('/','_')+"__"+child[1:].replace('/','_')] = np.array(pq)  # matlab friendly


    # # PLot xyz
    # import matplotlib.pyplot as plt
    # # Create the plot
    # plt.plot(timelist, frames_pq["world-cf_gt"][:, 0:3]) # FRAME, time step, dimension
    # plt.plot(timelist, frames_pq["world-goal"][ :, 0:3]) # FRAME, time step, dimension
    # plt.show()

    # Collect all data
    data = {t: {} for t in topics if t != '/tf' and not t.endswith('image_raw') and not t.endswith('camera_info')}

    # Prepare all the dictionaries
    for t in data.keys():
        print t
        name, msg, t = bag.read_messages(topics=t).next()        
        data[name] = {slot: [] for slot in msg.__slots__ if slot != "header"}
        data[name]["time"] = []


    # print "USED DATA: \n",
    [k +": "+str(data[k].keys()) for k in data.keys()]


    # Fill all dictionaries
    for name, msg, t in bag.read_messages(topics=data.keys()):
        # They all have a time stamp
        data[name]["time"].append((t-start).to_sec())
        # Append data for each slot
        for slot in data[name].keys():
            if slot == "time":
                continue
            contents = msg.__getattribute__(slot)
            data[name][slot].append(contents)

    # Convert all to NP arrays
    mdata = {}
    for name in data.keys():
        mname = name[name.rfind('/')+1:]  # matlab friendly name
        mdata[mname] = {}
        for slot in data[name].keys():
            mdata[mname][slot] = np.array(data[name][slot])


    # add TF data
    mdata['tf'] = frames_pq

    # Save as MAT
    sio.savemat(bagfile[:-4]+'.mat', {'bag'+bagfile[:-4].replace('-','_'):mdata}, oned_as='column')

    return