world_model.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
38 #define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
39 
40 #include <vector>
41 #include <costmap_2d/observation.h>
42 #include <costmap_2d/footprint.h>
43 #include <geometry_msgs/Point.h>
45 
46 namespace base_local_planner {
52  class WorldModel{
53  public:
62  virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
63  double inscribed_radius, double circumscribed_radius) = 0;
64 
65  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){
66 
67  double cos_th = cos(theta);
68  double sin_th = sin(theta);
69  std::vector<geometry_msgs::Point> oriented_footprint;
70  for(unsigned int i = 0; i < footprint_spec.size(); ++i){
71  geometry_msgs::Point new_pt;
72  new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
73  new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
74  oriented_footprint.push_back(new_pt);
75  }
76 
77  geometry_msgs::Point robot_position;
78  robot_position.x = x;
79  robot_position.y = y;
80 
81  if(inscribed_radius==0.0){
82  costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
83  }
84 
85  return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
86  }
87 
96  double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
97  double inscribed_radius, double circumscribed_radius, double extra) {
98  return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
99  }
100 
104  virtual ~WorldModel(){}
105 
106  protected:
108  };
109 
110 };
111 #endif
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
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)
Definition: world_model.h:65
double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius, double extra)
Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid...
Definition: world_model.h:96
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
virtual ~WorldModel()
Subclass will implement a destructor.
Definition: world_model.h:104
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:25