Skip to content

Commit

Permalink
Merge pull request #8 from ut-amrl/cuda
Browse files Browse the repository at this point in the history 
merging QOL changes to cuda_depth_to_lidar from depth_to_lidar
  • Loading branch information
@sadanand1120
sadanand1120 authored Nov 5, 2024
2 parents 2ea9c7f + 667bde8 commit 3ff5139
Showing 1 changed file with 100 additions and 3 deletions.
103 changes: 100 additions & 3 deletions src/cuda_depth_to_lidar.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/point_cloud2_iterator.h"
Expand Down Expand Up @@ -66,7 +67,10 @@ DECLARE_int32(v);
DEFINE_bool(depth, false, "Publish depth images");
DEFINE_bool(points, false, "Publish point cloud");
DEFINE_bool(rgb, false, "Publish color images");
DEFINE_bool(imu, false, "Publish IMU data");
DEFINE_string(config_file, "config/kinect.lua", "Name of config file to use");
DEFINE_uint32(resolution, 720, "RGB Image Resolution");
DEFINE_uint32(fps, 15, "RGB image frame rate");

CONFIG_STRING(serial, "kinect_serial");
CONFIG_STRING(costmap_topic, "costmap_topic");
Expand All @@ -77,6 +81,8 @@ CONFIG_STRING(rgb_frame, "rgb_image_frame");
CONFIG_STRING(depth_frame, "depth_image_frame");
CONFIG_STRING(scan_topic, "scan_topic");
CONFIG_STRING(scan_frame, "scan_frame");
CONFIG_STRING(imu_topic, "imu_topic");
CONFIG_STRING(imu_frame, "imu_frame");
CONFIG_BOOL(registered, "registered_rgbd");

