DO NOT MERGE Azure Kinect Auto Tune Exposure Node #6
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,158 @@ | ||
| // PlusOne Robotics | ||
| // Author: Shannon Stoehr | ||
| // email: [email protected] | ||
|
|
||
| #pragma once | ||
|
|
||
| // Library headers | ||
| #include <ros/ros.h> | ||
| #include <sensor_msgs/Image.h> | ||
| #include <image_transport/image_transport.h> | ||
| #include <cv_bridge/cv_bridge.h> | ||
| #include <opencv2/opencv.hpp> | ||
| #include <dynamic_reconfigure/client.h> | ||
| #include <k4a/k4a.h> | ||
| #include <mutex> | ||
|
|
||
| // Project headers | ||
| #include "azure_kinect_ros_driver/AzureKinectParamsConfig.h" | ||
| #include "azure_kinect_ros_driver/k4a_update_exposure.h" | ||
| #include "azure_kinect_ros_driver/k4a_auto_tune_exposure.h" | ||
| #include "azure_kinect_ros_driver/k4aCameraExposureServiceErrorCode.h" | ||
|
|
||
| /** | ||
| * @brief Class that allows the user to tune exposure in real time from the command line | ||
| */ | ||
| class K4AExposureCalibration | ||
| { | ||
| public: | ||
| /** @brief default constructor and destructor*/ | ||
| K4AExposureCalibration(); | ||
| ~K4AExposureCalibration(); | ||
|
|
||
| /** | ||
| * @brief initializes calibrator with a provided node handle | ||
| * @details Subscribes the node to /rgb/raw/image. | ||
| * Advertises the k4a_update_exposure service and the k4a_auto_tune_exposure service. | ||
| * @param[in] nh ROS node handle | ||
| */ | ||
| K4AExposureCalibration(ros::NodeHandle& nh); | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /** | ||
| * @brief call k4a_nodelet_manager/set_parameters to update exposure value | ||
| * @param[in] req_exposure new exposure | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| * @return true if exposure is successfully updated | ||
| */ | ||
| bool k4aUpdateExposure(int req_exposure, int& error_code, std::string& res_msg); | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /** | ||
| * @brief auto tune exposure with given target blue value | ||
| * @param[in] target_blue_value requested blue value to tune exposure to | ||
| * @param[out] final_exposure exposure camera is set to after call | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| * @return true if auto tuning exposure is successfully completed | ||
| */ | ||
| bool k4aAutoTuneExposure(int target_blue_value, int& final_exposure, int& error_code, std::string& res_msg); | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /** | ||
| * @brief check if dynamic_reconfigure response has correctly updated exposure | ||
| * @param[in] requested_exposure exposure originally requested in k4aUpdateExposure | ||
| * @param[out] updated_exposure exposure that was returned by the dynamic_reconfigure call | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| * @return true if requested_exposure == updated_exposure | ||
| */ | ||
| bool k4aCameraExposureUpdateCheck(int requested_exposure, int updated_exposure, int& error_code, std::string& res_msg); | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /** | ||
| * @brief check if requested_exposure is in appropriate bounds | ||
| * @param[in] requested_exposure exposure originally requested in k4aUpdateExposure | ||
| * @param[out] updated_exposure exposure that was returned by the dynamic_reconfigure call | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| * @return true if requested_exposure == updated_exposure | ||
| */ | ||
| bool k4aCameraExposureBoundsCheck(int requested_exposure, int& error_code, std::string& res_msg); | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /** | ||
| * @brief check if target_blue_value has been achieved | ||
| * @details When conducting the auto tune loop, this method compares the average blue | ||
| * value of the image at a particular exposure to the desired target average | ||
| * blue value provided by the user in k4aAutoTuneExposure | ||
| * @param[in] target_blue_val target blue value originally requested in k4aAutoTuneExposure | ||
| * @param[in] current_average_blue_value the average blue value of the image at a point in time | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| */ | ||
| bool k4aTargetBlueCheck(int target_blue_val, int current_avg_blue_value, int& error_code, std::string& res_msg); | ||
|
|
||
| /** | ||
| * @brief check if target_blue_value is in appropriate range | ||
| * @param[in] target_blue_value target blue value originally requested in k4aAutoTuneExposure | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| */ | ||
| bool k4aBlueBoundsCheck(int target_blue_value, int& error_code, std::string& res_msg); | ||
|
|
||
| /** | ||
| * @brief did the node receive an image at all? | ||
| * @param[in] mat current OpenCV mat stored in latest_k4a_image for k4aAutoTuneExposure loop | ||
| * @param[out] error_code error code included in response | ||
| * @param[out] res_msg human-readable error message included in response | ||
| */ | ||
| bool k4aImagePopulatedCheck(cv::Mat& mat, int& error_code, std::string& res_msg); | ||
|
|
||
| private: | ||
| /** | ||
| * @brief callback for /rgb/raw/image subscription | ||
| * @details This callback handles updating the current image for the node. | ||
| * It converts the ROS image message to an OpenCV mat for auto tuning calculations. | ||
| * @param[out] msg message received from image transport subscriber subscribed to /rgb/raw/image | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| */ | ||
| void rgbRawImageCallback(const sensor_msgs::ImageConstPtr& msg); | ||
|
|
||
| /** | ||
| * @brief callback for k4aUpdateExposure | ||
| * @details This callback handles requests to update the camera exposure from the | ||
| * k4a_update_exposure service. | ||
| * @param[in] req request received from calling k4a_update_exposure | ||
| * @param[out] res response sent from k4a_update_exposure | ||
| */ | ||
| bool k4aUpdateExposureCallback(azure_kinect_ros_driver::k4a_update_exposure::Request &req, | ||
| azure_kinect_ros_driver::k4a_update_exposure::Response &res); | ||
|
|
||
| /** | ||
| * @brief callback for k4aAutoTuneExposure | ||
| * @details This callback handles requests to update the camera exposure from the | ||
| * k4a_auto_tune_exposure service. | ||
| * @param[in] req request received from calling k4a_auto_tune_exposure | ||
| * @param[out] res response sent from k4a_auto_tune_exposure | ||
| */ | ||
| bool k4aAutoTuneExposureCallback(azure_kinect_ros_driver::k4a_auto_tune_exposure::Request &req, | ||
| azure_kinect_ros_driver::k4a_auto_tune_exposure::Response &res); | ||
|
|
||
| // private members | ||
| ros::NodeHandle nh_; | ||
| image_transport::Subscriber subRGBRaw; | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| ros::ServiceServer update_exposure_service; | ||
| ros::ServiceServer auto_tune_exposure_service; | ||
|
|
||
| // allocate memory space to store latest image | ||
| cv::Mat latest_k4a_image; /** @brief latest image*/ | ||
| cv::Mat* const latest_k4a_image_ptr = &latest_k4a_image; /** @brief pointer to latest image*/ | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| cv_bridge::CvImageConstPtr k4aCvImagePtr; /** @brief pointer to convert ROS image to OpenCV mat*/ | ||
| std::mutex latest_k4a_image_mutex; /** @brief mutex to protect latest image*/ | ||
| azure_kinect_ros_driver::k4aCameraExposureServiceErrorCode k4a_error_code; /** @brief error codes*/ | ||
|
|
||
| // config file info | ||
| // TODO: PULL THESE VALUES FROM CONFIG FILE "azure_kinect_ros_driver/AzureKinectParamsConfig.h" | ||
| const int MIN_EXPOSURE = 488; | ||
|
There was a problem hiding this comment. use uint32_t or uint64_t instead of int. Make it unsigned and either 32 bit or 64 bit. Up to you. |
||
| const int MAX_EXPOSURE = 1000000; | ||
| const int DEFAULT_EXPOSURE = 15625; | ||
| const int EXPOSURE_INC = 250; | ||
| const int MIN_BLUE = 0; | ||
| const int MAX_BLUE = 255; | ||
| }; | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,17 @@ | ||
| # Overall successful exposure calibration | ||
| int8 SUCCESS = 1 | ||
|
|
||
| # Failure to call service '/set_parameters' | ||
| int8 CAMERA_EXPOSURE_SET_FAILURE = -1 | ||
|
|
||
| # Failure to receive Image from callback | ||
| int8 IMAGE_NOT_RECEIVED_FAILURE = -2 | ||
|
|
||
| # Requested camera exposure is out of bounds Failure | ||
ShannonPlusOne marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| int8 REQUESTED_CAMERA_EXPOSURE_OUT_OF_BOUNDS_FAILURE = -3 | ||
|
|
||
| # Requested camera blue value is out of bounds Failure | ||
| int8 REQUESTED_CAMERA_BLUE_VALUE_OUT_OF_BOUNDS_FAILURE = -4 | ||
|
|
||
| # Requested camera blue value hasn't been achieved yet | ||
| int8 REQUESTED_CAMERA_BLUE_VALUE_NOT_MET = -5 | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'd remove the default constructor so as not to allow it. It doesn't make much sense without a NodeHandle anyway.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
So, unfortunately for now these are required in order for the current catkin testing setup to work.