ROS_Background_Extraction_Box.rar_ROS_extraction

#include "ros/ros.h" #include "std_msgs/String.h" #include "std_msgs/Float32.h" #include "sensor_msgs/LaserScan.h" #include <message_filters/subscriber.h> #include <message_filters/time_synchronizer.h> #include <fstream> #include <rosbag/bag.h> #include <cmath> #define TODEG (0.5) //Since 0.5° resultion #define ROTATE (45.0) //How much the cartesian representation is rotated using namespace std; using namespace ros; using namespace sensor_msgs; using namespace std_msgs; using namespace message_filters; /* * IMPORTANT: * THIS PROGRAM ONLY WORKS, IF THE BACKGROUND-FILE IS PLAYED BEFORE THE DATA-FILE. * * Alina Zoz * Fabian Schiemenz * Michael Valentin */ int backgroundCnt = 10; //Take x-th input measurement as background int totalNrOfCalls = 0; //To detect when the input-data has ended int msgPerSet = 541; //2*270+1 for 0.5° angular resolution and 270° FOV //Filter Parameter float mindist = 0.2; //min distance to background in meter. 0.2 (to make sure that there //really is no change to the background during the measurement) int n = 20; //width of change corresponds to n/2 degrees. Here: ~10° width used for filtering //Corresponds to one measurements with all 541 datapoints typedef struct { LaserScanPtr scan; Time time; string topic; } data; vector<data> filteredVals; //recorded data vector<float> background; //dataset corresponding to background bool firstTime = true; //To make sure end-time is only set once ros::Time end; //Signal for processing the data ros::Duration dur(11); //Approx. duration of data-bagfile in secs /************************* METHODS **************************/ //Does the filtering void filterOneDataset(LaserScan::ConstPtr newArrayOfRangeMeasurements, const string& topic, int index) { sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan()); output->angle_increment = newArrayOfRangeMeasurements->angle_increment; output->angle_max = newArrayOfRangeMeasurements->angle_max; output->angle_min = newArrayOfRangeMeasurements->angle_min; output->time_increment = newArrayOfRangeMeasurements->time_increment; output->scan_time = newArrayOfRangeMeasurements->scan_time; output->range_min = newArrayOfRangeMeasurements->range_min; output->range_max = newArrayOfRangeMeasurements->range_max; output->header = newArrayOfRangeMeasurements->header; output->intensities = newArrayOfRangeMeasurements->intensities; //subtract background //eliminate changes which do not have a width of at least n scan-points //Thus, positive diffs only remain, if the next n measurements are also positive int width = 0; for (unsigned int i = 0; i < newArrayOfRangeMeasurements->ranges.size(); i++) { float diff = abs( background[i] - newArrayOfRangeMeasurements->ranges[i]); if (diff > mindist) { output->ranges.push_back(newArrayOfRangeMeasurements->ranges[i]); width++; } else { output->ranges.push_back(0.0); //Check width of change. If smaller than n, set last "width"-elements to zero if (width < n || i == newArrayOfRangeMeasurements->ranges.size()) { for (int j = width; j > 0; j--) { output->ranges[i - j] = 0.0; } } width = 0; } } data dat; dat.scan = output; dat.topic = topic; dat.time = newArrayOfRangeMeasurements->header.stamp; filteredVals.push_back(dat); } void chatterCallback(const LaserScan::ConstPtr& msg, const string& topic) { totalNrOfCalls++; if (strcmp(topic.c_str(), "LM_Background") == 0) { if (totalNrOfCalls == backgroundCnt) { totalNrOfCalls = 0; backgroundCnt = -1; //Background-data --> store one of all measurements as background-vector //Here we take the first measurement for (int i = 0; i < msgPerSet; i++) { background.push_back(msg.get()->ranges[i]); } ROS_INFO("Background successfully saved!\n Please play the dataset-file.\n"); } } else { if (firstTime) { firstTime = false; end = ros::Time::now() + dur; ROS_INFO("Processing data.\n"); } filterOneDataset(msg, topic, totalNrOfCalls); //Filter dataset --> subtract background and //store in filteredVals-attribute } } int main(int argc, char **argv) { init(argc, argv, "boxFilter"); NodeHandle n; ros::Subscriber sub1 = n.subscribe<LaserScan>("LMS", 2000,//Subscriber for data-topic boost::bind(chatterCallback, _1, "LMS")); ros::Subscriber sub2 = n.subscribe<LaserScan>("LM_Background", 2000, //Subscriber for background-file boost::bind(chatterCallback, _1, "LM_Background")); end = ros::Time::now() + ros::Duration(999); //999s to play the bag-file while (ros::Time::now() < end) { ros::spinOnce(); } //Save filtered Data into a bag-file and txt-file ROS_INFO("Saving new bagFile...\n"); rosbag::Bag resultbag; resultbag.open("result.bag", rosbag::bagmode::Write); ofstream movBox; char path[99]; for (unsigned int j = 0; j < filteredVals.size(); j++) { data dat = filteredVals.at(j); resultbag.write(dat.topic, dat.time, dat.scan); //Also write to file sprintf(path, "movBox_%d.txt", j); movBox.open(path, std::ios_base::app); for (int k = 0; k < msgPerSet; k++) { float x = dat.scan.get()->ranges[k] * cos(((k * TODEG) - ROTATE) * M_PI / 180.0); float y = dat.scan.get()->ranges[k] * sin(((k * TODEG) - ROTATE) * M_PI / 180.0); movBox << x << ",\t" << y << "\n"; } movBox.close(); } resultbag.close(); ROS_INFO("%s\n", "Done!"); shutdown(); return 0; }