Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dynamic loading of different mesh formats #66

Merged
merged 17 commits into from
Nov 20, 2024
Merged
Show file tree
Hide file tree
Changes from 8 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion mesh_layers/CMakeLists.txt
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
3 changes: 3 additions & 0 deletions mesh_layers/include/mesh_layers/border_layer.h
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,9 @@ class BorderLayer : public mesh_map::AbstractLayer
double border_cost = 1.0;
double factor = 1.0;
} config_;

// put this to base class?
std::string name;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

codestyle

Suggested change
std::string name;
std::string name_;

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wait, is this even used? I think you're only using the layer_name_ attribute from the base class in the cpp file.

Looks like this is a leftover from experimenting.

Copy link
Collaborator Author

@amock amock Nov 19, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, it's basically how it is written in the comments. "Put this to base class" then I jumped to the base class and saw, there is a variable already. So "name" can be removed safely.

};

} /* namespace mesh_layers */
Expand Down
43 changes: 31 additions & 12 deletions mesh_layers/src/border_layer.cpp
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Whoops, was the descriptor never used when declaring the parameter? 🫢
Nice fix.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, but at least it was described for anyone reading the code 😅

}
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
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
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
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