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

Reconnect #5

Open
wants to merge 3 commits into
base: jazzy-devel
Choose a base branch
from
Open

Reconnect #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
95d058a
Reconnect
Tacha-S Dec 26, 2024
2e70552
Do not create multi nodes
Tacha-S Dec 27, 2024
3962811
Spin correctly
Tacha-S Dec 27, 2024
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
6 changes: 4 additions & 2 deletions include/azure_kinect_ros_driver/k4a_calibration_transform_data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,10 @@
//
#include "azure_kinect_ros_driver/k4a_ros_device_params.h"

class K4ACalibrationTransformData : public rclcpp::Node
class K4ACalibrationTransformData
{
public:
K4ACalibrationTransformData();
K4ACalibrationTransformData(const rclcpp::Node::SharedPtr& node);
void initialize(const k4a::device& device, const k4a_depth_mode_t depthMode, const k4a_color_resolution_t resolution,
const K4AROSDeviceParams& params);
void initialize(const k4a::playback& k4a_playback_handle, const K4AROSDeviceParams& params);
Expand All @@ -41,6 +41,8 @@ class K4ACalibrationTransformData : public rclcpp::Node
void getRgbCameraInfo(sensor_msgs::msg::CameraInfo& camera_info);
void print();

rclcpp::Node::SharedPtr node_;

k4a::calibration k4a_calibration_;
k4a::transformation k4a_transformation_;

Expand Down
12 changes: 7 additions & 5 deletions include/azure_kinect_ros_driver/k4a_ros_device.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,18 +36,18 @@
#include "azure_kinect_ros_driver/k4a_calibration_transform_data.h"
#include "azure_kinect_ros_driver/k4a_ros_device_params.h"

class K4AROSDevice : public rclcpp::Node
class K4AROSDevice
{
public:
K4AROSDevice();
K4AROSDevice(const rclcpp::Node::SharedPtr& node);

~K4AROSDevice();

k4a_result_t startCameras();
k4a_result_t startImu();

void stopCameras();
void stopImu();
bool stopCameras();
bool stopImu();

// Get camera calibration information for the depth camera
void getDepthCameraInfo(sensor_msgs::msg::CameraInfo& camera_info);
Expand Down Expand Up @@ -77,6 +77,7 @@ class K4AROSDevice : public rclcpp::Node
k4a_result_t renderBodyIndexMapToROS(std::shared_ptr<sensor_msgs::msg::Image> body_index_map_image, k4a::image& k4a_body_index_map,
const k4abt::frame& body_frame);
#endif
bool isRunning();

private:
k4a_result_t renderBGRA32ToROS(std::shared_ptr<sensor_msgs::msg::Image>& rgb_frame, k4a::image& k4a_bgra_frame);
Expand Down Expand Up @@ -116,6 +117,7 @@ class K4AROSDevice : public rclcpp::Node

void printTimestampDebugMessage(const std::string& name, const rclcpp::Time& timestamp);

rclcpp::Node::SharedPtr node_;

image_transport::Publisher rgb_raw_publisher_;
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr rgb_jpeg_publisher_;
Expand Down Expand Up @@ -160,7 +162,7 @@ class K4AROSDevice : public rclcpp::Node
std::chrono::nanoseconds device_to_realtime_offset_{0};

// Thread control
volatile bool running_;
volatile bool initialized_, running_;

// Last capture timestamp for synchronizing playback capture and imu thread
std::atomic_uint64_t last_capture_time_usec_;
Expand Down
6 changes: 4 additions & 2 deletions include/azure_kinect_ros_driver/k4a_ros_device_params.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,10 +85,10 @@
"The namespace of RGB topics", \
std::string, std::string("rgb"))

class K4AROSDeviceParams : public rclcpp::Node
class K4AROSDeviceParams
{
public:
K4AROSDeviceParams();
K4AROSDeviceParams(const rclcpp::Node::SharedPtr& node);

// Get a device configuration from a a set of parameters
k4a_result_t GetDeviceConfig(k4a_device_configuration_t* configuration);
Expand All @@ -99,6 +99,8 @@ class K4AROSDeviceParams : public rclcpp::Node
// Print the value of all parameters
void Print();

rclcpp::Node::SharedPtr node_;

// Parameters
#define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) param_type param_variable;
ROS_PARAM_LIST
Expand Down
93 changes: 49 additions & 44 deletions src/k4a_calibration_transform_data.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@
// Project headers
//
#include "azure_kinect_ros_driver/k4a_ros_types.h"
K4ACalibrationTransformData::K4ACalibrationTransformData() : Node("k4a_calibration_transform_data")
K4ACalibrationTransformData::K4ACalibrationTransformData(const rclcpp::Node::SharedPtr& node) : node_(node)
{
static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
}
void K4ACalibrationTransformData::initialize(const k4a::device& device, const k4a_depth_mode_t depth_mode,
const k4a_color_resolution_t resolution, const K4AROSDeviceParams& params)
Expand Down Expand Up @@ -95,68 +95,73 @@ int K4ACalibrationTransformData::getColorHeight()

void K4ACalibrationTransformData::print()
{
RCLCPP_INFO(this->get_logger(),"K4A Calibration Blob:");
RCLCPP_INFO(this->get_logger(),"\t Depth:");
RCLCPP_INFO(node_->get_logger(), "K4A Calibration Blob:");
RCLCPP_INFO(node_->get_logger(), "\t Depth:");
printCameraCalibration(k4a_calibration_.depth_camera_calibration);

RCLCPP_INFO(this->get_logger(),"\t Color:");
RCLCPP_INFO(node_->get_logger(), "\t Color:");
printCameraCalibration(k4a_calibration_.color_camera_calibration);

RCLCPP_INFO(this->get_logger(),"\t IMU (Depth to Color):");
RCLCPP_INFO(node_->get_logger(), "\t IMU (Depth to Color):");
printExtrinsics(k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_COLOR]);

RCLCPP_INFO(this->get_logger(),"\t IMU (Depth to IMU):");
RCLCPP_INFO(node_->get_logger(), "\t IMU (Depth to IMU):");
printExtrinsics(k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_ACCEL]);

RCLCPP_INFO(this->get_logger(),"\t IMU (IMU to Depth):");
RCLCPP_INFO(node_->get_logger(), "\t IMU (IMU to Depth):");
printExtrinsics(k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH]);

