/** \brief Create a single Cell by initializing his parameters
*
* \param char : a letter
* \return Cell
*
*/
Cell createCell(char c) {
Cell cell;
cell.letter = c;
cell.score = scoreCell(c);
cell.isVisited = 0;
strcpy(cell.bonus, " ");
return cell;
}
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0.0;
if (aggregationType_ == Product) {
cost = 1.0;
}
double px, py, pth;
double grid_dist;
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
grid_dist = scoreCell(px, py, pth);
if(stop_on_failure_){
if (grid_dist == map_.obstacleCosts()) {
return -3.0;
} else if (grid_dist == map_.unreachableCellCosts()) {
return -2.0;
}
}
switch( aggregationType_ ) {
case Last:
cost = grid_dist;
break;
case Sum:
cost += grid_dist;
break;
case Product:
if (cost > 0) {
cost *= grid_dist;
}
break;
}
}
double factor = costmap_->getResolution() * 0.5;
return cost * factor;
}
