Skip to content

Commit

Permalink
Merge pull request #66 from amock/dynamic-loading
Browse files Browse the repository at this point in the history 
Dynamic loading of different mesh formats
  • Loading branch information
@amock
amock authored Nov 20, 2024
2 parents 1f95987 + ef42e0a commit fedea89
Show file tree
Hide file tree
Showing 14 changed files with 453 additions and 137 deletions.
2 changes: 1 addition & 1 deletion mesh_layers/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 3.8)
project(mesh_layers)

find_package(ament_cmake_ros COMPONENTS)
find_package(ament_cmake_ros REQUIRED)
find_package(mesh_map)
find_package(LVR2)

Expand Down
43 changes: 31 additions & 12 deletions mesh_layers/src/border_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,12 @@ namespace mesh_layers
bool BorderLayer::readLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read border costs from map file...");
auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>("border");
auto border_costs_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float> >(layer_name_);

if (border_costs_opt)
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Border costs have been read successfully.");
border_costs_ = border_costs_opt.get();

return computeLethals();
}

Expand All @@ -80,7 +79,7 @@ bool BorderLayer::computeLethals()
bool BorderLayer::writeLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saving border costs to map file...");
if (mesh_io_ptr_->addDenseAttributeMap(border_costs_, "border"))
if (mesh_io_ptr_->addDenseAttributeMap(border_costs_, layer_name_))
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saved border costs to map file.");
return true;
Expand Down Expand Up @@ -113,56 +112,76 @@ rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::v
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;

bool has_threshold_changed = false;
for (auto parameter : parameters) {
// bool has_threshold_changed = false;
bool recompute_costs = false;
bool recompute_lethals = false;

for (auto parameter : parameters)
{
if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") {
config_.threshold = parameter.as_double();
has_threshold_changed = true;
recompute_lethals = true;
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost") {
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();
}
}

if (has_threshold_changed) {
RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change.");
if(recompute_costs)
{
RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute layer costs due to cfg change.");
computeLayer();
}

if (recompute_lethals)
{
RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute lethals due to cfg change.");
computeLethals();
}

if(recompute_costs || recompute_lethals)
{
RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Notify changes.");
notifyChange();
}

return result;
}


bool BorderLayer::initialize()
{
{ // threshold
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Threshold for the local border costs to be counted as lethal.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.05;
range.to_value = 1.0;
descriptor.floating_point_range.push_back(range);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor);
}
{ // border cost
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "The cost value used for the border vertices.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.02;
range.to_value = 10.0;
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);
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);
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));
Expand Down
20 changes: 12 additions & 8 deletions mesh_layers/src/height_diff_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ namespace mesh_layers
bool HeightDiffLayer::readLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read height differences from map file...");
auto height_diff_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>("height_diff");
auto height_diff_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>(layer_name_);

if (height_diff_opt)
{
Expand Down Expand Up @@ -78,7 +78,7 @@ bool HeightDiffLayer::computeLethals()
bool HeightDiffLayer::writeLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saving height_differences to map file...");
if (mesh_io_ptr_->addDenseAttributeMap(height_diff_, "height_diff"))
if (mesh_io_ptr_->addDenseAttributeMap(height_diff_, layer_name_))
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saved height differences to map file.");
return true;
Expand Down Expand Up @@ -111,19 +111,20 @@ rcl_interfaces::msg::SetParametersResult HeightDiffLayer::reconfigureCallback(st
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;

bool has_threshold_changed = false;
bool recompute_lethals = false;
for (auto parameter : parameters) {
if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") {
config_.threshold = parameter.as_double();
has_threshold_changed = true;
recompute_lethals = true;
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") {
config_.radius = parameter.as_double();
recompute_lethals = true;
} else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") {
config_.factor = parameter.as_double();
}
}

