46 : costmap_(costmap), sum_scores_(false) {
47 if (costmap != NULL) {
80 ROS_ERROR(
"Footprint spec is empty, maybe missing call to setFootprint?");
84 for (
unsigned int i = 0; i < traj.getPointsSize(); ++i) {
85 traj.getPoint(i, px, py, pth);
97 cost = std::max(cost, f_cost);
103 double vmag = hypot(traj.
xv_, traj.
yv_);
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)));