46 : costmap_(costmap), sum_scores_(false) {
47 if (costmap != NULL) {
80 ROS_ERROR(
"Footprint spec is empty, maybe missing call to setFootprint?");
97 cost = std::max(cost, f_cost);
108 if (vmag > scaling_speed) {
110 double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
111 scale = max_scaling_factor * ratio + 1.0;
121 std::vector<geometry_msgs::Point> footprint_spec,
125 std::vector<geometry_msgs::Point> scaled_footprint;
126 for(
unsigned int i = 0; i < footprint_spec.size(); ++i) {
127 geometry_msgs::Point new_pt;
128 new_pt.x = scale * footprint_spec[i].x;
129 new_pt.y = scale * footprint_spec[i].y;
130 scaled_footprint.push_back(new_pt);
135 double footprint_cost = world_model->
footprintCost(x, y, th, scaled_footprint);
137 if (footprint_cost < 0) {
140 unsigned int cell_x, cell_y;
143 if ( ! costmap->
worldToMap(x, y, cell_x, cell_y)) {
147 double occ_cost = std::max(std::max(0.0, footprint_cost),
double(costmap->
getCost(cell_x, cell_y)));
void setFootprint(std::vector< geometry_msgs::Point > footprint_spec)
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...
base_local_planner::WorldModel * world_model_
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
unsigned int getPointsSize() const
Return the number of points in the trajectory.
ObstacleCostFunction(costmap_2d::Costmap2D *costmap)
void getPoint(unsigned int index, double &x, double &y, double &th) const
Get a point within the trajectory.
static double footprintCost(const double &x, const double &y, const double &th, double scale, std::vector< geometry_msgs::Point > footprint_spec, costmap_2d::Costmap2D *costmap, base_local_planner::WorldModel *world_model)
double scoreTrajectory(Trajectory &traj)
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed)
double max_scaling_factor_
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
std::vector< geometry_msgs::Point > footprint_spec_
costmap_2d::Costmap2D * costmap_
unsigned char getCost(unsigned int mx, unsigned int my) const
Holds a trajectory generated by considering an x, y, and theta velocity.