Gazebo | Ignition | Community
Ask Your Question
0

ROS2 Subscriber not triggering callback when more than 3 subscribers in plugin

asked 2022-09-20 06:19:28 -0500

Hello !

I have been writing a plugin to control 4 joints on my model. Each joint command is obtained from a ros2 topic through a subscriber. Everything works as expected when there is only 3 subscribers. However, when I setup all 4 subscribers, the callbacks are not triggered.

Here is a minimal example (ROS2 foxy and Gazebo 11):

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo_ros/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>


namespace gazebo_plugins {

class TestPlugin : public gazebo::ModelPlugin {
public:
    TestPlugin() = default;


    void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {

        // Store the model pointer for convenience.
        this->model_ = model;

        // Initialize ROS node
        this->node_ = gazebo_ros::Node::Get(sdf);

        // Get QoS profiles
        const gazebo_ros::QoS &qos = this->node_->get_qos();

        // Create a topic name
        std::string topic_1 = "topic_1";
        std::string topic_2 = "topic_2";
        std::string topic_3 = "topic_3";
        std::string topic_4 = "topic_4";

        // Create subscribers
        this->topic_1_subscriber_ = this->node_->create_subscription<std_msgs::msg::Float32>(
                topic_1,
                qos.get_subscription_qos(topic_1, rclcpp::QoS(1)),
                std::bind(&TestPlugin::OnTopic1CmdMsg, this, std::placeholders::_1));
        this->topic_2_subscriber_ = this->node_->create_subscription<std_msgs::msg::Float32>(
                topic_2,
                qos.get_subscription_qos(topic_2, rclcpp::QoS(1)),
                std::bind(&TestPlugin::OnTopic2CmdMsg, this, std::placeholders::_1));
        this->topic_3_subscriber_ = this->node_->create_subscription<std_msgs::msg::Float32>(
                topic_3,
                qos.get_subscription_qos(topic_3, rclcpp::QoS(1)),
                std::bind(&TestPlugin::OnTopic3CmdMsg, this, std::placeholders::_1));
        this->topic_4_subscriber_ = this->node_->create_subscription<std_msgs::msg::Float32>(
                topic_4,
                qos.get_subscription_qos(topic_4, rclcpp::QoS(1)),
                std::bind(&TestPlugin::OnTopic4CmdMsg, this, std::placeholders::_1));

        // Listen to the update event. This event is broadcast every
        // simulation iteration.
        this->updateConnection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
                std::bind(&TestPlugin::OnUpdate, this));

        RCLCPP_INFO(this->node_->get_logger(), "Plugin loaded successfully.");
    }

    void OnUpdate() {

        RCLCPP_INFO_THROTTLE(this->node_->get_logger(), *this->node_->get_clock(), 1000, "Update");
    }

    void OnTopic1CmdMsg(const std_msgs::msg::Float32::SharedPtr msg) {
        RCLCPP_INFO_THROTTLE(this->node_->get_logger(), *this->node_->get_clock(), 1000, "Received topic 1 : %f", msg->data);
    }

    void OnTopic2CmdMsg(const std_msgs::msg::Float32::SharedPtr msg) {
        RCLCPP_INFO_THROTTLE(this->node_->get_logger(), *this->node_->get_clock(), 1000, "Received topic 2 : %f", msg->data);

    }

    void OnTopic3CmdMsg(const std_msgs::msg::Float32::SharedPtr msg) {
        RCLCPP_INFO_THROTTLE(this->node_->get_logger(), *this->node_->get_clock(), 1000, "Received topic 3 : %f", msg->data);

    }

    void OnTopic4CmdMsg(const std_msgs::msg::Float32::SharedPtr msg) {
        RCLCPP_INFO_THROTTLE(this->node_->get_logger(), *this->node_->get_clock(), 1000, "Received topic 4 : %f", msg->data);

    }

private:
    // Pointer to the update event connection
    gazebo::event::ConnectionPtr updateConnection_;

    gazebo_ros::Node::SharedPtr node_;
    gazebo::physics::ModelPtr model_;

    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr topic_1_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr topic_2_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr topic_3_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr topic_4_subscriber_;
};
    // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
    GZ_REGISTER_MODEL_PLUGIN(TestPlugin)
}

This code does not trigger the callbacks, however, by simply commenting any one of the subscriber definition, the callbacks are triggered.

I find this very puzzling... Any help would be most welcome, thank you !

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-09-21 02:45:13 -0500

After some testing I have found that I am able to use 4 subscribers if I launch Gazebo 11 with this command :

gazebo -s libgazebo_ros_factory.so

However, if I use the launch file from the gazebo_ros package (ROS foxy), the subscribers do not work all together. I modified my launch file to take this into account and it works now.

I am still interested in knowing why the gazebo launch file creates this problem, if anyone has more information...

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2022-09-20 06:19:28 -0500

Seen: 7 times

Last updated: Sep 20