Not receiving messages in ros2 while subscribing to topic from Gazebo 11 plugin demo world
Problem
Starting a demo world in Gazebo 11 by using
gazebo --verbose /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_depth_camera_demo.world
causes all topics to appear as expected when checking with ros2 command line tools. Even when echoing the topics I can see the messages containing actual, seemingly plausible data.
However, when using a custom written script (C++, see below) to subscribe to the topics, only some of the topics published by the demo worlds are actually received. The subscription process works correctly, at least the subscriber count of each topic increases when starting the script. But receiving a message/invoking the callback function works only on few topics.
Specifically the topics /demo_cam/mycamera/depth_cam_info_demo
and /demo_cam/mycamera/raw_cam_info_demo
, both of type sensor_msgs/msg/CameraInfo
, show different behaviour. Subscription to both works, but only ever the depth_cam.. topic triggers callbacks (regardless of which subscriber in the code is set to it, even both at the same time work).
I encountered this problem while running a custom .sdf model that publishes a pointcloud2 by using the libgazebo_ros_ray_sensor.so plugin. Using the cli tool the messages appear to be plausible and containing data. But when trying to visualize it in rviz2, it does not show, but also does not throw any errors (neither tf nor topic errors). Triggering the callback for this topic in a custom script does not work either.
Is it likely that these discrepancies between topics of the same type are caused by configurations on my system? Would Gazebo 11 from source be a better option? Any ideas how I could investigate this problem further, since I do not know if it is related to ros2, gazebo, or some middle ware communication? Thanks for any helpful tips.
Edit: Apparently another user experiences same symptoms with /imu messages published by a gazebo model and asked a question over at ros forum. But I am not sure whether these issues are caused by ros or gazebo.
Setup
- Ubuntu 20.04
- ros2 foxy installed as package
ros-foxy-desktop
- gazebo 11 installed as package
ros-foxy-gazebo-ros-pkgs
- script to test whether messages are received (compiled in a colcon workspace):
```
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float32.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "gazebo_msgs/msg/contacts_state.hpp"
/*
working topics so far:
/odom - odometry (ackerman, diff drive)
/distance - float32 (ackerman)
/depth_cam_info_demo - sensor/CameraInfo (depth cam demo)
/points_demo - PointCloud2 (depth cam demo)
not working topics:
/bumper_demo - gazebo_msgs/contactsstate (bumper demo)
/pointcloud2 - pointcloud2 (sensor in sdf model)
/camera_info - sensor/CameraInfo (cam demo)
/image_raw - sensor/Image (cam demo)
/raw_cam_info_demo - sensor/CameraInfo (depth cam demo)
/image_demo - sensor/Image (depth cam demo)
/depth_demo - sensor/Image (depth cam demo)
*/
typedef sensor_msgs::msg::CameraInfo custom_type_one;
typedef sensor_msgs::msg::CameraInfo custom_type_two;
const std::string topic_one = "demo_cam/mycamera/depth_cam_info_demo";
const std::string topic_two = "demo_cam/mycamera/raw_cam_info_demo";
using std::placeholders::_1;
class ReBroadcaster : public rclcpp::Node
{
public:
ReBroadcaster() : Node("rebroadcaster"), count_ ...
I have not been able to get a depth camera sensor working in Gazebo 11 either. Even using the Gazebo provided world and no ROS plugins. I do have a standard camera and laser scanner working, so I think something is wrong with the depth camera in the current version.
~$ gazebo --verbose ./depth_camera.world
Gazebo multi-robot simulator, version 11.2.0
[Wrn] [msgs.cc:1842] Conversion of sensor type[depth] not supported.
I am experiencing very similar behavior, no visualisation in rviz2, for anything.