0
$\begingroup$

I want to send a dummy PointCloud2 message to be visualized with RVIZ2, but when I try to subscribe to the pointCloud2 message, I received the following error :

[INFO] [1698326059.380456033] [rviz]: Message Filter dropping message: frame '625' at time 1698326054,377 for reason 'Unknown'

And I can't get rviz2 to show any frame, I populated every field of the pointcloud2 message.

The code to generate the pointCloud2 is the following :

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

using namespace std::chrono;

class TestPcloud2Pub : public rclcpp::Node
{
public:
    TestPcloud2Pub() : Node("test_pcloud2_pub")
    {
        mPcloudMsg.height = 5;
        mPcloudMsg.width = 3;
        mPcloudMsg.is_dense = true;
        mPcloudMsg.is_bigendian = true;
        mPcloudMsg.point_step = 4 * 3;
        mPcloudMsg.row_step = mPcloudMsg.width * 4 * 3;

        std::vector<sensor_msgs::msg::PointField> pointFields;

        sensor_msgs::msg::PointField pointFieldX;
        pointFieldX.name = 'x';
        pointFieldX.offset = 0;
        pointFieldX.datatype = 7; // float 32
        pointFieldX.count = 1;

        sensor_msgs::msg::PointField pointFieldY;
        pointFieldY.name = 'y';
        pointFieldY.offset = 4;
        pointFieldY.datatype = 7; // float 32
        pointFieldY.count = 1;

        sensor_msgs::msg::PointField pointFieldZ;
        pointFieldZ.name = 'z';
        pointFieldZ.offset = 8;
        pointFieldZ.datatype = 7; // float 32
        pointFieldZ.count = 1;

        pointFields.push_back(pointFieldX);
        pointFields.push_back(pointFieldY);
        pointFields.push_back(pointFieldZ);

        mPcloudMsg.fields = pointFields;

        mPublisher = this->create_publisher<sensor_msgs::msg::PointCloud2>("test_pcloud", 10);
        mTimer = this->create_wall_timer(500ms, std::bind(&TestPcloud2Pub::timer_callback, this));
    }

private:
    void timer_callback()
    {
        mPcloudMsg.header.frame_id = std::to_string(mFrameCounter);

        auto tp = system_clock::now();
        auto tpSec = time_point_cast<seconds>(tp);
        nanoseconds ns = tp - tpSec;

        uint32_t timeSec = static_cast<uint32_t>(tpSec.time_since_epoch().count());
        uint32_t timeNsec = static_cast<uint32_t>(ns.count());
        mPcloudMsg.header.stamp.sec = timeSec;
        mPcloudMsg.header.stamp.nanosec = timeNsec;

        mPcloudMsg.data.clear();
        // mPcloudMsg.data.resize(mPcloudMsg.height * mPcloudMsg.width * 4 * 3);

        for (int i = 0; i < mPcloudMsg.width; ++i)
        {
            for (int j = 0; j < mPcloudMsg.height; ++j)
            {
                float tempX = 9.5;
                uint8_t* x = reinterpret_cast<uint8_t*>(&tempX);
                float tempY = i - 1.;
                uint8_t* y = reinterpret_cast<uint8_t*>(&tempY);
                float tempZ = j;
                uint8_t* z = reinterpret_cast<uint8_t*>(&tempZ);

                mPcloudMsg.data.insert(mPcloudMsg.data.end(), x, x + 4);
                mPcloudMsg.data.insert(mPcloudMsg.data.end(), y, y + 4);
                mPcloudMsg.data.insert(mPcloudMsg.data.end(), z, z + 4);
            }
        }
        /*std::cout << "pcloud size : " << mPcloudMsg.data.size() << std::endl;
        std::cout << "Pcloud data : " << std::endl;
        for (int i = 0; i < mPcloudMsg.data.size(); ++i)
        {
            std::cout << std::hex << (int)mPcloudMsg.data[i];
            if (i % 4 == 0)
                std::cout << " ";
        }
        std::cout << std::endl;*/

        mFrameCounter++;
        mPublisher->publish(mPcloudMsg);
    }
    uint64_t mFrameCounter = 0;
    rclcpp::TimerBase::SharedPtr mTimer;
    sensor_msgs::msg::PointCloud2 mPcloudMsg;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mPublisher;
};

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

I displayed the content of the message with

ros2 topic echo /test_pcloud

and the content seems fine to me.

Does anyone have any clue on the subject? How do I do to debug it when the error outputed i 'Unknown' ?

Thansk for your help !

$\endgroup$

1 Answer 1

1
$\begingroup$

What is your fixed frame?

I see you are changing the frame_id every time. I am thinking that without proper connection between your pointcloud frame and rviz fixed frame, it is not able to visualize your message. Please try with a constant frame_id like "laser" and make sure you are viewing it in "laser" frame or another connected frame

$\endgroup$
1
  • $\begingroup$ Yes that was the problem ! by setting the frame_id to "Laser" I was able to view it in rviz, thanks ! $\endgroup$
    – Gregzz
    Oct 26, 2023 at 13:42

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.