RCLCPP_INFO(this->get_logger(),"\t IMU (Color to IMU):");
RCLCPP_INFO(node_->get_logger(), "\t IMU (Color to IMU):");
printExtrinsics(k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_COLOR][K4A_CALIBRATION_TYPE_ACCEL]);

RCLCPP_INFO(this->get_logger(),"\t IMU (IMU to Color):");
RCLCPP_INFO(node_->get_logger(), "\t IMU (IMU to Color):");
printExtrinsics(k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR]);
}

void K4ACalibrationTransformData::printCameraCalibration(k4a_calibration_camera_t& calibration)
{
printExtrinsics(calibration.extrinsics);

RCLCPP_INFO(this->get_logger(),"\t\t Resolution:");
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Width: " << calibration.resolution_width);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Height: " << calibration.resolution_height);

RCLCPP_INFO(this->get_logger(),"\t\t Intrinsics:");
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Model Type: " << calibration.intrinsics.type);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Parameter Count: " << calibration.intrinsics.parameter_count);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t cx: " << calibration.intrinsics.parameters.param.cx);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t cy: " << calibration.intrinsics.parameters.param.cy);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t fx: " << calibration.intrinsics.parameters.param.fx);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t fy: " << calibration.intrinsics.parameters.param.fy);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k1: " << calibration.intrinsics.parameters.param.k1);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k2: " << calibration.intrinsics.parameters.param.k2);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k3: " << calibration.intrinsics.parameters.param.k3);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k4: " << calibration.intrinsics.parameters.param.k4);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k5: " << calibration.intrinsics.parameters.param.k5);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t k6: " << calibration.intrinsics.parameters.param.k6);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t codx: " << calibration.intrinsics.parameters.param.codx);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t cody: " << calibration.intrinsics.parameters.param.cody);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t p2: " << calibration.intrinsics.parameters.param.p2);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t p1: " << calibration.intrinsics.parameters.param.p1);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t metric_radius: " << calibration.intrinsics.parameters.param.metric_radius);
RCLCPP_INFO(node_->get_logger(), "\t\t Resolution:");
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Width: " << calibration.resolution_width);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Height: " << calibration.resolution_height);

