rviz2 performance issues with CUBE_LIST marker
Hi,
I just ported some 3d map visualization to ros2, foxy. I publish a marker in the CUBE_LIST mode. The number of cubes in the marker is between 5000-30000. This was never a problem in rviz1 but with foxy I can barely move the camera in rviz2, it is about 1 fps. When I disable the topic, everything is smooth again. The publisher is publishing once every few seconds so that shouldn't be an issue.
I tried to debug a little:
- rviz2 debug output says GPU is used and works fine.
- Sending a delete_all marker before every update makes no difference
- compile rviz2 from source instead of the -> No difference except I noticed that it is not built as "Release" per default. This seems strange but the release build does only make a small difference.
- I fired up the ros1 bridge and bridged the topic to display it in rviz1 -> everything is super smooth.
- Played around with the QOS settings -> no difference
The marker basically has the following form plus a couple of thousand points and colors in the respective fields:
visualization_msgs::msg::Marker marker;
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.color.a = 1.0;
marker.pose.orientation.w = 1.0;
marker.frame_locked = true;
marker.header.frame_id = vdb_frame_id_;
marker.header.stamp = this->get_clock()->now();
marker.action = visualization_msgs::msg::Marker::ADD;
Am I doing something wrong or is this more of a general issue? Thanks!
EDIT
As requested here is a MWE which reproduces the issue. It is simply the code from the ros2 simple publisher example with a marker publisher. If you run a ros2 run ros1_bridge dynamic_bridge --bridge-all-2to1-topics
and visualize the cube list in rviz1 instead, everything is smooth. In rviz2 it is super slow.
#include "rclcpp/rclcpp.hpp"
#include <visualization_msgs/msg/marker.hpp>
using namespace std::chrono_literals;
static constexpr int pt_count = 30; // 27000 points
class CubePublisher : public rclcpp::Node
{
public:
CubePublisher() : Node("cube_publisher")
{
publisher_ = this->create_publisher<visualization_msgs::msg::Marker>("/cubes", 10);
timer_ = this->create_wall_timer(3000ms, std::bind(&CubePublisher::timer_callback, this));
}
private:
void timer_callback()
{
visualization_msgs::msg::Marker marker;
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
marker.scale.x = 0.25;
marker.scale.y = 0.25;
marker.scale.z = 0.25;
marker.color.a = 1.0;
marker.pose.orientation.w = 1.0;
marker.frame_locked = true;
geometry_msgs::msg::Point cube_center;
for (int i = 0; i < pt_count; ++i)
{
for (int j = 0; j < pt_count; ++j)
{
for (int k = 0; k < pt_count; ++k)
{
cube_center.x = i / 2.0;
cube_center.y = j / 2.0;
cube_center.z = k / 2.0;
marker.points.push_back(cube_center);
}
}
}
marker.header.frame_id = "map";
marker.header.stamp = this->get_clock()->now();
marker.action = visualization_msgs::msg::Marker::ADD;
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing " << marker.points.size() << " cubes.");
publisher_->publish(marker);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr publisher_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<CubePublisher>());
rclcpp::shutdown();
return 0;
}
Can you please post your full code that you use to generate the marker messages?
My full code is too complex to show but I created a MWE derived from the basic ROS2 publisher example. I updated the original post.