Skip to content

Commit ae5be64

Browse files
amockamock
authored
Merge pull request #81 from amock/better-inflation-ranges
InflationLayer: Better Radius Parameter Limits + Standard Decay Function
2 parents e80fd30 + 8756323 commit ae5be64

2 files changed

Lines changed: 49 additions & 157 deletions

File tree

‎mesh_layers/include/mesh_layers/inflation_layer.h‎

Lines changed: 4 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -81,19 +81,6 @@ class InflationLayer : public mesh_map::AbstractLayer
8181
*/
8282
virtual float threshold() override;
8383

84-
/**
85-
* @brief inflate around lethal vertices by inflating to neighbours on mesh and using squared distances and assign
86-
* riskiness values to vertices
87-
*
88-
* @param lethals set of current lethal vertices
89-
* @param inflation_radius radius of the inflation
90-
* @param inscribed_radius radius of the inscribed area
91-
* @param inscribed_value value assigned to vertices in inscribed area
92-
* @param lethal_value value assigned to lethal vertices
93-
*/
94-
void lethalCostInflation(const std::set<lvr2::VertexHandle>& lethals, const float inflation_radius,
95-
const float inscribed_radius, const float inscribed_value, const float lethal_value);
96-
9784
inline float computeUpdateSethianMethod(const float& d1, const float& d2, const float& a, const float& b,
9885
const float& dot, const float& F);
9986

@@ -125,7 +112,7 @@ class InflationLayer : public mesh_map::AbstractLayer
125112
*
126113
* @return resulting cost value
127114
*/
128-
float fading(const float val);
115+
float fading(const float squared_distance);
129116

130117
/**
131118
* @brief inflate around lethal vertices by using an wave front propagation and assign riskiness values to vertices
@@ -241,8 +228,9 @@ class InflationLayer : public mesh_map::AbstractLayer
241228
double inscribed_radius = 0.25;
242229
double inflation_radius = 0.4;
243230
double factor = 1.0;
244-
double lethal_value = 2.0;
245-
double inscribed_value = 1.0;
231+
double lethal_value = 1.0;
232+
double inscribed_value = 0.99;
233+
double cost_scaling_factor = 1.0;
246234
int min_contour_size = 3;
247235
bool repulsive_field = true;
248236
} config_;

‎mesh_layers/src/inflation_layer.cpp‎

Lines changed: 45 additions & 141 deletions
Original file line numberDiff line numberDiff line change
@@ -94,13 +94,6 @@ void InflationLayer::updateLethal(std::set<lvr2::VertexHandle>& added_lethal,
9494
RCLCPP_INFO_STREAM(node_->get_logger(), "Update lethal for inflation layer.");
9595
waveCostInflation(lethal_vertices_, config_.inflation_radius, config_.inscribed_radius, config_.inscribed_value,
9696
std::numeric_limits<float>::infinity());
97-
98-
/*lethalCostInflation(lethal_vertices_, config_.inflation_radius,
99-
config_.inscribed_radius, config_.inscribed_value,
100-
config_.lethal_value == -1
101-
? std::numeric_limits<float>::infinity()
102-
: config_.lethal_value);
103-
*/
10497
}
10598

10699
inline float InflationLayer::computeUpdateSethianMethod(const float& d1, const float& d2, const float& a,
@@ -233,22 +226,29 @@ inline bool InflationLayer::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
233226
return false;
234227
}
235228