if (has_threshold_changed) {
if (recompute_lethals) {
RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change.");
computeLethals();
notifyChange();
Expand All @@ -138,29 +139,32 @@ bool HeightDiffLayer::initialize()
{ // threshold
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Threshold for the local height difference to be counted as lethal.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.05;
range.to_value = 1.0;
descriptor.floating_point_range.push_back(range);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor);
}
{ // radius
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "The radius used for calculating the local height difference.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.02;
range.to_value = 1.0;
descriptor.floating_point_range.push_back(range);
config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius);
config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, 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);
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(
&HeightDiffLayer::reconfigureCallback, this, std::placeholders::_1));
Expand Down
25 changes: 16 additions & 9 deletions mesh_layers/src/inflation_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ bool InflationLayer::readLayer()
{
// riskiness
RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read riskiness from map file...");
auto riskiness_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>("riskiness");
auto riskiness_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>(layer_name_);

if (riskiness_opt)
{
Expand All @@ -68,7 +68,7 @@ bool InflationLayer::writeLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saving " << riskiness_.numValues() << " riskiness values to map file...");

if (mesh_io_ptr_->addDenseAttributeMap(riskiness_, "riskiness"))
if (mesh_io_ptr_->addDenseAttributeMap(riskiness_, layer_name_))
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saved riskiness to map file.");
return true;
Expand Down Expand Up @@ -678,61 +678,68 @@ bool InflationLayer::initialize()
{ // inscribed_radius
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Defines the inscribed radius.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.01;
range.to_value = 1.0;
descriptor.floating_point_range.push_back(range);
config_.inscribed_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius", config_.inscribed_radius);
config_.inscribed_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_radius", config_.inscribed_radius, descriptor);
}
{ // inflation_radius
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Defines the maximum inflation radius.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.01;
range.to_value = 3.0;
descriptor.floating_point_range.push_back(range);
config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius);
config_.inflation_radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inflation_radius", config_.inflation_radius, descriptor);
}
{ // factor
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "The 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);
config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor);
}
{ // lethal_value
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Defines the 'lethal' value for obstacles. -1 results in infinity";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = -1.0;
range.to_value = 100000.0;
descriptor.floating_point_range.push_back(range);
config_.lethal_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".lethal_value", config_.lethal_value);
config_.lethal_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".lethal_value", config_.lethal_value, descriptor);
}
{ // inscribed_value
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Defines the 'inscribed' value for obstacles.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.0;
range.to_value = 100000.0;
descriptor.floating_point_range.push_back(range);
config_.inscribed_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value", config_.inscribed_value);
config_.inscribed_value = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".inscribed_value", config_.inscribed_value, descriptor);
}
{ // min_contour_size
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Defines the minimum size for a contour to be classified as 'lethal'.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
rcl_interfaces::msg::IntegerRange range;
range.from_value = 0;
range.to_value = 100000;
descriptor.integer_range.push_back(range);
config_.min_contour_size = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".min_contour_size", config_.min_contour_size);
config_.min_contour_size = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".min_contour_size", config_.min_contour_size, descriptor);
}
{ // repulsive_field
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Enable the repulsive vector field.";
config_.repulsive_field = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".repulsive_field", config_.repulsive_field);
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
config_.repulsive_field = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".repulsive_field", config_.repulsive_field, descriptor);
}
dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind(&InflationLayer::reconfigureCallback, this, std::placeholders::_1));
return true;
Expand Down
13 changes: 8 additions & 5 deletions mesh_layers/src/ridge_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ namespace mesh_layers
bool RidgeLayer::readLayer()
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read ridge from map file...");
auto ridge_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>("ridge");
auto ridge_opt = mesh_io_ptr_->getDenseAttributeMap<lvr2::DenseVertexMap<float>>(layer_name_);
if (ridge_opt)
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read ridge from map file.");
Expand All @@ -63,7 +63,7 @@ bool RidgeLayer::readLayer()

bool RidgeLayer::writeLayer()
{
if (mesh_io_ptr_->addDenseAttributeMap(ridge_, "ridge"))
if (mesh_io_ptr_->addDenseAttributeMap(ridge_, layer_name_))
{
RCLCPP_INFO_STREAM(node_->get_logger(), "Saved ridge to map file.");
return true;
Expand Down Expand Up @@ -219,29 +219,32 @@ bool RidgeLayer::initialize()
{ // threshold
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Threshold for the local ridge to be counted as lethal.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.01;
range.to_value = 3.1415;
descriptor.floating_point_range.push_back(range);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold);
config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor);
}
{ // radius
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "Radius of the inscribed area.";
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
rcl_interfaces::msg::FloatingPointRange range;
range.from_value = 0.01;
range.to_value = 1.0;
descriptor.floating_point_range.push_back(range);
config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius);
config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor);
}
{ // factor
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "The local ridge 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);
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(
&RidgeLayer::reconfigureCallback, this, std::placeholders::_1));
Expand Down
Loading

0 comments on commit fedea89

Please sign in to comment.