Adding topological information to ppl logging

[cdondrup]

Closes #113

Needs testing

[marc-hanheide]

@nilsbore if you've got a minute, could you confirm this working, please?

[nilsbore]

@RaresAmbrus is the one with more time on his hands right now, can you have a look?

[cdondrup]

Just quickly tried it on linda:

{
    "_id" : ObjectId("55240198eaf45049451b1be6"),
    "_meta" : {
        "stored_type" : "bayes_people_tracker_logging/PeopleTrackerLogging",
        "inserted_at" : ISODate("2015-04-07T16:11:04.351Z"),
        "stored_class" : "bayes_people_tracker_logging.msg._PeopleTrackerLogging.PeopleTrackerLogging",
        "inserted_by" : "/save_people_locations",
        "people" : "tracks"
    },
    "people" : [ 
        {
            "header" : {
                "stamp" : {
                    "secs" : 1.42842e+09,
                    "nsecs" : 3.05034e+08
                },
                "frame_id" : "/map",
                "seq" : 0
            },
            "pose" : {
                "position" : {
                    "y" : 3.2007473214712174,
                    "x" : -1.5609576568858543,
                    "z" : 0.0000000000000000
                },
                "orientation" : {
                    "y" : 0.0000000000000000,
                    "x" : 0.0000000000000000,
                    "z" : -0.4311613773019872,
                    "w" : 0.9022748288204949
                }
            }
        }
    ],
    "robot" : {
        "position" : {
            "y" : -0.1846698991593005,
            "x" : -1.4362595879770197,
            "z" : 0.0000000000000000
        },
        "orientation" : {
            "y" : -0.0000000000000000,
            "x" : -0.0000000000000000,
            "z" : 0.7346206715253480,
            "w" : 0.6784780534163554
        }
    },
    "header" : {
        "stamp" : {
            "secs" : 1.42842e+09,
            "nsecs" : 3.05034e+08
        },
        "frame_id" : "/map",
        "seq" : 937
    },
    "current_node" : "WayPoint8",
    "uuids" : [ 
        "d676dd1f-6174-522b-a451-a30d417c6b8a"
    ],
    "people_tracker" : {
        "distances" : [ 
            3.3877129993745423
        ],
        "header" : {
            "stamp" : {
                "secs" : 1.42842e+09,
                "nsecs" : 3.05034e+08
            },
            "frame_id" : "/map",
            "seq" : 937
        },
        "uuids" : [ 
            "d676dd1f-6174-522b-a451-a30d417c6b8a"
        ],
        "angles" : [ 
            -0.0426012811329153
        ],
        "poses" : [ 
            {
                "position" : {
                    "y" : 3.2007473214712174,
                    "x" : -1.5609576568858543,
                    "z" : 0.0000000000000000
                },
                "orientation" : {
                    "y" : 0.0000000000000000,
                    "x" : 0.0000000000000000,
                    "z" : -0.4311613773019872,
                    "w" : 0.9022748288204949
                }
            }
        ],
        "min_distance_angle" : -0.0426012811329153,
        "min_distance" : 3.3877129993745423
    },
    "closest_node" : "WayPoint8",
    "current_edge" : "none"
}

Seems to work fine for me.

[marc-hanheide]

Looks ok to me.