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 4c722de..4aafce6 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 @@ -31,11 +31,11 @@ typedef pcl::PointCloud point_cloud_t; namespace scancontrol_driver { - class ScanControlDriver + class ScanControlDriver: public rclcpp::Node { public: // Constructor and destructor - ScanControlDriver(rclcpp::Node::SharedPtr& nh, rclcpp::Node::SharedPtr& private_nh); + ScanControlDriver(); ~ScanControlDriver() {} // Profile functions @@ -97,8 +97,6 @@ namespace scancontrol_driver bool transfer_active_ = false; // ROS handles - rclcpp::Node::SharedPtr nh_; - rclcpp::Node::SharedPtr private_nh_; rclcpp::Publisher::SharedPtr publisher; rclcpp::Service::SharedPtr get_feature_srv; rclcpp::Service::SharedPtr set_feature_srv; diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index e2360a7..1f20523 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -5,35 +5,33 @@ namespace scancontrol_driver static const rclcpp::Logger LOGGER = rclcpp::get_logger("scancontrol_driver"); - ScanControlDriver::ScanControlDriver(rclcpp::Node::SharedPtr& nh, rclcpp::Node::SharedPtr& private_nh) + ScanControlDriver::ScanControlDriver():Node("scancontrol_driver") { /* - Store the ros::NodeHandle objects and extract the relevant parameters. + Extract the relevant parameters. */ - nh_ = nh; - private_nh_ = private_nh; // Device settings - private_nh_->declare_parameter("resolution", -1); - private_nh_->get_parameter_or("resolution", config_.resolution, -1); + this->declare_parameter("resolution", -1); + this->get_parameter_or("resolution", config_.resolution, -1); // Multiple device parameters - private_nh_->declare_parameter("serial", std::string("")); - private_nh_->get_parameter_or("serial", config_.serial, std::string("")); - private_nh_->declare_parameter("frame_id", std::string(DEFAULT_FRAME_ID)); - private_nh_->get_parameter_or("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); - private_nh_->declare_parameter("topic_name", std::string(DEFAULT_TOPIC_NAME)); - private_nh_->get_parameter_or("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); + this->declare_parameter("serial", std::string("")); + this->get_parameter_or("serial", config_.serial, std::string("")); + this->declare_parameter("frame_id", std::string(DEFAULT_FRAME_ID)); + this->get_parameter_or("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); + this->declare_parameter("topic_name", std::string(DEFAULT_TOPIC_NAME)); + this->get_parameter_or("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); // TODO: Are these parameters needed? - private_nh_->declare_parameter("partial_profile_start_point", 0); - private_nh_->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0); - private_nh_->declare_parameter("partial_profile_start_point_data", 4); - private_nh_->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 4); - private_nh_->declare_parameter("partial_profile_point_count", -1); - private_nh_->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1); - private_nh_->declare_parameter("partial_profile_data_width", 4); - private_nh_->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4); + this->declare_parameter("partial_profile_start_point", 0); + this->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0); + this->declare_parameter("partial_profile_start_point_data", 4); + this->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 4); + this->declare_parameter("partial_profile_point_count", -1); + this->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1); + this->declare_parameter("partial_profile_data_width", 4); + this->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4); // Create driver interface object: device_interface_ptr = std::make_unique(); @@ -232,25 +230,25 @@ namespace scancontrol_driver } // Advertise topic - publisher = nh_->create_publisher(config_.topic_name, 10); + publisher = this->create_publisher(config_.topic_name, 10); using std::placeholders::_1; using std::placeholders::_2; // Advertise services - get_feature_srv = private_nh_->create_service( + get_feature_srv = this->create_service( "~/get_feature", std::bind(&ScanControlDriver::ServiceGetFeature, this, _1, _2)); - set_feature_srv = private_nh_->create_service( + set_feature_srv = this->create_service( "~/set_feature", std::bind(&ScanControlDriver::ServiceSetFeature, this, _1, _2)); - get_resolution_srv = private_nh_->create_service( + get_resolution_srv = this->create_service( "~/get_resolution", std::bind(&ScanControlDriver::ServiceGetResolution, this, _1, _2)); - set_resolution_srv = private_nh_->create_service( + set_resolution_srv = this->create_service( "~/set_resolution", std::bind(&ScanControlDriver::ServiceSetResolution, this, _1, _2)); - get_available_resolutions_srv = private_nh_->create_service( + get_available_resolutions_srv = this->create_service( "~/get_available_resolutions", std::bind(&ScanControlDriver::ServiceGetAvailableResolutions, this, _1, _2)); - invert_z_srv = private_nh_->create_service( + invert_z_srv = this->create_service( "~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2)); - invert_x_srv = private_nh_->create_service( + invert_x_srv = this->create_service( "~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2)); } @@ -289,8 +287,8 @@ namespace scancontrol_driver 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; iwidth = resolution; + for (int i=0; ipoints.push_back(point); } @@ -341,7 +339,7 @@ namespace scancontrol_driver /* Process and publish profile */ int ScanControlDriver::ProcessAndPublishProfile(const void * data, size_t data_size){ // Timestamp - pcl_conversions::toPCL(nh_->get_clock()->now(), point_cloud_msg->header.stamp); + pcl_conversions::toPCL(this->get_clock()->now(), point_cloud_msg->header.stamp); // Copy sensor data to local buffer if (data != NULL && data_size == profile_buffer.size()){ @@ -421,23 +419,29 @@ namespace scancontrol_driver const std::shared_ptr request, std::shared_ptr response) { - if (response->return_code = StopProfileTransfer() < GENERAL_FUNCTION_OK){ - // return true; + response->return_code = StopProfileTransfer(); + if (response->return_code < GENERAL_FUNCTION_OK){ + RCLCPP_WARN_STREAM(LOGGER, "Error while stopping transmission! Code: " << response->return_code); + return; } - if (response->return_code = device_interface_ptr->SetResolution(request->resolution) < GENERAL_FUNCTION_OK){ + response->return_code = device_interface_ptr->SetResolution(request->resolution); + if (response->return_code < GENERAL_FUNCTION_OK){ RCLCPP_WARN_STREAM(LOGGER, "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){ + response->return_code = SetPartialProfile(temp_resolution); + if (response->return_code < GENERAL_FUNCTION_OK){ RCLCPP_WARN_STREAM(LOGGER, "Error while setting partial profile. Code: " << response->return_code); - // return true; + return; } - if (response->return_code = StartProfileTransfer() < GENERAL_FUNCTION_OK){ - // return true; + response->return_code = StartProfileTransfer(); + if (response->return_code < GENERAL_FUNCTION_OK){ + RCLCPP_WARN_STREAM(LOGGER, "Error while starting transmission! Code: " << response->return_code); + return; } - // Change of resolution was succesull + // // Change of resolution was succesull config_.resolution = request->resolution; // return true; } diff --git a/micro_epsilon_scancontrol_driver/src/node.cpp b/micro_epsilon_scancontrol_driver/src/node.cpp index 4547622..f44d3be 100644 --- a/micro_epsilon_scancontrol_driver/src/node.cpp +++ b/micro_epsilon_scancontrol_driver/src/node.cpp @@ -1,27 +1,30 @@ #include "rclcpp/rclcpp.hpp" #include "micro_epsilon_scancontrol_driver/driver.h" - +#include +#include static const rclcpp::Logger logger = rclcpp::get_logger("scancontrol_driver"); int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("scancontrol_driver"); - rclcpp::Node::SharedPtr private_node = rclcpp::Node::make_shared("scancontrol_driver"); - + // Start the driver try { - scancontrol_driver::ScanControlDriver driver(node, private_node); + std::shared_ptr driver = std::make_shared(); RCLCPP_INFO(logger, "Driver started"); + //Turn On Laser + driver->SetFeature(FEATURE_FUNCTION_LASERPOWER,2); + // Loop driver until shutdown - driver.StartProfileTransfer(); - while(rclcpp::ok()) - { - rclcpp::spin_some(node); - } - driver.StopProfileTransfer(); + driver->StartProfileTransfer(); + rclcpp::spin(driver); + + //Turn Off Laser + driver->SetFeature(FEATURE_FUNCTION_LASERPOWER,0); + driver->StopProfileTransfer(); + return 0; } catch(const std::runtime_error& error)