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..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 @@ -16,9 +16,12 @@ #include #include #include +#include +#include #include #include +#include #define MAX_DEVICE_INTERFACE_COUNT 6 #define MAX_RESOLUTION_COUNT 6 @@ -30,14 +33,25 @@ typedef pcl::PointCloud point_cloud_t; namespace scancontrol_driver -{ +{ + 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 - ScanControlDriver(); - ~ScanControlDriver() {} - + explicit ScanControlDriver(const std::string& name); + ~ScanControlDriver() = default; + // Profile functions int SetPartialProfile(int &resolution); int StartProfileTransfer(); @@ -47,6 +61,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 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;}; @@ -74,7 +91,19 @@ namespace scancontrol_driver void ServiceInvertX( 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 int Profile2PointCloud(); @@ -103,6 +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 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..dc0697d 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. @@ -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_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)); + } 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; } @@ -441,7 +451,7 @@ namespace scancontrol_driver return; } - // // Change of resolution was succesull + // Change of resolution was successful config_.resolution = request->resolution; // return true; } @@ -554,6 +564,94 @@ 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::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 + + // 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 encoded_value = remainder + quotient; + + ret_code = SetFeature(setting_id, encoded_value); + + // success if value was sent AND set. + if (ret_code < GENERAL_FUNCTION_OK){ + RCLCPP_ERROR_STREAM(LOGGER, "SetFeature failed. Return code:"<> 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::ServiceSetExposureDuration( + 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); + response->return_code = ret_code; + } + + // a wrapper on getfeature to use proper decoding + 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::ServiceSetIdleDuration( + 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); + response->return_code = ret_code; + } + + // a wrapper on getfeature to use proper decoding + 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); + } + /* 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_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 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/CMakeLists.txt b/micro_epsilon_scancontrol_msgs/CMakeLists.txt index 2997947..76a3e77 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/GetDuration.srv" + "srv/SetDuration.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/micro_epsilon_scancontrol_msgs/srv/GetDuration.srv b/micro_epsilon_scancontrol_msgs/srv/GetDuration.srv new file mode 100644 index 0000000..326dd66 --- /dev/null +++ b/micro_epsilon_scancontrol_msgs/srv/GetDuration.srv @@ -0,0 +1,4 @@ +--- +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/SetDuration.srv b/micro_epsilon_scancontrol_msgs/srv/SetDuration.srv new file mode 100644 index 0000000..18387ed --- /dev/null +++ b/micro_epsilon_scancontrol_msgs/srv/SetDuration.srv @@ -0,0 +1,4 @@ +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