CODE TRACK DANIELE GUSMINI

ID DATA FILE
map_grid_visualizer.cpp
1 21/04/2020
map_grid_visualizer.h
Added 4 explicit topics to visualize separately goal_cost,
DESCRIPTION
path_cost, occ_cost and total cost
CODE COMMENTS
/ /

ID DATA
2 21/04/2020
Added description in the brief .h of setTargetCells and
DESCRIPTION
computeTargetDistance. Modified setTargetCell.
CODE PRE
void
MapGrid::setTargetCells(const
costmap_2d::Costmap2D& costmap,
const
std::vector<geometry_msgs::PoseSta
mped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(
), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*>
path_dist_queue;
std::vector<geometry_msgs::PoseSta
mped> adjusted_global_plan;
adjustPlanResolution(global_plan,
adjusted_global_plan,
costmap.getResolution());
if
(adjusted_global_plan.size() !=
global_plan.size()) {
ROS_DEBUG("Adjusted global
plan resolution, added %zu
points",
adjusted_global_plan.size() -
global_plan.size());
} of the local map
unsigned int i;
FILE
map_grid.cpp
map_grid.h
CODE POST
void MapGrid::setTargetCells(const
costmap_2d::Costmap2D& costmap,
const
std::vector<geometry_msgs::PoseStamp
ed>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(),
costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamp
ed> adjusted_global_plan;
adjustPlanResolution(global_plan,
adjusted_global_plan,
costmap.getResolution());
if (adjusted_global_plan.size()
!= global_plan.size()) {
ROS_DEBUG("Adjusted global
plan resolution, added %zu points",
adjusted_global_plan.size() -
global_plan.size());
}
unsigned int i;
// put global path points into
local map until we reach the border
for (i = 0; i <
 // put global path points into
local map until we reach the
border of the local map
for (i = 0; i <
adjusted_global_plan.size(); ++i)
{ adjusted_global_plan[i].pose.positio
double g_x =
adjusted_global_plan[i].pose.posit
ion.x;
double g_y =
adjusted_global_plan[i].pose.posit
ion.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x,
g_y, map_x, map_y) &&
costmap.getCost(map_x, map_y) !=
costmap_2d::NO_INFORMATION) {
MapCell& current =
getCell(map_x, map_y);
current.target_dist = 0;
//Modfied to ALG C Daniele Gusmini
current.target_mark =
true;
path_dist_queue.push(&current);
started_path = true;
} else if (started_path) {
break;
} i,
} adjusted_global_plan.size(),
if (!started_path) {
ROS_ERROR("None of the %d
first of %zu (%zu) points of the
global plan were in the local
costmap and free",
i,
adjusted_global_plan.size(),
global_plan.size());
return;
} }
adjusted_global_plan.size(); ++i) {
double g_x =
adjusted_global_plan[i].pose.positio
n.x;
double g_y =
n.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x,
g_y, map_x, map_y) &&
costmap.getCost(map_x, map_y) !=
costmap_2d::NO_INFORMATION) {
MapCell& current =
getCell(map_x, map_y);
current.target_dist = 10-i*0.1;
current.target_mark = true;
path_dist_queue.push(&current);
started_path = true;
} else if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d
first of %zu (%zu) points of the
global plan were in the local
costmap and free",
global_plan.size());
return;
}
computeTargetDistance(path_dist_queu
e, costmap);

computeTargetDistance(path_dist_qu
eue, costmap);

}
COMMENTS
Modified to introduce a cost proportional to the distance from the robot
along the curvilinear coordinate
 ID DATA
