Skip to content

Use point cloud transport to allow compressed point clouds #3424

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: ros2-master
Choose a base branch
from
+20 −13
Open

Use point cloud transport to allow compressed point clouds #3424

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions realsense2_camera/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,7 @@ find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(diagnostic_updater REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core)
find_package(point_cloud_transport REQUIRED)

find_package(realsense2 2.56)
if (BUILD_ACCELERATE_GPU_WITH_GLSL)
Expand Down Expand Up @@ -263,6 +264,7 @@ set(dependencies
tf2
tf2_ros
diagnostic_updater
point_cloud_transport
)

set (targets
Expand All @@ -278,6 +280,7 @@ set (targets
tf2::tf2
tf2_ros::tf2_ros
diagnostic_updater::diagnostic_updater
point_cloud_transport::point_cloud_transport
)

if (BUILD_ACCELERATE_GPU_WITH_GLSL)
Expand All @@ -293,7 +296,7 @@ if(USE_LIFECYCLE_NODE)
find_package(rclcpp_lifecycle REQUIRED)
find_package(lifecycle_msgs REQUIRED)
list(APPEND dependencies rclcpp_lifecycle lifecycle_msgs)
list(APPEND targets
list(APPEND targets
rclcpp_lifecycle::rclcpp_lifecycle
lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_cpp)
add_definitions(-DUSE_LIFECYCLE_NODE)
Expand Down Expand Up @@ -342,13 +345,13 @@ install(
)

# Install launch files
install(DIRECTORY
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)

# Install example files
install(DIRECTORY
install(DIRECTORY
examples
DESTINATION share/${PROJECT_NAME}
)
Expand Down
9 changes: 6 additions & 3 deletions realsense2_camera/include/pointcloud_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,11 @@
#include <string>
#include <memory>
#include <librealsense2/rs.hpp>
#include <point_cloud_transport/point_cloud_transport.hpp>
#include <point_cloud_transport/publisher.hpp>
#include <ros_sensor.h>
#include <sensor_params.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <ros_sensor.h>
#include "named_filter.h"

namespace realsense2_camera
Expand All @@ -28,7 +30,7 @@ namespace realsense2_camera
{
public:
PointcloudFilter(std::shared_ptr<rs2::filter> filter, RosNodeBase& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);

void setPublisher();
void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id);

Expand All @@ -41,7 +43,8 @@ namespace realsense2_camera
bool _allow_no_texture_points;
bool _ordered_pc;
std::mutex _mutex_publisher;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pointcloud_publisher;
std::shared_ptr<point_cloud_transport::PointCloudTransport> _pct;
std::shared_ptr<point_cloud_transport::Publisher> _pointcloud_publisher;
std::string _pointcloud_qos;
};
}
7 changes: 4 additions & 3 deletions realsense2_camera/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,12 @@
<description>RealSense camera package allowing access to Intel D400 3D cameras</description>
<maintainer email="[email protected]">LibRealSense ROS Team</maintainer>
<license>Apache License 2.0</license>

<url type="website">http://www.ros.org/wiki/RealSense</url>
<url type="bugtracker">https://github.com/intel-ros/realsense/issues</url>

<author email="[email protected]">LibRealSense ROS Team</author>

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>eigen</depend>
<depend>builtin_interfaces</depend>
Expand All @@ -34,6 +34,7 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>diagnostic_updater</depend>
<depend>point_cloud_transport</depend>
<test_depend condition="$ROS_DISTRO != foxy">ament_cmake_gtest</test_depend>
<test_depend condition="$ROS_DISTRO != foxy">launch_testing</test_depend>
<test_depend condition="$ROS_DISTRO != foxy">ament_cmake_pytest</test_depend>
Expand All @@ -45,7 +46,7 @@
<test_depend condition="$ROS_DISTRO != foxy">python3-requests</test_depend>
<test_depend condition="$ROS_DISTRO != foxy">tf2_ros_py</test_depend>
<test_depend condition="$ROS_DISTRO != foxy">ros2topic</test_depend>

<exec_depend>launch_ros</exec_depend>
<build_depend>ros_environment</build_depend>

Expand Down
8 changes: 4 additions & 4 deletions realsense2_camera/src/pointcloud_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,9 @@ void PointcloudFilter::setPublisher()
std::lock_guard<std::mutex> lock_guard(_mutex_publisher);
if ((_is_enabled) && (!_pointcloud_publisher))
{
_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/depth/color/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)),
qos_string_to_qos(_pointcloud_qos)));
_pct = std::make_shared<point_cloud_transport::PointCloudTransport>(_node.shared_from_this());
_pointcloud_publisher = std::make_shared<point_cloud_transport::Publisher>(
_pct->advertise("~/depth/color/points", qos_string_to_qos(_pointcloud_qos)));
}
else if ((!_is_enabled) && (_pointcloud_publisher))
{
Expand All @@ -92,7 +92,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
{
{
std::lock_guard<std::mutex> lock_guard(_mutex_publisher);
if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count())))
if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->getNumSubscribers())))
return;
}
rs2_stream texture_source_id = static_cast<rs2_stream>(_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER));
Expand Down