diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b991ec3..49cad972 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ add_executable(${PROJECT_NAME}_node src/k4a_ros_bridge_node.cpp src/k4a_ros_device.cpp src/k4a_ros_device_params.cpp + src/k4a_ros_color_params.cpp src/k4a_calibration_transform_data.cpp ) @@ -67,7 +68,7 @@ set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "" #### AZURE KINECT SDK ###### ############################ -message("Finding K4A SDK binaries") +message(STATUS "Finding K4A SDK binaries") # Disable cached locations for K4A SDK binaries. # Do this to force the search logic to happen correctly. @@ -79,7 +80,7 @@ message("Finding K4A SDK binaries") unset(k4a_DIR CACHE) # Force running the Findk4a.cmake module -find_package(k4a 1.3.0 QUIET MODULE REQUIRED) +find_package(k4a 1.4.0 QUIET MODULE REQUIRED) set(K4A_LIBS k4a::k4a;k4a::k4arecord) # Try to find and enable the body tracking SDK @@ -89,7 +90,7 @@ if (k4abt_FOUND) message(STATUS "Body Tracking SDK found: compiling support for Body Tracking") target_compile_definitions(${PROJECT_NAME}_node PUBLIC K4A_BODY_TRACKING) else() - message("!!! Body Tracking SDK not found: body tracking features will not be available !!!") + message(STATUS "!!! Body Tracking SDK not found: body tracking features will not be available !!!") endif() # This reads the K4A_LIBS and K4A_INSTALL_REQUIRED variables and decides how to install @@ -114,9 +115,6 @@ target_link_libraries(${PROJECT_NAME}_node ## Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin DESTINATION lib/${PROJECT_NAME} ) @@ -128,4 +126,4 @@ install( DESTINATION share/${PROJECT_NAME}/ ) -ament_package() \ No newline at end of file +ament_package() diff --git a/README.md b/README.md index a70b9e94..0fe23313 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,25 @@ This project is a node which publishes sensor data from the [Azure Kinect Develo This repository uses the [Azure Kinect Sensor SDK](https://github.com/microsoft/Azure-Kinect-Sensor-SDK) to communicate with the Azure Kinect DK. It supports both Linux and Windows installations of ROS. -[![Build Status](https://dev.azure.com/ms/Azure_Kinect_ROS_Driver/_apis/build/status/microsoft.Azure_Kinect_ROS_Driver?branchName=melodic)](https://dev.azure.com/ms/Azure_Kinect_ROS_Driver/_build/latest?definitionId=166&branchName=melodic) +## Dependencies +``` +cd -- +sudo apt install libsoundio2 +wget https://github.com/asukiaaa/Azure-Kinect-Sensor-SDK/releases/download/2923-08-26-ubuntu-22-04/libk4a1.4_1.4.1_amd64.deb +sudo dpkg -i libk4a1.4_1.4.1_amd64.deb +rm libk4a1.4_1.4.1_amd64.deb +sudo ldconfig + +pip3 install xacro +sudo apt install ros-humble-joint-state-publisher +``` + +# INSTALL AZURE KINECT ROS DRIVER + +cd ~/otto_ws/src +pip3 install xacro +sudo apt install ros-humble-joint-state-publisher + ## Features diff --git a/cmake/Installk4a.cmake b/cmake/Installk4a.cmake index 750bc19b..b6cb318b 100644 --- a/cmake/Installk4a.cmake +++ b/cmake/Installk4a.cmake @@ -41,9 +41,9 @@ foreach(_lib ${K4A_LIBS}) endif() endforeach() -message("K4A Libs: ${K4A_LIBS}") -message("K4A DLLs: ${K4A_DLL_FILES}") -message("K4A Install Needed: ${K4A_INSTALL_NEEDED}") +message(STATUS "K4A Libs: ${K4A_LIBS}") +message(STATUS "K4A DLLs: ${K4A_DLL_FILES}") +message(STATUS "K4A Install Needed: ${K4A_INSTALL_NEEDED}") if (${K4A_INSTALL_NEEDED}) # Tell cmake that we need to reconfigure if any of the DLL files change diff --git a/include/azure_kinect_ros_driver/k4a_ros_color_params.h b/include/azure_kinect_ros_driver/k4a_ros_color_params.h new file mode 100644 index 00000000..8f09c166 --- /dev/null +++ b/include/azure_kinect_ros_driver/k4a_ros_color_params.h @@ -0,0 +1,66 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef K4A_ROS_COLOR_PARAMS_H +#define K4A_ROS_COLOR_PARAMS_H + +// System headers +// + +// Library headers +// +#include +#include +#include "rclcpp/rclcpp.hpp" +// Project headers +// + +// The format of these list entries is : +// +// LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) +// +// param_variable: the variable name which will be created in the k4a_ros_device class to hold the contents of the +// parameter +// param_help_string: a string containing help information for the parameter +// param_type: the type of the parameter +// param_default_val: the default value of the parameter +// +// Example: +// LIST_ENTRY(sensor_sn, "The serial number of the sensor this node should connect with", std::string, std::string("")) +#define ROS_PARAM_LIST_COLOR \ + LIST_ENTRY(exposure_auto, "True if using auto exposure time", bool, true) \ + LIST_ENTRY(exposure_time, "Exposure time is measured in microseconds", int, 16670) \ + LIST_ENTRY(whitebalance_auto, "True if using auto whitebalance", bool, true) \ + LIST_ENTRY(whitebalance_val, "The unit is degrees Kelvin." \ + "The setting must be set to a value evenly divisible by 10 degrees, " \ + "between 2500 and 12500.", int, 4500) \ + LIST_ENTRY(brightness, "Brightness value, the valid range is 0 to 255.", int, 128) \ + LIST_ENTRY(contrast, "Contrast value, the valid range is 0 to 10", int, 5) \ + LIST_ENTRY(saturation, "Saturation value, the valid range is 0 to 63", int, 32) \ + LIST_ENTRY(sharpness, "Sharpness value, the valid range is 0 to 4", int, 2) \ + LIST_ENTRY(gain, "Gain value, the valid range is 0 to 255", int, 128) \ + LIST_ENTRY(powerline_frequency, "Value of 1 sets the powerline compensation to 50 Hz. " \ + "Value of 2 sets the powerline compensation to 60 Hz.", int, 1) \ + LIST_ENTRY(backlight_compensation, "Backlight compensation enable", bool, false) + +class K4AROSColorParams : public rclcpp::Node +{ +public: + K4AROSColorParams(); + + // Set color config from a set of parameters + bool SetColorConfig(k4a::device &device); + + // Print help messages to the console + void Help(); + + // Print the value of all parameters + void Print(); + +// Parameters +#define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) param_type param_variable; + ROS_PARAM_LIST_COLOR +#undef LIST_ENTRY +}; + +#endif // K4A_ROS_COLOR_PARAMS_H diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h index 2c2de2f4..4f03b22a 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device.h @@ -33,6 +33,7 @@ // #include "azure_kinect_ros_driver/k4a_calibration_transform_data.h" #include "azure_kinect_ros_driver/k4a_ros_device_params.h" +#include "azure_kinect_ros_driver/k4a_ros_color_params.h" class K4AROSDevice : public rclcpp::Node { @@ -143,6 +144,7 @@ class K4AROSDevice : public rclcpp::Node // Parameters K4AROSDeviceParams params_; + K4AROSColorParams color_params_; // K4A device k4a::device k4a_device_; diff --git a/launch/driver.launch.py b/launch/driver.launch.py index 711b4cf3..6922615e 100644 --- a/launch/driver.launch.py +++ b/launch/driver.launch.py @@ -137,6 +137,53 @@ def generate_launch_description(): 'subordinate_delay_off_master_usec', default_value="0", description="Delay subordinate camera off master camera by specified amount in usec."), + DeclareLaunchArgument( + 'exposure_auto', + default_value="true", + description="Set exposure to automatic (true) or manual (false)."), + DeclareLaunchArgument( + 'exposure_time', + default_value="16670", + description="Exposure time is measured in microseconds"), + DeclareLaunchArgument( + 'whitebalance_auto', + default_value="true", + description="Set whitebalance to automatic (true) or manual (false)."), + DeclareLaunchArgument( + 'whitebalance_val', + default_value="4500", + description="Whitebalance value unit in degrees Kelvin." \ + "The setting must be set to a value evenly divisible by 10 degrees, " \ + "between 2500 and 12500 "), + DeclareLaunchArgument( + 'brightness', + default_value="127", + description="Brightness value, the valid range is 0 to 255"), + DeclareLaunchArgument( + 'contrast', + default_value="5", + description="Contrast value, the valid range is 0 to 10"), + DeclareLaunchArgument( + 'saturation', + default_value="32", + description="Saturation value, the valid range is 0 to 63"), + DeclareLaunchArgument( + 'sharpness', + default_value="2", + description="Sharpness value, the valid range is 0 to 4"), + DeclareLaunchArgument( + 'gain', + default_value="128", + description="Gain value, the valid range is 0 to 255"), + DeclareLaunchArgument( + 'powerline_frequency', + default_value="1", + description="Value of 1 sets the powerline compensation to 50 Hz. " + "Value of 2 sets the powerline compensation to 60 Hz."), + DeclareLaunchArgument( + 'backlight_compensation', + default_value="false", + description="Backlight compensation enable"), launch_ros.actions.Node( package='azure_kinect_ros_driver', executable='node', @@ -161,7 +208,19 @@ def generate_launch_description(): {'ir_mono8_scaling_factor': launch.substitutions.LaunchConfiguration('ir_mono8_scaling_factor')}, {'imu_rate_target': launch.substitutions.LaunchConfiguration('imu_rate_target')}, {'wired_sync_mode': launch.substitutions.LaunchConfiguration('wired_sync_mode')}, - {'subordinate_delay_off_master_usec': launch.substitutions.LaunchConfiguration('subordinate_delay_off_master_usec')}]), + {'subordinate_delay_off_master_usec': launch.substitutions.LaunchConfiguration('subordinate_delay_off_master_usec')}, + {'exposure_auto': launch.substitutions.LaunchConfiguration('exposure_auto')}, + {'exposure_time': launch.substitutions.LaunchConfiguration('exposure_time')}, + {'whitebalance_auto': launch.substitutions.LaunchConfiguration('whitebalance_auto')}, + {'whitebalance_val': launch.substitutions.LaunchConfiguration('whitebalance_val')}, + {'brightness': launch.substitutions.LaunchConfiguration('brightness')}, + {'contrast': launch.substitutions.LaunchConfiguration('contrast')}, + {'saturation': launch.substitutions.LaunchConfiguration('saturation')}, + {'sharpness': launch.substitutions.LaunchConfiguration('sharpness')}, + {'gain': launch.substitutions.LaunchConfiguration('gain')}, + {'powerline_frequency': launch.substitutions.LaunchConfiguration('powerline_frequency')}, + {'backlight_compensation': launch.substitutions.LaunchConfiguration('backlight_compensation')} + ]), # If flag overwrite_robot_description is set: launch_ros.actions.Node( package='robot_state_publisher', diff --git a/launch/driver_simple.launch.py b/launch/driver_simple.launch.py new file mode 100644 index 00000000..23d44c4b --- /dev/null +++ b/launch/driver_simple.launch.py @@ -0,0 +1,47 @@ +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions +import os + +def generate_launch_description(): + azure_kinect_ros_driver_node = launch_ros.actions.Node( + package='azure_kinect_ros_driver', + executable='node', + output='screen', + parameters=[ + {'depth_enabled': True}, + {'depth_mode': 'NFOV_2X2BINNED'}, + {'depth_unit': '16UC1'}, + {'color_enabled': True}, + {'color_format': 'bgra'}, + {'color_resolution': '1080P'}, + {'fps': 30}, + {'point_cloud': True}, + {'rgb_point_cloud': True}, + {'point_cloud_in_depth_frame': True}, + {'imu_rate_target': 0}, + {'exposure_auto': True}, + {'exposure_time': 15000}, + {'whitebalance_auto': True}, + {'whitebalance_val': 4500}, + {'brightness': 128}, + {'contrast': 5}, + {'saturation': 32}, + {'sharpness': 2}, + {'gain': 128} + ] + ) + azure_kinect_debugger_node = launch_ros.actions.Node( + package='azure_kinect_debugger', + executable='kinect_subscriber' + ) + + return launch.LaunchDescription([ + azure_kinect_ros_driver_node, + azure_kinect_debugger_node, + ]) + + +if __name__ == '__main__': + generate_launch_description() diff --git a/src/k4a_ros_color_params.cpp b/src/k4a_ros_color_params.cpp new file mode 100644 index 00000000..15c24fe8 --- /dev/null +++ b/src/k4a_ros_color_params.cpp @@ -0,0 +1,151 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +// Associated header +// +#include "azure_kinect_ros_driver/k4a_ros_color_params.h" + +// System headers +// + +// Library headers +// +#include +#include + +// Project headers +// + +K4AROSColorParams::K4AROSColorParams() : rclcpp::Node("k4a_ros_color_params_node") {} + +bool K4AROSColorParams::SetColorConfig(k4a::device &device) +{ + if (exposure_auto) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting exposure mode to AUTO"); + device.set_color_control(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, K4A_COLOR_CONTROL_MODE_AUTO, 0); + } + else + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting exposure mode to MANUAL and exposure time: " << exposure_time); + device.set_color_control(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, K4A_COLOR_CONTROL_MODE_MANUAL, exposure_time); + } + + if (whitebalance_auto) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting whitebalance mode to AUTO"); + device.set_color_control(K4A_COLOR_CONTROL_WHITEBALANCE, K4A_COLOR_CONTROL_MODE_AUTO, 0); + } + else + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting whitebalance mode to MANUAL and value: " << whitebalance_val); + + if (whitebalance_val >= 2500 && whitebalance_val <= 12500 && whitebalance_val % 10 == 0) + device.set_color_control(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, K4A_COLOR_CONTROL_MODE_MANUAL, + whitebalance_val); + else + RCLCPP_ERROR_STREAM(this->get_logger(), "Whitebalance value not valid: " << whitebalance_val + << "\n Value must be between 2500 and 12500 and evenly divisible by 10"); + } + + if (brightness >= 0 && brightness <= 255) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting brightness to: " << brightness); + device.set_color_control(K4A_COLOR_CONTROL_BRIGHTNESS, K4A_COLOR_CONTROL_MODE_MANUAL, brightness); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Brightness value not valid: " << brightness << "\n Value must be between 0 and 255"); + } + + if (contrast >= 0 && contrast <= 10) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting contrast to: " << contrast); + device.set_color_control(K4A_COLOR_CONTROL_CONTRAST, K4A_COLOR_CONTROL_MODE_MANUAL, contrast); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Contrast value not valid: " << contrast << "\n Value must be between 0 and 10"); + } + + if (saturation >= 0 && contrast <= 63) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting saturation to: " << saturation); + device.set_color_control(K4A_COLOR_CONTROL_SATURATION, K4A_COLOR_CONTROL_MODE_MANUAL, saturation); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Saturation value not valid: " << saturation << "\n Value must be between 0 and 63"); + } + + if (sharpness >= 0 && sharpness <= 4) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting sharpness to: " << sharpness); + device.set_color_control(K4A_COLOR_CONTROL_SHARPNESS, K4A_COLOR_CONTROL_MODE_MANUAL, sharpness); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Sharpness value not valid: " << sharpness << "\n Value must be between 0 and 4"); + } + + if (gain >= 0 && gain <= 255) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting gain to: " << gain); + device.set_color_control(K4A_COLOR_CONTROL_GAIN, K4A_COLOR_CONTROL_MODE_MANUAL, gain); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Gain value not valid: " << gain << "\n Value must be between 0 and 255"); + } + + if (backlight_compensation) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting backlight compensation to TRUE"); + device.set_color_control(K4A_COLOR_CONTROL_BACKLIGHT_COMPENSATION, K4A_COLOR_CONTROL_MODE_MANUAL, 1); + } + else + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting backlight compensation to FALSE"); + device.set_color_control(K4A_COLOR_CONTROL_BACKLIGHT_COMPENSATION, K4A_COLOR_CONTROL_MODE_MANUAL, 0); + } + + if (powerline_frequency == 1) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting powerline frequency to 50 Hz"); + device.set_color_control(K4A_COLOR_CONTROL_POWERLINE_FREQUENCY, K4A_COLOR_CONTROL_MODE_MANUAL, 1); + } + else if (powerline_frequency == 2) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Setting powerline frequency to 60 Hz"); + device.set_color_control(K4A_COLOR_CONTROL_POWERLINE_FREQUENCY, K4A_COLOR_CONTROL_MODE_MANUAL, 2); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Powerline frequency must be either 1 (50 Hz) or 2 (60Hz), and was set as: " + << powerline_frequency); + } + + return true; +} + +void K4AROSColorParams::Help() +{ +#define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) \ + RCLCPP_INFO(this->get_logger(),"#param_variable - #param_type : param_help_string (#param_default_val)"); + + ROS_PARAM_LIST_COLOR +#undef LIST_ENTRY +} + +void K4AROSColorParams::Print() +{ +#define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) \ + RCLCPP_INFO_STREAM(this->get_logger(),"" << #param_variable << " - " << #param_type " : " << param_variable); + + ROS_PARAM_LIST_COLOR +#undef LIST_ENTRY +} diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index b17884fd..a037fec1 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -84,6 +84,24 @@ K4AROSDevice::K4AROSDevice() ROS_PARAM_LIST #undef LIST_ENTRY + this->declare_parameter("exposure_auto", rclcpp::ParameterValue(true)); + this->declare_parameter("exposure_time", rclcpp::ParameterValue(0)); + this->declare_parameter("whitebalance_auto", rclcpp::ParameterValue(true)); + this->declare_parameter("whitebalance_val", rclcpp::ParameterValue(4500)); + this->declare_parameter("brightness", rclcpp::ParameterValue(128)); + this->declare_parameter("contrast", rclcpp::ParameterValue(5)); + this->declare_parameter("saturation", rclcpp::ParameterValue(32)); + this->declare_parameter("sharpness", rclcpp::ParameterValue(2)); + this->declare_parameter("gain", rclcpp::ParameterValue(128)); + this->declare_parameter("powerline_frequency", rclcpp::ParameterValue(1)); + this->declare_parameter("backlight_compensation", rclcpp::ParameterValue(false)); + +#define LIST_ENTRY(param_variable, param_help_string, param_type, param_default_val) \ + this->get_parameter_or(#param_variable, color_params_.param_variable, param_default_val); + ROS_PARAM_LIST_COLOR +#undef LIST_ENTRY + + if (params_.recording_file != "") { RCLCPP_INFO(this->get_logger(),"Node is started in playback mode"); @@ -354,6 +372,8 @@ k4a_result_t K4AROSDevice::startCameras() if (k4a_device_) { + RCLCPP_INFO_STREAM(this->get_logger(),"Setting color control"); + color_params_.SetColorConfig(k4a_device_); RCLCPP_INFO_STREAM(this->get_logger(),"STARTING CAMERAS"); k4a_device_.start_cameras(&k4a_configuration); }