Ask Your Question
0

Testing node throws 'ros::serialization::StreamOverrunException' BufferOverrun

asked 2021-07-12 07:11:52 -0500

ignacio gravatar image

updated 2021-07-15 02:47:38 -0500

I am trying to have a pcl::PointCloud<pcl::PointXYZ> message being published in a test, so I can process that data and then publish it as a filtered point cloud.

Test header file

#pragma once

// point_cloud_filters
#include <point_cloud_filters/remove_box_filter_node.h>

#include "test_utils.h"

// Gtest
#include <gtest/gtest.h>

// ros
#include <ros/ros.h>

class RemoveBoxFilterPCLPointNodeTest : public ::testing::Test {
 protected:
  std::vector<geometry_msgs::Point32> points_;
  pcl::PointCloud<pcl::PointXYZ> received_msg_;
  ros::Publisher pcl_pub_pcl_point_;
  ros::Subscriber filtered_pcl_sub_point_;
  std::shared_ptr<ros::NodeHandle> nh_pcl_point;

  virtual void SetUp() {
    nh_pcl_point = std::make_shared<ros::NodeHandle>("~");
    pcl_pub_pcl_point_ =
        nh_pcl_point->advertise<pcl::PointCloud<pcl::PointXYZ>>(
            "pcl_point/point_cloud_in", 1, true);
    filtered_pcl_sub_point_ = nh_pcl_point->subscribe(
        "pcl_point/filtered_point_cloud", 10,
        &RemoveBoxFilterPCLPointNodeTest::filteredPCLPointCB, this);
    createTestPoints();
  }
  virtual void TearDown() {}

  void filteredPCLPointCB(
      const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& pc_msg) {
    ROS_WARN("AHHHHHHHHHHHHH");
    ROS_WARN_STREAM(*pc_msg);
    received_msg_ = *pc_msg;
  }

  void createTestPoints() {
    points_.clear();
    points_.reserve(4);
    geometry_msgs::Point32 point;

    // Out
    point.x = 2;
    point.y = 0;
    point.z = 0;
    points_.push_back(point);
    point.x = -2;
    point.y = 0;
    point.z = 0;
    points_.push_back(point);

    // In X
    point.x = 0.9;
    point.y = 0;
    point.z = 0;
    points_.push_back(point);
    point.x = -0.9;
    point.y = 0;
    point.z = 0;
    points_.push_back(point);

    // In Y
    point.x = 0;
    point.y = -0.4;
    point.z = 0;
    points_.push_back(point);

    // In Z
    point.x = 0;
    point.y = 0;
    point.z = -0.05;
    points_.push_back(point);
  }
};

TEST_F(RemoveBoxFilterPCLPointNodeTest, OperatorPointCloud1) {
  ros::Duration(1).sleep();
  publishPCLByPoints(points_, pcl_pub_pcl_point_);
  ros::Duration(1).sleep();

  ROS_WARN_STREAM(received_msg_);
  ASSERT_EQ(pcl_pub_pcl_point_.getNumSubscribers(), 1);
  ASSERT_EQ(filtered_pcl_sub_point_.getNumPublishers(), 1);
  ASSERT_EQ(received_msg_.points.size(), 2);
  ASSERT_FLOAT_EQ(received_msg_.points[0].x, 2.0);
  ASSERT_FLOAT_EQ(received_msg_.points[0].y, 0);
  ASSERT_FLOAT_EQ(received_msg_.points[0].z, 0);
  ASSERT_FLOAT_EQ(received_msg_.points[1].x, -2.0);
  ASSERT_FLOAT_EQ(received_msg_.points[2].y, 0);
  ASSERT_FLOAT_EQ(received_msg_.points[3].z, 0);
}

Test main file

#include "../include/remove_box_filter_pcl_point_node_test.h"

// Gtest
#include <gtest/gtest.h>

// std
#include <fstream>
#include <memory>
#include <sstream>
#include <string>

// ros
#include <ros/ros.h>

// Eigen
#include <Eigen/Core>

int main(int argc, char** argv) {
  ros::init(argc, argv, "point_cloud_node_test");
  ros::AsyncSpinner spinner(0);
  spinner.start();
  testing::InitGoogleTest(&argc, argv);
  auto result = RUN_ALL_TESTS();
  spinner.stop();
  return result;
}

