ROS2 CALLBACK DONT TRIGGRED IN GAZEBO PLUGIN

asked 2023-01-09 01:32:35 -0500

omer_1903 gravatar image

wrote a simple model plugin in GAZEBO-11. This plugin subscribe a "topic". The topic named "topic" is publishing a float data. I use ROS2 example publisher codes. When I launch publisher and gazebo, model plugin's callback sometimes don't triggred and I write to terminal "ros2 topic echo /topic", callback start to work. Sometimes I launch publisher and gazebo-11 callback start normally but after 1-2 minutes callback function stopped,gazebo plugin continue to work. I think the problem is caused by the model plugin because when the ros2 subscriber code runs, the callback there never stops. I add my codes as image.

ModelPlugin.h

#ifndef PLUGIN_DENEME
#define PLUGIN_DENEME
 #include <gazebo/gazebo.hh>
 #include "std_msgs/msg/float32.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "gazebo/physics/physics.hh"
 #include "gazebo/gazebo.hh"
#include "gazebo/common/Events.hh"
#include "gazebo_ros/node.hpp"
 namespace gazebo {

  class DenemePlugin : public gazebo::ModelPlugin {

    public: 


    DenemePlugin();

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


    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr float_veri_subs;
    void FloatVeriCallback(std_msgs::msg::Float32 x);

    //gazebo-ros node
    gazebo_ros::Node::SharedPtr node_;


    gazebo::physics::ModelPtr model_;
    gazebo::physics::WorldPtr world_;


     gazebo::event::ConnectionPtr update_connection_;

       };

   }

  #endif

ModelPlugin.cpp

 #include "plugin_deneme.h"
 int i = 0;
namespace gazebo
{

DenemePlugin::DenemePlugin()
{
    printf("const");
}

void DenemePlugin::FloatVeriCallback(std_msgs::msg::Float32 msg)
  {
    RCLCPP_INFO(node_->get_logger(), " PLUGIN  CALLBACK %f \n:", msg.data);
   }

void DenemePlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 {
    node_ = gazebo_ros::Node::Get(sdf);
    RCLCPP_INFO(node_->get_logger(), "  LOAD \n:");
    printf("load\n");

    // Get model and world references
    model_ = model;
    world_ = model_->GetWorld();

    float_veri_subs = node_->create_subscription<std_msgs::msg::Float32>("topic", 10,
            std::bind(&DenemePlugin::FloatVeriCallback, this, std::placeholders::_1));

    // Hook into simulation update loop
    update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
            std::bind(&DenemePlugin::Update, this));
  }

 void DenemePlugin::Update()
  {
    //RCLCPP_INFO(node_->get_logger(), "  UPDATE %f \n:", i++);
  }

 GZ_REGISTER_MODEL_PLUGIN(DenemePlugin)
}

Publisher.cpp

enter code here

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float32.hpp"
using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
 {
     public:
         float i=0;
        MinimalPublisher()
             : Node("minimal_publisher"), count_(0)
         {
              publisher_ = this->create_publisher<std_msgs::msg::Float32>("topic", 10);
              timer_ = this->create_wall_timer(
             500ms, std::bind(&MinimalPublisher::timer_callback, this));
          }

         private:
        void timer_callback()
          {
            std_msgs::msg::Float32 veri;
            veri.data=i++;
            RCLCPP_INFO(this->get_logger(), "Publishing: '%f'",veri.data);
            publisher_->publish(veri);
          }
          rclcpp::TimerBase::SharedPtr timer_;
         rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr publisher_;
        size_t count_;
   };

 int main(int argc, char * argv[]) 
 {
      rclcpp::init(argc, argv);
      rclcpp::spin(std::make_shared<MinimalPublisher>());
      rclcpp::shutdown();
     return 0;
   }

Operating System:

Ubuntu 22.04

Version or commit hash:

GAZEBO-11, ROS2 HUMBLE

DDS implementation:

-Fast RTPS

Expected behavior

When the model plugin code starts to run, the callback function in the plugin works as long as the publisher publishes data.

Actual behavior

When the model plugin code starts running, the callback function doesn't work at first. But when the ros2 topic echo command is entered from the terminal ,the callback function starts to work . Sometimes it works properly but after a few minutes the callback is not triggered.

edit retag flag offensive close merge delete

Comments

Cross-post of ros2/ros2#1366.

gvdhoorn gravatar image gvdhoorn  ( 2023-01-09 01:36:34 -0500 )edit