Node publishes only half of expected frequency

edit

I'm writing a ROS2 C++ node for a camera. In the code we create a callback from the camera driver which then publishes the image. When we measure the topic frequency we only get half the actual callback frequency. If we set the camera to 10fps we get around 5fps in ROS and when we double the camera rate to 20fps we get around 10fps in ROS. This leads us to believe that we are not limited in processing. The topic frequency is measured with ros2 topic hz and for the callback measuring we use this:

void BufferReceived(void *callBackOwner, BGAPI2::Buffer *pBufferFilled)

{
    count++;
    // Pack the OpenCV image into the ROS image.
    sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
    // Read from buffer and convert to ros msg
    pub_->publish(std::move(msg));

    if (count == 60){
        count = 0;
        auto duration = std::chrono::system_clock::now() - start_time;

        auto fps = 60.0 / (duration.count());
        std::cout << "fps: " << fps * 1000000000 << std::endl;
        start_time = std::chrono::system_clock::now();
    }
}

We have written something similar in Python and there we had no problems for it to publish at the correct frequency. We work in ROS galactic with the Fast DDS middleware. I don't really have an idea about how I should start to debug this, because everything is working as expected with the exception of losing some frames.

This is how we create the publisher:

pub_ = this->create_publisher<sensor_msgs::msg::Image>("/output", rclcpp::SystemDefaultsQoS());

Whereby we modified the default QOS via the XML to be reliable and to have sufficiently big buffers.

<?xml version="1.0" encoding="UTF-8" ?>
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<transport_descriptors>
    <!-- Create a descriptor for the new transport -->
    <transport_descriptor>
        <transport_id>shm_transport</transport_id>
        <type>SHM</type>
        <!-- <sendBufferSize>100000000</sendBufferSize>
        <receiveBufferSize>100000000</receiveBufferSize> -->
        <segment_size>1000000000</segment_size>
        <maxMessageSize>10000000</maxMessageSize>
    </transport_descriptor>
</transport_descriptors>

<participant profile_name="SHMParticipant" is_default_profile="true">
    <rtps>
        <!-- Link the Transport Layer to the Participant -->
        <userTransports>
            <transport_id>shm_transport</transport_id>
        </userTransports>
    </rtps>
</participant>

<data_writer profile_name="default publisher profile" is_default_profile="true">
    <qos>
        <publishMode>
            <kind>SYNCHRONOUS</kind>
        </publishMode>
        <durability>
            <kind>VOLATILE</kind>
        </durability>
        <reliability>
            <kind>RELIABLE</kind>
        </reliability>
    </qos>
    <topic>
        <historyQos>
            <kind>KEEP_LAST</kind>
            <depth>20</depth>
        </historyQos>
    </topic>
    <historyMemoryPolicy>DYNAMIC</historyMemoryPolicy>
</data_writer>

<data_reader profile_name="default subscriber profile" is_default_profile="true">
    <qos>
        <durability>
            <kind>VOLATILE</kind>
        </durability>
        <reliability>
            <kind>RELIABLE</kind>
        </reliability>
    </qos>
    <historyMemoryPolicy>DYNAMIC</historyMemoryPolicy>
    <topic>
        <historyQos>
            <kind>KEEP_LAST</kind>
            <depth>20</depth>
        </historyQos>
    </topic>

</data_reader>

</profiles>