Merge pull request #82 from amock/cost-aware-planning

Cost-aware planning

Author: amock

cvp_mesh_planner/src/cvp_mesh_planner.cpp

@@ -230,7 +230,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& start, con
                                               std::list<std::pair<mesh_map::Vector, lvr2::FaceHandle>>& path,
                                               std::string& message)
 {
-  return waveFrontPropagation(start, goal, mesh_map_->edgeDistances(), mesh_map_->vertexCosts(), path, message,
+  return waveFrontPropagation(start, goal, mesh_map_->edgeWeights(), mesh_map_->vertexCosts(), path, message,
                               potential_, predecessors_);
 }
 
@@ -633,7 +633,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
     }
   }
 
-return false;
+  return false;
 }
 
 uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_start,

dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp

@@ -198,7 +198,7 @@ void DijkstraMeshPlanner::computeVectorMap()
 uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& start, const mesh_map::Vector& goal,
                                        std::list<lvr2::VertexHandle>& path)
 {
-  return dijkstra(start, goal, mesh_map_->edgeDistances(), mesh_map_->vertexCosts(), path, potential_, predecessors_);
+  return dijkstra(start, goal, mesh_map_->edgeWeights(), mesh_map_->vertexCosts(), path, potential_, predecessors_);
 }
 
 uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, const mesh_map::Vector& original_goal,

mesh_layers/include/mesh_layers/border_layer.h

@@ -139,7 +139,6 @@ class BorderLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.5;
     double border_cost = 1.0;
-    double factor = 1.0;
   } config_;
 };
 

mesh_layers/include/mesh_layers/clearance_layer.h

@@ -142,7 +142,6 @@ class ClearanceLayer : public mesh_map::AbstractLayer
   struct {
     double robot_height = 0.5;
     double height_inflation = 0.3;
-    double factor = 1.0;
   } config_;
 };
 

mesh_layers/include/mesh_layers/height_diff_layer.h

@@ -139,7 +139,6 @@ class HeightDiffLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.185;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 

mesh_layers/include/mesh_layers/inflation_layer.h

@@ -227,7 +227,6 @@ class InflationLayer : public mesh_map::AbstractLayer
   struct {
     double inscribed_radius = 0.25;
     double inflation_radius = 0.4;
-    double factor = 1.0;
     double lethal_value = 1.0;
     double inscribed_value = 0.99;
     double cost_scaling_factor = 1.0;

mesh_layers/include/mesh_layers/ridge_layer.h

@@ -142,7 +142,6 @@ class RidgeLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.3;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 

mesh_layers/include/mesh_layers/roughness_layer.h

@@ -140,7 +140,6 @@ class RoughnessLayer : public mesh_map::AbstractLayer
   struct {
     double threshold = 0.3;
     double radius = 0.3;
-    double factor = 1.0;
   } config_;
 };
 

mesh_layers/include/mesh_layers/steepness_layer.h

@@ -138,7 +138,6 @@ class SteepnessLayer : public mesh_map::AbstractLayer
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   struct {
     double threshold = 0.3;
-    double factor = 1.0;
   } config_;
 
 };

mesh_layers/src/border_layer.cpp

@@ -127,8 +127,6 @@ rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::v
       config_.border_cost = parameter.as_double();
       recompute_costs = true;
       recompute_lethals = true;
-    } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
-      config_.factor = parameter.as_double();
     }
   }
 
@@ -175,16 +173,6 @@ bool BorderLayer::initialize()
     descriptor.floating_point_range.push_back(range);
     config_.border_cost = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost", config_.border_cost, descriptor);
   }
-  { // factor
-    rcl_interfaces::msg::ParameterDescriptor descriptor;
-    descriptor.description = "Using this factor to weight this layer.";
-    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-    rcl_interfaces::msg::FloatingPointRange range;
-    range.from_value = 0.0;
-    range.to_value = 1.0;
-    descriptor.floating_point_range.push_back(range);
-    config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
-  }
   dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(
       &BorderLayer::reconfigureCallback, this, std::placeholders::_1));
   return true;