int main(int argc, char** argv) {
LMS1xx laser;
scanCfg sCfg;
if(argc < 4) {
print_usage();
return 0;
}
laser.connect(argv[1]);
if(!laser.isConnected()) {
std::cout << "Unable to connect to device at address : " << argv[1] << std::endl;
return 0;
}
sCfg.angleResolution = (int)(boost::lexical_cast<double>(std::string(argv[2])) * 10000);
sCfg.scaningFrequency = boost::lexical_cast<int>(std::string(argv[3])) * 100;
laser.login();
laser.setScanCfg(sCfg);
laser.saveConfig();
sCfg = laser.getScanCfg();
std::cout << "Configuration set to : " << std::endl;
std::cout << "resolution : " << (double)sCfg.angleResolution/10000.0 << std::endl;
std::cout << "frequency : " << (double)sCfg.scaningFrequency/100.0 << std::endl;
laser.disconnect();
return 0;
}
int main()
{
LMS1xx laser;
scanData data;
laser.connect("192.168.0.1");
if(!laser.isConnected())
{
std::cout << "connection failend" << std::endl;
return 0;
}
std::cout << "Connected to laser" << std::endl;
std::cout << "Loging in ..." << std::endl;
laser.login();
laser.stopMeas();
std::cout << "Geting scan configuration ..." << ::std::endl;
scanCfg c = laser.getScanCfg();
//std::cout << "Scanning Frequency : " << c.scaningFrequency/100.0 << "Hz AngleResolution : " << c.angleResolution/10000.0 << "deg " << std::endl;
c.angleResolution = 5000;
c.scaningFrequency = 5000;
laser.setScanCfg(c);
scanDataCfg cc;
cc.deviceName = false;
cc.encoder = 0;
cc.outputChannel = 3;
cc.remission = true;
cc.resolution = 0;
cc.position = false;
cc.outputInterval = 1;
laser.setScanDataCfg(cc);
int ret = 0;
std::cout << "Start measurements ..." << std::endl;
laser.startMeas();
std::cout << "Wait for ready status ..." << std::endl;
ret = 0;
while (ret != 7)
{
ret = laser.queryStatus();
std::cout << "status : " << ret << std::endl;
sleep(1);
}
std::cout << "Laser ready" << std::endl;
std::cout << "Start continuous data transmission ..." << std::endl;
laser.scanContinous(1);
for(int i =0; i < 3; i++)
{
std::cout << "Receive data sample ..." << std::endl;
laser.getData(data);
}
std::cout << "Stop continuous data transmission ..." << std::endl;
laser.scanContinous(0);
laser.stopMeas();
std::cout << "Disconnect from laser" << std::endl;
laser.disconnect();
return 0;
}
int main(int argc, char **argv)
{
// laser data
LMS1xx laser;
scanCfg cfg;
scanDataCfg dataCfg;
scanData data;
// published data
sensor_msgs::LaserScan scan_msg;
// parameters
std::string host;
std::string frame_id;
ros::init(argc, argv, "lms1xx");
ros::NodeHandle nh;
ros::NodeHandle n("~");
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
n.param<std::string>("host", host, "192.168.0.1");
n.param<std::string>("frame_id", frame_id, "laser");
ROS_INFO("connecting to laser at : %s", host.c_str());
// initialize hardware
laser.connect(host);
if (laser.isConnected())
{
ROS_INFO("Connected to laser.");
laser.login();
cfg = laser.getScanCfg();
scan_msg.header.frame_id = frame_id;
scan_msg.range_min = 0.01;
scan_msg.range_max = 20.0;
scan_msg.scan_time = 100.0/cfg.scaningFrequency;
scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 * DEG2RAD;
scan_msg.angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD - M_PI/2;
scan_msg.angle_max = (double)cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2;
std::cout << "resolution : " << (double)cfg.angleResolution/10000.0 << " deg " << std::endl;
std::cout << "frequency : " << (double)cfg.scaningFrequency/100.0 << " Hz " << std::endl;
int num_values;
if (cfg.angleResolution == 2500)
{
num_values = 1081;
}
else if (cfg.angleResolution == 5000)
{
num_values = 541;
}
else
{
ROS_ERROR("Unsupported resolution");
return 0;
}
scan_msg.time_increment = scan_msg.scan_time/num_values;
scan_msg.ranges.resize(num_values);
scan_msg.intensities.resize(num_values);
dataCfg.outputChannel = 1;
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
laser.setScanDataCfg(dataCfg);
laser.startMeas();
status_t stat;
do // wait for ready status
{
stat = laser.queryStatus();
ros::Duration(1.0).sleep();
}
while (stat != ready_for_measurement);
laser.startDevice(); // Log out to properly re-enable system after config
laser.scanContinous(1);
while (ros::ok())
{
ros::Time start = ros::Time::now();
scan_msg.header.stamp = start;
++scan_msg.header.seq;
laser.getData(data);
for (int i = 0; i < data.dist_len1; i++)
{
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
for (int i = 0; i < data.rssi_len1; i++)
{
scan_msg.intensities[i] = data.rssi1[i];
}
scan_pub.publish(scan_msg);
ros::spinOnce();
}
laser.scanContinous(0);
laser.stopMeas();
laser.disconnect();
}
else
{
ROS_ERROR("Connection to device failed");
}
return 0;
}
int main(int argc, char **argv)
{
// laser data
LMS1xx laser;
scanCfg cfg;
scanDataCfg dataCfg;
scanData data;
// published data
sensor_msgs::LaserScan scan_msg;
// parameters
std::string host;
std::string frame_id;
double range_max = 20.0;
double start_angle = -135.0*DEG2RAD;
double end_angle = 135.0*DEG2RAD;
double frequency = 25.0;
ros::init(argc, argv, "lms1xx");
ros::NodeHandle nh;
ros::NodeHandle n("~");
n.param<std::string>("host", host, "192.168.1.2");
n.param<std::string>("frame_id", frame_id, "laser");
n.param<double>("range_max", range_max, 20.0);
n.param<double>("start_angle", start_angle,-135.0*DEG2RAD);
n.param<double>("end_angle", end_angle,135.0*DEG2RAD);
n.param<double>("frequency", frequency,25.0);
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
ROS_INFO("connecting to laser at : %s", host.c_str());
// initialize hardware
laser.connect(host);
if (laser.isConnected())
{
ROS_INFO("Connected to laser.");
laser.login();
cfg = laser.getScanCfg();
int num_values;
if (cfg.angleResolution == 2500)
{
num_values = 1081;
}
else if (cfg.angleResolution == 5000)
{
num_values = 541;
}
else
{
ROS_ERROR("Unsupported resolution");
return 0;
}
double sensor_start_angle = cfg.startAngle/10000.0 * DEG2RAD - M_PI/2.0;
double sensor_end_angle = cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2.0;
ROS_INFO_STREAM("\n\nLaser measurement config received from the sensor :");
ROS_INFO("\t sensor start angle %.2f [deg] - %.6f [rad]", cfg.startAngle/10000.0 - 90, sensor_start_angle );
ROS_INFO("\t sensor stop angle %.2f [deg] - %.6f [rad]", cfg.stopAngle/10000.0 - 90, sensor_end_angle );
ROS_INFO("\t sensor angle resolution %.2f [deg] - %.6f [rad]", cfg.angleResolution/10000.0, cfg.angleResolution/10000.0 * DEG2RAD);
ROS_INFO("\t sensor scan frequency %.2f [hz]", cfg.scaningFrequency/100.0);
ROS_INFO("\t sensor num readings %d", num_values);
/// Setting fields of scan msg
scan_msg.header.frame_id = frame_id;
scan_msg.range_min = 0.01;
scan_msg.range_max = range_max;
int frequency_reducer = static_cast<int>( cfg.scaningFrequency/(100.0 * frequency) );
frequency = cfg.scaningFrequency/(100.0* frequency_reducer);
scan_msg.scan_time = 1.0/frequency;
scan_msg.angle_increment = cfg.angleResolution/10000.0 * DEG2RAD;
/// Swap start/end angle in case of inverted sign
if(start_angle > end_angle)
{
double tmp = start_angle;
start_angle = end_angle;
end_angle = tmp;
}
/// Check if sensor starting angle is greater than requested starting angle
if(start_angle < sensor_start_angle )
start_angle = sensor_start_angle;
/// Check if sensor ending angle is smaller than requested ending angle
if(end_angle > sensor_end_angle)
end_angle = sensor_end_angle;
/// Adjusting min max angle values considering angular resolution
start_angle = sensor_start_angle + scan_msg.angle_increment* (static_cast<int>( ( fabs(start_angle - sensor_start_angle)) /scan_msg.angle_increment ) + 1);
end_angle = sensor_end_angle - scan_msg.angle_increment* ( static_cast<int>( ( fabs(sensor_end_angle - end_angle)) /scan_msg.angle_increment ) + 1);
/// Setting min max angle values in ros scan msg
scan_msg.angle_min = start_angle;
scan_msg.angle_max = end_angle;
int filtered_num_values = static_cast<int>((end_angle - start_angle)/scan_msg.angle_increment ) + 1;
// ROS_INFO("Sensor num of readings %d, filtered num of readings %d", num_values, filtered_num_values);
num_values = filtered_num_values;
ROS_INFO_STREAM("\n\nLaser measurement output from the driver :");
ROS_INFO("\t msg start angle %.2f [deg] - %.6f [rad]", start_angle*RAD2DEG, start_angle);
ROS_INFO("\t msg stop angle %.2f [deg - %.6f [rad]", end_angle*RAD2DEG, end_angle);
ROS_INFO("\t msg angle resolution %.2f [deg] - %.6f [rad]", cfg.angleResolution/10000.0, scan_msg.angle_increment);
ROS_INFO("\t msg topic frequency %.2f [hz]", frequency);
ROS_INFO("\t msg min range %.2f [m]", scan_msg.range_min);
ROS_INFO("\t msg max range %.2f [m]", scan_msg.range_max);
ROS_INFO("\t msg num readings %d", num_values);
ROS_INFO("\t frequency reduction factor is %d", frequency_reducer);
scan_msg.time_increment = scan_msg.scan_time/num_values;
scan_msg.ranges.resize(num_values);
scan_msg.intensities.resize(num_values);
dataCfg.outputChannel = 1;
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
laser.setScanDataCfg(dataCfg);
laser.startMeas();
status_t stat;
do // wait for ready status
{
stat = laser.queryStatus();
ros::Duration(1.0).sleep();
}
while (stat != ready_for_measurement);
laser.startDevice(); // Log out to properly re-enable system after config
laser.scanContinous(1);
int counter = -1;
while (ros::ok())
{
ros::Time start = ros::Time::now();
scan_msg.header.stamp = start;
++scan_msg.header.seq;
laser.getData(data);
/// frequency publishing reduction
counter++;
if(counter%frequency_reducer != 0)
continue;
double angle = sensor_start_angle;
int j = 0;
for (int i = 0; i < data.dist_len1; i++)
{
if( angle - scan_msg.angle_min < -1e-4 || angle - scan_msg.angle_max > 1e-4 )
{
angle += scan_msg.angle_increment;
continue;
}
scan_msg.ranges[j] = data.dist1[i] * 0.001;
angle += scan_msg.angle_increment;
j++;
}
angle = sensor_start_angle;
j = 0;
for (int i = 0; i < data.rssi_len1; i++)
{
if(angle - scan_msg.angle_min < -1e-4 || angle - scan_msg.angle_max > 1e-4)
{
angle += scan_msg.angle_increment;
continue;
}
scan_msg.intensities[j] = data.rssi1[i];
angle += scan_msg.angle_increment;
j++;
}
scan_pub.publish(scan_msg);
ros::spinOnce();
}
laser.scanContinous(0);
laser.stopMeas();
laser.disconnect();
}
else
{
ROS_ERROR("Connection to device failed");
}
return 0;
}
int main(int argc, char **argv)
{
/* laser data */
LMS1xx laser;
scanCfg cfg;
scanDataCfg dataCfg;
scanData data;
/* published data */
sensor_msgs::LaserScan scan_msg;
/* parameters */
std::string host;
std::string topic_id;
std::string frame_id;
/* variables */
int retrycount = 0;
ros::init(argc, argv, "LMS111_node");
ros::NodeHandle nh;
ros::NodeHandle n("~");
n.param<std::string> ("host", host, "192.168.0.10");
n.param<std::string> ("topic_id", topic_id, "scan_msg");
n.param<std::string> ("frame_id", frame_id, "/map");
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan> (topic_id, 1);
ROS_INFO("Connecting to device at : %s", host.c_str());
/* initialize hardware */
laser.connect(host);
if (laser.isConnected())
{
ROS_INFO("Connected to device at : %s", host.c_str());
laser.login();
cfg = laser.getScanCfg();
scan_msg.header.frame_id = frame_id;
scan_msg.range_min = 0.01;
scan_msg.range_max = 6.0;
scan_msg.scan_time = 100.0 / cfg.scaningFrequency;
scan_msg.angle_increment = cfg.angleResolution / 10000.0 * DEG2RAD;
scan_msg.angle_min = cfg.startAngle / 10000.0 * DEG2RAD - M_PI / 2;
scan_msg.angle_max = cfg.stopAngle / 10000.0 * DEG2RAD - M_PI / 2;
//std::cout << "res : " << cfg.angleResolution << " freq : " << cfg.scaningFrequency << std::endl;
int num_values;
if (cfg.angleResolution == 2500)
{
num_values = 1081;
}
else if (cfg.angleResolution == 5000)
{
num_values = 541;
}
else
{
ROS_ERROR("Unsupported resolution");
return 0;
}
scan_msg.time_increment = scan_msg.scan_time / num_values;
scan_msg.ranges.resize(num_values);
scan_msg.intensities.resize(num_values);
dataCfg.outputChannel = 1;
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
laser.setScanDataCfg(dataCfg);
laser.startMeas();
status_t stat;
do // wait for ready status
{
retrycount++;
stat = laser.queryStatus();
ros::Duration(1.0).sleep();
} while ((stat != ready_for_measurement) && retrycount < MAXRETRYES);
if (stat == ready_for_measurement)
{
ROS_INFO("Device ready - receiving measurement");
laser.scanContinous(1);
while (ros::ok())
{
ros::Time start = ros::Time::now();
scan_msg.header.stamp = start;
++scan_msg.header.seq;
laser.getData(data);
for (int i = 0; i < data.dist_len1; i++)
{
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
for (int i = 0; i < data.rssi_len1; i++)
{
scan_msg.intensities[i] = data.rssi1[i];
}
scan_pub.publish(scan_msg);
ros::spinOnce();
}
laser.scanContinous(0);
laser.stopMeas();
laser.disconnect();
}
else
{
ROS_ERROR("Device not ready for measurement");
return 0;
}
}
else
{
ROS_ERROR("Connection to device @ %s failed", host.c_str());
}
return 0;
}
/**
* Main entry of program
*/
int main(int argc, char **argv)
{
// laser data
LMS1xx laser;
scanCfg cfg;
scanDataCfg dataCfg;
scanData data;
// published data
sensor_msgs::LaserScan scan_msg;
// parameters
string host;
string frame_id;
// ROS
ros::init(argc, argv, "lms1xx");
ros::NodeHandle nh;
ros::NodeHandle n("~");
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("/sick/scan", 1);
ros::Publisher scanfil_pub = nh.advertise<sensor_msgs::LaserScan>("/sick/scan_filtered", 1);
n.param<string>("host", host, "192.168.1.2");
n.param<string>("frame_id", frame_id, "laser");
n.param<bool>( "EnableRawOutput", EnableRawOutput, false);
n.param<bool>( "UpsideDown", UpsideDown, true);
n.param< int>( "FilterSelect", FilterSelect, 2); // Median filter is standard
// initialize hardware
ROS_INFO("connecting to laser at : %s", host.c_str());
while (!laser.isConnected())
laser.connect(host);
while(!laser.isLoggedin())
laser.login();
ROS_INFO("Connected to laser and logged in.");
cfg = laser.getScanCfg();
scan_msg.header.frame_id = frame_id;
scan_msg.range_min = 0.01;
scan_msg.range_max = 20.0;
scan_msg.scan_time = 100.0/cfg.scaningFrequency;
float angle_incr = (double)cfg.angleResolution/10000.0 * DEG2RAD;
float angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD;
float angle_max = (double)cfg.stopAngle /10000.0 * DEG2RAD;
cout << "resolution : " << (double)cfg.angleResolution/10000.0 << " deg " << endl;
cout << "frequency : " << (double)cfg.scaningFrequency/100.0 << " Hz " << endl;
int num_values;
if (cfg.angleResolution == 2500)
{
num_values = 1081;
}
else if (cfg.angleResolution == 5000)
{
num_values = 541;
}
else
{
ROS_ERROR("Unsupported resolution");
return 0;
}
// Init filters
InitFilterNone ( num_values);
InitFilterAverage ( num_values);
InitFilterMedian ( num_values);
// Set some scan_msg defaults
scan_msg.time_increment = scan_msg.scan_time/num_values;
scan_msg.ranges.resize(num_values);
#if SEND_INTENSITIES
scan_msg.intensities.resize(num_values);
dataCfg.remission = true;
#else
dataCfg.remission = false;
#endif
dataCfg.outputChannel = 1;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
laser.setScanDataCfg(dataCfg);
laser.startMeas();
status_t stat;
do // wait for ready status
{
stat = laser.queryStatus();
ros::Duration(1.0).sleep();
}
while (stat != ready_for_measurement);
laser.startDevice(); // Log out to properly re-enable system after config
laser.scanContinous(1);
while (ros::ok())
{
ros::Time start = ros::Time::now();
scan_msg.header.stamp = start;
++scan_msg.header.seq;
laser.getData(data);
if( UpsideDown)
{
scan_msg.angle_increment = -angle_incr;
scan_msg.angle_min = angle_max;
scan_msg.angle_max = angle_min;
}
else
{
scan_msg.angle_increment = angle_incr;
scan_msg.angle_min = angle_min;
scan_msg.angle_max = angle_max;
}
#if SEND_INTENSITIES
for (int i = 0; i < data.rssi_len1; i++)
{
scan_msg.intensities[i] = data.rssi1[i];
}
#endif
// Send to /sick/scan if raw data is wanted
if( EnableRawOutput)
{
for (int i = 0; i < data.dist_len1; i++)
{
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
scan_pub.publish(scan_msg);
}
switch( FilterSelect)
{
case 1:
ExecuteFilterAverage( data, scan_msg);
break;
case 2:
ExecuteFilterMedian( data, scan_msg);
break;
default:
ExecuteFilterNone( data, scan_msg);
break;
}
scanfil_pub.publish(scan_msg);
ros::spinOnce();
}
// de-init filters
DestroyFilterNone();
DestroyFilterAverage();
DestroyFilterMedian();
laser.scanContinous(0);
laser.stopMeas();
laser.disconnect();
return 0;
}
int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
static int index = 0;
///Laser Data
LMS1xx LaserSensor;
scanCfg cfg;
scanData data;
scanDataCfg dataCfg;
status_t status;
std::ofstream file;
double start_angle=0;
double stop_angle=0;
double resolution=0;
double frequency=0;
///Kmeans realated variables
KMeans<> K;
mat dataset;
size_t cluster;
Col<size_t> assignments;
mat centroid;
///Connect to the Lasersensor
LaserSensor.connect(host);
if(LaserSensor.isConnected())
{
std::cout << "\nConnected !!!\n";
LaserSensor.login();
///Get Laser Configurations
cfg = LaserSensor.getScanCfg();
//cfg.angleResolution = 0.25*10000.0;
//cfg.scaningFrequency = 25*100;
//LaserSensor.setScanCfg(cfg);
//LaserSensor.saveConfig();
// sleep(3);
cfg = LaserSensor.getScanCfg();
start_angle = cfg.startAngle/10000.0; //* DEG2RAD - M_PI/2;
stop_angle = cfg.stopAngle/10000.0; //* DEG2RAD - M_PI/2;
resolution = cfg.angleResolution/10000.0;
frequency = cfg.scaningFrequency/100;
std::cout << "Start Angle: " << start_angle;
std::cout << "\tStop Angle: " << stop_angle;
std::cout << "\tResolution: " << resolution;
std::cout << "\tFrequency: " << frequency;
std::cout << std::endl;
dataCfg.outputChannel = 1;
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
LaserSensor.setScanDataCfg(dataCfg); ///Set Data Configuration of the laser data
LaserSensor.startMeas(); ///Start Measurement
do
{
status = LaserSensor.queryStatus();
usleep(200);
}
while(status != ready_for_measurement);
{
LaserSensor.startDevice();
LaserSensor.scanContinous(1);
while(LaserSensor.isConnected())
{
LaserSensor.getData(data); ///Get the Laser Data
// u_int16_t range[data.dist_len1];
// u_int16_t intensity[data.rssi_len1];
int range[data.dist_len1];
int intensity[data.rssi_len1];
for(int i=0; i<data.dist_len1;i++)
range[i] = data.dist1[i];
for(int i=0; i<data.rssi_len1;i++)
intensity[i] = data.rssi1[i];
if (index == 0)
{
index++;
std::cout << std::endl << "Data len = " << data.dist_len1 << std::endl;
std::cout << "Intensity len = " << data.rssi_len1 << std::endl;
///distance assumed to be in mm
///Start angle is -45 end is 225
float angle_scan = -45.0;
float x[1081], y[1081]; ///The resolution is 0.5 degress so 541 values
int index_range = 0;
double slope;
cluster = 2;
//centroid.zeros();
dataset.resize(2,1081);
dataset.zeros();
file.open("LaserData.txt");
while(1)
{
x[index_range] = range[index_range]*cos(angle_scan*DEG2RAD)/1000.0;
y[index_range] = range[index_range] * sin(angle_scan*DEG2RAD)/1000.0;
//std::cout << "range: " << range[index_range] << " angle: " << angle_scan;
//std::cout << " x: " << x[index_range] << " y : " << y[index_range] << std::endl;
angle_scan += 0.25;
//if(intensity[index_range] >=850)
{
file << x[index_range] << "," << y[index_range] << "," << intensity[index_range] << std::endl;
}
if (angle_scan > 225.0)
{
break;
}
index_range++;
usleep(100);
}
int index_tmp = 0;
for(int i=0; i<1081;i++)
{
if (intensity[i] >= 900)
{
dataset(0,index_tmp) = x[i];
dataset(1,index_tmp) = y[i];
std::cout << "\n" << dataset[0,index_tmp] << "\t" << dataset[1,index_tmp];
index_tmp++;
}
}
std::cout << "\nKMeans Calculations!!!" << std::endl;
dataset.resize(2,index_tmp);
///Actual KMeans CLustering
K.Cluster((arma::mat) dataset,2,assignments,centroid);
/*************************************************************************************************************************
static double sum_x[2];
static double sum_y[2];
int number_dist1=0;
int number_dist2=0;
for(int i=0; i < assignments.size(); i++) {
switch(assignments[i])
{
case 0:
sum_x[0]+=dataset(0,i);
sum_y[0]+=dataset(1,i);
number_dist1++;
break;
case 1:
sum_x[1]+=dataset(0,i);
sum_y[1]+=dataset(1,i);
number_dist2++;
break;
};
std::cout << "\n" << assignments[i];
}
double center1_x, center1_y;
double center2_x, center2_y;
center1_x = sum_x[0]/number_dist1;
center1_y = sum_y[0]/number_dist1;
center2_x = sum_x[1]/number_dist2;
center2_y = sum_y[1]/number_dist2;
std::cout<<center1_x<<"," << center1_y<<" " << center2_x<<","<<center2_y<<endl;
**************************************************************************************************************************/
//std::cout << "\n" << centroid(0,0) << "\t" << centroid(1,0) << "\t"<< centroid(0,1) << "\t"<< centroid(1,1)<<"\n";
slope = (centroid(1,1) - centroid(1,0)) / (centroid(0,1) - centroid(0,0));
slope = (atan(slope))*RAD2DEG;
std::cout << "\nclusters= " << cluster << std::endl;
std::cout << "\nOrientation= " << slope << std::endl;
}
usleep(200);
}
std::cout << "\n Sensor Disconnected \n";
///Disconnect the Laser
LaserSensor.scanContinous(0);
LaserSensor.stopMeas();
LaserSensor.disconnect();
file.close();
}
}
else
{
std::cout <<"\nSensor Not Connected !!!\n";
}
return a.exec();
}
