Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
00038 #define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
00039
00040 #include <vector>
00041 #include <costmap_2d/observation.h>
00042 #include <costmap_2d/footprint.h>
00043 #include <geometry_msgs/Point.h>
00044 #include <base_local_planner/planar_laser_scan.h>
00045
00046 namespace base_local_planner {
00052 class WorldModel{
00053 public:
00062 virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
00063 double inscribed_radius, double circumscribed_radius) = 0;
00064
00065 double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){
00066
00067 double cos_th = cos(theta);
00068 double sin_th = sin(theta);
00069 std::vector<geometry_msgs::Point> oriented_footprint;
00070 for(unsigned int i = 0; i < footprint_spec.size(); ++i){
00071 geometry_msgs::Point new_pt;
00072 new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
00073 new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
00074 oriented_footprint.push_back(new_pt);
00075 }
00076
00077 geometry_msgs::Point robot_position;
00078 robot_position.x = x;
00079 robot_position.y = y;
00080
00081 if(inscribed_radius==0.0){
00082 costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
00083 }
00084
00085 return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
00086 }
00087
00096 double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
00097 double inscribed_radius, double circumscribed_radius, double extra) {
00098 return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
00099 }
00100
00104 virtual ~WorldModel(){}
00105
00106 protected:
00107 WorldModel(){}
00108 };
00109
00110 };
00111 #endif