40 #ifdef HAVE_SYS_TIME_H
52 printf(
"(%.2f, %.2f, %.2f)", pt.x, pt.y, pt.z);
57 printf(
"%%%%Creator: Eitan Marder-Eppstein (Willow Garage)\n");
58 printf(
"%%%%EndComments\n");
62 printf(
"showpage\n%%%%EOF\n");
65 void printPolygonPS(
const std::vector<geometry_msgs::Point32>& poly,
double line_width){
69 printf(
"%.2f setlinewidth\n", line_width);
71 printf(
"%.4f\t%.4f\tmoveto\n", poly[0].x * 10, poly[0].y * 10);
72 for(
unsigned int i = 1; i < poly.size(); ++i)
73 printf(
"%.4f\t%.4f\tlineto\n", poly[i].x * 10, poly[i].y * 10);
74 printf(
"%.4f\t%.4f\tlineto\n", poly[0].x * 10, poly[0].y * 10);
75 printf(
"closepath stroke\n");
81 int main(
int argc,
char** argv){
82 geometry_msgs::Point origin;
85 PointGrid pg(50.0, 50.0, 0.2, origin, 2.0, 3.0, 0.0);
102 std::vector<geometry_msgs::Point> footprint, footprint2, footprint3;
103 geometry_msgs::Point pt;
107 footprint.push_back(pt);
111 footprint.push_back(pt);
115 footprint.push_back(pt);
119 footprint.push_back(pt);
123 footprint.push_back(pt);
127 footprint2.push_back(pt);
131 footprint2.push_back(pt);
135 footprint2.push_back(pt);
139 footprint2.push_back(pt);
143 footprint3.push_back(pt);
147 footprint3.push_back(pt);
151 footprint3.push_back(pt);
155 footprint3.push_back(pt);
159 footprint3.push_back(pt);
164 geometry_msgs::Point32 point;
169 #ifdef HAVE_SYS_TIME_H
170 struct timeval
start, end;
171 double start_t, end_t, t_diff;
176 #ifdef HAVE_SYS_TIME_H
177 gettimeofday(&
start, NULL);
180 for(
unsigned int i = 0; i < 2000; ++i){
184 #ifdef HAVE_SYS_TIME_H
185 gettimeofday(&end, NULL);
186 start_t =
start.tv_sec + double(
start.tv_usec) / 1e6;
187 end_t = end.tv_sec + double(end.tv_usec) / 1e6;
188 t_diff = end_t - start_t;
189 printf(
"%%Insertion Time: %.9f \n", t_diff);
192 vector<Observation> obs;
193 vector<PlanarLaserScan> scan;
195 #ifdef HAVE_SYS_TIME_H
196 gettimeofday(&
start, NULL);
204 #ifdef HAVE_SYS_TIME_H
205 gettimeofday(&end, NULL);
206 start_t =
start.tv_sec + double(
start.tv_usec) / 1e6;
207 end_t = end.tv_sec + double(end.tv_usec) / 1e6;
208 t_diff = end_t - start_t;
210 printf(
"%%Footprint calc: %.9f \n", t_diff);
214 printf(
"%%Legal footprint %.4f, %.4f\n", legal, legal2);
216 printf(
"%%Illegal footprint\n");