Gazebo Callback not firing

Question

I am attempting to write a small library to interface Gazebo with Python (I tried using the pygazebo library and had no success). I am attempting to get the output from a camera and find April Tags and store the position data in a class variable using C++ code wrapped with SWIG to be used in Python. I had a standalone C++ application printing this data, but I have not been able to get it to work inside a class. From my testing the line sub = node->Subscribe(IMAGE_TOPIC, &april::callback, this); may be the problem. The code is included below.

gazeboApril.cpp

#include "gazeboApril.hpp"

april::april(void) {
    this->tag_size = (0.3 * (8.0 / 10.0)) / 2.0;
    apriltag_family_t *tf = tag36h11_create();
    this->td = apriltag_detector_create();
    apriltag_detector_add_family(this->td, tf);
    gazebo::client::setup();
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, &april::callback, this);
}

void april::callback(ConstImageStampedPtr &msg) {
    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::Mat gray;
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
    image_u8_t im = { .width = gray.cols,
        .height = gray.rows,
        .stride = gray.cols,
        .buf = gray.data
    };
    zarray_t *detections = apriltag_detector_detect(td, &im);
    apriltag_detection *det;
    this->id.resize(zarray_size(detections));
    this->d.resize(zarray_size(detections));
    this->theta.resize(zarray_size(detections));
    for (int i = 0; i < zarray_size(detections); i++) {
        zarray_get(detections, i, &det);
        this->id.at(i) = det->id;
        matd_t *pose = homography_to_pose(det->H, -1108.77, 1108.77, 1280 / 2, 720 / 2);
        this->d.at(i) = this->tag_size * sqrt(pow(MATD_EL(pose, 0, 3), 2) + pow(MATD_EL(pose, 2, 3), 2));
        this->theta.at(i) = atan2(MATD_EL(pose, 0, 3), MATD_EL(pose, 2, 3));
    }
    delete data;
}

void april::stop(void) {
    gazebo::client::shutdown();
}

gazeboApril.hpp

#include <vector>

#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>

#include "apriltag.h"
#include "tag36h11.h"
#include "common/homography.h"

#pragma once

#define IMAGE_TOPIC "/gazebo/default/pioneer3at/camera/link/camera/image"

class april {
public:
    april(void);
    void stop(void);
public:
    void callback(ConstImageStampedPtr &msg);
public:
    std::vector<int> id;
    std::vector<double> d;
    std::vector<double> theta;
protected:
    apriltag_detector_t *td;
    double tag_size;
};

gazeboApril.i

%module gazeboApril
%include "typemaps.i"
%include "std_vector.i"
namespace std {
    %template(IntVector) vector<int>;
    %template(DoubleVector) vector<double>;
};
%{
#include <Python.h>
#include "gazeboApril.hpp"
%}
%naturalvar april::id;
%naturalvar april::d;
%naturalvar april::theta;
%include "gazeboApril.hpp"

You will not get any meaningful answers if you do not provide some more information. What error do you get? Compile error or runtime error? Please provide as much information as possible. — Andreas, Oct 23 '17 at 15:06
I get no errors. It compiles and runs, but the callback function is never called. — Cameron D, Oct 23 '17 at 17:31
The question has been updated to include the SWIG interface file. — Cameron D, Oct 23 '17 at 19:34
If I remember correctly I moved away from a class structure which fixed my problem. I see that you added an answer. If that fixed the problem for you I will accept it as the answer. — Cameron D, Mar 24 '19 at 17:54

score 0 · Answer 1 · answered Mar 23 '19 at 17:19

You need to make the return value of Node::Subscribe a local variable and keep it.

You only stay subscribed to the topic until the the destructor of the SubscribePtr is called. In your example, the sub variable is local to the constructor so you unsubscribe as soon as the constructor is left.

Gazebo Callback not firing

1 Answers1