Skip to content

Commit

Permalink
Clang format updated
Browse files Browse the repository at this point in the history
  • Loading branch information
@NithishkumarS
NithishkumarS committed Nov 29, 2023
1 parent 2a303a0 commit bff5c76
Show file tree
Hide file tree
Showing 5 changed files with 826 additions and 706 deletions.
269 changes: 140 additions & 129 deletions micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,17 @@
#ifndef _SCANCONTROL_DRIVER_H_
#define _SCANCONTROL_DRIVER_H_

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

#include <std_srvs/SetBool.h>
#include <llt.h>
#include <mescan.h>
#include <micro_epsilon_scancontrol_msgs/GetAvailableResolutions.h>
#include <micro_epsilon_scancontrol_msgs/GetFeature.h>
#include <micro_epsilon_scancontrol_msgs/SetFeature.h>
#include <micro_epsilon_scancontrol_msgs/GetResolution.h>
#include <micro_epsilon_scancontrol_msgs/SetFeature.h>
#include <micro_epsilon_scancontrol_msgs/SetResolution.h>
#include <micro_epsilon_scancontrol_msgs/GetAvailableResolutions.h>

#include <llt.h>
#include <mescan.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <std_srvs/SetBool.h>

#define MAX_DEVICE_INTERFACE_COUNT 6
#define MAX_RESOLUTION_COUNT 6
Expand All @@ -26,122 +24,135 @@ typedef pcl::PointCloud<pcl::PointXYZI> 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<CInterfaceLLT> device_interface_ptr;
TScannerType device_type;
TPartialProfile t_partial_profile_;
std::vector<guint8> profile_buffer;
std::vector<double> value_x, value_z;
int lost_values;
unsigned int lost_profiles;

point_cloud_t::Ptr point_cloud_msg;

std::map<std::string, unsigned int> 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<unsigned int> 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_
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<CInterfaceLLT> device_interface_ptr;
TScannerType device_type;
TPartialProfile t_partial_profile_;
std::vector<guint8> profile_buffer;
std::vector<double> value_x, value_z;
int lost_values;
unsigned int lost_profiles;

point_cloud_t::Ptr point_cloud_msg;

std::map<std::string, unsigned int> 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<unsigned int> 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_
67 changes: 34 additions & 33 deletions micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/nodelet.h
View file
Original file line number Diff line number Diff line change
@@ -1,42 +1,43 @@
#ifndef _SCANCONTROL_NODELET_H_
#define _SCANCONTROL_NODELET_H_

#include <string>
#include <boost/thread.hpp>

#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

#include <boost/thread.hpp>
#include <string>

#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<ScanControlDriver> driver_;
};

}

#endif // _SCANCONTROL_NODELET_H_
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<ScanControlDriver> driver_;
};

} // namespace scancontrol_driver

#endif // _SCANCONTROL_NODELET_H_
Loading

0 comments on commit bff5c76

Please sign in to comment.