3 22/04/2020
Added a while cycle that read again the queue, assign a
DESCRIPTION cost = max_cost-steep at each cell and give the cells to the
compute target distance function.
CODE PRE
void
MapGrid::setTargetCells(const
costmap_2d::Costmap2D& costmap,
const
std::vector<geometry_msgs::PoseSta
mped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(
), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*>
path_dist_queue;
std::vector<geometry_msgs::PoseSta
mped> adjusted_global_plan;
adjustPlanResolution(global_plan,
adjusted_global_plan,
costmap.getResolution());
if
(adjusted_global_plan.size() !=
global_plan.size()) {
ROS_DEBUG("Adjusted global
plan resolution, added %zu
points",
adjusted_global_plan.size() -
global_plan.size());
} if (adjusted_global_plan.size()
unsigned int i;
// put global path points into
local map until we reach the
border of the local map
for (i = 0; i <
adjusted_global_plan.size(); ++i)
{ unsigned int i;
double g_x =
adjusted_global_plan[i].pose.posit
ion.x;
double g_y =
adjusted_global_plan[i].pose.posit
ion.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x,
g_y, map_x, map_y) &&
costmap.getCost(map_x, map_y) !=
costmap_2d::NO_INFORMATION) {
FILE
map_grid.cpp
map_grid.h
CODE POST
void MapGrid::setTargetCells(const
costmap_2d::Costmap2D& costmap,
const
std::vector<geometry_msgs::PoseStamp
ed>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(),
costmap.getSizeInCellsY());
bool started_path = false;
//Check if a local_path as been
initialized
queue<MapCell*> path_dist_queue;
//Initialize a queue of the cell
whoose contains the local path
queue<MapCell*>
path_dist_queue_modified;
//Initialize a queue of the cell
whoose contains the local path,
modified with a decreasing cost
std::vector<geometry_msgs::PoseStamp
ed> adjusted_global_plan;
adjustPlanResolution(global_plan,
adjusted_global_plan,
costmap.getResolution()); //Resize
the global path to fit the
resolution of the cost_grid (local)
!= global_plan.size()) {
ROS_DEBUG("Adjusted global
plan resolution, added %zu points",
adjusted_global_plan.size() -
global_plan.size());
}
// put global path points into local
map until we reach the border of the
local map
for (i = 0; i <
adjusted_global_plan.size(); ++i) {
//For each point of the local path
find the cell and if it has not been
check before update cost
double g_x =
adjusted_global_plan[i].pose.positio
 MapCell& current = n.x;
getCell(map_x, map_y); double g_y =
current.target_dist = 10-i*0.1; adjusted_global_plan[i].pose.positio
current.target_mark = n.y;
true; unsigned int map_x, map_y;
if (costmap.worldToMap(g_x,
g_y, map_x, map_y) &&
path_dist_queue.push(&current); costmap.getCost(map_x, map_y) !=
started_path = true; costmap_2d::NO_INFORMATION) {
} else if (started_path) { MapCell& current =
break; getCell(map_x, map_y);
} //Pointer to the current cell of
} grid
if (!started_path) { // current.target_dist = 10-
ROS_ERROR("None of the %d i*0.1;
first of %zu (%zu) points of the //-->Modfied to ALG C Daniele
global plan were in the local Gusmini
costmap and free", current.target_dist = 0;
i, current.target_mark = true;
adjusted_global_plan.size(), //Cell marked as updated
global_plan.size());
return; path_dist_queue.push(&current);
} //Add the cell to the queue
started_path = true;
//Declare the path started
computeTargetDistance(path_dist_qu } else if (started_path) {
eue, costmap); break;
}
} }
if (!started_path) {
ROS_ERROR("None of the %d
first of %zu (%zu) points of the
global plan were in the local
costmap and free",
i,
adjusted_global_plan.size(),
global_plan.size());
return;
}

//Update the cost of the cell

containing the path base on the
dimension of queue

double steep;
double cost;
unsigned long lPathLenght;

steep = 0.1;
lPathLenght =
path_dist_queue.size();
cost = lPathLenght * steep;

while(!path_dist_queue.empty()){
path_dist_queue.front()-
>target_dist = cost;

path_dist_queue_modified.push(path_d
ist_queue.front());
path_dist_queue.pop();
cost = cost - steep;
 }

computeTargetDistance(path_dist_queu
e_modified, costmap);
//Compute the remaining cell costs
starting from the path

}
COMMENTS
The while cycle read and create another queue, unable to access at each
element of a queue without decomposing it.

