From 7cb609236c961ef4408b9f787141416a86147541 Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Thu, 3 Oct 2024 14:38:54 +0200 Subject: [PATCH 1/5] Add exposure and idle time services --- .../micro_epsilon_scancontrol_driver/driver.h | 23 +++- .../src/driver.cpp | 106 +++++++++++++++++- micro_epsilon_scancontrol_msgs/CMakeLists.txt | 2 + .../srv/GetTime.srv | 3 + .../srv/SetTime.srv | 4 + 5 files changed, 136 insertions(+), 2 deletions(-) create mode 100644 micro_epsilon_scancontrol_msgs/srv/GetTime.srv create mode 100644 micro_epsilon_scancontrol_msgs/srv/SetTime.srv diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index 4aafce6..c21392f 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include #include @@ -47,6 +49,9 @@ namespace scancontrol_driver // Device setting functions int GetFeature(unsigned int setting_id, unsigned int *value); int SetFeature(unsigned int setting_id, unsigned int value); + // specialised features specific to encoding/decoding pattern + int SetTime(unsigned int setting_id, unsigned int value); + int GetTime(unsigned int setting_id, unsigned int* value); // Get configuration parameters std::string serial() const {return config_.serial;}; @@ -74,7 +79,19 @@ namespace scancontrol_driver void ServiceInvertX( const std::shared_ptr request, std::shared_ptr response); - + void ServiceSetExposureTime( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetExposureTime( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceSetIdleTime( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetIdleTime( + const std::shared_ptr request, + std::shared_ptr response); + private: // Profile functions int Profile2PointCloud(); @@ -103,6 +120,10 @@ namespace scancontrol_driver rclcpp::Service::SharedPtr get_resolution_srv; rclcpp::Service::SharedPtr set_resolution_srv; rclcpp::Service::SharedPtr get_available_resolutions_srv; + rclcpp::Service::SharedPtr set_exposure_time_srv; + rclcpp::Service::SharedPtr get_exposure_time_srv; + rclcpp::Service::SharedPtr set_idle_time_srv; + rclcpp::Service::SharedPtr get_idle_time_srv; rclcpp::Service::SharedPtr invert_z_srv; rclcpp::Service::SharedPtr invert_x_srv; diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index 1f20523..d710ab2 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -250,6 +250,15 @@ namespace scancontrol_driver "~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2)); invert_x_srv = this->create_service( "~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2)); + set_exposure_time_srv = this->create_service( + "~/set_exposure_time", std::bind(&ScanControlDriver::ServiceSetExposureTime, this, _1, _2)); + get_exposure_time_srv = this->create_service( + "~/get_exposure_time", std::bind(&ScanControlDriver::ServiceGetExposureTime, this, _1, _2)); + set_idle_time_srv = this->create_service( + "~/set_idle_time", std::bind(&ScanControlDriver::ServiceSetIdleTime, this, _1, _2)); + get_idle_time_srv = this->create_service( + "~/get_idle_time", std::bind(&ScanControlDriver::ServiceGetIdleTime, this, _1, _2)); + } int ScanControlDriver::SetPartialProfile(int &resolution){ @@ -360,7 +369,8 @@ namespace scancontrol_driver /* Retrieve the current value of a setting/feature. Consult the scanCONTROL API documentation for a list of available features */ int ScanControlDriver::GetFeature(unsigned int setting_id, unsigned int *value){ int return_code = 0; - if (return_code = device_interface_ptr->GetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ + return_code = device_interface_ptr->GetFeature(setting_id, value); + if (return_code < GENERAL_FUNCTION_OK){ RCLCPP_WARN_STREAM(LOGGER, "Setting could not be retrieved. Code: " << return_code); return return_code; } @@ -554,6 +564,100 @@ namespace scancontrol_driver // return true; } + // Generic function for setting a times like exposure and idle time. + // setting_id: + // value: time in microseconds. min: 1 mus, max: 40950 mus ; eg. 1005 = 1.005ms + int ScanControlDriver::SetTime(unsigned int setting_id, unsigned int value){ + int ret_code; + + // encoded value in 1 mus steps. + uint32_t remainder = ((value % 10) << 12) & 0xF000; // Remainder is left shifted first and bits 0-12 are masked. the quotient will occupy this area. + uint32_t quotient = ((value / 10)) & 0xFFF; // take the quotient and mask bits 12-15 + uint32_t value_encoded = remainder + quotient; + + ret_code = SetFeature(setting_id, value_encoded); + + // success if value was sent AND set. + if (ret_code < GENERAL_FUNCTION_OK){ + RCLCPP_INFO_STREAM(LOGGER, "SetFeature failed."); + return ret_code; + } + + unsigned int actual_value = 0; + ret_code = GetTime(setting_id, &actual_value); + if (actual_value != value){ + RCLCPP_INFO(LOGGER, "Requested value and actual value do not match. "); + return ret_code; + } + return GENERAL_FUNCTION_OK; + } + + int ScanControlDriver::GetTime(unsigned int setting_id, unsigned int* value){ + int ret_code = 0; + ret_code = GetFeature(setting_id, value); + + if (ret_code < GENERAL_FUNCTION_OK){ + return ret_code; + } + + // Decode + uint32_t quotient = *value & 0xFFF; // Extract ExposureTime / 10 + uint32_t remainder = (*value >> 12) & 0xF; // Extract ExposureTime % 10 + *value = (quotient * 10) + remainder; // Reconstruct ExposureTime + + return GENERAL_FUNCTION_OK; + + } + + + // a wrapper on setfeature to use proper encoding + void ScanControlDriver::ServiceSetExposureTime( + const std::shared_ptr request, + std::shared_ptr response){ + + int ret_code = SetTime(FEATURE_FUNCTION_EXPOSURE_TIME, request->time); + + if (ret_code < GENERAL_FUNCTION_OK){ + RCLCPP_INFO_STREAM(LOGGER, "Setting exposure time failed with error code: "<success = false; + } + else{ + response->success = true; + } + response->return_code = ret_code; + } + + // a wrapper on getfeature to use proper decoding + void ScanControlDriver::ServiceGetExposureTime( + const std::shared_ptr request, + std::shared_ptr response){ + response->return_code = GetTime(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->time)); + } + + // a wrapper on setfeature to use proper encoding + void ScanControlDriver::ServiceSetIdleTime( + const std::shared_ptr request, + std::shared_ptr response){ + + int ret_code = SetTime(FEATURE_FUNCTION_IDLE_TIME, request->time); + + if (ret_code < GENERAL_FUNCTION_OK){ + RCLCPP_INFO_STREAM(LOGGER, "Setting idle time failed with error code: "<success = false; + } + else{ + response->success = true; + } + response->return_code = ret_code; + } + + // a wrapper on getfeature to use proper decoding + void ScanControlDriver::ServiceGetIdleTime( + const std::shared_ptr request, + std::shared_ptr response){ + response->return_code = GetTime(FEATURE_FUNCTION_IDLE_TIME, &(response->time)); + } + /* Callback for when a new profile is read, for use with the scanCONTROL API. */ void NewProfileCallback(const void * data, size_t data_size, gpointer user_data){ // Cast user_data to a driver class pointer and process and publish point cloud diff --git a/micro_epsilon_scancontrol_msgs/CMakeLists.txt b/micro_epsilon_scancontrol_msgs/CMakeLists.txt index 2997947..1dc75d7 100644 --- a/micro_epsilon_scancontrol_msgs/CMakeLists.txt +++ b/micro_epsilon_scancontrol_msgs/CMakeLists.txt @@ -16,6 +16,8 @@ set(srv_files "srv/GetResolution.srv" "srv/SetFeature.srv" "srv/SetResolution.srv" + "srv/GetTime.srv" + "srv/SetTime.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/micro_epsilon_scancontrol_msgs/srv/GetTime.srv b/micro_epsilon_scancontrol_msgs/srv/GetTime.srv new file mode 100644 index 0000000..7326601 --- /dev/null +++ b/micro_epsilon_scancontrol_msgs/srv/GetTime.srv @@ -0,0 +1,3 @@ +--- +uint32 time +int32 return_code \ No newline at end of file diff --git a/micro_epsilon_scancontrol_msgs/srv/SetTime.srv b/micro_epsilon_scancontrol_msgs/srv/SetTime.srv new file mode 100644 index 0000000..0178f33 --- /dev/null +++ b/micro_epsilon_scancontrol_msgs/srv/SetTime.srv @@ -0,0 +1,4 @@ +uint32 time +--- +int32 return_code +int32 success \ No newline at end of file From 360b3e839a4e5adc766a7e719f850f04773a8aeb Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Thu, 10 Oct 2024 13:56:53 +0200 Subject: [PATCH 2/5] Address PR review from #38 --- .../src/driver.cpp | 36 ++-- micro_epsilon_scancontrol_driver/src/test.cpp | 163 ++++++++++++++++++ .../srv/GetTime.srv | 5 +- .../srv/SetTime.srv | 6 +- 4 files changed, 184 insertions(+), 26 deletions(-) create mode 100644 micro_epsilon_scancontrol_driver/src/test.cpp diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index d710ab2..eb87e9c 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -451,7 +451,7 @@ namespace scancontrol_driver return; } - // // Change of resolution was succesull + // Change of resolution was successful config_.resolution = request->resolution; // return true; } @@ -570,23 +570,26 @@ namespace scancontrol_driver int ScanControlDriver::SetTime(unsigned int setting_id, unsigned int value){ int ret_code; + // detailed docs about encoding and decoding here: https://samxl.atlassian.net/l/cp/3fr1eQD0 + // encoded value in 1 mus steps. uint32_t remainder = ((value % 10) << 12) & 0xF000; // Remainder is left shifted first and bits 0-12 are masked. the quotient will occupy this area. uint32_t quotient = ((value / 10)) & 0xFFF; // take the quotient and mask bits 12-15 - uint32_t value_encoded = remainder + quotient; + uint32_t encoded_value = remainder + quotient; - ret_code = SetFeature(setting_id, value_encoded); + ret_code = SetFeature(setting_id, encoded_value); // success if value was sent AND set. if (ret_code < GENERAL_FUNCTION_OK){ - RCLCPP_INFO_STREAM(LOGGER, "SetFeature failed."); + RCLCPP_ERROR_STREAM(LOGGER, "SetFeature failed. Return code:"<> 12) & 0xF; // Extract ExposureTime % 10 *value = (quotient * 10) + remainder; // Reconstruct ExposureTime @@ -616,14 +622,7 @@ namespace scancontrol_driver std::shared_ptr response){ int ret_code = SetTime(FEATURE_FUNCTION_EXPOSURE_TIME, request->time); - - if (ret_code < GENERAL_FUNCTION_OK){ - RCLCPP_INFO_STREAM(LOGGER, "Setting exposure time failed with error code: "<success = false; - } - else{ - response->success = true; - } + response->success = !(ret_code < GENERAL_FUNCTION_OK); response->return_code = ret_code; } @@ -632,6 +631,7 @@ namespace scancontrol_driver const std::shared_ptr request, std::shared_ptr response){ response->return_code = GetTime(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->time)); + response->success = !(response->return_code < GENERAL_FUNCTION_OK); } // a wrapper on setfeature to use proper encoding @@ -640,14 +640,7 @@ namespace scancontrol_driver std::shared_ptr response){ int ret_code = SetTime(FEATURE_FUNCTION_IDLE_TIME, request->time); - - if (ret_code < GENERAL_FUNCTION_OK){ - RCLCPP_INFO_STREAM(LOGGER, "Setting idle time failed with error code: "<success = false; - } - else{ - response->success = true; - } + response->success = !(ret_code < GENERAL_FUNCTION_OK); response->return_code = ret_code; } @@ -656,6 +649,7 @@ namespace scancontrol_driver const std::shared_ptr request, std::shared_ptr response){ response->return_code = GetTime(FEATURE_FUNCTION_IDLE_TIME, &(response->time)); + response->success = !(response->return_code < GENERAL_FUNCTION_OK); } /* Callback for when a new profile is read, for use with the scanCONTROL API. */ diff --git a/micro_epsilon_scancontrol_driver/src/test.cpp b/micro_epsilon_scancontrol_driver/src/test.cpp new file mode 100644 index 0000000..82ab403 --- /dev/null +++ b/micro_epsilon_scancontrol_driver/src/test.cpp @@ -0,0 +1,163 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + + +#define MAX_INTERFACE_COUNT 5 +#define MAX_RESOLUTION 6 + + +guint32 resolution; +std::vector profile_buffer; +TScannerType llt_type; +guint32 profile_count, needed_profile_count; +guint32 profile_data_size; + +EHANDLE* event; + + + +class TestNode : public rclcpp::Node{ + public: + TestNode(); + void getResolutionWrapper(const std::shared_ptr request, std::shared_ptr response); + void setResolutionWrapper(const std::shared_ptr request, std::shared_ptr response); + // void getResolutionWrapper(); + private: + rclcpp::Service::SharedPtr service; + rclcpp::Service::SharedPtr set_resolution_srv; + + std::unique_ptr hLLT; + +}; + +TestNode::TestNode():rclcpp::Node("test_node"){ + RCLCPP_INFO(this->get_logger(), "test node started"); + + gint32 ret = 0; + + char *interfaces[MAX_INTERFACE_COUNT]; + guint32 resolutions[MAX_RESOLUTION]; + guint32 interface_count = 0; + + guint32 idle_time = 3900; + guint32 exposure_time = 100; + + + service = this->create_service("get_resolution2", std::bind(&TestNode::getResolutionWrapper, this, std::placeholders::_1, std::placeholders::_2)); + set_resolution_srv = this->create_service("set_resolution", std::bind(&TestNode::setResolutionWrapper, this, std::placeholders::_1, std::placeholders::_2)); + + if ((ret = CInterfaceLLT::GetDeviceInterfaces(&interfaces[0], MAX_INTERFACE_COUNT)) == + ERROR_GETDEVINTERFACE_REQUEST_COUNT) { + std::cout << "There are more than " << MAX_INTERFACE_COUNT << " scanCONTROL connected" << std::endl; + interface_count = MAX_INTERFACE_COUNT; + } else if (ret < 1) { + std::cout << "A error occured during searching for connected scanCONTROL" << std::endl; + interface_count = 0; + } else { + interface_count = ret; + } + + if (interface_count == 0) { + std::cout << "There is no scanCONTROL connected - Exiting" << std::endl; + exit(0); + } else if (interface_count == 1) { + std::cout << "There is 1 scanCONTROL connected " << std::endl; + } else { + std::cout << "There are " << interface_count << " scanCONTROL connected" << std::endl; + } + + for (guint32 i = 0; i < interface_count; i++) { + std::cout << interfaces[i] << "" << std::endl; + } + + // new LLT instance + hLLT = std::make_unique(); + event = CInterfaceLLT::CreateEvent(); + + if ((ret = hLLT->SetDeviceInterface(interfaces[1])) < GENERAL_FUNCTION_OK) { + std::cout << "Error while setting dev id " << ret << "!" << std::endl; + goto cleanup; + } + + // connect to sensor + if ((ret = hLLT->Connect()) < GENERAL_FUNCTION_OK) { + std::cout << "Error while connecting to camera - Error " << ret << "!" << std::endl; + goto cleanup; + } + + if ((ret = hLLT->GetLLTType(&llt_type)) < GENERAL_FUNCTION_OK) { + std::cout << "Error while GetLLTType!" << std::endl; + goto cleanup; + } + + if (ret == GENERAL_FUNCTION_DEVICE_NAME_NOT_SUPPORTED) { + std::cout << "Can't decode scanCONTROL type. Please contact Micro Epsilon for a newer version of the library." + << std::endl; + goto cleanup; + } + + if (llt_type >= scanCONTROL27xx_25 && llt_type <= scanCONTROL27xx_xxx) { + std::cout << "The scanCONTROL is a scanCONTROL27xx" << std::endl; + } else if (llt_type >= scanCONTROL26xx_25 && llt_type <= scanCONTROL26xx_xxx) { + std::cout << "The scanCONTROL is a scanCONTROL26xx" << std::endl; + } else if (llt_type >= scanCONTROL29xx_25 && llt_type <= scanCONTROL29xx_xxx) { + std::cout << "The scanCONTROL is a scanCONTROL29xx" << std::endl; + } else if (llt_type >= scanCONTROL30xx_25 && llt_type <= scanCONTROL30xx_xxx) { + std::cout << "The scanCONTROL is a scanCONTROL30xx" << std::endl; + } else if (llt_type >= scanCONTROL25xx_25 && llt_type <= scanCONTROL25xx_xxx) { + std::cout << "The scanCONTROL is a scanCONTROL25xx" << std::endl; + } else { + std::cout << "The scanCONTROL is a undefined type\nPlease contact Micro-Epsilon for a newer SDK" << std::endl; + goto cleanup; + } + + std::cout << "Get all possible resolutions" << std::endl; + if ((ret = hLLT->GetResolutions(&resolutions[0], MAX_RESOLUTION)) < GENERAL_FUNCTION_OK) { + std::cout << "Error GetResolutions!" << std::endl; + goto cleanup; + } + uint32_t res; + hLLT->GetResolution(&res); + std::cout< request, std::shared_ptr response){ + + RCLCPP_INFO(this->get_logger(), "service called"); + uint32_t res; + hLLT->GetResolution(&res); + std::cout<return_code = hLLT->GetResolution(&(response->resolution)); + +} + +void TestNode::setResolutionWrapper(const std::shared_ptr request, std::shared_ptr response){ + + RCLCPP_INFO(this->get_logger(), "set resolution srv called"); + + int return_code; + + if (return_code = hLLT->SetResolution(request->resolution) < 1){ + RCLCPP_FATAL_STREAM(this->get_logger(), "Error while setting device resolution! CodeL " << return_code); + } +} + +int main(int argc, char** argv){ + rclcpp::init(argc, argv); + + std::shared_ptr node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + +} \ No newline at end of file diff --git a/micro_epsilon_scancontrol_msgs/srv/GetTime.srv b/micro_epsilon_scancontrol_msgs/srv/GetTime.srv index 7326601..e3797e1 100644 --- a/micro_epsilon_scancontrol_msgs/srv/GetTime.srv +++ b/micro_epsilon_scancontrol_msgs/srv/GetTime.srv @@ -1,3 +1,4 @@ --- -uint32 time -int32 return_code \ No newline at end of file +uint32 time # time in microseconds. Range: 0 - 40959 +int32 return_code # 1 if success, otherwise errorcode from scancontrol SDK +int32 success # True if no errors were encountered from scancontrol SDK diff --git a/micro_epsilon_scancontrol_msgs/srv/SetTime.srv b/micro_epsilon_scancontrol_msgs/srv/SetTime.srv index 0178f33..3659d3b 100644 --- a/micro_epsilon_scancontrol_msgs/srv/SetTime.srv +++ b/micro_epsilon_scancontrol_msgs/srv/SetTime.srv @@ -1,4 +1,4 @@ -uint32 time +uint32 time # time to set in microseconds. Range: 0 - 40959 --- -int32 return_code -int32 success \ No newline at end of file +int32 return_code # 1 if success, otherwise errorcode from scancontrol SDK +int32 success # True if the value was sent successfull , and matches the GetTime value From 0e7d87f53e1a34b6e2748d1ad9b838554f8fe198 Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Mon, 14 Oct 2024 15:52:56 +0200 Subject: [PATCH 3/5] Address review from PR #36 --- .../include/micro_epsilon_scancontrol_driver/driver.h | 3 ++- micro_epsilon_scancontrol_driver/src/driver.cpp | 4 ++-- micro_epsilon_scancontrol_driver/src/node.cpp | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index c21392f..969f1e3 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -21,6 +21,7 @@ #include #include +#include #define MAX_DEVICE_INTERFACE_COUNT 6 #define MAX_RESOLUTION_COUNT 6 @@ -37,7 +38,7 @@ namespace scancontrol_driver { public: // Constructor and destructor - ScanControlDriver(); + ScanControlDriver(const std::string& name); ~ScanControlDriver() {} // Profile functions diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index eb87e9c..de0b815 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -3,9 +3,9 @@ namespace scancontrol_driver { - static const rclcpp::Logger LOGGER = rclcpp::get_logger("scancontrol_driver"); + static const rclcpp::Logger LOGGER = rclcpp::get_logger("scancontrol_driver_node"); // TODO change this into a class variable. or just use this->get_logger()? - ScanControlDriver::ScanControlDriver():Node("scancontrol_driver") + ScanControlDriver::ScanControlDriver(const std::string& name):Node(name) { /* Extract the relevant parameters. diff --git a/micro_epsilon_scancontrol_driver/src/node.cpp b/micro_epsilon_scancontrol_driver/src/node.cpp index f44d3be..cc41528 100644 --- a/micro_epsilon_scancontrol_driver/src/node.cpp +++ b/micro_epsilon_scancontrol_driver/src/node.cpp @@ -1,6 +1,6 @@ #include "rclcpp/rclcpp.hpp" #include "micro_epsilon_scancontrol_driver/driver.h" -#include + #include static const rclcpp::Logger logger = rclcpp::get_logger("scancontrol_driver"); @@ -11,7 +11,7 @@ int main(int argc, char** argv) // Start the driver try { - std::shared_ptr driver = std::make_shared(); + std::shared_ptr driver = std::make_shared("scancontrol_driver_node"); RCLCPP_INFO(logger, "Driver started"); //Turn On Laser From e01f1618d9298654147f901c5052562ad31a106e Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Wed, 16 Oct 2024 15:07:44 +0200 Subject: [PATCH 4/5] Address re-review from #38 --- .../micro_epsilon_scancontrol_driver/driver.h | 52 +++++++++--------- .../src/driver.cpp | 54 +++++++++---------- micro_epsilon_scancontrol_msgs/CMakeLists.txt | 4 +- .../srv/{GetTime.srv => GetDuration.srv} | 2 +- .../srv/{SetTime.srv => SetDuration.srv} | 2 +- 5 files changed, 59 insertions(+), 55 deletions(-) rename micro_epsilon_scancontrol_msgs/srv/{GetTime.srv => GetDuration.srv} (71%) rename micro_epsilon_scancontrol_msgs/srv/{SetTime.srv => SetDuration.srv} (70%) diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index 969f1e3..3666323 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -16,8 +16,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -33,14 +33,18 @@ typedef pcl::PointCloud point_cloud_t; namespace scancontrol_driver -{ +{ + // aliases + using set_duration_srv = micro_epsilon_scancontrol_msgs::srv::SetDuration; + using get_duration_srv = micro_epsilon_scancontrol_msgs::srv::GetDuration; + class ScanControlDriver: public rclcpp::Node { public: // Constructor and destructor - ScanControlDriver(const std::string& name); - ~ScanControlDriver() {} - + explicit ScanControlDriver(const std::string& name); + ~ScanControlDriver() = default; + // Profile functions int SetPartialProfile(int &resolution); int StartProfileTransfer(); @@ -51,8 +55,8 @@ namespace scancontrol_driver int GetFeature(unsigned int setting_id, unsigned int *value); int SetFeature(unsigned int setting_id, unsigned int value); // specialised features specific to encoding/decoding pattern - int SetTime(unsigned int setting_id, unsigned int value); - int GetTime(unsigned int setting_id, unsigned int* value); + int SetDuration(unsigned int setting_id, unsigned int value); + int GetDuration(unsigned int setting_id, unsigned int* value); // Get configuration parameters std::string serial() const {return config_.serial;}; @@ -80,18 +84,18 @@ namespace scancontrol_driver void ServiceInvertX( const std::shared_ptr request, std::shared_ptr response); - void ServiceSetExposureTime( - const std::shared_ptr request, - std::shared_ptr response); - void ServiceGetExposureTime( - const std::shared_ptr request, - std::shared_ptr response); - void ServiceSetIdleTime( - const std::shared_ptr request, - std::shared_ptr response); - void ServiceGetIdleTime( - const std::shared_ptr request, - std::shared_ptr response); + void ServiceSetExposureDuration( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetExposureDuration( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceSetIdleDuration( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetIdleDuration( + const std::shared_ptr request, + std::shared_ptr response); private: // Profile functions @@ -121,10 +125,10 @@ namespace scancontrol_driver rclcpp::Service::SharedPtr get_resolution_srv; rclcpp::Service::SharedPtr set_resolution_srv; rclcpp::Service::SharedPtr get_available_resolutions_srv; - rclcpp::Service::SharedPtr set_exposure_time_srv; - rclcpp::Service::SharedPtr get_exposure_time_srv; - rclcpp::Service::SharedPtr set_idle_time_srv; - rclcpp::Service::SharedPtr get_idle_time_srv; + rclcpp::Service::SharedPtr set_exposure_duration_srv; + rclcpp::Service::SharedPtr get_exposure_duration_srv; + rclcpp::Service::SharedPtr set_idle_duration_srv; + rclcpp::Service::SharedPtr get_idle_duration_srv; rclcpp::Service::SharedPtr invert_z_srv; rclcpp::Service::SharedPtr invert_x_srv; diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index de0b815..b9f0fe7 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -250,14 +250,14 @@ namespace scancontrol_driver "~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2)); invert_x_srv = this->create_service( "~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2)); - set_exposure_time_srv = this->create_service( - "~/set_exposure_time", std::bind(&ScanControlDriver::ServiceSetExposureTime, this, _1, _2)); - get_exposure_time_srv = this->create_service( - "~/get_exposure_time", std::bind(&ScanControlDriver::ServiceGetExposureTime, this, _1, _2)); - set_idle_time_srv = this->create_service( - "~/set_idle_time", std::bind(&ScanControlDriver::ServiceSetIdleTime, this, _1, _2)); - get_idle_time_srv = this->create_service( - "~/get_idle_time", std::bind(&ScanControlDriver::ServiceGetIdleTime, this, _1, _2)); + set_exposure_duration_srv = this->create_service( + "~/set_exposure_duration", std::bind(&ScanControlDriver::ServiceSetExposureDuration, this, _1, _2)); + get_exposure_duration_srv = this->create_service( + "~/get_exposure_duration", std::bind(&ScanControlDriver::ServiceGetExposureDuration, this, _1, _2)); + set_idle_duration_srv = this->create_service( + "~/set_idle_duration", std::bind(&ScanControlDriver::ServiceSetIdleDuration, this, _1, _2)); + get_idle_duration_srv = this->create_service( + "~/get_idle_duration", std::bind(&ScanControlDriver::ServiceGetIdleDuration, this, _1, _2)); } @@ -567,7 +567,7 @@ namespace scancontrol_driver // Generic function for setting a times like exposure and idle time. // setting_id: // value: time in microseconds. min: 1 mus, max: 40950 mus ; eg. 1005 = 1.005ms - int ScanControlDriver::SetTime(unsigned int setting_id, unsigned int value){ + int ScanControlDriver::SetDuration(unsigned int setting_id, unsigned int value){ int ret_code; // detailed docs about encoding and decoding here: https://samxl.atlassian.net/l/cp/3fr1eQD0 @@ -587,7 +587,7 @@ namespace scancontrol_driver // Check if returned value from laser matches the request unsigned int actual_value = 0; - ret_code = GetTime(setting_id, &actual_value); + ret_code = GetDuration(setting_id, &actual_value); if (actual_value != value){ RCLCPP_WARN(LOGGER, "Requested value and actual value do not match. "); return ret_code; @@ -595,7 +595,7 @@ namespace scancontrol_driver return GENERAL_FUNCTION_OK; } - int ScanControlDriver::GetTime(unsigned int setting_id, unsigned int* value){ + int ScanControlDriver::GetDuration(unsigned int setting_id, unsigned int* value){ int ret_code = 0; ret_code = GetFeature(setting_id, value); @@ -617,38 +617,38 @@ namespace scancontrol_driver // a wrapper on setfeature to use proper encoding - void ScanControlDriver::ServiceSetExposureTime( - const std::shared_ptr request, - std::shared_ptr response){ + void ScanControlDriver::ServiceSetExposureDuration( + const std::shared_ptr request, + std::shared_ptr response){ - int ret_code = SetTime(FEATURE_FUNCTION_EXPOSURE_TIME, request->time); + int ret_code = SetDuration(FEATURE_FUNCTION_EXPOSURE_TIME, request->duration); response->success = !(ret_code < GENERAL_FUNCTION_OK); response->return_code = ret_code; } // a wrapper on getfeature to use proper decoding - void ScanControlDriver::ServiceGetExposureTime( - const std::shared_ptr request, - std::shared_ptr response){ - response->return_code = GetTime(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->time)); + void ScanControlDriver::ServiceGetExposureDuration( + const std::shared_ptr request, + std::shared_ptr response){ + response->return_code = GetDuration(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->duration)); response->success = !(response->return_code < GENERAL_FUNCTION_OK); } // a wrapper on setfeature to use proper encoding - void ScanControlDriver::ServiceSetIdleTime( - const std::shared_ptr request, - std::shared_ptr response){ + void ScanControlDriver::ServiceSetIdleDuration( + const std::shared_ptr request, + std::shared_ptr response){ - int ret_code = SetTime(FEATURE_FUNCTION_IDLE_TIME, request->time); + int ret_code = SetDuration(FEATURE_FUNCTION_IDLE_TIME, request->duration); response->success = !(ret_code < GENERAL_FUNCTION_OK); response->return_code = ret_code; } // a wrapper on getfeature to use proper decoding - void ScanControlDriver::ServiceGetIdleTime( - const std::shared_ptr request, - std::shared_ptr response){ - response->return_code = GetTime(FEATURE_FUNCTION_IDLE_TIME, &(response->time)); + void ScanControlDriver::ServiceGetIdleDuration( + const std::shared_ptr request, + std::shared_ptr response){ + response->return_code = GetDuration(FEATURE_FUNCTION_IDLE_TIME, &(response->duration)); response->success = !(response->return_code < GENERAL_FUNCTION_OK); } diff --git a/micro_epsilon_scancontrol_msgs/CMakeLists.txt b/micro_epsilon_scancontrol_msgs/CMakeLists.txt index 1dc75d7..76a3e77 100644 --- a/micro_epsilon_scancontrol_msgs/CMakeLists.txt +++ b/micro_epsilon_scancontrol_msgs/CMakeLists.txt @@ -16,8 +16,8 @@ set(srv_files "srv/GetResolution.srv" "srv/SetFeature.srv" "srv/SetResolution.srv" - "srv/GetTime.srv" - "srv/SetTime.srv" + "srv/GetDuration.srv" + "srv/SetDuration.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/micro_epsilon_scancontrol_msgs/srv/GetTime.srv b/micro_epsilon_scancontrol_msgs/srv/GetDuration.srv similarity index 71% rename from micro_epsilon_scancontrol_msgs/srv/GetTime.srv rename to micro_epsilon_scancontrol_msgs/srv/GetDuration.srv index e3797e1..326dd66 100644 --- a/micro_epsilon_scancontrol_msgs/srv/GetTime.srv +++ b/micro_epsilon_scancontrol_msgs/srv/GetDuration.srv @@ -1,4 +1,4 @@ --- -uint32 time # time in microseconds. Range: 0 - 40959 +uint32 duration # time in microseconds. Range: 0 - 40959 int32 return_code # 1 if success, otherwise errorcode from scancontrol SDK int32 success # True if no errors were encountered from scancontrol SDK diff --git a/micro_epsilon_scancontrol_msgs/srv/SetTime.srv b/micro_epsilon_scancontrol_msgs/srv/SetDuration.srv similarity index 70% rename from micro_epsilon_scancontrol_msgs/srv/SetTime.srv rename to micro_epsilon_scancontrol_msgs/srv/SetDuration.srv index 3659d3b..18387ed 100644 --- a/micro_epsilon_scancontrol_msgs/srv/SetTime.srv +++ b/micro_epsilon_scancontrol_msgs/srv/SetDuration.srv @@ -1,4 +1,4 @@ -uint32 time # time to set in microseconds. Range: 0 - 40959 +uint32 duration # time to set in microseconds. Range: 0 - 40959 --- int32 return_code # 1 if success, otherwise errorcode from scancontrol SDK int32 success # True if the value was sent successfull , and matches the GetTime value From d1e8e9559594703e9097ee67cfb3a0ec3468cb42 Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Wed, 16 Oct 2024 15:59:45 +0200 Subject: [PATCH 5/5] Create more specific aliases (review #38) --- .../micro_epsilon_scancontrol_driver/driver.h | 37 +++++++++++-------- .../src/driver.cpp | 16 ++++---- 2 files changed, 30 insertions(+), 23 deletions(-) diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index 3666323..e7b16db 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -34,12 +34,19 @@ typedef pcl::PointCloud point_cloud_t; namespace scancontrol_driver { - // aliases - using set_duration_srv = micro_epsilon_scancontrol_msgs::srv::SetDuration; - using get_duration_srv = micro_epsilon_scancontrol_msgs::srv::GetDuration; class ScanControlDriver: public rclcpp::Node { + // aliases + using SetDurationSrv = micro_epsilon_scancontrol_msgs::srv::SetDuration; + using SetDurationRequest = SetDurationSrv::Request; + using SetDurationResponse = SetDurationSrv::Response; + + using GetDurationSrv = micro_epsilon_scancontrol_msgs::srv::GetDuration; + using GetDurationRequest = GetDurationSrv::Request; + using GetDurationResponse = GetDurationSrv::Response; + + public: // Constructor and destructor explicit ScanControlDriver(const std::string& name); @@ -85,17 +92,17 @@ namespace scancontrol_driver const std::shared_ptr request, std::shared_ptr response); void ServiceSetExposureDuration( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void ServiceGetExposureDuration( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void ServiceSetIdleDuration( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void ServiceGetIdleDuration( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); private: // Profile functions @@ -125,10 +132,10 @@ namespace scancontrol_driver rclcpp::Service::SharedPtr get_resolution_srv; rclcpp::Service::SharedPtr set_resolution_srv; rclcpp::Service::SharedPtr get_available_resolutions_srv; - rclcpp::Service::SharedPtr set_exposure_duration_srv; - rclcpp::Service::SharedPtr get_exposure_duration_srv; - rclcpp::Service::SharedPtr set_idle_duration_srv; - rclcpp::Service::SharedPtr get_idle_duration_srv; + rclcpp::Service::SharedPtr set_exposure_duration_srv; + rclcpp::Service::SharedPtr get_exposure_duration_srv; + rclcpp::Service::SharedPtr set_idle_duration_srv; + rclcpp::Service::SharedPtr get_idle_duration_srv; rclcpp::Service::SharedPtr invert_z_srv; rclcpp::Service::SharedPtr invert_x_srv; diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index b9f0fe7..dc0697d 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -618,8 +618,8 @@ namespace scancontrol_driver // a wrapper on setfeature to use proper encoding void ScanControlDriver::ServiceSetExposureDuration( - const std::shared_ptr request, - std::shared_ptr response){ + const std::shared_ptr request, + std::shared_ptr response){ int ret_code = SetDuration(FEATURE_FUNCTION_EXPOSURE_TIME, request->duration); response->success = !(ret_code < GENERAL_FUNCTION_OK); @@ -628,16 +628,16 @@ namespace scancontrol_driver // a wrapper on getfeature to use proper decoding void ScanControlDriver::ServiceGetExposureDuration( - const std::shared_ptr request, - std::shared_ptr response){ + const std::shared_ptr request, + std::shared_ptr response){ response->return_code = GetDuration(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->duration)); response->success = !(response->return_code < GENERAL_FUNCTION_OK); } // a wrapper on setfeature to use proper encoding void ScanControlDriver::ServiceSetIdleDuration( - const std::shared_ptr request, - std::shared_ptr response){ + const std::shared_ptr request, + std::shared_ptr response){ int ret_code = SetDuration(FEATURE_FUNCTION_IDLE_TIME, request->duration); response->success = !(ret_code < GENERAL_FUNCTION_OK); @@ -646,8 +646,8 @@ namespace scancontrol_driver // a wrapper on getfeature to use proper decoding void ScanControlDriver::ServiceGetIdleDuration( - const std::shared_ptr request, - std::shared_ptr response){ + const std::shared_ptr request, + std::shared_ptr response){ response->return_code = GetDuration(FEATURE_FUNCTION_IDLE_TIME, &(response->duration)); response->success = !(response->return_code < GENERAL_FUNCTION_OK); }