CONFIG_FLOAT(yaw, "rotation.yaw");
Expand All @@ -102,12 +108,15 @@ class DepthToLidar : public K4AWrapper {
const k4a_device_configuration_t& config) :
K4AWrapper(serial, config, true),
image_transport_(n) {
boot_timestamp_ = ros::Time::now();
costmap_publisher_ =
n.advertise<sensor_msgs::Image>(CONFIG_costmap_topic, 1, false);
cloud_publisher_ =
n.advertise<sensor_msgs::PointCloud2>(CONFIG_points_topic, 1, false);
scan_publisher_ =
n.advertise<sensor_msgs::LaserScan>(CONFIG_scan_topic, 1, false);
imu_publisher_ =
n.advertise<sensor_msgs::Imu>(CONFIG_imu_topic, 1, false);
rgb_publisher_ = image_transport_.advertise(CONFIG_rgb_topic, 1);
depth_publisher_ = image_transport_.advertise(CONFIG_depth_topic, 1);
InitMessages();
Expand All @@ -117,11 +126,14 @@ class DepthToLidar : public K4AWrapper {
void InitMessages() {
depth_msg_.header.seq = heightmap_msg_.header.seq = rgb_msg_.header.seq =
cloud_msg_.header.seq = scan_msg_.header.seq = 0;

imu_msg_.header.seq = 0;

rgb_msg_.header.frame_id = CONFIG_rgb_frame;
depth_msg_.header.frame_id = CONFIG_depth_frame;
scan_msg_.header.frame_id = CONFIG_scan_frame;
cloud_msg_.header.frame_id = CONFIG_scan_frame;
imu_msg_.header.frame_id = CONFIG_imu_frame;

heightmap_msg_.header = scan_msg_.header;
// OpenCV Image format, float, 2 channel.
Expand Down Expand Up @@ -169,6 +181,7 @@ class DepthToLidar : public K4AWrapper {
cloud_msg_.data.resize(width * height * cloud_msg_.point_step);
cloud_msg_.width = width;
cloud_msg_.height = height;
imu_msg_.orientation_covariance.fill(-1); // Signify that the orientation field should be ignored.
}

void InitLookups() {
Expand Down Expand Up @@ -302,6 +315,40 @@ class DepthToLidar : public K4AWrapper {
void PublishHeightMap() {
}

void ImuCallback(k4a_imu_sample_t& imu_sample) override {
if (!FLAGS_imu) {
return;
}

// Use the message's timestamp-since-boot because we might be processing
// from a queue.
uint64_t sec =
boot_timestamp_.sec + imu_sample.acc_timestamp_usec / 1'000'000;
uint64_t nsec = boot_timestamp_.nsec +
(imu_sample.acc_timestamp_usec % 1'000'000) * 1'000;

sec += nsec / 1'000'000'000;
nsec %= 1'000'000'000;

imu_msg_.header.stamp = ros::Time(sec, nsec);
imu_msg_.angular_velocity.x = imu_sample.gyro_sample.xyz.x;
imu_msg_.angular_velocity.y = imu_sample.gyro_sample.xyz.y;
imu_msg_.angular_velocity.z = imu_sample.gyro_sample.xyz.z;
imu_msg_.linear_acceleration.x = imu_sample.acc_sample.xyz.x;
imu_msg_.linear_acceleration.y = imu_sample.acc_sample.xyz.y;
imu_msg_.linear_acceleration.z = imu_sample.acc_sample.xyz.z;

imu_publisher_.publish(imu_msg_);
}

void ColorCallback(k4a_image_t image) override {
ros::Time stamp_time = ros::Time::now();
if (image != nullptr && FLAGS_rgb) {
// TODO consider publishing camera info also with same timestamp
PublishRGBImage(image, stamp_time);
}
}

void RegisteredRGBDCallback(k4a_image_t color_image,
k4a_image_t depth_image) override {
ros::Time stamp_time = ros::Time::now();
Expand All @@ -313,12 +360,12 @@ class DepthToLidar : public K4AWrapper {
}

if (depth_image == nullptr) return;
DepthToPointCloud(color_image, depth_image);
PublishScan(stamp_time);
if (FLAGS_depth) {
PublishDepthImage(depth_image, stamp_time);
}
if (FLAGS_points) {
DepthToPointCloud(color_image, depth_image);
PublishPointCloud(stamp_time);
}
}
Expand All @@ -331,21 +378,26 @@ class DepthToLidar : public K4AWrapper {
sensor_msgs::Image rgb_msg_;
sensor_msgs::Image depth_msg_;
sensor_msgs::Image heightmap_msg_;
sensor_msgs::Imu imu_msg_;
sensor_msgs::PointCloud2 cloud_msg_;
ros::Publisher costmap_publisher_;
ros::Publisher cloud_publisher_;
ros::Publisher scan_publisher_;
ros::Publisher imu_publisher_;
image_transport::Publisher rgb_publisher_;
image_transport::Publisher depth_publisher_;
image_transport::Publisher heightmap_publisher_;
image_transport::ImageTransport image_transport_;
// Translation component of extrinsics.
Eigen::Vector3f ext_translation_;
ros::Time boot_timestamp_;
};

int main(int argc, char* argv[]) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, false);
FLAGS_logtostderr = true;
FLAGS_colorlogtostderr = true;

if (FLAGS_v > 0) {
processing_kernels::GetCudaCapabilities();
Expand All @@ -355,9 +407,54 @@ int main(int argc, char* argv[]) {
ros::init(argc, argv, "k4a_ros");
ros::NodeHandle n;
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
config.color_resolution = K4A_COLOR_RESOLUTION_720P;

switch (FLAGS_resolution) {
case 720:
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
break;
case 1080:
config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
break;
case 1440:
config.color_resolution = K4A_COLOR_RESOLUTION_1440P;
break;
case 1536:
config.color_resolution = K4A_COLOR_RESOLUTION_1536P;
break;
case 2160:
config.color_resolution = K4A_COLOR_RESOLUTION_2160P;
break;
case 3072:
config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
break;
default:
LOG(WARNING) << "Unknown resolution \"" << FLAGS_resolution << "\", defaulting to 720p";
config.color_resolution = K4A_COLOR_RESOLUTION_720P;
}

switch (FLAGS_fps) {
case 5:
config.camera_fps = K4A_FRAMES_PER_SECOND_5;
break;
case 15:
config.camera_fps = K4A_FRAMES_PER_SECOND_15;
break;
case 30:
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
break;
default:
LOG(WARNING) << "Unknown fps \"" << FLAGS_fps << "\", defaulting to 15";
config.camera_fps = K4A_FRAMES_PER_SECOND_15;
}

config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
config.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;

if (FLAGS_depth) {
config.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
} else {
config.depth_mode = K4A_DEPTH_MODE_OFF;
}

config.synchronized_images_only = false;
DepthToLidar interface(n, CONFIG_serial, config);

Expand Down

0 comments on commit 3ff5139

Please sign in to comment.