Merge branch 'devel' into intensity_pcd

micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h

@@ -16,11 +16,12 @@
 #include <micro_epsilon_scancontrol_msgs/srv/get_resolution.hpp>
 #include <micro_epsilon_scancontrol_msgs/srv/set_resolution.hpp>
 #include <micro_epsilon_scancontrol_msgs/srv/get_available_resolutions.hpp>
-#include <micro_epsilon_scancontrol_msgs/srv/get_time.hpp>
-#include <micro_epsilon_scancontrol_msgs/srv/set_time.hpp>
+#include <micro_epsilon_scancontrol_msgs/srv/get_duration.hpp>
+#include <micro_epsilon_scancontrol_msgs/srv/set_duration.hpp>
 
 #include <llt.h>
 #include <mescan.h>
+#include <string>
 
 #define MAX_DEVICE_INTERFACE_COUNT 6
 #define MAX_RESOLUTION_COUNT 6
@@ -32,14 +33,25 @@
 typedef pcl::PointCloud<pcl::PointXYZI> 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();
@@ -50,8 +62,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;};
@@ -79,18 +91,18 @@ namespace scancontrol_driver
             void ServiceInvertX(
                 const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
                 std::shared_ptr<std_srvs::srv::SetBool::Response> response);
-            void ServiceSetExposureTime(
-                const std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Request> request,
-                std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Response> response);
-            void ServiceGetExposureTime(
-                const std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Request> request,
-                std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Response> response);
-            void ServiceSetIdleTime(
-                const std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Request> request,
-                std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Response> response);
-            void ServiceGetIdleTime(
-                const std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Request> request,
-                std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Response> response);
+            void ServiceSetExposureDuration(
+                const std::shared_ptr<SetDurationRequest> request,
+                std::shared_ptr<SetDurationResponse> response);
+            void ServiceGetExposureDuration(
+                const std::shared_ptr<GetDurationRequest> request,
+                std::shared_ptr<GetDurationResponse> response);
+            void ServiceSetIdleDuration(
+                const std::shared_ptr<SetDurationRequest> request,
+                std::shared_ptr<SetDurationResponse> response);
+            void ServiceGetIdleDuration(
+                const std::shared_ptr<GetDurationRequest> request,
+                std::shared_ptr<GetDurationResponse> response);
 
         private:
             // Profile functions
@@ -120,10 +132,10 @@ namespace scancontrol_driver
             rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::GetResolution>::SharedPtr get_resolution_srv;
             rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::SetResolution>::SharedPtr set_resolution_srv;
             rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::GetAvailableResolutions>::SharedPtr get_available_resolutions_srv;
-            rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::SetTime>::SharedPtr set_exposure_time_srv;
-            rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::GetTime>::SharedPtr get_exposure_time_srv;
-            rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::SetTime>::SharedPtr set_idle_time_srv;
-            rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::GetTime>::SharedPtr get_idle_time_srv;
+            rclcpp::Service<SetDurationSrv>::SharedPtr set_exposure_duration_srv;
+            rclcpp::Service<GetDurationSrv>::SharedPtr get_exposure_duration_srv;
+            rclcpp::Service<SetDurationSrv>::SharedPtr set_idle_duration_srv;
+            rclcpp::Service<GetDurationSrv>::SharedPtr get_idle_duration_srv;
             rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr invert_z_srv;
             rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr invert_x_srv;
 

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,14 +250,14 @@ namespace scancontrol_driver
             "~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2));
         invert_x_srv = this->create_service<std_srvs::srv::SetBool>(
             "~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2));
-        set_exposure_time_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetTime>(
-            "~/set_exposure_time", std::bind(&ScanControlDriver::ServiceSetExposureTime, this, _1, _2));
-        get_exposure_time_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetTime>(
-            "~/get_exposure_time", std::bind(&ScanControlDriver::ServiceGetExposureTime, this, _1, _2));
-        set_idle_time_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetTime>(
-            "~/set_idle_time", std::bind(&ScanControlDriver::ServiceSetIdleTime, this, _1, _2));
-        get_idle_time_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetTime>(
-            "~/get_idle_time", std::bind(&ScanControlDriver::ServiceGetIdleTime, this, _1, _2));
+        set_exposure_duration_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetDuration>(
+            "~/set_exposure_duration", std::bind(&ScanControlDriver::ServiceSetExposureDuration, this, _1, _2));
+        get_exposure_duration_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetDuration>(
+            "~/get_exposure_duration", std::bind(&ScanControlDriver::ServiceGetExposureDuration, this, _1, _2));
+        set_idle_duration_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetDuration>(
+            "~/set_idle_duration", std::bind(&ScanControlDriver::ServiceSetIdleDuration, this, _1, _2));
+        get_idle_duration_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetDuration>(
+            "~/get_idle_duration", std::bind(&ScanControlDriver::ServiceGetIdleDuration, this, _1, _2));
 
     }
 
@@ -571,7 +571,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
@@ -591,15 +591,15 @@ 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;
         }
         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);
 
@@ -621,38 +621,38 @@ namespace scancontrol_driver
 
 
     // a wrapper on setfeature to use proper encoding 
-    void ScanControlDriver::ServiceSetExposureTime(
-        const std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Request> request,
-        std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Response> response){
+    void ScanControlDriver::ServiceSetExposureDuration(
+        const std::shared_ptr<SetDurationRequest> request,
+        std::shared_ptr<SetDurationResponse> 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<micro_epsilon_scancontrol_msgs::srv::GetTime::Request> request,
-        std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Response> response){
-        response->return_code = GetTime(FEATURE_FUNCTION_EXPOSURE_TIME, &(response->time));
+    void ScanControlDriver::ServiceGetExposureDuration(
+        const std::shared_ptr<GetDurationRequest> request,
+        std::shared_ptr<GetDurationResponse> 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<micro_epsilon_scancontrol_msgs::srv::SetTime::Request> request,
-        std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::SetTime::Response> response){
+    void ScanControlDriver::ServiceSetIdleDuration(
+        const std::shared_ptr<SetDurationRequest> request,
+        std::shared_ptr<SetDurationResponse> 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<micro_epsilon_scancontrol_msgs::srv::GetTime::Request> request,
-        std::shared_ptr<micro_epsilon_scancontrol_msgs::srv::GetTime::Response> response){
-        response->return_code = GetTime(FEATURE_FUNCTION_IDLE_TIME, &(response->time));
+    void ScanControlDriver::ServiceGetIdleDuration(
+        const std::shared_ptr<GetDurationRequest> request,
+        std::shared_ptr<GetDurationResponse> response){
+        response->return_code = GetDuration(FEATURE_FUNCTION_IDLE_TIME, &(response->duration));
         response->success = !(response->return_code < GENERAL_FUNCTION_OK);
     }  
 

micro_epsilon_scancontrol_driver/src/node.cpp

@@ -1,6 +1,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "micro_epsilon_scancontrol_driver/driver.h"
-#include <thread>
+
 #include <memory>
 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<scancontrol_driver::ScanControlDriver> driver = std::make_shared<scancontrol_driver::ScanControlDriver>();
+        std::shared_ptr<scancontrol_driver::ScanControlDriver> driver = std::make_shared<scancontrol_driver::ScanControlDriver>("scancontrol_driver_node");
         RCLCPP_INFO(logger, "Driver started");
 
         //Turn On Laser

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}

micro_epsilon_scancontrol_msgs/srv/GetTime.srv → 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

micro_epsilon_scancontrol_msgs/srv/SetTime.srv → 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