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");
void updateWorld(const std::vector< geometry_msgs::Point > &footprint, const std::vector< costmap_2d::Observation > &observations, const std::vector< PlanarLaserScan > &laser_scans)
Inserts observations from sensors into the point grid.
A class that implements the WorldModel interface to provide free-space collision checks for the traje...
void printPolygonPS(const std::vector< geometry_msgs::Point32 > &poly, double line_width)
int main(int argc, char **argv)
void printPoint(const geometry_msgs::Point &pt)
void insert(const geometry_msgs::Point32 &pt)
Insert a point into the point grid.
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
Checks if any points in the grid lie inside a convex footprint.