3

我正在尝试编写一个小库来将 Gazebo 与 Python 接口（我尝试使用 pygazebo 库但没有成功）。我正在尝试从相机获取输出并找到 April Tags 并将位置数据存储在使用 SWIG 包装的 C++ 代码的类变量中，以便在 Python 中使用。我有一个独立的 C++ 应用程序打印这些数据，但我无法让它在一个类中工作。从我的测试来看，这条线sub = node->Subscribe(IMAGE_TOPIC, &april::callback, this);可能是问题所在。代码包含在下面。

凉亭April.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();
}

凉亭April.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;
};

凉亭April.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"
4

1 回答 1

0

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.

于 2019-03-23T17:19:50.250 回答