diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index dd38c5a0e4..2a7500905c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -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; @@ -292,6 +294,7 @@ namespace realsense2_camera bool _use_intra_process; std::map> _image_publishers; rclcpp::Publisher::SharedPtr _labeled_pointcloud_publisher; + rclcpp::Publisher::SharedPtr _occupancy_publisher; std::map::SharedPtr> _imu_publishers; std::shared_ptr _synced_imu_publisher; std::map::SharedPtr> _info_publishers; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9083406b77..82599a5f82 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -568,6 +568,10 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) { publishPointCloud(f.as(), t, frameset); } + else if(stream_type == RS2_STREAM_OCCUPANCY) + { + publishOccupancyFrame(f, t); + } else { if (stream_type == RS2_STREAM_DEPTH) @@ -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()) + if(stream_type == RS2_STREAM_OCCUPANCY) { - if (_clipping_distance > 0) + publishOccupancyFrame(frame, t); + } + else + { + if (frame.is()) { - 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()) { @@ -828,7 +839,51 @@ 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(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_COLUMNS)); // grid cells width + auto rows = static_cast(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_ROWS)); // grid cells height + auto cell_size = static_cast(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) + { + // AICV algo is packing each 8 cells into one byte. Each byte include 8 bits <--> 8 cells + // The rightest bit (LSB) inside the packed byte from AICV algo represnts the closest cell we want to work with in the grid. + // e.g. Original Occupancy Cells: 0 0 1 1 0 0 1 0 ---> AICV packing algo ---> 01001100 (not the opposite order) + // In this if we check if current cell is occupied. + // Note that we start working from the most left bit, aka, the farest point of the grid. + if ((frame_as_uint8_arr[i / 8U] & (1U << i % 8)) != 0) + { + // Find x,y,z positions of current index + // Remember, in ROS CS: (X: Forward, Y: Left, Z: Up) + geometry_msgs::msg::Point p3d; + uint32_t row = (i / cols); + uint32_t col = (i % cols); + p3d.x = (cell_size * static_cast(rows)) - cell_size * (static_cast(row) + 0.5f); + p3d.y = (cell_size * static_cast(cols)) / 2 - cell_size * (static_cast(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; diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 5d676b5032..afdabf5d55 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -218,7 +218,21 @@ void BaseRealSenseNode::startPublishers(const std::vector& 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("~/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("~/labeled_point_cloud/points", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos)); + } + else { std::stringstream image_raw, camera_info; bool rectified_image = false; @@ -275,13 +289,6 @@ void BaseRealSenseNode::startPublishers(const std::vector& 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("~/labeled_point_cloud/points", - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos)); - } } else if (profile.is()) {