Message generator:

void publishPCLByPoints(const std::vector<geometry_msgs::Point32>& points,
                        const ros::Publisher& pub) {
  pcl::PointCloud<pcl::PointXYZ> pc;
  pc.reserve(points.size());
  for (const auto point32 : points) {
    pcl::PointXYZ p;
    p.x = point32.x;
    p.y = point32.y;
    p.z = point32.z;
    pc.push_back(p);
  }
  pcl_conversions::toPCL(ros::Time::now(), pc.header.stamp);
  pc.header.frame_id = "origin_frame";
  pub.publish(pc);
  ROS_WARN_STREAM(pc);
}

Class handling the point cloud:

template <class T>
void RemoveBoxFilterNode<T>::removeBoxCallback(
    const boost::shared_ptr<const T>& pc_msg) {
  T new_pc = *pc_msg;
  filter_(new_pc);
  remove_box_pcl_pub.publish(new_pc);
  filter_.vis(new_pc.header.frame_id);
}

template <class T>
RemoveBoxFilterNode<T>::RemoveBoxFilterNode(
    const std::shared_ptr<ros::NodeHandle>& nh)
    : nh_ptr_(nh),
      frame_name_(),
      filter_(nh),
      filter_sub_(nh_ptr_->subscribe("point_cloud_in", 10,
                                     &RemoveBoxFilterNode<T>::removeBoxCallback,
                                     this)),
      remove_box_pcl_pub(nh_ptr_->advertise<T>("filtered_point_cloud", 10)) {}

template class RemoveBoxFilterNode<sensor_msgs::PointCloud2>;
template class RemoveBoxFilterNode<sensor_msgs::PointCloud>;
template class RemoveBoxFilterNode<pcl::PointCloud<pcl::PointXYZ>>;

What am I probably missing here?

edit retag flag offensive close merge delete

Comments

I have no idea whether it matters, but where does nh_pcl_point go after SetUp() exits?

gvdhoorn gravatar image gvdhoorn  ( 2021-07-12 07:52:09 -0500 )edit

nowhere. After the SetUp() I use the fixture's subscriber and publisher to check the information from my node

ignacio gravatar image ignacio  ( 2021-07-12 08:51:23 -0500 )edit

Officially you're supposed to hold on to your NodeHandle instance. Weird things may happen if you don't.

gvdhoorn gravatar image gvdhoorn  ( 2021-07-12 08:53:26 -0500 )edit

I tried adding the NodeHandle as a class attribute. It still doesn't work, unfortunately

ignacio gravatar image ignacio  ( 2021-07-13 07:01:07 -0500 )edit

Is your node using 100% cpu? How many points per second are you trying to process? Are you using an asynchronous spinner?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-07-13 16:16:03 -0500 )edit

I am running that in a test scenario. For that I am processing 5 points only and using ros::AsyncSpinner spinner(0);. I don't know how to check if I am using GPU/CPU, but I would guess that I am using 100% cpu

ignacio gravatar image ignacio  ( 2021-07-14 09:00:13 -0500 )edit

Processing five PointXYZ per second is not going to stress the system. Do you get the error for every message published, or just occasionally? I suspect you are doing something that is not thread-safe. Does the error go away if you use a single spinner?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-07-14 12:25:32 -0500 )edit

Is there any reason for that to happen exclusively with pcl::PointCloud<pcl::PointXYZ>? Because the test works with both sensor_msgs::PointCloud2 and sensor_msgs::PointCloud

ignacio gravatar image ignacio  ( 2021-07-15 02:44:29 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-07-16 06:09:35 -0500

Mike Scheutzow gravatar image

updated 2021-07-16 06:10:56 -0500

I think you are getting the error message because you are creating a new publisher to an existing topic, but using a different message type. ROS is a distributed system, and was designed to publish only one message type to a topic.

The code you've shown us is not safe to run using an AsyncSpinner with multiple threads. My advice is to look into the rules for multi-threaded programming.

edit flag offensive delete link more

Comments

In my case, the nodes are created with private namespaces and with different naming, so the topics should be different

ignacio gravatar image ignacio  ( 2021-07-16 08:23:51 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-07-12 07:11:52 -0500

Seen: 22 times

Last updated: Jul 16