Pointcloud conversion slows down simulation
After spawning a model in rviz, ROS Time (indicator on the bottom left) runs smoothly until I run a pointCloud conversion node. Then, ROS Time seems to update based on the update_rate gazebo tag of the sensor that returns the PointCloud I'm trying to convert to PointCloud2. This is the code I'm using for the conversion node:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <ros/publisher.h>
ros::Publisher pointcloud2Publisher;
void p2p2Converter(const sensor_msgs::PointCloud& msg) {
sensor_msgs::PointCloud2 pointcloud2;
if (sensor_msgs::convertPointCloudToPointCloud2(msg,pointcloud2))
pointcloud2Publisher.publish(pointcloud2);
else
ROS_INFO("Failed to publish PointCloud2");
}
int main(int argc, char **argv) {
ros::init(argc, argv, "p2p2_node");
ros::NodeHandle n;
pointcloud2Publisher = n.advertise<sensor_msgs::PointCloud2>("/lidar/pointcloud2", 1);
ros::Subscriber pointcloudListener = n.subscribe("/lidar/pointcloud", 1, p2p2Converter);
ros::spin();
return 0;
}
Here's my CMakeLists.txt:
cmake_minimum_required(VERSION 2.8.3)
project(turtle_gazebo)
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
)
catkin_package(
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(p2p2_node src/p2p2.cpp)
target_link_libraries(p2p2_node
${catkin_LIBRARIES}
)
EDIT: I cloned the following point_cloud_converter pkg but the issue remains.
What do you actually mean by "slows down simulation"? You can try echoing the topics to see if the timestamps make sense.
Sorry for not clarifying but by slow-down I mean that every second in real life is the equivalent of 1/update_rate of the sensor. When not running the conversion node, ROS real time factor seems to be almost 1 (meaning that 1 second in real life is 1 second in ROS). After running
rostopic echo /lidar/pointcloud2
androstopic echo /lidar/pointcloud
, it seems that ROS moves to the next simulation step everytime a PointCloud is published on the /lidar/pointcloud topic