Skip to content

Commit f0546b7

Browse files
amockamock
authored
Merge pull request #86 from naturerobots/feature/dynamic-cost-updates
Dynamic Cost Updates
2 parents c6e4dc7 + 749b9e7 commit f0546b7

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

56 files changed

+2718
-761
lines changed

‎README.md‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,7 @@ The package structure is as follows:
103103
- ClearanceLayer - `mesh_layers/ClearanceLayer`
104104
- InflationLayer - `mesh_layers/InflationLayer`
105105
- BorderLayer - `mesh_layers/BorderLayer`
106+
- ObstacleLayer - `mesh_layers/ObstacleLayer`
106107

107108
- `dijkstra_mesh_planner` contains a mesh planner plugin providing a path planning method based on Dijkstra's algorithm. It plans by using the edges of the mesh map. The propagation start a the goal pose, thus a path from every accessed vertex to the goal pose can be computed. This leads to a sub-optimal potential field, which highly depends on the mesh structure.
108109

@@ -125,6 +126,7 @@ The following table gives an overview of all currently implemented layer plugins
125126
| **ClearanceLayer** | `mesh_layers/ClearanceLayer` | comparison of robot height and clearance along each vertex normal | ![ClearanceLayer](docs/images/costlayers/clearance.jpg?raw=true "Clearance Layer") |
126127
| **InflationLayer** | `mesh_layers/InflationLayer` | by distance to a lethal vertex | ![InflationLayer](docs/images/costlayers/inflation.jpg?raw=true "Inflation Layer") |
127128
| **BorderLayer** | `mesh_layers/BorderLayer` | give vertices close to the border a certain cost | ![BorderLayer](docs/images/costlayers/border.png?raw=true "Border Layer") |
129+
| **ObstacleLayer** | `mesh_layers/ObstacleLayer` | marks vertices blocked by dynamic obstacles as lethal. Cost layer (left) and gazebo sim with unmapped obstacle (right) | ![ObstacleLayer](docs/images/costlayers/obstacle.png?raw=true "ObstacleLayer") |
128130

129131
## Planners
130132
Currently the following planners are available:

‎cvp_mesh_planner/CHANGELOG.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,10 @@
22
Changelog for package cvp_mesh_planner
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
3.0.0 (2025-09-10)
6+
------------------
7+
* Cost-aware planning
8+
59
2.0.1 (2025-08-11)
610
------------------
711
* Bug fixes and cleanup since initial ROS 2 port

‎cvp_mesh_planner/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="3">
33
<name>cvp_mesh_planner</name>
4-
<version>2.0.1</version>
4+
<version>3.0.0</version>
55
<description>The Continuous Vector Field Planner (CVP) mesh planner package</description>
66
<maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
77
<maintainer email="sebastian.puetz@naturerobots.com">Sebastian Pütz</maintainer>

‎cvp_mesh_planner/src/cvp_mesh_planner.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -742,7 +742,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
742742
if (distances[current_vh] > goal_dist)
743743
continue;
744744

745-
if (vertex_costs[current_vh] > config_.cost_limit)
745+
if (vertex_costs[current_vh] >= config_.cost_limit)
746746
continue;
747747

748748
if (invalid[current_vh])
@@ -786,8 +786,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
786786
else if (fixed[a] && fixed[b] && !fixed[c])
787787
{
788788
// c is free
789-
// Skip vertices above the cost limit
790-
if (costs[c] > config_.cost_limit)
789+
// Skip vertices at or above the cost limit
790+
if (costs[c] >= config_.cost_limit)
791791
{
792792
continue;
793793
}
@@ -809,8 +809,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
809809
else if (fixed[a] && !fixed[b] && fixed[c])
810810
{
811811
// b is free
812-
// Skip vertices above the cost limit
813-
if (costs[b] > config_.cost_limit)
812+
// Skip vertices at or above the cost limit
813+
if (costs[b] >= config_.cost_limit)
814814
{
815815
continue;
816816
}
@@ -832,8 +832,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
832832
else if (!fixed[a] && fixed[b] && fixed[c])
833833
{
834834
// a if free
835-
// Skip vertices above the cost limit
836-
if (costs[a] > config_.cost_limit)
835+
// Skip vertices at or above the cost limit
836+
if (costs[a] >= config_.cost_limit)
837837
{
838838
continue;
839839
}

‎dijkstra_mesh_planner/CHANGELOG.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,10 @@
22
Changelog for package dijkstra_mesh_planner
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
3.0.0 (2025-09-10)
6+
------------------
7+
* Cost-aware planning
8+
59
2.0.1 (2025-08-11)
610
------------------
711
* Bug fixes and cleanup since initial ROS 2 port

‎dijkstra_mesh_planner/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="3">
33
<name>dijkstra_mesh_planner</name>
4-
<version>2.0.1</version>
4+
<version>3.0.0</version>
55
<description>The dijkstra_mesh_planner package</description>
66
<maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>
77
<maintainer email="sebastian.puetz@naturerobots.com">Sebastian Pütz</maintainer>

‎docs/images/costlayers/obstacle.png‎

70.1 KB
Loading

‎mbf_mesh_core/CHANGELOG.rst‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
22
Changelog for package mbf_mesh_core
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
3.0.0 (2025-09-10)
5+
------------------
46

57
2.0.1 (2025-08-11)
68
------------------

‎mbf_mesh_core/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="3">
33
<name>mbf_mesh_core</name>
4-
<version>2.0.1</version>
4+
<version>3.0.0</version>
55
<description>The mbf_mesh_core package</description>
66

77
<maintainer email="matthias.holoch@naturerobots.com">Matthias Holoch</maintainer>

‎mbf_mesh_nav/CHANGELOG.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,10 @@
22
Changelog for package mbf_mesh_nav
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
3.0.0 (2025-09-10)
6+
------------------
7+
* Multi-threaded executor
8+
59
2.0.1 (2025-08-11)
610
------------------
711
* Bug fixes and cleanup since initial ROS 2 port

0 commit comments

Comments
 (0)