236-
float InflationLayer::fading(const float val)
229+
float InflationLayer::fading(const float squared_distance)
237230
{
238-
if (val > config_.inflation_radius)
239-
return 0;
231+
const float distance = sqrt(squared_distance);
240232

241-
// Inflation radius
242-
if (val > config_.inscribed_radius)
233+
if (distance > config_.inflation_radius)
243234
{
244-
// Map values from [config_.inscribed_radius, config_.inflation_radius] to [0, pi]
245-
float alpha = ((val - config_.inscribed_radius) / (config_.inflation_radius - config_.inscribed_radius)) * M_PI;
246-
return config_.inscribed_value * (cos(alpha) + 1) / 2.0;
235+
return 0;
236+
}
237+
238+
// <= Inflation radius
239+
if (distance > config_.inscribed_radius)
240+
{
241+
// http://wiki.ros.org/costmap_2d - cost decay function
242+
const float factor = exp(-1.0f * config_.cost_scaling_factor * (distance - config_.inscribed_radius));
243+
const float cost = config_.inscribed_value * factor;
244+
return cost;
247245
}
248246

249-
// Inscribed radius
250-
if (val > 0)
247+
// <= Inscribed radius
248+
if (distance > 0)
249+
{
251250
return config_.inscribed_value;
251+
}
252252

253253
// Lethality
254254
return config_.lethal_value;
@@ -310,7 +310,6 @@ void InflationLayer::waveCostInflation(const std::set<lvr2::VertexHandle>& letha
310310

311311
if (map_ptr_->invalid[current_vh])
312312
continue;
313-
314313
// check if already fixed
315314
// if(fixed[current_vh]) continue;
316315
fixed[current_vh] = true;
@@ -517,117 +516,14 @@ void InflationLayer::backToSource(const lvr2::VertexHandle& current_vertex,
517516
}
518517
}
519518

520-
void InflationLayer::lethalCostInflation(const std::set<lvr2::VertexHandle>& lethals, const float inflation_radius,
521-
const float inscribed_radius, const float inscribed_value,
522-
const float lethal_value)
523-
{
524-
RCLCPP_INFO_STREAM(node_->get_logger(), "lethal cost inflation.");
525-
526-
struct Cmp
527-
{
528-
const lvr2::DenseVertexMap<float>& distances;
529-
Cmp(const lvr2::DenseVertexMap<float>& distances) : distances(distances)
530-
{
531-
}
532-
533-
bool operator()(lvr2::VertexHandle& left, lvr2::VertexHandle& right)
534-
{
535-
return distances[left] > distances[right];
536-
}
537-
};
538-
539-
const auto mesh = map_ptr_->mesh();
540-
541-
lvr2::DenseVertexMap<bool> seen(mesh->nextVertexIndex(), false);
542-
lvr2::DenseVertexMap<float> distances(mesh->nextVertexIndex(), std::numeric_limits<float>::max());
543-
lvr2::DenseVertexMap<lvr2::VertexHandle> prev;
544-
prev.reserve(mesh->nextVertexIndex());
545-
546-
for (auto vH : mesh->vertices())
547-
{
548-
prev.insert(vH, vH);
549-
}
550-
551-
Cmp cmp(distances);
552-
std::priority_queue<lvr2::VertexHandle, std::vector<lvr2::VertexHandle>, Cmp> pq(cmp);
553-
554-
for (auto vH : lethals)
555-
{
556-
distances[vH] = 0;
557-
pq.push(vH);
558-
}
559-
560-
float inflation_radius_squared = inflation_radius * inflation_radius;
561-
float inscribed_radius_squared = inscribed_radius * inscribed_radius;
562-
563-
while (!pq.empty())
564-
{
565-
lvr2::VertexHandle vH = pq.top();
566-
pq.pop();
567-
568-
if (seen[vH])
569-
continue;
570-
seen[vH] = true;
571-
572-
std::vector<lvr2::VertexHandle> neighbours;
573-
mesh->getNeighboursOfVertex(vH, neighbours);
574-
for (auto n : neighbours)
575-
{
576-
if (distances[n] == 0 || seen[n])
577-
continue;
578-
float n_dist = mesh->getVertexPosition(n).squaredDistanceFrom(mesh->getVertexPosition(prev[vH]));
579-
if (n_dist < distances[n] && n_dist < inflation_radius_squared)
580-
{
581-
prev[n] = prev[vH];
582-
distances[n] = n_dist;
583-
pq.push(n);
584-
}
585-
}
586-
}
587-
588-
for (auto vH : mesh->vertices())
589-
{
590-
if (distances[vH] > inflation_radius_squared)
591-
{
592-
riskiness_.insert(vH, 0);
593-
}
594-
595-
// Inflation radius
596-
else if (distances[vH] > inscribed_radius_squared)
597-
{
598-
float alpha = (sqrt(distances[vH]) - inscribed_radius) / (inflation_radius - inscribed_radius) * M_PI;
599-
riskiness_.insert(vH, inscribed_value * (cos(alpha) + 1) / 2.0);
600-
}
601-
602-
// Inscribed radius
603-
else if (distances[vH] > 0)
604-
{
605-
riskiness_.insert(vH, inscribed_value);
606-
}
607-
608-
// Lethality
609-
else
610-
{
611-
riskiness_.insert(vH, lethal_value);
612-
}
613-
}
614-
615-
RCLCPP_INFO_STREAM(node_->get_logger(), "lethal cost inflation finished.");
616-
}
617-
618519
bool InflationLayer::computeLayer()
619520
{
620-
/*waveCostInflation(lethal_vertices_, config_.inflation_radius,
521+
waveCostInflation(lethal_vertices_, config_.inflation_radius,
621522
config_.inscribed_radius, config_.inscribed_value,
622523
std::numeric_limits<float>::infinity());
623-
*/
624-
// lethalCostInflation(lethal_vertices_, config_.inflation_radius,
625-
// config_.inscribed_radius, config_.inscribed_value,
626-
// std::numeric_limits<float>::infinity());
627-
// config_.lethal_value);
628-
629524
return true;
630525
}
526+
631527
lvr2::VertexMap<float>& InflationLayer::costs()
632528
{
633529
return riskiness_;
@@ -638,40 +534,38 @@ rcl_interfaces::msg::SetParametersResult InflationLayer::reconfigureCallback(std
638534
rcl_interfaces::msg::SetParametersResult result;
639535
result.successful = true;
640536

641-
bool has_inflation_radius_changed = false;
642-
bool has_vector_field_parameter_changed = false;
537+
bool recompute_inflation_costs = false;
538+
643539
for (auto parameter : parameters) {
644540
if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius") {
645541
config_.inflation_radius = parameter.as_double();
646-
has_inflation_radius_changed = true;
647-
has_vector_field_parameter_changed = true;
542+
recompute_inflation_costs = true;
648543
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius") {
649544
config_.inscribed_radius = parameter.as_double();
650-
has_vector_field_parameter_changed = true;
545+
recompute_inflation_costs = true;
651546
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".lethal_value") {
652547
config_.lethal_value = parameter.as_double();
653-
has_vector_field_parameter_changed = true;
548+
recompute_inflation_costs = true;
654549
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value") {
655550
config_.inscribed_value = parameter.as_double();
656-
has_vector_field_parameter_changed = true;
551+
recompute_inflation_costs = true;
552+
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".cost_scaling_factor") {
553+
config_.cost_scaling_factor = parameter.as_double();
554+
recompute_inflation_costs = true;
657555
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".min_contour_size") {
658556
config_.min_contour_size = parameter.as_int();
557+
recompute_inflation_costs = true;
659558
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".repulsive_field") {
660559
config_.repulsive_field = parameter.as_bool();
560+
recompute_inflation_costs = true;
661561
}
662562
}
663563

664-
if (has_inflation_radius_changed){
665-
waveCostInflation(lethal_vertices_, config_.inflation_radius, config_.inscribed_radius, config_.inscribed_value, std::numeric_limits<float>::infinity());
666-
}
667-
668-
if (has_vector_field_parameter_changed)
564+
if (recompute_inflation_costs)
669565
{
566+
waveCostInflation(lethal_vertices_, config_.inflation_radius, config_.inscribed_radius, config_.inscribed_value, std::numeric_limits<float>::infinity());
670567
map_ptr_->publishVectorField("inflation", vector_map_, distances_,
671568
std::bind(&mesh_layers::InflationLayer::fading, this, std::placeholders::_1));
672-
}
673-
674-
if (has_vector_field_parameter_changed || has_inflation_radius_changed) {
675569
RCLCPP_INFO_STREAM(node_->get_logger(), "Inflation layer (name: " << layer_name_ << ") changed due to cfg update.");
676570
notifyChange();
677571
}
@@ -687,7 +581,7 @@ bool InflationLayer::initialize()
687581
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
688582
rcl_interfaces::msg::FloatingPointRange range;
689583
range.from_value = 0.01;
690-
range.to_value = 1.0;
584+
range.to_value = 5.0;
691585
descriptor.floating_point_range.push_back(range);
692586
config_.inscribed_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius", config_.inscribed_radius, descriptor);
693587
}
@@ -697,7 +591,7 @@ bool InflationLayer::initialize()
697591
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
698592
rcl_interfaces::msg::FloatingPointRange range;
699593
range.from_value = 0.01;
700-
range.to_value = 3.0;
594+
range.to_value = 10.0;
701595
descriptor.floating_point_range.push_back(range);
702596
config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius, descriptor);
703597
}
@@ -707,7 +601,7 @@ bool InflationLayer::initialize()
707601
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
708602
rcl_interfaces::msg::FloatingPointRange range;
709603
range.from_value = 0.0;
710-
range.to_value = 1.-0;
604+
range.to_value = 1.0;
711605
descriptor.floating_point_range.push_back(range);
712606
config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
713607
}
@@ -731,6 +625,16 @@ bool InflationLayer::initialize()
731625
descriptor.floating_point_range.push_back(range);
732626
config_.inscribed_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value", config_.inscribed_value, descriptor);
733627
}
628+
{ // cost_scaling_factor
629+
rcl_interfaces::msg::ParameterDescriptor descriptor;
630+
descriptor.description = "Defines the cost scaling factor for the exponential fading outside the inscribed radius.";
631+
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
632+
rcl_interfaces::msg::FloatingPointRange range;
633+
range.from_value = 0.0;
634+
range.to_value = 5.0;
635+
descriptor.floating_point_range.push_back(range);
636+
config_.cost_scaling_factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".cost_scaling_factor", config_.cost_scaling_factor, descriptor);
637+
}
734638
{ // min_contour_size
735639
rcl_interfaces::msg::ParameterDescriptor descriptor;
736640
descriptor.description = "Defines the minimum size for a contour to be classified as 'lethal'.";

0 commit comments

Comments
 (0)