38 #include <sensor_msgs/LaserScan.h>
63 std::ofstream outFile;
67 outFile <<
"#Laser Scan in Text Format, version: " <<
version<< std::endl;
68 outFile <<
"#Created by Jorge Almeida from ros/bag"<< std::endl;
69 outFile <<
"angle_min: " << scan->angle_min<<
" #rad "<<std::endl;
70 outFile <<
"angle_max: " << scan->angle_max<<
" #rad "<<std::endl;
71 outFile <<
"angle_increment: " << scan->angle_increment<<
" #rad "<<std::endl;
72 outFile <<
"time_increment: " << scan->time_increment<<
" #sec"<<std::endl;
73 outFile <<
"scan_time: " << scan->scan_time<<
" #sec"<<std::endl;
74 outFile <<
"range_min: " << scan->range_min<<
" #m"<< std::endl;
75 outFile <<
"range_max: " << scan->range_max<<
" #m"<< std::endl;
76 outFile <<
"#Format of each scan: TIMESTAMP RANGE_1 RANGE_2 ... [RANGE_N]"<<std::endl;
83 outFile << scan->header.stamp <<
" ";
84 for(
float range : scan->ranges){
85 outFile << range <<
" ";
102 int main(
int argc,
char** argv)
104 std::cout<<
"Starting laserWriter"<<std::endl;
105 ros::init(argc, argv,
"laserWriter", ros::init_options::AnonymousName);
108 LaserWriter laserWriter(nh,
"/home/jorge14/Desktop/test1.txt");
113 std::cout<<
"In main loop"<<std::endl;
ros::NodeHandle nh
Ros node handler.
void laserScanHandler(const sensor_msgs::LaserScan::ConstPtr &scan)
ros::Subscriber laserSubscriber
Ros command subscriber.
LaserWriter(const ros::NodeHandle &nh_, std::string outFile)
int main(int argc, char **argv)