Skip to content

Commit

Permalink
support occupancy grid cells
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed Mar 4, 2024
1 parent ec7c093 commit ebdc11e
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 13 deletions.
3 changes: 3 additions & 0 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/grid_cells.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
Expand Down Expand Up @@ -202,6 +203,7 @@ namespace realsense2_camera
void startDynamicTf();
void publishDynamicTransforms();
void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
void publishOccupancyFrame(rs2::frame f, const rclcpp::Time& t);
void publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t);
bool shouldPublishCameraInfo(const stream_index_pair& sip);
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;
Expand Down Expand Up @@ -292,6 +294,7 @@ namespace realsense2_camera
bool _use_intra_process;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _labeled_pointcloud_publisher;
rclcpp::Publisher<nav_msgs::msg::GridCells>::SharedPtr _occupancy_publisher;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
std::shared_ptr<SyncedImuPublisher> _synced_imu_publisher;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _info_publishers;
Expand Down
61 changes: 56 additions & 5 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -568,6 +568,10 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
{
publishPointCloud(f.as<rs2::points>(), t, frameset);
}
else if(stream_type == RS2_STREAM_OCCUPANCY)
{
publishOccupancyFrame(f, t);
}
else
{
if (stream_type == RS2_STREAM_DEPTH)
Expand Down Expand Up @@ -610,14 +614,21 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.nanoseconds());

stream_index_pair sip{stream_type,stream_index};
if (frame.is<rs2::depth_frame>())
if(stream_type == RS2_STREAM_OCCUPANCY)
{
if (_clipping_distance > 0)
publishOccupancyFrame(frame, t);
}
else
{
if (frame.is<rs2::depth_frame>())
{
clip_depth(frame, _clipping_distance);
if (_clipping_distance > 0)
{
clip_depth(frame, _clipping_distance);
}
}
publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers);
}
publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers);
}
else if (frame.is<rs2::labeled_points>())
{
Expand Down Expand Up @@ -828,7 +839,47 @@ bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip)
return (stream != RS2_STREAM_SAFETY && stream != RS2_STREAM_OCCUPANCY && stream != RS2_STREAM_LABELED_POINT_CLOUD);
}

void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t){
void BaseRealSenseNode::publishOccupancyFrame(rs2::frame f, const rclcpp::Time& t)
{
if(!_occupancy_publisher || 0 == _occupancy_publisher->get_subscription_count())
return;

ROS_DEBUG("Publishing Occupancy GridCells Frame");

// get frame bytes and frame metadata relevant info
auto frame_as_uint8_arr = (uint8_t*)f.get_data();
auto cols = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_COLUMNS));
auto rows = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_ROWS));
auto cell_size = static_cast<float>(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_CELL_SIZE) / 100.0f); // convert to meters

// create GridCells msg and start filling it
nav_msgs::msg::GridCells msg;
msg.header.stamp = t;
msg.header.frame_id = FRAME_ID(OCCUPANCY);
msg.cell_width = cell_size;
msg.cell_height = cell_size;

for (auto i = 0; i < cols * rows; ++i)
{
// check if current cell is occupied
// each byte include 8 bits <--> 8 cells
if ((frame_as_uint8_arr[i / 8U] & (1U << i % 8)) != 0)
{
// find x,y,z positions of current index
geometry_msgs::msg::Point p3d;
uint32_t row = (i / cols);
uint32_t col = (i % cols);
p3d.x = (cell_size * static_cast<float>(rows)) - cell_size * (static_cast<float>(row) + 0.5f);
p3d.y = (cell_size * static_cast<float>(cols)) / 2 - cell_size * (static_cast<float>(col) + 0.5f);
p3d.z = 0;
msg.cells.push_back(p3d);
}
}
_occupancy_publisher->publish(msg);
}

void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t)
{
if(!_labeled_pointcloud_publisher || 0 == _labeled_pointcloud_publisher->get_subscription_count())
return;

Expand Down
23 changes: 15 additions & 8 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,7 +218,21 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
else if (profile.stream_type() == RS2_STREAM_DEPTH)
_is_depth_enabled = true;

if (profile.stream_type() != RS2_STREAM_LABELED_POINT_CLOUD)
if (profile.stream_type() == RS2_STREAM_OCCUPANCY)
{
// special handling for occupancy stream, since it is a topic of nav_msgs/msg/GridCells messages
// and not a normal image publisher
_occupancy_publisher = _node.create_publisher<nav_msgs::msg::GridCells>("~/occupancy",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
}
else if(profile.stream_type() == RS2_STREAM_LABELED_POINT_CLOUD)
{
// special handling for labeled point cloud stream, since it is a topic of PointCloud messages
// and not a normal image publisher
_labeled_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/labeled_point_cloud/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
}
else
{
std::stringstream image_raw, camera_info;
bool rectified_image = false;
Expand Down Expand Up @@ -275,13 +289,6 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
}
}
else {

// special handling for labeled point cloud stream, since it a topic of PointCloud messages
// and not a normal image publisher
_labeled_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/labeled_point_cloud/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
}
}
else if (profile.is<rs2::motion_stream_profile>())
{
Expand Down

0 comments on commit ebdc11e

Please sign in to comment.