Instantiating messages from bagfile yields nullpointer

asked 2016-08-08 07:51:36 -0600

Robert Grönsfeld gravatar image

updated 2016-08-10 09:10:18 -0600

Hi, I have problems changing the code from this example so that it fits my needs. This is the scenario: I want to benchmark my code with some real world data. So I'm trying to read a previously recorded bagfile and feed it into my benchmarking code. I use a library that executes a given function n times and then tells me how long it took. My goal is to read a new sensor_msgs::PointCloud2 and some transformation data in each of the n iterations and perform some calculations on it. The example I changed used a BOOST_FOREACH loop to iterate through the whole bagfile. As I only want to do 100 iterations, I only have to read the first 100 point clouds from the bagfile. This is why I want to access the rosbag::View::iterator directly.

Now my problem is, that I always end up with a nullpointer when I instantiate a message via the iterator. This is how I implemented it:

BagReader.h:

class BagReader {
public:
    BagReader(std::string const &filename);
    ~BagReader();
    void addTopic(std::string const &name);
    void readNextMessage();

    template <typename MessageType>
    boost::shared_ptr<MessageType> getMessage() {
        if (!view) {
            view.reset(new rosbag::View(*bag, rosbag::TopicQuery(topics)));
            messageIterator = view->begin();
        }
        // The instantiation of the message will be a smart pointer.
        boost::shared_ptr<MessageType> ptr = messageIterator->instantiate<MessageType>();
        return ptr; //  >>> THIS IS NULL. IT SHOULD BE AN INSTANCE OF MessageType. <<<
    }

private:
    boost::scoped_ptr<rosbag::Bag> bag;
    boost::scoped_ptr<rosbag::View> view;
    rosbag::View::iterator messageIterator;
    std::vector<std::string> topics;
};

BagReader.cpp:

#include "BagReader.h"

BagReader::BagReader(std::string const &filename) : bag(new rosbag::Bag(filename)) {
}

BagReader::~BagReader() {
    bag->close();
}

void BagReader::addTopic(std::string const &name) {
    topics.push_back(name);
}

void BagReader::readNextMessage() {
    if (messageIterator != view->end())
        ++messageIterator;
    else
        messageIterator = view->begin();
}

Now I initialize my reader giving it the path to a bagfile and adding the two topics i want to read from:

std::string const &path = bagfilesDirectory + "/dice_detection.bag";
bagReader.reset(new BagReader(path));
bagReader->addTopic("/camera/depth_registered/points");
bagReader->addTopic("/tf");

Then I do the following to iterate through the messages of the bagfile:

sensor_msgs::PointCloud2ConstPtr cloud = bagReader->getMessage<sensor_msgs::PointCloud2>();
// Do something with the cloud...
bagReader->readNextMessage();
edit retag flag offensive close merge delete