RCLCPP_INFO(node_->get_logger(), "\t\t Intrinsics:");
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Model Type: " << calibration.intrinsics.type);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Parameter Count: " << calibration.intrinsics.parameter_count);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t cx: " << calibration.intrinsics.parameters.param.cx);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t cy: " << calibration.intrinsics.parameters.param.cy);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t fx: " << calibration.intrinsics.parameters.param.fx);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t fy: " << calibration.intrinsics.parameters.param.fy);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k1: " << calibration.intrinsics.parameters.param.k1);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k2: " << calibration.intrinsics.parameters.param.k2);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k3: " << calibration.intrinsics.parameters.param.k3);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k4: " << calibration.intrinsics.parameters.param.k4);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k5: " << calibration.intrinsics.parameters.param.k5);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t k6: " << calibration.intrinsics.parameters.param.k6);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t codx: " << calibration.intrinsics.parameters.param.codx);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t cody: " << calibration.intrinsics.parameters.param.cody);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t p2: " << calibration.intrinsics.parameters.param.p2);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t p1: " << calibration.intrinsics.parameters.param.p1);
RCLCPP_INFO_STREAM(node_->get_logger(),
"\t\t\t metric_radius: " << calibration.intrinsics.parameters.param.metric_radius);
}

void K4ACalibrationTransformData::printExtrinsics(k4a_calibration_extrinsics_t& extrinsics)
{
RCLCPP_INFO(this->get_logger(),"\t\t Extrinsics:");
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Translation: " << extrinsics.translation[0] << ", " << extrinsics.translation[1] << ", "
<< extrinsics.translation[2]);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Rotation[0]: " << extrinsics.rotation[0] << ", " << extrinsics.rotation[1] << ", "
<< extrinsics.rotation[2]);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Rotation[1]: " << extrinsics.rotation[3] << ", " << extrinsics.rotation[4] << ", "
<< extrinsics.rotation[5]);
RCLCPP_INFO_STREAM(this->get_logger(),"\t\t\t Rotation[2]: " << extrinsics.rotation[6] << ", " << extrinsics.rotation[7] << ", "
<< extrinsics.rotation[8]);
RCLCPP_INFO(node_->get_logger(), "\t\t Extrinsics:");
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Translation: " << extrinsics.translation[0] << ", "
<< extrinsics.translation[1] << ", "
<< extrinsics.translation[2]);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Rotation[0]: " << extrinsics.rotation[0] << ", "
<< extrinsics.rotation[1] << ", "
<< extrinsics.rotation[2]);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Rotation[1]: " << extrinsics.rotation[3] << ", "
<< extrinsics.rotation[4] << ", "
<< extrinsics.rotation[5]);
RCLCPP_INFO_STREAM(node_->get_logger(), "\t\t\t Rotation[2]: " << extrinsics.rotation[6] << ", "
<< extrinsics.rotation[7] << ", "
<< extrinsics.rotation[8]);
}

void K4ACalibrationTransformData::publishRgbToDepthTf()
Expand All @@ -175,7 +180,7 @@ void K4ACalibrationTransformData::publishRgbToDepthTf()
geometry_msgs::msg::TransformStamped static_transform;
static_transform.transform = tf2::toMsg(depth_to_rgb_transform.inverse());

static_transform.header.stamp = this->get_clock()->now();
static_transform.header.stamp = node_->get_clock()->now();
static_transform.header.frame_id = tf_prefix_ + depth_camera_frame_;
static_transform.child_frame_id = tf_prefix_ + rgb_camera_frame_;

Expand All @@ -198,7 +203,7 @@ void K4ACalibrationTransformData::publishImuToDepthTf()
geometry_msgs::msg::TransformStamped static_transform;
static_transform.transform = tf2::toMsg(depth_to_imu_transform.inverse());

static_transform.header.stamp = this->get_clock()->now();
static_transform.header.stamp = node_->get_clock()->now();
static_transform.header.frame_id = tf_prefix_ + depth_camera_frame_;
static_transform.child_frame_id = tf_prefix_ + imu_frame_;

