Merge pull request #38 from nikhil-sethi/devel

Add exposure time and idle time services

micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h

@@ -16,9 +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_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
@@ -30,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();
@@ -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<std_srvs::srv::SetBool::Request> request,
                 std::shared_ptr<std_srvs::srv::SetBool::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
             int Profile2PointCloud();
@@ -103,6 +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<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,6 +250,15 @@ 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_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));
+
     }
 
     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:"<<ret_code);
+            return ret_code;
+        }
+
+        // Check if returned value from laser matches the request
+        unsigned int actual_value = 0;
+        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::GetDuration(unsigned int setting_id, unsigned int* value){
+        int ret_code = 0;
+        ret_code = GetFeature(setting_id, value);
+
+        if (ret_code < GENERAL_FUNCTION_OK){
+            RCLCPP_ERROR_STREAM(LOGGER, "GetFeature failed. Return code: "<<ret_code);
+            return ret_code;
+        }
+
+        // Decode
+        // detailed docs about encoding and decoding here: https://samxl.atlassian.net/l/cp/3fr1eQD0
+
+        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::ServiceSetExposureDuration(
+        const std::shared_ptr<SetDurationRequest> request,
+        std::shared_ptr<SetDurationResponse> 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<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::ServiceSetIdleDuration(
+        const std::shared_ptr<SetDurationRequest> request,
+        std::shared_ptr<SetDurationResponse> 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<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);
+    }  
+
     /* 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

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