Hello,
We were trying to use the Oak-D-Lite camera and ROS to get depth information but we were not getting the correct output. We expected the points to give height, depth, and width for the entire image but the pointcloud returns values that look like RGB given that they are from 0 to 255. We thought that the docs said that the camera did not return any RGB values. An issue might be that we do not understand the format of the Pointcloud Messages and we didn't understand the docs.
The launch script for our camera is
roslaunch depthai_ros_driver pointcloud.launch
Our code for reading the pointcloud values is here
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"
#include "sensor_msgs/PointCloud2.h"
#include <sensor_msgs/point_cloud2_iterator.h>
#include <stdio.h>
#include <chrono>
ros::Publisher occupancy_grid_publisher;
// in meters
float chunk_size = 0.3; // size of each cell
int map_size = 5; // size of observed area (side length of square)
int noise_thresh = 10000; // # of points in each cell to be considered an obstacle
void ptCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
auto start = std::chrono::high_resolution_clock::now();
int grid_size = (int)map_size / chunk_size;
std::vector<std::vector<float>> occupancy_grid(grid_size, std::vector<float>(grid_size, -100));
std::vector<std::vector<float>> counts(grid_size, std::vector<float>(grid_size, 0));
std::vector<std::vector<float>> gradient_map(grid_size, std::vector<float>(grid_size, 0));
for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it)
{
float x = it[0];
float z = it[1];
float y = it[2];
int x_index = (int)(x / chunk_size);
int y_index = (int)(y / chunk_size);
x_index += (int)(grid_size/2);
if (x_index < 0 || x_index >= grid_size || y_index < 0 || y_index >= grid_size)
continue;
occupancy_grid[x_index][y_index] = std::max(occupancy_grid[x_index][y_index], (float)(z));
counts[x_index][y_index] += 1;
}
printf("\n[");
for (int i = 0; i < grid_size; i++)
{
for (int j = 0; j < grid_size; j++)
{
printf("%.1f ", counts[i][j]);
}
printf("\n");
}
printf("]\n");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "obj_detect");
ros::NodeHandle n;
ROS_INFO("Starting obj_detect node");
occupancy_grid_publisher = n.advertise<std_msgs::Float64MultiArray>("/obstacle_avoidance/occupancy_grid", 1000);
ros::Subscriber sub = n.subscribe("/oak/points", 1, ptCallback);
ros::spin();
return 0;
}
Thank you for your help.
