Skip to content

Commit

Permalink
Merge pull request #36 from nikhil-sethi/ros2-devel
Browse files Browse the repository at this point in the history 
Fix ros2 port using node class
  • Loading branch information
@nikhil-sethi
nikhil-sethi authored Oct 15, 2024
2 parents 12940b7 + 145e675 commit 6e3fb66
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 44 deletions.
6 changes: 2 additions & 4 deletions micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ typedef pcl::PointCloud<pcl::PointXYZI> 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
Expand Down Expand Up @@ -97,8 +97,6 @@ namespace scancontrol_driver
bool transfer_active_ = false;

// ROS handles
rclcpp::Node::SharedPtr nh_;
rclcpp::Node::SharedPtr private_nh_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher;
rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::GetFeature>::SharedPtr get_feature_srv;
rclcpp::Service<micro_epsilon_scancontrol_msgs::srv::SetFeature>::SharedPtr set_feature_srv;
Expand Down
56 changes: 27 additions & 29 deletions micro_epsilon_scancontrol_driver/src/driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>("resolution", -1);
private_nh_->get_parameter_or("resolution", config_.resolution, -1);
this->declare_parameter<int>("resolution", -1);
this->get_parameter_or("resolution", config_.resolution, -1);

// Multiple device parameters
private_nh_->declare_parameter<std::string>("serial", std::string(""));
private_nh_->get_parameter_or("serial", config_.serial, std::string(""));
private_nh_->declare_parameter<std::string>("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<std::string>("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<std::string>("serial", std::string(""));
this->get_parameter_or("serial", config_.serial, std::string(""));
this->declare_parameter<std::string>("frame_id", std::string(DEFAULT_FRAME_ID));
this->get_parameter_or("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID));
this->declare_parameter<std::string>("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<int>("partial_profile_start_point", 0);
private_nh_->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0);
private_nh_->declare_parameter<int>("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<int>("partial_profile_point_count", -1);
private_nh_->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1);
private_nh_->declare_parameter<int>("partial_profile_data_width", 4);
private_nh_->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4);
this->declare_parameter<int>("partial_profile_start_point", 0);
this->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0);
this->declare_parameter<int>("partial_profile_start_point_data", 4);
this->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 4);
this->declare_parameter<int>("partial_profile_point_count", -1);
this->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1);
this->declare_parameter<int>("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<CInterfaceLLT>();
Expand Down Expand Up @@ -232,25 +230,25 @@ namespace scancontrol_driver
}

// Advertise topic
publisher = nh_->create_publisher<sensor_msgs::msg::PointCloud2>(config_.topic_name, 10);
publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(config_.topic_name, 10);

using std::placeholders::_1;
using std::placeholders::_2;

// Advertise services
get_feature_srv = private_nh_->create_service<micro_epsilon_scancontrol_msgs::srv::GetFeature>(
get_feature_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetFeature>(
"~/get_feature", std::bind(&ScanControlDriver::ServiceGetFeature, this, _1, _2));
set_feature_srv = private_nh_->create_service<micro_epsilon_scancontrol_msgs::srv::SetFeature>(
set_feature_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetFeature>(
"~/set_feature", std::bind(&ScanControlDriver::ServiceSetFeature, this, _1, _2));
get_resolution_srv = private_nh_->create_service<micro_epsilon_scancontrol_msgs::srv::GetResolution>(
get_resolution_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetResolution>(
"~/get_resolution", std::bind(&ScanControlDriver::ServiceGetResolution, this, _1, _2));
set_resolution_srv = private_nh_->create_service<micro_epsilon_scancontrol_msgs::srv::SetResolution>(
set_resolution_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::SetResolution>(
"~/set_resolution", std::bind(&ScanControlDriver::ServiceSetResolution, this, _1, _2));
get_available_resolutions_srv = private_nh_->create_service<micro_epsilon_scancontrol_msgs::srv::GetAvailableResolutions>(
get_available_resolutions_srv = this->create_service<micro_epsilon_scancontrol_msgs::srv::GetAvailableResolutions>(
"~/get_available_resolutions", std::bind(&ScanControlDriver::ServiceGetAvailableResolutions, this, _1, _2));
invert_z_srv = private_nh_->create_service<std_srvs::srv::SetBool>(
invert_z_srv = this->create_service<std_srvs::srv::SetBool>(
"~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2));
invert_x_srv = private_nh_->create_service<std_srvs::srv::SetBool>(
invert_x_srv = this->create_service<std_srvs::srv::SetBool>(
"~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2));
}

Expand Down Expand Up @@ -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()){
Expand Down
25 changes: 14 additions & 11 deletions micro_epsilon_scancontrol_driver/src/node.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,27 +1,30 @@
#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");

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<scancontrol_driver::ScanControlDriver> driver = std::make_shared<scancontrol_driver::ScanControlDriver>();
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)
Expand Down

0 comments on commit 6e3fb66

Please sign in to comment.