Subscribe to Gazebo Plugin Topic

edit

Hello,

you should: - create a gazebo::transport::Node to subscribe the Gazebo topic - create a ROS 2 node and create the gith publisher. - In the callback you need to fill the ROS 2 message with the Gazebo IMU message.

Here you can a find a snippet of how to implement this idea.

gazebo_ros::Node::SharedPtr ros_node;

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub;

gazebo::transport::NodePtr gz_node;

// Function is called everytime a message is received.
void cb(ConstImuPtr &_msg)
{
  gazebo_msgs::msg::IMU imu_msg;

 // fill imu_msg message based on _msg

  imu_pub->publish(imu_msg);

}

/////////////////////////////////////////////////
int main(int argc, char ** argv)
{
  // Load gazebo
  gazebo::client::setup(argc, argv);

  if (!rclcpp::is_initialized()) {
    rclcpp::init(argc, argv);
    ros_node = gazebo_ros::Node::Get();
  } else {
    ros_node = gazebo_ros::Node::Get();
  }

  imu_pub = ros_node->create_publisher<sensor_msgs::msg::Imu>("/turtlebot/rack/imu_sensor/imu", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)));

  // Gazebo transport
  gz_node = gazebo::transport::NodePtr(new gazebo::transport::Node());
  gz_node->Init();

  // Listen to Gazebo world_stats topic
  gazebo::transport::SubscriberPtr sub = gz_node->Subscribe("~/turtlebot/rack/imu_sensor/imu", cb);

  while (true)
    gazebo::common::Time::MSleep(10);

  // Make sure to shut everything down.
  gazebo::client::shutdown();
}