Switch from the sensor sdk playback c-api to the c++ wrapper (opencv#73)

RoseFlunder · Stuart Alldritt · commit 60789f0a0fe3 · 2019-09-10T17:17:29.000-07:00
diff --git a/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h b/include/azure_kinect_ros_driver/k4a_calibration_transform_data.h
@@ -12,7 +12,7 @@
 //
 #include <k4a/k4a.h>
 #include <k4a/k4a.hpp>
-#include <k4arecord/playback.h>
+#include <k4arecord/playback.hpp>
 #include <ros/ros.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
@@ -29,7 +29,7 @@ class K4ACalibrationTransformData
 public:
 
     void initialize(const k4a::device &device, k4a_depth_mode_t depthMode, k4a_color_resolution_t resolution, K4AROSDeviceParams params);
-    void initialize(const k4a_playback_t &k4a_playback_handle, K4AROSDeviceParams params);
+    void initialize(const k4a::playback &k4a_playback_handle, K4AROSDeviceParams params);
     int getDepthWidth();
     int getDepthHeight();
     int getColorWidth();
diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h
@@ -14,7 +14,7 @@
 //
 #include <k4a/k4a.h>
 #include <k4a/k4a.hpp>
-#include <k4arecord/playback.h>
+#include <k4arecord/playback.hpp>
 #include <ros/ros.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
@@ -121,7 +121,7 @@ class K4AROSDevice
     K4ACalibrationTransformData calibration_data_;
 
     // K4A Recording
-    k4a_playback_t k4a_playback_handle_;
+    k4a::playback k4a_playback_handle_;
     std::mutex k4a_playback_handle_mutex_;
     
 #if defined(K4A_BODY_TRACKING)
diff --git a/src/k4a_calibration_transform_data.cpp b/src/k4a_calibration_transform_data.cpp
@@ -28,13 +28,10 @@ void K4ACalibrationTransformData::initialize(const k4a::device &device,
     initialize(params);
 }
 
-void K4ACalibrationTransformData::initialize(const k4a_playback_t &k4a_playback_handle,
+void K4ACalibrationTransformData::initialize(const k4a::playback &k4a_playback_handle,
                                              const K4AROSDeviceParams params)
 {
-    if (k4a_playback_get_calibration(k4a_playback_handle, &k4a_calibration_) != K4A_RESULT_SUCCEEDED)
-    {
-        throw std::runtime_error("Failed to get playback calibration!");
-    }
+    k4a_calibration_ = k4a_playback_handle.get_calibration();
     initialize(params);
 }
 
diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp
@@ -52,21 +52,14 @@ K4AROSDevice::K4AROSDevice(const NodeHandle &n, const NodeHandle &p) : k4a_devic
     {
         ROS_INFO("Node is started in playback mode");
         ROS_INFO_STREAM("Try to open recording file " << params_.recording_file);
-        // Open recording file
-        if (k4a_playback_open(params_.recording_file.c_str(), &k4a_playback_handle_) != K4A_RESULT_SUCCEEDED)
-        {
-            ROS_ERROR_STREAM("Failed to open recording");
-            return;
-        }
-        uint64_t recording_length = k4a_playback_get_last_timestamp_usec(k4a_playback_handle_);
-        ROS_INFO_STREAM("Successfully openend recording file. Recording is " << recording_length / 1000000 << " seconds long");
 
-        k4a_record_configuration_t record_config;
-        if (k4a_playback_get_record_configuration(k4a_playback_handle_, &record_config) != K4A_RESULT_SUCCEEDED)
-        {
-            ROS_ERROR_STREAM("Failed to get record configuration");
-            return;
-        }
+        // Open recording file and print its length
+        k4a_playback_handle_ = k4a::playback::open(params_.recording_file.c_str());
+        auto recording_length = k4a_playback_handle_.get_recording_length();
+        ROS_INFO_STREAM("Successfully openend recording file. Recording is " << recording_length.count() / 1000000 << " seconds long");
+
+        // Get the recordings configuration to overwrite node parameters
+        k4a_record_configuration_t record_config = k4a_playback_handle_.get_record_configuration();
 
         // Overwrite fps param with recording configuration for a correct loop rate in the frame publisher thread
         switch (record_config.camera_fps)
@@ -95,7 +88,7 @@ K4AROSDevice::K4AROSDevice(const NodeHandle &n, const NodeHandle &p) : k4a_devic
         // This is necessary because at the moment there are only checks in place which use BgraPixel size
         else if (params_.color_enabled && record_config.color_track_enabled && record_config.color_format != K4A_IMAGE_FORMAT_COLOR_BGRA32)
         {
-            k4a_playback_set_color_conversion(k4a_playback_handle_, K4A_IMAGE_FORMAT_COLOR_BGRA32);
+            k4a_playback_handle_.set_color_conversion(K4A_IMAGE_FORMAT_COLOR_BGRA32);
         }
 
         // Disable depth if the recording has neither ir track nor depth track
@@ -256,7 +249,7 @@ K4AROSDevice::~K4AROSDevice()
 
     if (k4a_playback_handle_)
     {
-        k4a_playback_close(k4a_playback_handle_);
+        k4a_playback_handle_.close();
     }
 
 #if defined(K4A_BODY_TRACKING)
@@ -833,8 +826,6 @@ void K4AROSDevice::framePublisherThread()
 
     while (running_ && ros::ok() && !ros::isShuttingDown())
     {
-        bool success = true;
-
         if (k4a_device_)
         {
             // TODO: consider appropriate capture timeout based on camera framerate
@@ -848,21 +839,13 @@ void K4AROSDevice::framePublisherThread()
         else if (k4a_playback_handle_)
         {
             std::lock_guard<std::mutex> guard(k4a_playback_handle_mutex_);
-            k4a_capture_t capture_temp;
-            k4a_stream_result_t stream_result = k4a_playback_get_next_capture(k4a_playback_handle_, &capture_temp);
-
-            if (stream_result == K4A_STREAM_RESULT_EOF)
+            if (!k4a_playback_handle_.get_next_capture(&capture))
             {
                 // rewind recording if looping is enabled
                 if (params_.recording_loop_enabled)
                 {
-                    if (k4a_playback_seek_timestamp(k4a_playback_handle_, 0, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED)
-                    {
-                        ROS_FATAL("Error seeking recording to beginning. node cannot continue.");
-                        ros::requestShutdown();
-                        return;
-                    }
-                    stream_result = k4a_playback_get_next_capture(k4a_playback_handle_, &capture_temp);
+                    k4a_playback_handle_.seek_timestamp(std::chrono::microseconds(0), K4A_PLAYBACK_SEEK_BEGIN);
+                    k4a_playback_handle_.get_next_capture(&capture);
                     imu_stream_end_of_file_ = false;
                     last_imu_time_usec_ = 0;
                 }
@@ -873,14 +856,7 @@ void K4AROSDevice::framePublisherThread()
                     return;
                 }
             }
-            else if (stream_result != K4A_STREAM_RESULT_SUCCEEDED)
-            {
-                ROS_FATAL("Failed to get next capture from recording file: node cannot continue.");
-                ros::requestShutdown();
-                return;
-            }
 
-            capture = k4a::capture(capture_temp);
             last_capture_time_usec_ = getCaptureTimestamp(capture).count();
         }
 
@@ -1141,14 +1117,11 @@ void K4AROSDevice::imuPublisherThread()
 {
     ros::Rate loop_rate(300);
 
-    k4a_wait_result_t waitResult;
     k4a_result_t result;
-
     k4a_imu_sample_t sample;
 
     while (running_ && ros::ok() && !ros::isShuttingDown())
     {
-
         if (k4a_device_)
         {
             // IMU messages are delivered in batches at 300 Hz. Drain the queue of IMU messages by
@@ -1177,12 +1150,11 @@ void K4AROSDevice::imuPublisherThread()
             while (last_imu_time_usec_ <= last_capture_time_usec_ && !imu_stream_end_of_file_)
             {
                 std::lock_guard<std::mutex> guard(k4a_playback_handle_mutex_);
-                k4a_stream_result_t stream_result = k4a_playback_get_next_imu_sample(k4a_playback_handle_, &sample);
-                if (stream_result == K4A_STREAM_RESULT_EOF)
+                if (!k4a_playback_handle_.get_next_imu_sample(&sample))
                 {
                     imu_stream_end_of_file_ = true;
                 }
-                else if (stream_result == K4A_STREAM_RESULT_SUCCEEDED)
+                else
                 {
                     ImuPtr imu_msg(new Imu);
                     result = getImuFrame(sample, imu_msg);
@@ -1192,13 +1164,6 @@ void K4AROSDevice::imuPublisherThread()
 
                     last_imu_time_usec_ = sample.acc_timestamp_usec;
                 }
-                else
-                {
-                    ROS_FATAL("Failed to get next imu sample from recording. node cannot continue.");
-                    ros::requestShutdown();
-                    return;
-                }
-                
             }
         }
 

Original file line number	Diff line number	Diff line change
`@@ -28,13 +28,10 @@ void K4ACalibrationTransformData::initialize(const k4a::device &device,`
`28`	`28`	`initialize(params);`
`29`	`29`	`}`
`30`	`30`
`31`		`-void K4ACalibrationTransformData::initialize(const k4a_playback_t &k4a_playback_handle,`
	`31`	`+void K4ACalibrationTransformData::initialize(const k4a::playback &k4a_playback_handle,`
`32`	`32`	`const K4AROSDeviceParams params)`
`33`	`33`	`{`
`34`		`- if (k4a_playback_get_calibration(k4a_playback_handle, &k4a_calibration_) != K4A_RESULT_SUCCEEDED)`
`35`		`- {`
`36`		`- throw std::runtime_error("Failed to get playback calibration!");`
`37`		`- }`
	`34`	`+ k4a_calibration_ = k4a_playback_handle.get_calibration();`
`38`	`35`	`initialize(params);`
`39`	`36`	`}`
`40`	`37`
Original file line number	Diff line number	Diff line change
`@@ -52,21 +52,14 @@ K4AROSDevice::K4AROSDevice(const NodeHandle &n, const NodeHandle &p) : k4a_devic`
`52`	`52`	`{`
`53`	`53`	`ROS_INFO("Node is started in playback mode");`
`54`	`54`	`ROS_INFO_STREAM("Try to open recording file " << params_.recording_file);`
`55`		`- // Open recording file`
`56`		`- if (k4a_playback_open(params_.recording_file.c_str(), &k4a_playback_handle_) != K4A_RESULT_SUCCEEDED)`
`57`		`- {`
`58`		`- ROS_ERROR_STREAM("Failed to open recording");`
`59`		`- return;`
`60`		`- }`
`61`		`- uint64_t recording_length = k4a_playback_get_last_timestamp_usec(k4a_playback_handle_);`
`62`		`- ROS_INFO_STREAM("Successfully openend recording file. Recording is " << recording_length / 1000000 << " seconds long");`
`63`	`55`
`64`		`- k4a_record_configuration_t record_config;`
`65`		`- if (k4a_playback_get_record_configuration(k4a_playback_handle_, &record_config) != K4A_RESULT_SUCCEEDED)`
`66`		`- {`
`67`		`- ROS_ERROR_STREAM("Failed to get record configuration");`
`68`		`- return;`
`69`		`- }`
	`56`	`+ // Open recording file and print its length`
	`57`	`+ k4a_playback_handle_ = k4a::playback::open(params_.recording_file.c_str());`
	`58`	`+ auto recording_length = k4a_playback_handle_.get_recording_length();`
	`59`	`+ ROS_INFO_STREAM("Successfully openend recording file. Recording is " << recording_length.count() / 1000000 << " seconds long");`
	`60`	`+`
	`61`	`+ // Get the recordings configuration to overwrite node parameters`
	`62`	`+ k4a_record_configuration_t record_config = k4a_playback_handle_.get_record_configuration();`
`70`	`63`
`71`	`64`	`// Overwrite fps param with recording configuration for a correct loop rate in the frame publisher thread`
`72`	`65`	`switch (record_config.camera_fps)`
`@@ -95,7 +88,7 @@ K4AROSDevice::K4AROSDevice(const NodeHandle &n, const NodeHandle &p) : k4a_devic`
`95`	`88`	`// This is necessary because at the moment there are only checks in place which use BgraPixel size`
`96`	`89`	`else if (params_.color_enabled && record_config.color_track_enabled && record_config.color_format != K4A_IMAGE_FORMAT_COLOR_BGRA32)`
`97`	`90`	`{`
`98`		`- k4a_playback_set_color_conversion(k4a_playback_handle_, K4A_IMAGE_FORMAT_COLOR_BGRA32);`
	`91`	`+ k4a_playback_handle_.set_color_conversion(K4A_IMAGE_FORMAT_COLOR_BGRA32);`
`99`	`92`	`}`
`100`	`93`
`101`	`94`	`// Disable depth if the recording has neither ir track nor depth track`
`@@ -256,7 +249,7 @@ K4AROSDevice::~K4AROSDevice()`
`256`	`249`
`257`	`250`	`if (k4a_playback_handle_)`
`258`	`251`	`{`
`259`		`- k4a_playback_close(k4a_playback_handle_);`
	`252`	`+ k4a_playback_handle_.close();`
`260`	`253`	`}`
`261`	`254`
`262`	`255`	`#if defined(K4A_BODY_TRACKING)`
`@@ -833,8 +826,6 @@ void K4AROSDevice::framePublisherThread()`
`833`	`826`
`834`	`827`	`while (running_ && ros::ok() && !ros::isShuttingDown())`
`835`	`828`	`{`
`836`		`- bool success = true;`
`837`		`-`
`838`	`829`	`if (k4a_device_)`
`839`	`830`	`{`
`840`	`831`	`// TODO: consider appropriate capture timeout based on camera framerate`
`@@ -848,21 +839,13 @@ void K4AROSDevice::framePublisherThread()`
`848`	`839`	`else if (k4a_playback_handle_)`
`849`	`840`	`{`
`850`	`841`	`std::lock_guard<std::mutex> guard(k4a_playback_handle_mutex_);`
`851`		`- k4a_capture_t capture_temp;`
`852`		`- k4a_stream_result_t stream_result = k4a_playback_get_next_capture(k4a_playback_handle_, &capture_temp);`
`853`		`-`
`854`		`- if (stream_result == K4A_STREAM_RESULT_EOF)`
	`842`	`+ if (!k4a_playback_handle_.get_next_capture(&capture))`
`855`	`843`	`{`
`856`	`844`	`// rewind recording if looping is enabled`
`857`	`845`	`if (params_.recording_loop_enabled)`
`858`	`846`	`{`
`859`		`- if (k4a_playback_seek_timestamp(k4a_playback_handle_, 0, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED)`
`860`		`- {`
`861`		`- ROS_FATAL("Error seeking recording to beginning. node cannot continue.");`
`862`		`- ros::requestShutdown();`
`863`		`- return;`
`864`		`- }`
`865`		`- stream_result = k4a_playback_get_next_capture(k4a_playback_handle_, &capture_temp);`
	`847`	`+ k4a_playback_handle_.seek_timestamp(std::chrono::microseconds(0), K4A_PLAYBACK_SEEK_BEGIN);`
	`848`	`+ k4a_playback_handle_.get_next_capture(&capture);`
`866`	`849`	`imu_stream_end_of_file_ = false;`
`867`	`850`	`last_imu_time_usec_ = 0;`
`868`	`851`	`}`
`@@ -873,14 +856,7 @@ void K4AROSDevice::framePublisherThread()`
`873`	`856`	`return;`
`874`	`857`	`}`
`875`	`858`	`}`
`876`		`- else if (stream_result != K4A_STREAM_RESULT_SUCCEEDED)`
`877`		`- {`
`878`		`- ROS_FATAL("Failed to get next capture from recording file: node cannot continue.");`
`879`		`- ros::requestShutdown();`
`880`		`- return;`
`881`		`- }`
`882`	`859`
`883`		`- capture = k4a::capture(capture_temp);`
`884`	`860`	`last_capture_time_usec_ = getCaptureTimestamp(capture).count();`
`885`	`861`	`}`
`886`	`862`
`@@ -1141,14 +1117,11 @@ void K4AROSDevice::imuPublisherThread()`
`1141`	`1117`	`{`
`1142`	`1118`	`ros::Rate loop_rate(300);`
`1143`	`1119`
`1144`		`- k4a_wait_result_t waitResult;`
`1145`	`1120`	`k4a_result_t result;`
`1146`		`-`
`1147`	`1121`	`k4a_imu_sample_t sample;`
`1148`	`1122`
`1149`	`1123`	`while (running_ && ros::ok() && !ros::isShuttingDown())`
`1150`	`1124`	`{`
`1151`		`-`
`1152`	`1125`	`if (k4a_device_)`
`1153`	`1126`	`{`
`1154`	`1127`	`// IMU messages are delivered in batches at 300 Hz. Drain the queue of IMU messages by`
`@@ -1177,12 +1150,11 @@ void K4AROSDevice::imuPublisherThread()`
`1177`	`1150`	`while (last_imu_time_usec_ <= last_capture_time_usec_ && !imu_stream_end_of_file_)`
`1178`	`1151`	`{`
`1179`	`1152`	`std::lock_guard<std::mutex> guard(k4a_playback_handle_mutex_);`
`1180`		`- k4a_stream_result_t stream_result = k4a_playback_get_next_imu_sample(k4a_playback_handle_, &sample);`
`1181`		`- if (stream_result == K4A_STREAM_RESULT_EOF)`
	`1153`	`+ if (!k4a_playback_handle_.get_next_imu_sample(&sample))`
`1182`	`1154`	`{`
`1183`	`1155`	`imu_stream_end_of_file_ = true;`
`1184`	`1156`	`}`
`1185`		`- else if (stream_result == K4A_STREAM_RESULT_SUCCEEDED)`
	`1157`	`+ else`
`1186`	`1158`	`{`
`1187`	`1159`	`ImuPtr imu_msg(new Imu);`
`1188`	`1160`	`result = getImuFrame(sample, imu_msg);`
`@@ -1192,13 +1164,6 @@ void K4AROSDevice::imuPublisherThread()`
`1192`	`1164`
`1193`	`1165`	`last_imu_time_usec_ = sample.acc_timestamp_usec;`
`1194`	`1166`	`}`
`1195`		`- else`
`1196`		`- {`
`1197`		`- ROS_FATAL("Failed to get next imu sample from recording. node cannot continue.");`
`1198`		`- ros::requestShutdown();`
`1199`		`- return;`
`1200`		`- }`
`1201`		`-`
`1202`	`1167`	`}`
`1203`	`1168`	`}`
`1204`	`1169`