Expand All @@ -210,7 +215,7 @@ void K4ACalibrationTransformData::publishDepthToBaseTf()
// This is a purely cosmetic transform to make the base model of the URDF look good.
geometry_msgs::msg::TransformStamped static_transform;

static_transform.header.stamp = this->get_clock()->now();
static_transform.header.stamp = node_->get_clock()->now();
static_transform.header.frame_id = tf_prefix_ + camera_base_frame_;
static_transform.child_frame_id = tf_prefix_ + depth_camera_frame_;

Expand Down
103 changes: 72 additions & 31 deletions src/k4a_ros_bridge_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@

// System headers
//
#include <chrono>
#include <memory>
#include <sstream>

// Library headers
Expand All @@ -14,49 +16,88 @@
//
#include "azure_kinect_ros_driver/k4a_ros_device.h"

int main(int argc, char** argv)
class K4ADriver
{
rclcpp::init(argc, argv);

// Create Node for handling info and error messages
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("k4a_bridge");


// Setup the K4A device
std::shared_ptr<K4AROSDevice> device(new K4AROSDevice);

k4a_result_t result = device->startCameras();

if (result != K4A_RESULT_SUCCEEDED)
public:
K4ADriver(const rclcpp::Node::SharedPtr& node) : node_(node), device_(std::make_shared<K4AROSDevice>(node_))
{
RCLCPP_ERROR_STREAM(node->get_logger(),"Failed to start cameras");
return -1;
startKinect();
watchdog_timer_ =
node_->create_wall_timer(std::chrono::milliseconds(100), std::bind(&K4ADriver::watchdogTimerCallback, this));
}

result = device->startImu();
if (result != K4A_RESULT_SUCCEEDED)
void watchdogTimerCallback(){
if(!device_->isRunning()){
watchdog_timer_->cancel();
RCLCPP_ERROR(node_->get_logger(), "K4A is not running");
restartKinect();
watchdog_timer_->reset();
}
}
k4a_result_t startKinect()
{
RCLCPP_ERROR_STREAM(node->get_logger(),"Failed to start IMU");
return -2;
k4a_result_t result = device_->startCameras();

if (result != K4A_RESULT_SUCCEEDED)
{
RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to start cameras");
return result;
}

result = device_->startImu();
if (result != K4A_RESULT_SUCCEEDED)
{
RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to start IMU");
return result;
}

RCLCPP_INFO(node_->get_logger(), "K4A Started");
return result;
}
void restartKinect(){
device_.reset();
device_ = std::make_shared<K4AROSDevice>(node_);
bool succeed = false;
while(!succeed){
succeed = startKinect() == K4A_RESULT_SUCCEEDED;
if (!succeed)
{
device_.reset();
RCLCPP_WARN(node_->get_logger(), "Failed to restart K4A, retrying in 1 second");
std::this_thread::sleep_for(std::chrono::seconds(1));
device_ = std::make_shared<K4AROSDevice>(node_);
}
}
}
void run()
{
rclcpp::spin(node_);

RCLCPP_INFO(node->get_logger(),"K4A Started");
RCLCPP_INFO(node_->get_logger(), "ROS Exit Started");

if (result == K4A_RESULT_SUCCEEDED)
{
rclcpp::spin(node);
device_.reset();

RCLCPP_INFO(node->get_logger(),"ROS Exit Started");
}
RCLCPP_INFO(node_->get_logger(), "ROS Exit");

device.reset();
rclcpp::shutdown();

RCLCPP_INFO(node->get_logger(),"ROS Exit");
RCLCPP_INFO(node_->get_logger(), "ROS Shutdown complete");

RCLCPP_INFO(node_->get_logger(), "Finished ros bridge main");
}

rclcpp::shutdown();
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<K4AROSDevice> device_;
rclcpp::TimerBase::SharedPtr watchdog_timer_;
};

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);

RCLCPP_INFO(node->get_logger(),"ROS Shutdown complete");
auto node = rclcpp::Node::make_shared("k4a_ros_driver");
auto k4a_driver = std::make_shared<K4ADriver>(node);
k4a_driver->run();

RCLCPP_INFO(node->get_logger(),"Finished ros bridge main");
return 0;
}
Loading
Loading