Skip to content
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

Support occupancy grid cells #3032

Merged
merged 1 commit into from
Mar 17, 2024
Merged

Support occupancy grid cells #3032

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
3 changes: 3 additions & 0 deletions realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
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
65 changes: 60 additions & 5 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
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,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<int>(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_COLUMNS)); // grid cells width
auto rows = static_cast<int>(f.get_frame_metadata(RS2_FRAME_METADATA_OCCUPANCY_GRID_ROWS)); // grid cells height
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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SamerKhshiboun, just thinking out loud for future enhancement. Maybe it makes sense to parallelize this operation by using SSSE3 or GPU acceleration. your thoughts?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I think it worth checking if we can accelerate this.
If so, we can do this in a separated task.
Thanks for the advice !

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this means rows and columns represent the cells and not the bytes?
meaning they are in bit units?
raws = 8 is 1 byte?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rows = grid cell height, cols = grid cell width
added more documentation for this code

{
// 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);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

raw is calculates from cols?
Why not from RS2_FRAME_METADATA_OCCUPANCY_GRID_ROWS?

uint32_t col = (i % cols);
p3d.x = (cell_size * static_cast<float>(rows)) - cell_size * (static_cast<float>(row) + 0.5f);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would expect x axis will use columns and not rows, can you explain this line?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in ROS CS, X is forward, Y is left, and Z is up.
in AICV algo ROWS represents the height is and COLS represents the width.
image

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
Open in desktop
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
Loading