demo.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
32 #include <unistd.h>
33 
34 #include <iostream>
35 #include <string>
36 #include <vector>
37 #include <map>
38 #include <fstream>
39 
40 #include <ros/ros.h>
41 
42 #include <laser_geometry/laser_geometry.h>
43 #include <tf/transform_listener.h>
44 
45 #include <visualization_msgs/Marker.h>
46 #include <visualization_msgs/MarkerArray.h>
47 #include <sensor_msgs/LaserScan.h>
48 
49 #include <atlascar_base/AtlascarStatus.h>
50 #include <atlascar_base/AtlascarPartialStatus.h>
51 #include <atlascar_base/AtlascarVelocityStatus.h>
52 
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>
60 
61 using namespace std;
62 using namespace ros;
63 
64 class Conversion
65 {
66  public:
67 
68  Conversion():
69  n_("~"),
70  listener_(n_,ros::Duration(10))
71  {
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");
75 
76  atlascar_status_report_name_ = ros::names::remap("atlascar_status_report");
77  atlascar_velocity_status_report_name_ = ros::names::remap("atlascar_velocity_status_report");
78 
79  remove(laser_1_report_name_.c_str());
80  remove(laser_2_report_name_.c_str());
81  remove(laser_3_report_name_.c_str());
82 
83  remove(atlascar_status_report_name_.c_str());
84  remove(atlascar_velocity_status_report_name_.c_str());
85 
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_));
89 
90  atlascar_status_handler_ = n_.subscribe("atlascar_status", 1000, &Conversion::atlascarStatusHandler,this);
91  atlascar_velocity_status_handler_ = n_.subscribe("atlascar_velocity_status", 1000, &Conversion::atlascarVelocityStatusHandler,this);
92  }
93 
94  ~Conversion()
95  {
96 
97  }
98 
99  void laserHandler(const sensor_msgs::LaserScan::ConstPtr& scan_in,string report_name)
100  {
101  sensor_msgs::PointCloud2 cloud;
102 
103  listener_.waitForTransform(ros::names::remap("base_link"), scan_in->header.frame_id, ros::Time::now(), ros::Duration(10.0));
104 
105  try
106  {
107  projector_.transformLaserScanToPointCloud(ros::names::remap("base_link"),*scan_in,cloud,listener_);
108  }
109  catch (tf::TransformException ex)
110  {
111  cout<<"Error!! "<<ex.what()<<endl;
112  return;
113  }
114 
115  pcl::PointCloud<pcl::PointXYZ> data;
116  pcl::fromROSMsg(cloud,data);
117 
118  ofstream report_laser;
119  report_laser.open(report_name.c_str(),ios::app);
120 
121  boost::format fm("%.4f");
122  fm % scan_in->header.stamp.toSec();
123 
124  report_laser<<scan_in->header.seq<<" "<<fm.str()<<" "<<ros::names::remap("base_link");
125 
126  for(pcl::PointCloud<pcl::PointXYZ>::iterator it = data.begin();it!=data.end();it++)
127  report_laser<<" "<<it->x<<" "<<it->y<<" "<<it->z;
128 
129  report_laser<<endl;
130 
131  report_laser.close();
132  }
133 
134  void atlascarStatusHandler(const atlascar_base::AtlascarStatusPtr& status)
135  {
136  ofstream report_status;
137  report_status.open(atlascar_status_report_name_.c_str(),ios::app);
138 
139  boost::format fm_stamp("%.4f");
140  fm_stamp % status->header.stamp.toSec();
141 
142  report_status<<status->header.seq<<" "<<fm_stamp.str()<<" ";
143 
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;
146 
147  report_status<<fm_data.str()<<endl;
148  }
149 
150 
151  void atlascarVelocityStatusHandler(const atlascar_base::AtlascarVelocityStatusPtr& status)
152  {
153  ofstream report_status;
154  report_status.open(atlascar_velocity_status_report_name_.c_str(),ios::app);
155 
156  boost::format fm_stamp("%.4f");
157  fm_stamp % status->header.stamp.toSec();
158 
159  report_status<<status->header.seq<<" "<<fm_stamp.str()<<" ";
160 
161  boost::format fm_data("%.1f %.6f %.6f %.6f");
162  fm_data % status->counting % status->pulses_sec % status->revolutions_sec % status->velocity;
163 
164  report_status<<fm_data.str()<<endl;
165 
166  }
167 
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_;
173 
174  ros::Subscriber laser_1_handler_;
175  ros::Subscriber laser_2_handler_;
176  ros::Subscriber laser_3_handler_;
177 
178  ros::Subscriber atlascar_status_handler_;
179  ros::Subscriber atlascar_velocity_status_handler_;
180 
181  ros::NodeHandle n_;
182  tf::TransformListener listener_;
183  laser_geometry::LaserProjection projector_;
184 };
185 
186 int main(int argc,char**argv)
187 {
188  ros::init(argc, argv, "conversion");
189 
190  Conversion convert;
191 
192  cout<<"spining ..."<<endl;
193  ros::spin();
194 
195  return 0;
196 }
Conversion::atlascarStatusHandler
void atlascarStatusHandler(const atlascar_base::AtlascarStatusPtr &status)
Definition: conversion.cpp:134
main
int main(int argc, char **argv)
Definition: demo.cpp:186
Conversion::atlascarVelocityStatusHandler
void atlascarVelocityStatusHandler(const atlascar_base::AtlascarVelocityStatusPtr &status)
Definition: conversion.cpp:151
Conversion::Conversion
Conversion()
Definition: demo.cpp:68
Conversion::~Conversion
~Conversion()
Definition: demo.cpp:94
Conversion::laserHandler
void laserHandler(const sensor_msgs::LaserScan::ConstPtr &scan_in, string report_name)
Definition: conversion.cpp:99

mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18