42 #include <laser_geometry/laser_geometry.h>
43 #include <tf/transform_listener.h>
45 #include <visualization_msgs/Marker.h>
46 #include <visualization_msgs/MarkerArray.h>
47 #include <sensor_msgs/LaserScan.h>
49 #include <atlascar_base/AtlascarStatus.h>
50 #include <atlascar_base/AtlascarPartialStatus.h>
51 #include <atlascar_base/AtlascarVelocityStatus.h>
53 #include <pcl_ros/transforms.h>
54 #include <pcl/ros/conversions.h>
55 #include <pcl/point_cloud.h>
56 #include <pcl/point_types.h>
57 #include <pcl/segmentation/extract_polygonal_prism_data.h>
58 #include <pcl/filters/extract_indices.h>
59 #include <pcl/filters/project_inliers.h>
70 listener_(n_,ros::Duration(10))
72 laser_1_report_name_ = ros::names::remap(
"laser_1_report");
73 laser_2_report_name_ = ros::names::remap(
"laser_2_report");
74 laser_3_report_name_ = ros::names::remap(
"laser_3_report");
76 atlascar_status_report_name_ = ros::names::remap(
"atlascar_status_report");
77 atlascar_velocity_status_report_name_ = ros::names::remap(
"atlascar_velocity_status_report");
79 remove(laser_1_report_name_.c_str());
80 remove(laser_2_report_name_.c_str());
81 remove(laser_3_report_name_.c_str());
83 remove(atlascar_status_report_name_.c_str());
84 remove(atlascar_velocity_status_report_name_.c_str());
86 laser_1_handler_ = n_.subscribe<sensor_msgs::LaserScan> (
"laser_1",1000,boost::bind(&
Conversion::laserHandler,
this,_1,laser_1_report_name_));
87 laser_2_handler_ = n_.subscribe<sensor_msgs::LaserScan> (
"laser_2",1000,boost::bind(&
Conversion::laserHandler,
this,_1,laser_2_report_name_));
88 laser_3_handler_ = n_.subscribe<sensor_msgs::LaserScan> (
"laser_3",1000,boost::bind(&
Conversion::laserHandler,
this,_1,laser_3_report_name_));
99 void laserHandler(
const sensor_msgs::LaserScan::ConstPtr& scan_in,
string report_name)
101 sensor_msgs::PointCloud2 cloud;
103 listener_.waitForTransform(ros::names::remap(
"base_link"), scan_in->header.frame_id, ros::Time::now(), ros::Duration(10.0));
107 projector_.transformLaserScanToPointCloud(ros::names::remap(
"base_link"),*scan_in,cloud,listener_);
109 catch (tf::TransformException ex)
111 cout<<
"Error!! "<<ex.what()<<endl;
115 pcl::PointCloud<pcl::PointXYZ> data;
116 pcl::fromROSMsg(cloud,data);
118 ofstream report_laser;
119 report_laser.open(report_name.c_str(),ios::app);
121 boost::format fm(
"%.4f");
122 fm % scan_in->header.stamp.toSec();
124 report_laser<<scan_in->header.seq<<
" "<<fm.str()<<
" "<<ros::names::remap(
"base_link");
126 for(pcl::PointCloud<pcl::PointXYZ>::iterator it = data.begin();it!=data.end();it++)
127 report_laser<<
" "<<it->x<<
" "<<it->y<<
" "<<it->z;
131 report_laser.close();
136 ofstream report_status;
137 report_status.open(atlascar_status_report_name_.c_str(),ios::app);
139 boost::format fm_stamp(
"%.4f");
140 fm_stamp % status->header.stamp.toSec();
142 report_status<<status->header.seq<<
" "<<fm_stamp.str()<<
" ";
144 boost::format fm_data(
"%.6f %.6f %.6f %.6f %.6f %.6f");
145 fm_data % status->throttle % status->brake % status->clutch % status->steering_wheel % status->speed % status->rpm;
147 report_status<<fm_data.str()<<endl;
153 ofstream report_status;
154 report_status.open(atlascar_velocity_status_report_name_.c_str(),ios::app);
156 boost::format fm_stamp(
"%.4f");
157 fm_stamp % status->header.stamp.toSec();
159 report_status<<status->header.seq<<
" "<<fm_stamp.str()<<
" ";
161 boost::format fm_data(
"%.1f %.6f %.6f %.6f");
162 fm_data % status->counting % status->pulses_sec % status->revolutions_sec % status->velocity;
164 report_status<<fm_data.str()<<endl;
168 string laser_1_report_name_;
169 string laser_2_report_name_;
170 string laser_3_report_name_;
171 string atlascar_status_report_name_;
172 string atlascar_velocity_status_report_name_;
174 ros::Subscriber laser_1_handler_;
175 ros::Subscriber laser_2_handler_;
176 ros::Subscriber laser_3_handler_;
178 ros::Subscriber atlascar_status_handler_;
179 ros::Subscriber atlascar_velocity_status_handler_;
182 tf::TransformListener listener_;
183 laser_geometry::LaserProjection projector_;
188 ros::init(argc, argv,
"conversion");
192 cout<<
"spining ..."<<endl;
void atlascarStatusHandler(const atlascar_base::AtlascarStatusPtr &status)
int main(int argc, char **argv)
void atlascarVelocityStatusHandler(const atlascar_base::AtlascarVelocityStatusPtr &status)
void laserHandler(const sensor_msgs::LaserScan::ConstPtr &scan_in, string report_name)