凉亭回调不触发

Gazebo Callback not firing

本文关键字：回调 凉亭      更新时间：2023-10-16

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

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

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

凉亭四月

%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"

您需要将 Node::Subscribe 的返回值设为局部变量并保留它。

只有在调用SubscribePtr的析构函数之前，您才会保持对主题的订阅状态。在您的示例中，sub 变量是构造函数的本地变量，因此一旦构造函数离开，您就可以取消订阅。

相关文章：