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 1c20975..fff06bf 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 @@ -1,19 +1,17 @@ #ifndef _SCANCONTROL_DRIVER_H_ #define _SCANCONTROL_DRIVER_H_ -#include -#include -#include - -#include +#include +#include +#include #include -#include #include +#include #include -#include - -#include -#include +#include +#include +#include +#include #define MAX_DEVICE_INTERFACE_COUNT 6 #define MAX_RESOLUTION_COUNT 6 @@ -26,122 +24,135 @@ typedef pcl::PointCloud point_cloud_t; namespace scancontrol_driver { - class ScanControlDriver - { - public: - // Constructor and destructor - ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); - ~ScanControlDriver() {} - - // Profile functions - int SetPartialProfile(int &resolution); - int StartProfileTransfer(); - int StopProfileTransfer(); - int ProcessAndPublishProfile(const void * data, size_t data_size); - - // Device setting functions - int GetFeature(unsigned int setting_id, unsigned int *value); - int SetFeature(unsigned int setting_id, unsigned int value); - - // Get configuration parameters - std::string serial() const {return config_.serial;}; - int resolution() const {return config_.resolution;}; - - // Service Callback - bool ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request &request, micro_epsilon_scancontrol_msgs::SetFeature::Response &response); - bool ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request &request, micro_epsilon_scancontrol_msgs::GetFeature::Response &response); - bool ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request &request, micro_epsilon_scancontrol_msgs::SetResolution::Response &response); - bool ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request &request, micro_epsilon_scancontrol_msgs::GetResolution::Response &response); - bool ServiceGetAvailableResolutions(micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request &request, micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response &response); - bool ServiceInvertZ(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); - bool ServiceInvertX(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); - - private: - // Profile functions - int Profile2PointCloud(); - // Configuration storage - struct - { - std::string frame_id; - std::string model; - std::string serial; - std::string interface; - std::string topic_name; - int resolution; - int pp_start_point; - int pp_start_point_data; - int pp_point_count; - int pp_point_data_width; - } config_; - - bool initialized_ = false; - bool transfer_active_ = false; - - // ROS handles - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - ros::Publisher publisher; - ros::ServiceServer get_feature_srv; - ros::ServiceServer set_feature_srv; - ros::ServiceServer get_resolution_srv; - ros::ServiceServer set_resolution_srv; - ros::ServiceServer get_available_resolutions_srv; - ros::ServiceServer invert_z_srv; - ros::ServiceServer invert_x_srv; - - - // Driver objects - std::unique_ptr device_interface_ptr; - TScannerType device_type; - TPartialProfile t_partial_profile_; - std::vector profile_buffer; - std::vector value_x, value_z; - int lost_values; - unsigned int lost_profiles; - - point_cloud_t::Ptr point_cloud_msg; - - std::map feature2id = { - {"FEATURE_FUNCTION_SERIAL", FEATURE_FUNCTION_SERIAL}, - {"FEATURE_FUNCTION_PEAKFILTER_WIDTH", FEATURE_FUNCTION_PEAKFILTER_WIDTH}, - {"FEATURE_FUNCTION_PEAKFILTER_HEIGHT", FEATURE_FUNCTION_PEAKFILTER_HEIGHT}, - {"FEATURE_FUNCTION_FREE_MEASURINGFIELD_Z", FEATURE_FUNCTION_FREE_MEASURINGFIELD_Z}, - {"FEATURE_FUNCTION_FREE_MEASURINGFIELD_X", FEATURE_FUNCTION_FREE_MEASURINGFIELD_X}, - {"FEATURE_FUNCTION_DYNAMIC_TRACK_DIVISOR", FEATURE_FUNCTION_DYNAMIC_TRACK_DIVISOR}, - {"FEATURE_FUNCTION_DYNAMIC_TRACK_FACTOR", FEATURE_FUNCTION_DYNAMIC_TRACK_FACTOR}, - {"FEATURE_FUNCTION_CALIBRATION_0", FEATURE_FUNCTION_CALIBRATION_0}, - {"FEATURE_FUNCTION_CALIBRATION_1", FEATURE_FUNCTION_CALIBRATION_1}, - {"FEATURE_FUNCTION_CALIBRATION_2", FEATURE_FUNCTION_CALIBRATION_2}, - {"FEATURE_FUNCTION_CALIBRATION_3", FEATURE_FUNCTION_CALIBRATION_3}, - {"FEATURE_FUNCTION_CALIBRATION_4", FEATURE_FUNCTION_CALIBRATION_4}, - {"FEATURE_FUNCTION_CALIBRATION_5", FEATURE_FUNCTION_CALIBRATION_5}, - {"FEATURE_FUNCTION_CALIBRATION_6", FEATURE_FUNCTION_CALIBRATION_6}, - {"FEATURE_FUNCTION_CALIBRATION_7", FEATURE_FUNCTION_CALIBRATION_7}, - {"FEATURE_FUNCTION_LASERPOWER", FEATURE_FUNCTION_LASERPOWER}, - {"FEATURE_FUNCTION_MEASURINGFIELD", FEATURE_FUNCTION_MEASURINGFIELD}, - {"FEATURE_FUNCTION_TRIGGER", FEATURE_FUNCTION_TRIGGER}, - {"FEATURE_FUNCTION_SHUTTERTIME", FEATURE_FUNCTION_SHUTTERTIME}, - {"FEATURE_FUNCTION_IDLETIME", FEATURE_FUNCTION_IDLETIME}, - {"FEATURE_FUNCTION_PROCESSING_PROFILEDATA", FEATURE_FUNCTION_PROCESSING_PROFILEDATA}, - {"FEATURE_FUNCTION_THRESHOLD", FEATURE_FUNCTION_THRESHOLD}, - {"FEATURE_FUNCTION_MAINTENANCEFUNCTIONS", FEATURE_FUNCTION_MAINTENANCEFUNCTIONS}, - {"FEATURE_FUNCTION_REARRANGEMENT_PROFILE", FEATURE_FUNCTION_REARRANGEMENT_PROFILE}, - {"FEATURE_FUNCTION_PROFILE_FILTER", FEATURE_FUNCTION_PROFILE_FILTER}, - {"FEATURE_FUNCTION_RS422_INTERFACE_FUNCTION", FEATURE_FUNCTION_RS422_INTERFACE_FUNCTION}, - {"FEATURE_FUNCTION_PACKET_DELAY", FEATURE_FUNCTION_PACKET_DELAY}, - {"FEATURE_FUNCTION_SHARPNESS", FEATURE_FUNCTION_SHARPNESS}, - {"FEATURE_FUNCTION_TEMPERATURE", FEATURE_FUNCTION_TEMPERATURE}, - {"FEATURE_FUNCTION_SATURATION", FEATURE_FUNCTION_SATURATION}, - {"FEATURE_FUNCTION_CAPTURE_QUALITY", FEATURE_FUNCTION_CAPTURE_QUALITY} - }; - - // - std::list features_with_corruption_risk = {FEATURE_FUNCTION_LASERPOWER, FEATURE_FUNCTION_MEASURINGFIELD, FEATURE_FUNCTION_TRIGGER}; - }; - - void NewProfileCallback(const void * data, size_t data_size, gpointer user_data); - void ControlLostCallback(ArvGvDevice *mydevice, gpointer user_data); - -} // namespace scancontrol_driver - -#endif // _SCANCONTROL_DRIVER_H_ \ No newline at end of file +class ScanControlDriver +{ +public: + // Constructor and destructor + ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); + ~ScanControlDriver() + { + } + + // Profile functions + int SetPartialProfile(int& resolution); + int StartProfileTransfer(); + int StopProfileTransfer(); + int ProcessAndPublishProfile(const void* data, size_t data_size); + + // Device setting functions + int GetFeature(unsigned int setting_id, unsigned int* value); + int SetFeature(unsigned int setting_id, unsigned int value); + + // Get configuration parameters + std::string serial() const + { + return config_.serial; + }; + int resolution() const + { + return config_.resolution; + }; + + // Service Callback + bool ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request& request, + micro_epsilon_scancontrol_msgs::SetFeature::Response& response); + bool ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request& request, + micro_epsilon_scancontrol_msgs::GetFeature::Response& response); + bool ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request& request, + micro_epsilon_scancontrol_msgs::SetResolution::Response& response); + bool ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request& request, + micro_epsilon_scancontrol_msgs::GetResolution::Response& response); + bool ServiceGetAvailableResolutions(micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request& request, + micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response& response); + bool ServiceInvertZ(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); + bool ServiceInvertX(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); + +private: + // Profile functions + int Profile2PointCloud(); + // Configuration storage + struct + { + std::string frame_id; + std::string model; + std::string serial; + std::string interface; + std::string topic_name; + int resolution; + int pp_start_point; + int pp_start_point_data; + int pp_point_count; + int pp_point_data_width; + } config_; + + bool initialized_ = false; + bool transfer_active_ = false; + + // ROS handles + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Publisher publisher; + ros::ServiceServer get_feature_srv; + ros::ServiceServer set_feature_srv; + ros::ServiceServer get_resolution_srv; + ros::ServiceServer set_resolution_srv; + ros::ServiceServer get_available_resolutions_srv; + ros::ServiceServer invert_z_srv; + ros::ServiceServer invert_x_srv; + + // Driver objects + std::unique_ptr device_interface_ptr; + TScannerType device_type; + TPartialProfile t_partial_profile_; + std::vector profile_buffer; + std::vector value_x, value_z; + int lost_values; + unsigned int lost_profiles; + + point_cloud_t::Ptr point_cloud_msg; + + std::map feature2id = { + { "FEATURE_FUNCTION_SERIAL", FEATURE_FUNCTION_SERIAL }, + { "FEATURE_FUNCTION_PEAKFILTER_WIDTH", FEATURE_FUNCTION_PEAKFILTER_WIDTH }, + { "FEATURE_FUNCTION_PEAKFILTER_HEIGHT", FEATURE_FUNCTION_PEAKFILTER_HEIGHT }, + { "FEATURE_FUNCTION_FREE_MEASURINGFIELD_Z", FEATURE_FUNCTION_FREE_MEASURINGFIELD_Z }, + { "FEATURE_FUNCTION_FREE_MEASURINGFIELD_X", FEATURE_FUNCTION_FREE_MEASURINGFIELD_X }, + { "FEATURE_FUNCTION_DYNAMIC_TRACK_DIVISOR", FEATURE_FUNCTION_DYNAMIC_TRACK_DIVISOR }, + { "FEATURE_FUNCTION_DYNAMIC_TRACK_FACTOR", FEATURE_FUNCTION_DYNAMIC_TRACK_FACTOR }, + { "FEATURE_FUNCTION_CALIBRATION_0", FEATURE_FUNCTION_CALIBRATION_0 }, + { "FEATURE_FUNCTION_CALIBRATION_1", FEATURE_FUNCTION_CALIBRATION_1 }, + { "FEATURE_FUNCTION_CALIBRATION_2", FEATURE_FUNCTION_CALIBRATION_2 }, + { "FEATURE_FUNCTION_CALIBRATION_3", FEATURE_FUNCTION_CALIBRATION_3 }, + { "FEATURE_FUNCTION_CALIBRATION_4", FEATURE_FUNCTION_CALIBRATION_4 }, + { "FEATURE_FUNCTION_CALIBRATION_5", FEATURE_FUNCTION_CALIBRATION_5 }, + { "FEATURE_FUNCTION_CALIBRATION_6", FEATURE_FUNCTION_CALIBRATION_6 }, + { "FEATURE_FUNCTION_CALIBRATION_7", FEATURE_FUNCTION_CALIBRATION_7 }, + { "FEATURE_FUNCTION_LASERPOWER", FEATURE_FUNCTION_LASERPOWER }, + { "FEATURE_FUNCTION_MEASURINGFIELD", FEATURE_FUNCTION_MEASURINGFIELD }, + { "FEATURE_FUNCTION_TRIGGER", FEATURE_FUNCTION_TRIGGER }, + { "FEATURE_FUNCTION_SHUTTERTIME", FEATURE_FUNCTION_SHUTTERTIME }, + { "FEATURE_FUNCTION_IDLETIME", FEATURE_FUNCTION_IDLETIME }, + { "FEATURE_FUNCTION_PROCESSING_PROFILEDATA", FEATURE_FUNCTION_PROCESSING_PROFILEDATA }, + { "FEATURE_FUNCTION_THRESHOLD", FEATURE_FUNCTION_THRESHOLD }, + { "FEATURE_FUNCTION_MAINTENANCEFUNCTIONS", FEATURE_FUNCTION_MAINTENANCEFUNCTIONS }, + { "FEATURE_FUNCTION_REARRANGEMENT_PROFILE", FEATURE_FUNCTION_REARRANGEMENT_PROFILE }, + { "FEATURE_FUNCTION_PROFILE_FILTER", FEATURE_FUNCTION_PROFILE_FILTER }, + { "FEATURE_FUNCTION_RS422_INTERFACE_FUNCTION", FEATURE_FUNCTION_RS422_INTERFACE_FUNCTION }, + { "FEATURE_FUNCTION_PACKET_DELAY", FEATURE_FUNCTION_PACKET_DELAY }, + { "FEATURE_FUNCTION_SHARPNESS", FEATURE_FUNCTION_SHARPNESS }, + { "FEATURE_FUNCTION_TEMPERATURE", FEATURE_FUNCTION_TEMPERATURE }, + { "FEATURE_FUNCTION_SATURATION", FEATURE_FUNCTION_SATURATION }, + { "FEATURE_FUNCTION_CAPTURE_QUALITY", FEATURE_FUNCTION_CAPTURE_QUALITY } + }; + + // + std::list features_with_corruption_risk = { FEATURE_FUNCTION_LASERPOWER, + FEATURE_FUNCTION_MEASURINGFIELD, FEATURE_FUNCTION_TRIGGER }; +}; + +void NewProfileCallback(const void* data, size_t data_size, gpointer user_data); +void ControlLostCallback(ArvGvDevice* mydevice, gpointer user_data); + +} // namespace scancontrol_driver + +#endif // _SCANCONTROL_DRIVER_H_ diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/nodelet.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/nodelet.h index 72ccb34..ca9ac23 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/nodelet.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/nodelet.h @@ -1,42 +1,43 @@ #ifndef _SCANCONTROL_NODELET_H_ #define _SCANCONTROL_NODELET_H_ -#include -#include - -#include -#include #include +#include +#include + +#include +#include -#include "micro_epsilon_scancontrol_driver/nodelet.h" #include "micro_epsilon_scancontrol_driver/driver.h" +#include "micro_epsilon_scancontrol_driver/nodelet.h" namespace scancontrol_driver { - class DriverNodelet: public nodelet::Nodelet - { - public: - DriverNodelet(): - running_(false) - {} - ~DriverNodelet() - { - // if (running_) - // { - // NODELET_INFO("Shutting down driver thread."); - // running_ = false; - // device_thread_->join(); - // NODELET_INFO("Driver thread stopped."); - // } - } - - private: - virtual void onInit(); - - volatile bool running_; - boost::shared_ptr driver_; - }; - -} - -#endif // _SCANCONTROL_NODELET_H_ \ No newline at end of file +class DriverNodelet : public nodelet::Nodelet +{ +public: + DriverNodelet() + : running_(false) + { + } + ~DriverNodelet() + { + // if (running_) + // { + // NODELET_INFO("Shutting down driver thread."); + // running_ = false; + // device_thread_->join(); + // NODELET_INFO("Driver thread stopped."); + // } + } + +private: + virtual void onInit(); + + volatile bool running_; + boost::shared_ptr driver_; +}; + +} // namespace scancontrol_driver + +#endif // _SCANCONTROL_NODELET_H_ diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index af2dcac..194c862 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -2,523 +2,634 @@ namespace scancontrol_driver { - ScanControlDriver::ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) - { - /* - Store the ros::NodeHandle objects and extract the relevant parameters. - */ - nh_ = nh; - private_nh_ = private_nh; - - // Device settings - private_nh_.param("resolution", config_.resolution, -1); - - // Multiple device parameters - private_nh_.param("serial", config_.serial, std::string("")); - private_nh_.param("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); - private_nh_.param("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); - - // TODO: Are these parameters needed? - private_nh_.param("partial_profile_start_point", config_.pp_start_point, 0); - private_nh_.param("partial_profile_start_point_data", config_.pp_start_point_data, 4); - private_nh_.param("partial_profile_point_count", config_.pp_point_count, -1); - private_nh_.param("partial_profile_data_width", config_.pp_point_data_width, 4); - - // Create driver interface object: - device_interface_ptr = std::make_unique(); - - /* - Search for available scanCONTROL interfaces - The code only supports a maximum of MAX_LLT_INTERFACE_COUNT (6) devices. If - more are connected the devices found after the 6th are not considdered. This - is a limitation of the API, which needs a fixed number of devices to search - for. If 6 devices is insufficient, redefine MAX_LLT_INTERFACE_COUNT to - accomodate the additional devices. - */ - gint32 return_code = 0; - gint32 interface_count = 0; - std::vector available_interfaces(MAX_DEVICE_INTERFACE_COUNT); - - return_code = device_interface_ptr->GetDeviceInterfaces(&available_interfaces[0], MAX_DEVICE_INTERFACE_COUNT); - if (return_code == ERROR_GETDEVINTERFACE_REQUEST_COUNT){ - ROS_WARN_STREAM("There are more than " << MAX_DEVICE_INTERFACE_COUNT << " scanCONTROL sensors connected."); - interface_count = MAX_DEVICE_INTERFACE_COUNT; - } else if (return_code < 0) { - ROS_WARN_STREAM("An error occured while searching for connected scanCONTROL devices. Code: " << return_code); - interface_count = 0; - } else { - interface_count = return_code; - } - - /* - Select scanCONTROL interface - A preffered interface can be set by means of the 'serial' parameter. - */ - gint8 selected_interface = -1; - if (interface_count == 0){ - ROS_WARN("There is no scanCONTROL device connected. Exiting..."); - goto stop_initialization; - } else if (interface_count == 1){ - ROS_INFO("There is 1 scanCONTROL device connected."); - selected_interface = 0; - - // Check if the available device is the same as the prefered device (if a serial is provided): - std::string interface(available_interfaces[0]); - if ((config_.serial == "") || (interface.find(config_.serial) > -1)){ - ROS_INFO_STREAM("Interface found: " << interface); - } - else{ - ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); - ROS_INFO_STREAM("Selected interface: " << interface); - } - } else { - ROS_INFO_STREAM("There are " << interface_count << " scanCONTROL devices connected."); - - // Select prefered device based on the defined ip or serial. If both are set, this selects the device which ip or serial is encountered first. - if (config_.serial != ""){ - for (int i = 0; i < interface_count; i++){ - std::string interface(available_interfaces[i]); - if (interface.find(config_.serial) > -1){ - ROS_INFO_STREAM("Interface found: " << interface); - selected_interface = i; - break; - } - } - // Fallback if serial are not found: - if (selected_interface == -1){ - ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); - ROS_WARN("Available interfaces:"); - for (gint8 i = 0; i < interface_count; i++){ - ROS_WARN_STREAM(" " << available_interfaces[i]); - } - selected_interface = 0; - ROS_INFO_STREAM("\nSelecting first available interface: " << available_interfaces[selected_interface]); - } - } else{ - selected_interface = 0; - ROS_INFO_STREAM("No 'serial' set, selecting first interface: " << available_interfaces[selected_interface]); - } - } - - /* - Set the selected device to the driver interface class and catch possible errors - */ - config_.interface = std::string(available_interfaces[selected_interface]); - config_.serial = std::string(config_.interface.end() - 9, config_.interface.end()); - return_code = device_interface_ptr->SetDeviceInterface(available_interfaces[selected_interface]); - if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while setting device ID! Code: " << return_code); - goto stop_initialization; - } - - /* - Connect to scanCONTROL device - */ - return_code = device_interface_ptr->Connect(); - if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while connecting to device! Code: " << return_code); - goto stop_initialization; - } - - /* - Identify device type and store information on type and serial - */ - return_code = device_interface_ptr->GetLLTType(&device_type); - if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while retrieving device type! Code: " << return_code); - goto stop_initialization; - } - if (device_type >= scanCONTROL27xx_25 && device_type <= scanCONTROL27xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL27xx, with serial number " << config_.serial << "."); - config_.model = std::string("scanCONTROL27xx"); - } else if (device_type >= scanCONTROL26xx_25 && device_type <= scanCONTROL26xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL26xx, with serial number " << config_.serial << "."); - config_.model = std::string("scanCONTROL26xx"); - } else if (device_type >= scanCONTROL29xx_25 && device_type <= scanCONTROL29xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL29xx, with serial number " << config_.serial << "."); - config_.model = std::string("scanCONTROL29xx"); - } else if (device_type >= scanCONTROL30xx_25 && device_type <= scanCONTROL30xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL30xx, with serial number " << config_.serial << "."); - config_.model = std::string("scanCONTROL30xx"); - } else if (device_type >= scanCONTROL25xx_25 && device_type <= scanCONTROL25xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL25xx, with serial number " << config_.serial << "."); - config_.model = std::string("scanCONTROL25xx"); - } else { - ROS_FATAL("The scanCONTROL device is a undefined type.\nPlease contact Micro-Epsilon for a newer SDK or update the driver."); - goto stop_initialization; - } - - /* - Set the resolution of the scanner. - */ - // Find all available resolutions - guint32 available_resolutions[MAX_RESOLUTION_COUNT]; - if (return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Unable to request the available resolutions of the scanCONTROL device. Code: " << return_code); - goto stop_initialization; - } - - // Select prefered/first found resolution - if (config_.resolution > 0){ - gint8 selected_resolution = -1; - for (int i = 0; i < return_code; i++){ - std::string resolution = std::to_string(available_resolutions[i]); - if (resolution.find(config_.serial) > -1){ - selected_resolution = i; - break; - } - } - if (selected_resolution == -1){ - ROS_WARN_STREAM("Requested resolution of " << std::to_string(config_.resolution) <<" not found as available option."); - ROS_WARN("Available resolutions:"); - for (int i = 0; i < return_code; i++){ - ROS_WARN_STREAM(" " << std::to_string(available_resolutions[i])); - } - config_.resolution = available_resolutions[0]; - ROS_INFO_STREAM("Selecting first available resolution: " << std::to_string(config_.resolution)); - } - } else{ - config_.resolution = available_resolutions[0]; - ROS_INFO_STREAM("No resolution set, selecting first available: " << std::to_string(config_.resolution)); - } - - // Set the selected resolution - if (return_code = device_interface_ptr->SetResolution(config_.resolution) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while setting device resolution! CodeL " << return_code); - goto stop_initialization; - } - - /* - Prepare the partial profile - */ - if (return_code = SetPartialProfile(config_.resolution) < GENERAL_FUNCTION_OK){ - goto stop_initialization; - } - - /* - Register callback functions - RegisterBufferCallback > NewProfile: Triggered when the sensor has a new profile available in the buffer. - RegisterControlLostCallback > ControlLostCallback: Triggered when control of the device is lost. - */ - if ((return_code = device_interface_ptr->RegisterBufferCallback((gpointer) &NewProfileCallback, this)) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while registering buffer callback. Code: " << return_code); - goto stop_initialization; - } - if ((return_code = device_interface_ptr->RegisterControlLostCallback((gpointer) &ControlLostCallback, this)) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while registering control lost callback. Code: " << return_code); - goto stop_initialization; - } - - /* - Initialization finished and successfully connected to scanCONTROL device. - */ - initialized_ = true; - - // goto location when initialization fails - stop_initialization: - if (!initialized_) { - throw std::runtime_error("ScanControlDriver: Initialization failed."); - } - - // Advertise topic - publisher = nh.advertise(config_.topic_name, 10); - - // Advertise services - get_feature_srv = private_nh_.advertiseService("get_feature", &ScanControlDriver::ServiceGetFeature, this); - set_feature_srv = private_nh_.advertiseService("set_feature", &ScanControlDriver::ServiceSetFeature, this); - get_resolution_srv = private_nh_.advertiseService("get_resolution", &ScanControlDriver::ServiceGetResolution, this); - set_resolution_srv = private_nh_.advertiseService("set_resolution", &ScanControlDriver::ServiceSetResolution, this); - get_available_resolutions_srv = private_nh_.advertiseService("get_available_resolutions", &ScanControlDriver::ServiceGetAvailableResolutions, this); - invert_z_srv = private_nh_.advertiseService("invert_z", &ScanControlDriver::ServiceInvertZ, this); - invert_x_srv = private_nh_.advertiseService("invert_x", &ScanControlDriver::ServiceInvertX, this); +ScanControlDriver::ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) +{ + /* + Store the ros::NodeHandle objects and extract the relevant parameters. + */ + nh_ = nh; + private_nh_ = private_nh; + + // Device settings + private_nh_.param("resolution", config_.resolution, -1); + + // Multiple device parameters + private_nh_.param("serial", config_.serial, std::string("")); + private_nh_.param("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); + private_nh_.param("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); + + // TODO: Are these parameters needed? + private_nh_.param("partial_profile_start_point", config_.pp_start_point, 0); + private_nh_.param("partial_profile_start_point_data", config_.pp_start_point_data, 4); + private_nh_.param("partial_profile_point_count", config_.pp_point_count, -1); + private_nh_.param("partial_profile_data_width", config_.pp_point_data_width, 4); + + // Create driver interface object: + device_interface_ptr = std::make_unique(); + + /* + Search for available scanCONTROL interfaces + The code only supports a maximum of MAX_LLT_INTERFACE_COUNT (6) devices. If + more are connected the devices found after the 6th are not considdered. This + is a limitation of the API, which needs a fixed number of devices to search + for. If 6 devices is insufficient, redefine MAX_LLT_INTERFACE_COUNT to + accomodate the additional devices. + */ + gint32 return_code = 0; + gint32 interface_count = 0; + std::vector available_interfaces(MAX_DEVICE_INTERFACE_COUNT); + + return_code = device_interface_ptr->GetDeviceInterfaces(&available_interfaces[0], MAX_DEVICE_INTERFACE_COUNT); + if (return_code == ERROR_GETDEVINTERFACE_REQUEST_COUNT) + { + ROS_WARN_STREAM("There are more than " << MAX_DEVICE_INTERFACE_COUNT << " scanCONTROL sensors connected."); + interface_count = MAX_DEVICE_INTERFACE_COUNT; + } + else if (return_code < 0) + { + ROS_WARN_STREAM("An error occured while searching for connected scanCONTROL devices. Code: " << return_code); + interface_count = 0; + } + else + { + interface_count = return_code; + } + + /* + Select scanCONTROL interface + A preffered interface can be set by means of the 'serial' parameter. + */ + gint8 selected_interface = -1; + if (interface_count == 0) + { + ROS_WARN("There is no scanCONTROL device connected. Exiting..."); + goto stop_initialization; + } + else if (interface_count == 1) + { + ROS_INFO("There is 1 scanCONTROL device connected."); + selected_interface = 0; + + // Check if the available device is the same as the prefered device (if a serial is provided): + std::string interface(available_interfaces[0]); + if ((config_.serial == "") || (interface.find(config_.serial) > -1)) + { + ROS_INFO_STREAM("Interface found: " << interface); } - - int ScanControlDriver::SetPartialProfile(int &resolution){ - gint32 return_code = 0; - // Set profile configuration to partial profile - if ((return_code = device_interface_ptr->SetProfileConfig(PARTIAL_PROFILE)) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting profile config to PARTIAL_PROFILE. Code: " << return_code); - return GENERAL_FUNCTION_FAILED; - } - - // Create partial profile object and send to device - t_partial_profile_.nStartPoint = config_.pp_start_point; - t_partial_profile_.nStartPointData = config_.pp_start_point_data; - t_partial_profile_.nPointDataWidth = config_.pp_point_data_width; - t_partial_profile_.nPointCount = (config_.pp_point_count == -1 || config_.pp_point_count > resolution) ? resolution : config_.pp_point_count; - - // Send partial profile settings to device - if ((return_code = device_interface_ptr->SetPartialProfile(&t_partial_profile_)) < GENERAL_FUNCTION_OK){ - // Restore nPointCount to old value - t_partial_profile_.nPointCount = (config_.pp_point_count == -1 || config_.pp_point_count > resolution) ? config_.resolution : config_.pp_point_count; - - // Send warning and return failed - ROS_WARN_STREAM("Error while setting partial profile settings. Code: " << return_code); - return GENERAL_FUNCTION_FAILED; - } - - // Resize buffers - values contain less than the point count, due to the timestamp data - profile_buffer.resize(t_partial_profile_.nPointCount*t_partial_profile_.nPointDataWidth); - lost_values = (16 + t_partial_profile_.nPointDataWidth - 1)/t_partial_profile_.nPointDataWidth; - value_x.resize(t_partial_profile_.nPointCount); - value_z.resize(t_partial_profile_.nPointCount); - ROS_INFO_STREAM("Profile is losing " << std::to_string(lost_values) << " values due to timestamp of 16 byte at the end of the profile."); - - // Prepare new point cloud message - point_cloud_msg.reset(new point_cloud_t); - point_cloud_msg->header.frame_id = config_.frame_id; - point_cloud_msg->height = 1; - point_cloud_msg->width = config_.resolution; - for (int i=0; ipoints.push_back(point); - } - - return GENERAL_FUNCTION_OK; + else + { + ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); + ROS_INFO_STREAM("Selected interface: " << interface); } - - /* Start the transfer of new profiles */ - int ScanControlDriver::StartProfileTransfer(){ - int return_code = 0; - if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, true)) < GENERAL_FUNCTION_OK) { - ROS_WARN_STREAM("Error while starting profile transfer! Code: " << return_code); - return return_code; + } + else + { + ROS_INFO_STREAM("There are " << interface_count << " scanCONTROL devices connected."); + + // Select prefered device based on the defined ip or serial. If both are set, this selects the device which ip or + // serial is encountered first. + if (config_.serial != "") + { + for (int i = 0; i < interface_count; i++) + { + std::string interface(available_interfaces[i]); + if (interface.find(config_.serial) > -1) + { + ROS_INFO_STREAM("Interface found: " << interface); + selected_interface = i; + break; + } + } + // Fallback if serial are not found: + if (selected_interface == -1) + { + ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); + ROS_WARN("Available interfaces:"); + for (gint8 i = 0; i < interface_count; i++) + { + ROS_WARN_STREAM(" " << available_interfaces[i]); } - transfer_active_ = true; - return GENERAL_FUNCTION_OK; + selected_interface = 0; + ROS_INFO_STREAM("\nSelecting first available interface: " << available_interfaces[selected_interface]); + } } - - /* Stop the transfer of new profiles */ - int ScanControlDriver::StopProfileTransfer(){ - int return_code = 0; - if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, false)) < GENERAL_FUNCTION_OK) { - ROS_WARN_STREAM("Error while stopping profile transfer. Code: " << return_code); - return return_code; - } - transfer_active_ = false; - return GENERAL_FUNCTION_OK; + else + { + selected_interface = 0; + ROS_INFO_STREAM("No 'serial' set, selecting first interface: " << available_interfaces[selected_interface]); } - - /* Process raw profile data and create the point cloud message */ - int ScanControlDriver::Profile2PointCloud(){ - - device_interface_ptr->ConvertPartProfile2Values(&profile_buffer[0], profile_buffer.size(), &t_partial_profile_, device_type, 0, NULL, NULL, NULL, &value_x[0], &value_z[0], NULL, NULL); - for (int i = 0; i < config_.resolution; i++){ - point_cloud_msg->points[i].x = value_x[i]/1000; - - // Fill in NaN if the scanner is to close or far away (sensor returns ~32.232) and for the final few points which are overwritten by the timestamp data - if ((value_z[i] < 32.5) || (i >= config_.resolution - lost_values)){ - point_cloud_msg->points[i].z = std::numeric_limits::infinity(); - } - else{ - point_cloud_msg->points[i].z = value_z[i]/1000; - } - } - return GENERAL_FUNCTION_OK; + } + + /* + Set the selected device to the driver interface class and catch possible errors + */ + config_.interface = std::string(available_interfaces[selected_interface]); + config_.serial = std::string(config_.interface.end() - 9, config_.interface.end()); + return_code = device_interface_ptr->SetDeviceInterface(available_interfaces[selected_interface]); + if (return_code < GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while setting device ID! Code: " << return_code); + goto stop_initialization; + } + + /* + Connect to scanCONTROL device + */ + return_code = device_interface_ptr->Connect(); + if (return_code < GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while connecting to device! Code: " << return_code); + goto stop_initialization; + } + + /* + Identify device type and store information on type and serial + */ + return_code = device_interface_ptr->GetLLTType(&device_type); + if (return_code < GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while retrieving device type! Code: " << return_code); + goto stop_initialization; + } + if (device_type >= scanCONTROL27xx_25 && device_type <= scanCONTROL27xx_xxx) + { + ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL27xx, with serial number " << config_.serial << "."); + config_.model = std::string("scanCONTROL27xx"); + } + else if (device_type >= scanCONTROL26xx_25 && device_type <= scanCONTROL26xx_xxx) + { + ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL26xx, with serial number " << config_.serial << "."); + config_.model = std::string("scanCONTROL26xx"); + } + else if (device_type >= scanCONTROL29xx_25 && device_type <= scanCONTROL29xx_xxx) + { + ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL29xx, with serial number " << config_.serial << "."); + config_.model = std::string("scanCONTROL29xx"); + } + else if (device_type >= scanCONTROL30xx_25 && device_type <= scanCONTROL30xx_xxx) + { + ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL30xx, with serial number " << config_.serial << "."); + config_.model = std::string("scanCONTROL30xx"); + } + else if (device_type >= scanCONTROL25xx_25 && device_type <= scanCONTROL25xx_xxx) + { + ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL25xx, with serial number " << config_.serial << "."); + config_.model = std::string("scanCONTROL25xx"); + } + else + { + ROS_FATAL( + "The scanCONTROL device is a undefined type.\nPlease contact Micro-Epsilon for a newer SDK or update the " + "driver."); + goto stop_initialization; + } + + /* + Set the resolution of the scanner. + */ + // Find all available resolutions + guint32 available_resolutions[MAX_RESOLUTION_COUNT]; + if (return_code = + device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT) < GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Unable to request the available resolutions of the scanCONTROL device. Code: " << return_code); + goto stop_initialization; + } + + // Select prefered/first found resolution + if (config_.resolution > 0) + { + gint8 selected_resolution = -1; + for (int i = 0; i < return_code; i++) + { + std::string resolution = std::to_string(available_resolutions[i]); + if (resolution.find(config_.serial) > -1) + { + selected_resolution = i; + break; + } } - - /* Process and publish profile */ - int ScanControlDriver::ProcessAndPublishProfile(const void * data, size_t data_size){ - // Timestamp - pcl_conversions::toPCL(ros::Time::now(), point_cloud_msg->header.stamp); - - // Copy sensor data to local buffer - if (data != NULL && data_size == profile_buffer.size()){ - memcpy(&profile_buffer[0], data, data_size); - } - - // Process buffer and publish point cloud - ScanControlDriver::Profile2PointCloud(); - publisher.publish(point_cloud_msg); - - return GENERAL_FUNCTION_OK; + if (selected_resolution == -1) + { + ROS_WARN_STREAM("Requested resolution of " << std::to_string(config_.resolution) + << " not found as available option."); + ROS_WARN("Available resolutions:"); + for (int i = 0; i < return_code; i++) + { + ROS_WARN_STREAM(" " << std::to_string(available_resolutions[i])); + } + config_.resolution = available_resolutions[0]; + ROS_INFO_STREAM("Selecting first available resolution: " << std::to_string(config_.resolution)); } - - /* 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){ - ROS_WARN_STREAM("Setting could not be retrieved. Code: " << return_code); - return return_code; - } - return GENERAL_FUNCTION_OK; + } + else + { + config_.resolution = available_resolutions[0]; + ROS_INFO_STREAM("No resolution set, selecting first available: " << std::to_string(config_.resolution)); + } + + // Set the selected resolution + if (return_code = device_interface_ptr->SetResolution(config_.resolution) < GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while setting device resolution! CodeL " << return_code); + goto stop_initialization; + } + + /* + Prepare the partial profile + */ + if (return_code = SetPartialProfile(config_.resolution) < GENERAL_FUNCTION_OK) + { + goto stop_initialization; + } + + /* + Register callback functions + RegisterBufferCallback > NewProfile: Triggered when the sensor has a new profile available in the buffer. + RegisterControlLostCallback > ControlLostCallback: Triggered when control of the device is lost. + */ + if ((return_code = device_interface_ptr->RegisterBufferCallback((gpointer)&NewProfileCallback, this)) < + GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while registering buffer callback. Code: " << return_code); + goto stop_initialization; + } + if ((return_code = device_interface_ptr->RegisterControlLostCallback((gpointer)&ControlLostCallback, this)) < + GENERAL_FUNCTION_OK) + { + ROS_FATAL_STREAM("Error while registering control lost callback. Code: " << return_code); + goto stop_initialization; + } + + /* + Initialization finished and successfully connected to scanCONTROL device. + */ + initialized_ = true; + +// goto location when initialization fails +stop_initialization: + if (!initialized_) + { + throw std::runtime_error("ScanControlDriver: Initialization failed."); + } + + // Advertise topic + publisher = nh.advertise(config_.topic_name, 10); + + // Advertise services + get_feature_srv = private_nh_.advertiseService("get_feature", &ScanControlDriver::ServiceGetFeature, this); + set_feature_srv = private_nh_.advertiseService("set_feature", &ScanControlDriver::ServiceSetFeature, this); + get_resolution_srv = private_nh_.advertiseService("get_resolution", &ScanControlDriver::ServiceGetResolution, this); + set_resolution_srv = private_nh_.advertiseService("set_resolution", &ScanControlDriver::ServiceSetResolution, this); + get_available_resolutions_srv = private_nh_.advertiseService( + "get_available_resolutions", &ScanControlDriver::ServiceGetAvailableResolutions, this); + invert_z_srv = private_nh_.advertiseService("invert_z", &ScanControlDriver::ServiceInvertZ, this); + invert_x_srv = private_nh_.advertiseService("invert_x", &ScanControlDriver::ServiceInvertX, this); +} + +int ScanControlDriver::SetPartialProfile(int& resolution) +{ + gint32 return_code = 0; + // Set profile configuration to partial profile + if ((return_code = device_interface_ptr->SetProfileConfig(PARTIAL_PROFILE)) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Error while setting profile config to PARTIAL_PROFILE. Code: " << return_code); + return GENERAL_FUNCTION_FAILED; + } + + // Create partial profile object and send to device + t_partial_profile_.nStartPoint = config_.pp_start_point; + t_partial_profile_.nStartPointData = config_.pp_start_point_data; + t_partial_profile_.nPointDataWidth = config_.pp_point_data_width; + t_partial_profile_.nPointCount = + (config_.pp_point_count == -1 || config_.pp_point_count > resolution) ? resolution : config_.pp_point_count; + + // Send partial profile settings to device + if ((return_code = device_interface_ptr->SetPartialProfile(&t_partial_profile_)) < GENERAL_FUNCTION_OK) + { + // Restore nPointCount to old value + t_partial_profile_.nPointCount = (config_.pp_point_count == -1 || config_.pp_point_count > resolution) ? + config_.resolution : + config_.pp_point_count; + + // Send warning and return failed + ROS_WARN_STREAM("Error while setting partial profile settings. Code: " << return_code); + return GENERAL_FUNCTION_FAILED; + } + + // Resize buffers - values contain less than the point count, due to the timestamp data + profile_buffer.resize(t_partial_profile_.nPointCount * t_partial_profile_.nPointDataWidth); + lost_values = (16 + t_partial_profile_.nPointDataWidth - 1) / t_partial_profile_.nPointDataWidth; + value_x.resize(t_partial_profile_.nPointCount); + value_z.resize(t_partial_profile_.nPointCount); + ROS_INFO_STREAM("Profile is losing " << std::to_string(lost_values) + << " values due to timestamp of 16 byte at the end of the profile."); + + // Prepare new point cloud message + point_cloud_msg.reset(new point_cloud_t); + point_cloud_msg->header.frame_id = config_.frame_id; + point_cloud_msg->height = 1; + point_cloud_msg->width = config_.resolution; + for (int i = 0; i < config_.resolution; i++) + { + pcl::PointXYZI point(1.0); + point_cloud_msg->points.push_back(point); + } + + return GENERAL_FUNCTION_OK; +} + +/* Start the transfer of new profiles */ +int ScanControlDriver::StartProfileTransfer() +{ + int return_code = 0; + if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, true)) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Error while starting profile transfer! Code: " << return_code); + return return_code; + } + transfer_active_ = true; + return GENERAL_FUNCTION_OK; +} + +/* Stop the transfer of new profiles */ +int ScanControlDriver::StopProfileTransfer() +{ + int return_code = 0; + if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, false)) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Error while stopping profile transfer. Code: " << return_code); + return return_code; + } + transfer_active_ = false; + return GENERAL_FUNCTION_OK; +} + +/* Process raw profile data and create the point cloud message */ +int ScanControlDriver::Profile2PointCloud() +{ + device_interface_ptr->ConvertPartProfile2Values(&profile_buffer[0], profile_buffer.size(), &t_partial_profile_, + device_type, 0, NULL, NULL, NULL, &value_x[0], &value_z[0], NULL, + NULL); + for (int i = 0; i < config_.resolution; i++) + { + point_cloud_msg->points[i].x = value_x[i] / 1000; + + // Fill in NaN if the scanner is to close or far away (sensor returns ~32.232) and for the final few points which + // are overwritten by the timestamp data + if ((value_z[i] < 32.5) || (i >= config_.resolution - lost_values)) + { + point_cloud_msg->points[i].z = std::numeric_limits::infinity(); } - - /* - Attempt to set the a setting/feature to the requested value. - The function temporarily halts the transfer of new profiles if the setting can cause the profiles to be corrupted. - */ - int ScanControlDriver::SetFeature(unsigned int setting_id, unsigned int value){ - int return_code = 0; - if ((std::find(features_with_corruption_risk.begin(), features_with_corruption_risk.end(), setting_id) != features_with_corruption_risk.end()) && transfer_active_){ - ROS_INFO("Risk of profile corruption, temporarily stopping profile transfer."); - if (return_code = ScanControlDriver::StopProfileTransfer() < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Profile transfer could not be stopped. Code: " << return_code); - return -1; - } - if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); - return return_code; - } - if (return_code = ScanControlDriver::StartProfileTransfer() < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Profile transfer could not be restarted after changing feature. Code: " << return_code); - return -1; - } - return GENERAL_FUNCTION_OK; - } - if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); - return return_code; - } - return GENERAL_FUNCTION_OK; + else + { + point_cloud_msg->points[i].z = value_z[i] / 1000; } + } + return GENERAL_FUNCTION_OK; +} - /* Wrapper of the SetFeature call for use by the ServiceSetFeature service */ - bool ScanControlDriver::ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request &request, micro_epsilon_scancontrol_msgs::SetFeature::Response &response){ - response.return_code = ScanControlDriver::SetFeature(request.setting, request.value); - return true; - } +/* Process and publish profile */ +int ScanControlDriver::ProcessAndPublishProfile(const void* data, size_t data_size) +{ + // Timestamp + pcl_conversions::toPCL(ros::Time::now(), point_cloud_msg->header.stamp); - /* Wrapper of the GetFeature call for use by the ServiceGetFeature service */ - bool ScanControlDriver::ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request &request, micro_epsilon_scancontrol_msgs::GetFeature::Response &response){ - response.return_code = ScanControlDriver::GetFeature(request.setting, &(response.value)); - return true; - } + // Copy sensor data to local buffer + if (data != NULL && data_size == profile_buffer.size()) + { + memcpy(&profile_buffer[0], data, data_size); + } - /* Wrapper of the SetResolution call for use by the ServiceSetResolution service */ - bool ScanControlDriver::ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request &request, micro_epsilon_scancontrol_msgs::SetResolution::Response &response){ - if (response.return_code = StopProfileTransfer() < GENERAL_FUNCTION_OK){ - return true; - } - if (response.return_code = device_interface_ptr->SetResolution(request.resolution) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting device resolution! Code: " << response.return_code); - return true; - } - int temp_resolution = request.resolution; - if (response.return_code = SetPartialProfile(temp_resolution) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting partial profile. Code: " << response.return_code); - return true; - } - if (response.return_code = StartProfileTransfer() < GENERAL_FUNCTION_OK){ - return true; - } + // Process buffer and publish point cloud + ScanControlDriver::Profile2PointCloud(); + publisher.publish(point_cloud_msg); - // Change of resolution was succesull - config_.resolution = request.resolution; - return true; - } + return GENERAL_FUNCTION_OK; +} - /* Wrapper of the GetResolution call for use by the ServiceGetResolution service */ - bool ScanControlDriver::ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request &request, micro_epsilon_scancontrol_msgs::GetResolution::Response &response){ - response.return_code = device_interface_ptr->GetResolution(&(response.resolution)); - return true; +/* 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) + { + ROS_WARN_STREAM("Setting could not be retrieved. Code: " << return_code); + return return_code; + } + return GENERAL_FUNCTION_OK; +} + +/* + Attempt to set the a setting/feature to the requested value. + The function temporarily halts the transfer of new profiles if the setting can cause the profiles to be + corrupted. +*/ +int ScanControlDriver::SetFeature(unsigned int setting_id, unsigned int value) +{ + int return_code = 0; + if ((std::find(features_with_corruption_risk.begin(), features_with_corruption_risk.end(), setting_id) != + features_with_corruption_risk.end()) && + transfer_active_) + { + ROS_INFO("Risk of profile corruption, temporarily stopping profile transfer."); + if (return_code = ScanControlDriver::StopProfileTransfer() < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Profile transfer could not be stopped. Code: " << return_code); + return -1; } - - /* Wrapper of the GetResolutions call for use by the ServiceGetAvailableResolutions service */ - bool ScanControlDriver::ServiceGetAvailableResolutions(micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request &request, micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response &response){ - guint32 available_resolutions[MAX_RESOLUTION_COUNT] = {0}; - response.return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT); - for (int i = 0; i < MAX_RESOLUTION_COUNT; i++){ - if (available_resolutions[i] > 0){ - response.resolutions.push_back(available_resolutions[i]); - } - } - return true; + if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); + return return_code; } + if (return_code = ScanControlDriver::StartProfileTransfer() < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Profile transfer could not be restarted after changing feature. Code: " << return_code); + return -1; + } + return GENERAL_FUNCTION_OK; + } + if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); + return return_code; + } + return GENERAL_FUNCTION_OK; +} + +/* Wrapper of the SetFeature call for use by the ServiceSetFeature service */ +bool ScanControlDriver::ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request& request, + micro_epsilon_scancontrol_msgs::SetFeature::Response& response) +{ + response.return_code = ScanControlDriver::SetFeature(request.setting, request.value); + return true; +} - /* - Enable or disable the inversion of Z values on the scanCONTROL device: - If request.data == true > Enable the inversion of Z values on the scanCONTROL device. (Default of the scanCONTROL device) - If request.data == false > Disable the inversion of Z values on the scanCONTROL device. - */ - bool ScanControlDriver::ServiceInvertZ(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) - { - unsigned int value; - int return_code; - - // Retrieve current settings - if (return_code = ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA ,&value) < GENERAL_FUNCTION_OK) - { - // Failed to get PROCESSING feature - response.success = false; - response.message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; - } - - // Set 6th bit according to the SetBool service request - value = value & ~(1<<6); - if (request.data) - { - value |= (1<<6); - } - - // Send the updated settings - if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) - { - // Failed to set PROCESSING feature - response.success = false; - response.message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; - } - - response.success = true; - - return true; - } - - /* - Enable or disable the inversion of X values on the scanCONTROL device: - If request.data == true > Enable the inversion of X values on the scanCONTROL device. (Default of the scanCONTROL device) - If request.data == false > Disable the inversion of X values on the scanCONTROL device. - */ - bool ScanControlDriver::ServiceInvertX(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) - { - unsigned int value; - int return_code; - - // Retrieve current settings - if (return_code = ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA ,&value) < GENERAL_FUNCTION_OK) - { - // Failed to get PROCESSING feature - response.success = false; - response.message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; - } - - // Set 6th bit according to the SetBool service request - value = value & ~(1<<7); - if (request.data) - { - value |= (1<<7); - } - - // Send the updated settings - if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) - { - // Failed to set PROCESSING feature - response.success = false; - response.message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; - } - - response.success = true; - - return true; - } - - /* 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 - ScanControlDriver* driver_ptr = static_cast(user_data); +/* Wrapper of the GetFeature call for use by the ServiceGetFeature service */ +bool ScanControlDriver::ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request& request, + micro_epsilon_scancontrol_msgs::GetFeature::Response& response) +{ + response.return_code = ScanControlDriver::GetFeature(request.setting, &(response.value)); + return true; +} - // Call member function - driver_ptr->ProcessAndPublishProfile(data, data_size); +/* Wrapper of the SetResolution call for use by the ServiceSetResolution service */ +bool ScanControlDriver::ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request& request, + micro_epsilon_scancontrol_msgs::SetResolution::Response& response) +{ + if (response.return_code = StopProfileTransfer() < GENERAL_FUNCTION_OK) + { + return true; + } + if (response.return_code = device_interface_ptr->SetResolution(request.resolution) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Error while setting device resolution! Code: " << response.return_code); + return true; + } + int temp_resolution = request.resolution; + if (response.return_code = SetPartialProfile(temp_resolution) < GENERAL_FUNCTION_OK) + { + ROS_WARN_STREAM("Error while setting partial profile. Code: " << response.return_code); + return true; + } + if (response.return_code = StartProfileTransfer() < GENERAL_FUNCTION_OK) + { + return true; + } + + // Change of resolution was succesull + config_.resolution = request.resolution; + return true; +} + +/* Wrapper of the GetResolution call for use by the ServiceGetResolution service */ +bool ScanControlDriver::ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request& request, + micro_epsilon_scancontrol_msgs::GetResolution::Response& response) +{ + response.return_code = device_interface_ptr->GetResolution(&(response.resolution)); + return true; +} + +/* Wrapper of the GetResolutions call for use by the ServiceGetAvailableResolutions service */ +bool ScanControlDriver::ServiceGetAvailableResolutions( + micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request& request, + micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response& response) +{ + guint32 available_resolutions[MAX_RESOLUTION_COUNT] = { 0 }; + response.return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT); + for (int i = 0; i < MAX_RESOLUTION_COUNT; i++) + { + if (available_resolutions[i] > 0) + { + response.resolutions.push_back(available_resolutions[i]); } + } + return true; +} + +/* +Enable or disable the inversion of Z values on the scanCONTROL device: + If request.data == true > Enable the inversion of Z values on the scanCONTROL device. (Default of the scanCONTROL +device) If request.data == false > Disable the inversion of Z values on the scanCONTROL device. +*/ +bool ScanControlDriver::ServiceInvertZ(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) +{ + unsigned int value; + int return_code; + + // Retrieve current settings + if (return_code = + ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, &value) < GENERAL_FUNCTION_OK) + { + // Failed to get PROCESSING feature + response.success = false; + response.message = + std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + return true; + } + + // Set 6th bit according to the SetBool service request + value = value & ~(1 << 6); + if (request.data) + { + value |= (1 << 6); + } + + // Send the updated settings + if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) + { + // Failed to set PROCESSING feature + response.success = false; + response.message = + std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + return true; + } + + response.success = true; + + return true; +} + +/* +Enable or disable the inversion of X values on the scanCONTROL device: + If request.data == true > Enable the inversion of X values on the scanCONTROL device. (Default of the scanCONTROL +device) If request.data == false > Disable the inversion of X values on the scanCONTROL device. +*/ +bool ScanControlDriver::ServiceInvertX(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) +{ + unsigned int value; + int return_code; + + // Retrieve current settings + if (return_code = + ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, &value) < GENERAL_FUNCTION_OK) + { + // Failed to get PROCESSING feature + response.success = false; + response.message = + std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + return true; + } + + // Set 6th bit according to the SetBool service request + value = value & ~(1 << 7); + if (request.data) + { + value |= (1 << 7); + } + + // Send the updated settings + if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) + { + // Failed to set PROCESSING feature + response.success = false; + response.message = + std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + return true; + } + + response.success = true; + + return true; +} + +/* 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 + ScanControlDriver* driver_ptr = static_cast(user_data); - /* Callback for when connection to the device is lost, for use with the scanCONTROL API. */ - void ControlLostCallback(ArvGvDevice *mydevice, gpointer user_data){ - ROS_FATAL("Conrol of scanCONTROL device lost!"); - ros::shutdown(); - } + // Call member function + driver_ptr->ProcessAndPublishProfile(data, data_size); +} + +/* Callback for when connection to the device is lost, for use with the scanCONTROL API. */ +void ControlLostCallback(ArvGvDevice* mydevice, gpointer user_data) +{ + ROS_FATAL("Conrol of scanCONTROL device lost!"); + ros::shutdown(); +} -} // namespace scancontrol_driver \ No newline at end of file +} // namespace scancontrol_driver diff --git a/micro_epsilon_scancontrol_driver/src/node.cpp b/micro_epsilon_scancontrol_driver/src/node.cpp index 788c2fa..473615f 100644 --- a/micro_epsilon_scancontrol_driver/src/node.cpp +++ b/micro_epsilon_scancontrol_driver/src/node.cpp @@ -1,33 +1,32 @@ #include + #include "micro_epsilon_scancontrol_driver/driver.h" int main(int argc, char** argv) { - ros::init(argc, argv, "micro_epsilon_scancontrol_driver_node"); - ros::NodeHandle node; - ros::NodeHandle private_nh("~"); - - // Start the driver - try - { - scancontrol_driver::ScanControlDriver driver(node, private_nh); - ROS_INFO("Driver started"); + ros::init(argc, argv, "micro_epsilon_scancontrol_driver_node"); + ros::NodeHandle node; + ros::NodeHandle private_nh("~"); - // Loop driver until shutdown - driver.StartProfileTransfer(); - while(ros::ok()) - { - ros::spinOnce(); - } - driver.StopProfileTransfer(); - return 0; - } - catch(const std::runtime_error& error) + // Start the driver + try + { + scancontrol_driver::ScanControlDriver driver(node, private_nh); + ROS_INFO("Driver started"); + + // Loop driver until shutdown + driver.StartProfileTransfer(); + while (ros::ok()) { - ROS_FATAL_STREAM(error.what()); - ros::shutdown(); - return 0; + ros::spinOnce(); } - - + driver.StopProfileTransfer(); + return 0; + } + catch (const std::runtime_error& error) + { + ROS_FATAL_STREAM(error.what()); + ros::shutdown(); + return 0; + } } diff --git a/micro_epsilon_scancontrol_driver/src/nodelet.cpp b/micro_epsilon_scancontrol_driver/src/nodelet.cpp index e41e5c5..accd9ff 100644 --- a/micro_epsilon_scancontrol_driver/src/nodelet.cpp +++ b/micro_epsilon_scancontrol_driver/src/nodelet.cpp @@ -1,30 +1,28 @@ #include "micro_epsilon_scancontrol_driver/nodelet.h" -// Register this plugin with pluginlib. +// Register this plugin with pluginlib. PLUGINLIB_EXPORT_CLASS(scancontrol_driver::DriverNodelet, nodelet::Nodelet) namespace scancontrol_driver { - - void DriverNodelet::onInit() - { - // Start scanCONTROL driver - driver_.reset(new ScanControlDriver(getNodeHandle(), getPrivateNodeHandle())); - - // spawn device poll thread - // running_ = true; - // device_thread_ = boost::shared_ptr (new boost::thread(boost::bind(&DriverNodelet::devicePoll, this))); - } +void DriverNodelet::onInit() +{ + // Start scanCONTROL driver + driver_.reset(new ScanControlDriver(getNodeHandle(), getPrivateNodeHandle())); - /** @brief Device poll thread main loop. */ - // void DriverNodelet::devicePoll() - // { - // while(ros::ok()) - // { - - // } - // running_ = false; - // } -} + // spawn device poll thread + // running_ = true; + // device_thread_ = boost::shared_ptr (new boost::thread(boost::bind(&DriverNodelet::devicePoll, + // this))); +} +/** @brief Device poll thread main loop. */ +// void DriverNodelet::devicePoll() +// { +// while(ros::ok()) +// { +// } +// running_ = false; +// } +} // namespace scancontrol_driver