ID DATA FILE
dwa_planner.cpp
4 23/04/2020
dwa_planner.h
Added read parameters mod and steep from param
DESCRIPTION server and call setMod method on the path_cost.
Modified critics and getCellCost to exclude goal.
CODE PRE CODE POST
DWAPlanner::DWAPlanner(std::string DWAPlanner::DWAPlanner(std::string
name, name,
base_local_planner::LocalPlannerUt base_local_planner::LocalPlannerUtil
il *planner_util) : *planner_util) :
planner_util_(planner_util), planner_util_(planner_util),
obstacle_costs_(planner_util-
obstacle_costs_(planner_util- >getCostmap()),
>getCostmap()), path_costs_(planner_util-
path_costs_(planner_util- >getCostmap()),
>getCostmap()), goal_costs_(planner_util-
goal_costs_(planner_util- >getCostmap(), 0.0, 0.0, true),
>getCostmap(), 0.0, 0.0, true),
goal_front_costs_(planner_util-
goal_front_costs_(planner_util- >getCostmap(), 0.0, 0.0, true),
>getCostmap(), 0.0, 0.0, true), alignment_costs_(planner_util-
>getCostmap())
alignment_costs_(planner_util- {
>getCostmap()) ros::NodeHandle private_nh("~/"
{ + name);
ros::NodeHandle
private_nh("~/" + name);
goal_front_costs_.setStopOnFailure(
false );
goal_front_costs_.setStopOnFailure
( false ); alignment_costs_.setStopOnFailure( f
alse );
alignment_costs_.setStopOnFailure(
false ); //Assuming this planner is being
run within the navigation stack, we
//Assuming this planner is can
being run within the navigation //just do an upward search for
stack, we can
//just do an upward search for
the frequency at which its being
run. This
//also allows the frequency to
be overwritten locally.
std::string
controller_frequency_param_name;
if(!
private_nh.searchParam("controller
_frequency",
controller_frequency_param_name))
{ double controller_frequency =
sim_period_ = 0.05;
} else {
double controller_frequency
= 0;
private_nh.param(controller_freque
ncy_param_name,
controller_frequency, 20.0);
if(controller_frequency > 0)
{ ROS_WARN("A
sim_period_ = 1.0 /
controller_frequency;
} else {
ROS_WARN("A
controller_frequency less than 0
has been set. Ignoring the
parameter, assuming a rate of
20Hz");
sim_period_ = 0.05;
}
} oscillation_costs_.resetOscillationF
ROS_INFO("Sim period is set to
%.2f", sim_period_);
oscillation_costs_.resetOscillatio
nFlags();
bool sum_scores;
private_nh.param("sum_scores",
sum_scores, false);
obstacle_costs_.setSumScores(sum_s
cores);
private_nh.param("publish_cost_gri
d_pc", publish_cost_grid_pc_,
false);
map_viz_.initialize(name,
planner_util->getGlobalFrame(),
boost::bind(&DWAPlanner::getCellCo
sts, this, _1, _2, _3, _4, _5,
_6));
private_nh.param("global_frame_id"
the frequency at which its being
run. This
//also allows the frequency to
be overwritten locally.
std::string
controller_frequency_param_name;
if(!
private_nh.searchParam("controller_f
requency",
controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
0;
private_nh.param(controller_frequenc
y_param_name, controller_frequency,
20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 /
controller_frequency;
} else {
controller_frequency less than 0 has
been set. Ignoring the parameter,
assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to
%.2f", sim_period_);
lags();
bool sum_scores;
private_nh.param("sum_scores",
sum_scores, false);
obstacle_costs_.setSumScores(sum_sco
res);
private_nh.param("publish_cost_grid_
pc", publish_cost_grid_pc_, false);
map_viz_.initialize(name,
planner_util->getGlobalFrame(),
boost::bind(&DWAPlanner::getCellCost
s, this, _1, _2, _3, _4, _5, _6));
private_nh.param("global_frame_id",
frame_id_, std::string("odom"));
traj_cloud_pub_ =
private_nh.advertise<sensor_msgs::Po
intCloud2>("trajectory_cloud", 1);
private_nh.param("publish_traj_pc",
, frame_id_, std::string("odom"));
traj_cloud_pub_ =
private_nh.advertise<sensor_msgs::
PointCloud2>("trajectory_cloud",
1);
private_nh.param("publish_traj_pc"
, publish_traj_pc_, true);
// set up all the cost
functions that will be applied in
order
// (any function returning
negative values will abort
scoring, so the order can improve
performance)
std::vector<base_local_planner::Tr
ajectoryCostFunction*> critics;
critics.push_back(&oscillation_cos
ts_); // discards oscillating
motions (assisgns cost -1)
critics.push_back(&obstacle_costs_
); // discards trajectories that
move into obstacles
critics.push_back(&goal_front_cost
s_); // prefers trajectories that
make the nose go towards (local)
nose goal
critics.push_back(&alignment_costs
_); // prefers trajectories that
keep the robot nose on nose path
critics.push_back(&path_costs_); /
/ prefers trajectories on global
path
critics.push_back(&goal_costs_); /
/ prefers trajectories that go
towards (local) goal, based on
wave propagation
critics.push_back(&twirling_costs_
); // optionally prefer
trajectories that don't spin
// trajectory generators
std::vector<base_local_planner::Tr
ajectorySampleGenerator*>
generator_list;
generator_list.push_back(&generato
r_);
scored_sampling_planner_ =
publish_traj_pc_, false);
//-> Daniele
private_nh.param("mod", mod_,
false); //Read the
parameters from parameter server
private_nh.param("steep",
steep_, 0.0);
path_costs_.setMod(mod_,
steep_); //Method
defined in map_grid_cost_function.h
to send mod and steep params
// set up all the cost functions
that will be applied in order
// (any function returning
negative values will abort scoring,
so the order can improve
performance)
std::vector<base_local_planner::Traj
ectoryCostFunction*> critics;
if (mod_){ //in mod do
not update critics og local goal
critics.push_back(&oscillation_costs
_); // discards oscillating motions
(assisgns cost -1)
critics.push_back(&obstacle_costs_);
// discards trajectories that move
into obstacles
critics.push_back(&goal_front_costs_
); // prefers trajectories that make
the nose go towards (local) nose
goal
critics.push_back(&alignment_costs_)
; // prefers trajectories that keep
the robot nose on nose path
critics.push_back(&path_costs_); //
prefers trajectories on global path
critics.push_back(&twirling_costs_);
// optionally prefer trajectories
that don't spin
}
else {
critics.push_back(&oscillation_costs
_); // discards oscillating motions
(assisgns cost -1)
critics.push_back(&obstacle_costs_);
// discards trajectories that move
into obstacles
critics.push_back(&goal_front_costs_
base_local_planner::SimpleScoredSa
mplingPlanner(generator_list,
critics);
private_nh.param("cheat_factor",
cheat_factor_, 1.0);
//######################
MODIFIED 20200116 Singhiozzo in
curva ###########################
goal_costs_.setStopOnFailure(false
);
path_costs_.setStopOnFailure(false
);
alignment_costs_.setStopOnFailure(
false);
///
##################################
##################################
##################
} ectorySampleGenerator*>
// used for visualization only,
total_costs are not really total
costs
bool
DWAPlanner::getCellCosts(int cx,
int cy, float &path_cost, float
&goal_cost, float &occ_cost, float
&total_cost) {
path_cost =
path_costs_.getCellCosts(cx, cy);
goal_cost =
goal_costs_.getCellCosts(cx, cy);
occ_cost = planner_util_-
>getCostmap()->getCost(cx, cy);
ROS_INFO("Hey");
if (path_cost ==
path_costs_.obstacleCosts() ||
path_cost ==
path_costs_.unreachableCellCosts()
||
occ_cost >=
costmap_2d::INSCRIBED_INFLATED_OBS
TACLE) {
return false;
} ##############
total_cost =
pdist_scale_ * path_cost +
gdist_scale_ * goal_cost +
occdist_scale_ * occ_cost;
return true;
} &goal_cost, float &occ_cost, float
); // prefers trajectories that make
the nose go towards (local) nose
goal
critics.push_back(&alignment_costs_)
; // prefers trajectories that keep
the robot nose on nose path
critics.push_back(&path_costs_); //
prefers trajectories on global path
critics.push_back(&goal_costs_); //
prefers trajectories that go towards
(local) goal, based on wave
propagation
critics.push_back(&twirling_costs_);
// optionally prefer trajectories
that don't spin
}
// trajectory generators
std::vector<base_local_planner::Traj
generator_list;
generator_list.push_back(&generator_
);
scored_sampling_planner_ =
base_local_planner::SimpleScoredSamp
lingPlanner(generator_list,
critics);
private_nh.param("cheat_factor",
cheat_factor_, 1.0);
//######################
MODIFIED 20200116 Singhiozzo in
curva ###########################
goal_costs_.setStopOnFailure(false);
path_costs_.setStopOnFailure(false);
alignment_costs_.setStopOnFailure(fa
lse);
///
####################################
####################################
}
// used for visualization only,
total_costs are not really total
costs
bool DWAPlanner::getCellCosts(int
cx, int cy, float &path_cost, float
&total_cost) {
 path_cost =
path_costs_.getCellCosts(cx, cy);
goal_cost =
goal_costs_.getCellCosts(cx, cy);
occ_cost = planner_util_-
>getCostmap()->getCost(cx, cy);

if (path_cost ==
path_costs_.obstacleCosts() ||
path_cost ==
path_costs_.unreachableCellCosts()
||
occ_cost >=
costmap_2d::INSCRIBED_INFLATED_OBSTA
CLE) {
return false;
}

if(mod_){
total_cost =
pdist_scale_ * path_cost +
occdist_scale_ * occ_cost;
}
else{
total_cost =
pdist_scale_ * path_cost +
gdist_scale_ * goal_cost +
occdist_scale_ * occ_cost;
}

return true;
}
COMMENTS
Mod_ and steep_ are private parameters of the dwa_planner

ID DATA FILE
 5 23/04/2020 map_grid_cost_function.h
Introduced a method setMod that take the values and set
DESCRIPTION
mod and steep
CODE PRE CODE POST
/
/
COMMENTS

ID DATA
6 23/04/2020
Add a check in prepare(), if mod_ call seTargetCellsMod
DESCRIPTION
the modified version that take into parameter steep_
CODE PRE
MapGridCostFunction::MapGridCostFu
nction(costmap_2d::Costmap2D*
costmap,
double xshift,
double yshift,
bool is_local_goal_function,
CostAggregationType
aggregationType) :
costmap_(costmap),
map_(costmap-
>getSizeInCellsX(), costmap-
>getSizeInCellsY()),
aggregationType_(aggregationType),
xshift_(xshift),
yshift_(yshift),
is_local_goal_function_(is_local_g
oal_function),
stop_on_failure_(true) {}
FILE
map_grid_cost_function.cpp
CODE POST
MapGridCostFunction::MapGridCostFunc
tion(costmap_2d::Costmap2D* costmap,
double xshift,
double yshift,
bool is_local_goal_function,
CostAggregationType
aggregationType) :
costmap_(costmap),
map_(costmap->getSizeInCellsX(),
costmap->getSizeInCellsY()),
aggregationType_(aggregationType),
xshift_(xshift),
yshift_(yshift),
is_local_goal_function_(is_local_goa
l_function),
stop_on_failure_(true),
mod_(0), //
steep_(0){} //

void void
MapGridCostFunction::setTargetPose MapGridCostFunction::setTargetPoses(
s(std::vector<geometry_msgs::PoseS std::vector<geometry_msgs::PoseStamp
tamped> target_poses) { ed> target_poses) {
target_poses_ = target_poses; target_poses_ = target_poses;
} }

bool bool MapGridCostFunction::prepare()

MapGridCostFunction::prepare() { {
map_.resetPathDist(); map_.resetPathDist();

if (is_local_goal_function_) { if (is_local_goal_function_) {
map_.setLocalGoal(*costmap_, map_.setLocalGoal(*costmap_,
target_poses_); target_poses_);
} else { } else {
map_.setTargetCells(*costmap_, if(mod_){
target_poses_);
} map_.setTargetCellsMod(*costmap_,
return true; target_poses_, steep_);
} }
else {
map_.setTargetCells(*costmap_,
target_poses_);
}

}
return true;
}
COMMENTS

ID DATA FILE
map_grid.h
7 23/04/2020
map_grid.cpp
Split setTargetCell and *Mod that take as input the
DESCRIPTION
paramenter steep
CODE PRE CODE POST
/
/
COMMENTS

ID DATA FILE
map_grid.h
8 1/05/2020
map_grid.cpp
Added computeTargetDistance to use the quadratic cost
function if enabled from parameters, it calls
DESCRIPTION
updatepathCellMod instead of the standard version that
make uses of the quadratic cost function
CODE PRE CODE POST
void void
MapGrid::computeTargetDistance(que MapGrid::computeTargetDistanceQuad(q
ue<MapCell*>& dist_queue, const ueue<MapCell*>& dist_queue, const
costmap_2d::Costmap2D& costmap){ costmap_2d::Costmap2D& costmap,
MapCell* current_cell; double multi){
MapCell* check_cell; MapCell* current_cell;
unsigned int last_col = MapCell* check_cell;
size_x_ - 1; unsigned int last_col = size_x_
unsigned int last_row = - 1;
size_y_ - 1; unsigned int last_row = size_y_
while(!dist_queue.empty()){ - 1;
current_cell = while(!dist_queue.empty()){
dist_queue.front(); current_cell =
dist_queue.front();

dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell
- 1;
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell -
 if(!check_cell- 1;
>target_mark){ if(!check_cell->target_mark)
//mark the cell as {
visisted //mark the cell as
check_cell->target_mark visisted
= true; check_cell->target_mark =
true;
if(updatePathCell(current_cell,
check_cell, costmap)) { if(updatePathCellQuad(current_cell,
check_cell, costmap, multi)) {
dist_queue.push(check_cell);
} dist_queue.push(check_cell);
} }
} }
}
if(current_cell->cx <
last_col){ if(current_cell->cx <
check_cell = current_cell last_col){
+ 1; check_cell = current_cell +
if(!check_cell- 1;
>target_mark){ if(!check_cell->target_mark)
check_cell->target_mark {
= true; check_cell->target_mark =
true;
if(updatePathCell(current_cell,
check_cell, costmap)) { if(updatePathCellQuad(current_cell,
check_cell, costmap, multi)) {
dist_queue.push(check_cell);
} dist_queue.push(check_cell);
} }
} }
}
if(current_cell->cy > 0){
check_cell = current_cell if(current_cell->cy > 0){
- size_x_; check_cell = current_cell -
if(!check_cell- size_x_;
>target_mark){ if(!check_cell->target_mark)
check_cell->target_mark {
= true; check_cell->target_mark =
true;
if(updatePathCell(current_cell,
check_cell, costmap)) { if(updatePathCellQuad(current_cell,
check_cell, costmap, multi)) {
dist_queue.push(check_cell);
} dist_queue.push(check_cell);
} }
} }
}
if(current_cell->cy <
last_row){ if(current_cell->cy <
check_cell = current_cell last_row){
+ size_x_; check_cell = current_cell +
if(!check_cell- size_x_;
>target_mark){ if(!check_cell->target_mark)
check_cell->target_mark {
= true; check_cell->target_mark =
true;
if(updatePathCell(current_cell,
check_cell, costmap)) { if(updatePathCellQuad(current_cell,
check_cell, costmap, multi)) {
dist_queue.push(check_cell);
 } dist_queue.push(check_cell);
} }
} }
} }
} }
}
};
COMMENTS

ID DATA FILE
9 1/05/2020 map_cell.cpp
Added target_dist_real to indicate the real distance from
the path of the cell not taking into account the cost of the
cell containing the path, root_cost that take trace of the
DESCRIPTION cost of the root cell from which the cell take the cost, the
original target_dist id the alg is the modified version does
not corresponds to the dist but to the dist + the cost of
the root cell.
CODE PRE CODE POST
#include #include
<base_local_planner/map_cell.h> <base_local_planner/map_cell.h>

namespace base_local_planner{ namespace base_local_planner{

MapCell::MapCell() MapCell::MapCell()
: cx(0), cy(0), : cx(0), cy(0),
target_dist(DBL_MAX), target_dist(DBL_MAX),
target_mark(false), target_dist_real(DBL_MAX),
within_robot(false) root_cost(DBL_MAX),
target_mark(false),
{} within_robot(false)

MapCell::MapCell(const MapCell&
mc)
: cx(mc.cx), cy(mc.cy),
target_dist(mc.target_dist),
target_mark(mc.target_mark),
within_robot(mc.within_robot)
{}
};
{}
MapCell::MapCell(const MapCell&
mc)
: cx(mc.cx), cy(mc.cy),
target_dist(mc.target_dist),
target_dist_real(DBL_MAX),
root_cost(DBL_MAX),
target_mark(mc.target_mark),
within_robot(mc.within_robot)
{}
};

COMMENTS

ID DATA FILE
10 1/05/2020 map_cell.cpp
 Added updatePathCellQuad() that compute the cost of
DESCRIPTION the remaining cell as the cost of the root cell + the
quadratic distance*multi
CODE PRE CODE POST
/ inline bool
MapGrid::updatePathCellQuad(MapCell*
current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D&
costmap, double multi){

//if the cell is an obstacle set

the max path distance
unsigned char cost =
costmap.getCost(check_cell->cx,
check_cell->cy);
if(! getCell(check_cell->cx,
check_cell->cy).within_robot &&
(cost ==
costmap_2d::LETHAL_OBSTACLE ||
cost ==
costmap_2d::INSCRIBED_INFLATED_OBSTA
CLE ||
cost ==
costmap_2d::NO_INFORMATION)){
check_cell->target_dist =
obstacleCosts();
return false;
}

double new_target_dist =
current_cell->target_dist_real + 1;
//real distance
//double new_target_dist =
current_cell->target_dist + 100;

if (new_target_dist <
check_cell->target_dist) {
check_cell->root_cost =
current_cell->root_cost;
check_cell->target_dist_real =
new_target_dist;
check_cell->target_dist =
check_cell->root_cost +
pow(new_target_dist*multi,2);
}
return true;
}
COMMENTS