From 0e7d87f53e1a34b6e2748d1ad9b838554f8fe198 Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Mon, 14 Oct 2024 15:52:56 +0200 Subject: [PATCH 1/3] 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 2/3] 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 3/3] 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); }