Skip to content

Commit 0170583

Browse files
suchetanrssuchetanrs
committed
Bug fixes
1 parent a474d23 commit 0170583

File tree

16 files changed

+274
-249
lines changed

16 files changed

+274
-249
lines changed

‎roadmap_explorer/CMakeLists.txt‎

Lines changed: 86 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,6 @@ set(dependencies
5353
roadmap_explorer_msgs
5454
eigen3_cmake_module
5555
Eigen3
56-
yaml-cpp
5756
nav2_behavior_tree
5857
)
5958

@@ -97,19 +96,19 @@ ament_target_dependencies(${PROJECT_NAME}_utils ${dependencies})
9796
target_link_libraries(${PROJECT_NAME}_utils ${PROJECT_NAME}_frontier)
9897

9998

100-
add_library(roadmap_explorer_frontier_search_plugins SHARED
99+
add_library(${PROJECT_NAME}_frontier_search_plugins SHARED
101100
src/frontier_search/PluginBFSearch.cpp
102101
)
103-
ament_target_dependencies(roadmap_explorer_frontier_search_plugins ${dependencies})
102+
ament_target_dependencies(${PROJECT_NAME}_frontier_search_plugins ${dependencies})
104103

105104

106-
add_library(roadmap_explorer_information_gain_plugins SHARED
105+
add_library(${PROJECT_NAME}_information_gain_plugins SHARED
107106
src/information_gain/CountBasedGain.cpp
108107
)
109-
ament_target_dependencies(roadmap_explorer_information_gain_plugins ${dependencies})
108+
ament_target_dependencies(${PROJECT_NAME}_information_gain_plugins ${dependencies})
110109

111110

112-
add_library(roadmap_explorer_planner_plugins SHARED
111+
add_library(${PROJECT_NAME}_planner_plugins SHARED
113112
src/planners/ThetaStar.cpp
114113
src/planners/Astar.cpp
115114
src/planners/NavFn.cpp
@@ -119,49 +118,81 @@ src/planners/FrontierRoadmapPlugin.cpp
119118
src/planners/EuclideanPlugin.cpp
120119
src/planners/RandomDistancePlugin.cpp
121120
)
122-
ament_target_dependencies(roadmap_explorer_planner_plugins ${dependencies})
121+
ament_target_dependencies(${PROJECT_NAME}_planner_plugins ${dependencies})
123122

124-
# ########################### PLUGINLIB LIBRARIES ###########################
125123

126-
# add_library(roadmap_explorer_bt_plugins SHARED
127-
# src/bt_plugins/LogIterationPlugin.cpp
128-
# src/bt_plugins/UpdateBoundaryPlugin.cpp
129-
# src/bt_plugins/SearchForFrontiersPlugin.cpp
130-
# src/bt_plugins/CleanupRoadmapPlugin.cpp
131-
# src/bt_plugins/UpdateRoadmapPlugin.cpp
132-
# src/bt_plugins/ProcessFrontierCostsPlugin.cpp
133-
# src/bt_plugins/OptimizeFullPathPlugin.cpp
134-
# src/bt_plugins/SendNav2GoalPlugin.cpp
135-
# src/bt_plugins/BlacklistGoalPlugin.cpp
136-
# )
137-
# ament_target_dependencies(roadmap_explorer_bt_plugins ${dependencies})
138-
# target_link_libraries(roadmap_explorer_bt_plugins ${PROJECT_NAME}_frontier ${PROJECT_NAME}_utils)
124+
add_library(${PROJECT_NAME}_core SHARED
125+
src/CostAssigner.cpp
126+
src/Parameters.cpp
127+
src/Nav2Interface.cpp
128+
src/FullPathOptimizer.cpp
129+
src/SensorSimulator.cpp
130+
)
131+
ament_target_dependencies(${PROJECT_NAME}_core ${dependencies})
132+
target_link_libraries(${PROJECT_NAME}_core
133+
${PCL_LIBRARIES}
134+
${PROJECT_NAME}_frontier
135+
${PROJECT_NAME}_utils
136+
${PROJECT_NAME}_frontier_search_plugins
137+
${PROJECT_NAME}_planner_plugins
138+
${PROJECT_NAME}_information_gain_plugins
139+
)
139140

140-
# #############
141141

142+
add_library(${PROJECT_NAME}_bt_plugins SHARED
143+
src/bt_plugins/LogIterationPlugin.cpp
144+
src/bt_plugins/UpdateBoundaryPlugin.cpp
145+
src/bt_plugins/SearchForFrontiersPlugin.cpp
146+
src/bt_plugins/CleanupRoadmapPlugin.cpp
147+
src/bt_plugins/UpdateRoadmapPlugin.cpp
148+
src/bt_plugins/ProcessFrontierCostsPlugin.cpp
149+
src/bt_plugins/OptimizeFullPathPlugin.cpp
150+
src/bt_plugins/SendNav2GoalPlugin.cpp
151+
src/bt_plugins/BlacklistGoalPlugin.cpp
152+
)
153+
ament_target_dependencies(${PROJECT_NAME}_bt_plugins ${dependencies})
154+
target_link_libraries(${PROJECT_NAME}_bt_plugins
155+
${PROJECT_NAME}_frontier
156+
${PROJECT_NAME}_utils
157+
${PROJECT_NAME}_frontier_search_plugins
158+
${PROJECT_NAME}_planner_plugins
159+
${PROJECT_NAME}_information_gain_plugins
160+
)
142161

143-
# ########################### CORE LIBRARY ###########################
144162

145-
# add_library(roadmap_exploration_bt SHARED
146-
# src/Parameters.cpp
147-
# src/Nav2Interface.cpp
148-
# src/ExplorationBT.cpp
149-
# src/CostAssigner.cpp
150-
# src/FullPathOptimizer.cpp
151-
# src/ExplorationServer.cpp
152-
# src/SensorSimulator.cpp
153-
# )
154-
# ament_target_dependencies(roadmap_exploration_bt ${dependencies})
155-
# target_link_libraries(roadmap_exploration_bt ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ${PROJECT_NAME}_frontier ${PROJECT_NAME}_utils roadmap_explorer_bt_plugins roadmap_explorer_frontier_search_plugins roadmap_explorer_planner_plugins roadmap_explorer_information_gain_plugins)
163+
add_library(${PROJECT_NAME}_bt SHARED
164+
src/ExplorationBT.cpp
165+
src/ExplorationServer.cpp
166+
)
167+
ament_target_dependencies(${PROJECT_NAME}_bt ${dependencies})
168+
target_link_libraries(${PROJECT_NAME}_bt
169+
${PROJECT_NAME}_bt_plugins
170+
${PROJECT_NAME}_frontier
171+
${PROJECT_NAME}_utils
172+
${PROJECT_NAME}_frontier_search_plugins
173+
${PROJECT_NAME}_planner_plugins
174+
${PROJECT_NAME}_information_gain_plugins
175+
${PROJECT_NAME}_core
176+
)
156177

157-
# rclcpp_components_register_nodes(roadmap_exploration_bt "roadmap_exploration::ExplorationServer")
178+
rclcpp_components_register_nodes(${PROJECT_NAME}_bt "roadmap_exploration::ExplorationServer")
158179

159180
# ########################### EXECUTABLES ###########################
160-
# add_executable(roadmap_exploration_server
161-
# src/main.cpp
162-
# )
163-
# ament_target_dependencies(roadmap_exploration_server ${dependencies})
164-
# target_link_libraries(roadmap_exploration_server ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ${PROJECT_NAME}_frontier roadmap_exploration_bt ${PROJECT_NAME}_utils roadmap_explorer_bt_plugins roadmap_explorer_frontier_search_plugins roadmap_explorer_planner_plugins roadmap_explorer_information_gain_plugins)
181+
add_executable(roadmap_exploration_server
182+
src/main.cpp
183+
)
184+
ament_target_dependencies(roadmap_exploration_server ${dependencies})
185+
target_link_libraries(roadmap_exploration_server
186+
${PCL_LIBRARIES}
187+
${PROJECT_NAME}_frontier
188+
${PROJECT_NAME}_bt
189+
${PROJECT_NAME}_utils
190+
${PROJECT_NAME}_bt_plugins
191+
${PROJECT_NAME}_frontier_search_plugins
192+
${PROJECT_NAME}_planner_plugins
193+
${PROJECT_NAME}_information_gain_plugins
194+
${PROJECT_NAME}_core
195+
)
165196

166197
########################### COMPARISON SCRIPTS ###########################
167198
add_subdirectory(comparision_scripts)
@@ -171,19 +202,20 @@ add_subdirectory(comparision_scripts)
171202
install(TARGETS
172203
${PROJECT_NAME}_frontier
173204
${PROJECT_NAME}_utils
174-
roadmap_explorer_frontier_search_plugins
175-
roadmap_explorer_information_gain_plugins
176-
roadmap_explorer_planner_plugins
177-
# roadmap_exploration_bt
178-
# roadmap_explorer_bt_plugins
205+
${PROJECT_NAME}_frontier_search_plugins
206+
${PROJECT_NAME}_information_gain_plugins
207+
${PROJECT_NAME}_planner_plugins
208+
${PROJECT_NAME}_core
209+
${PROJECT_NAME}_bt_plugins
210+
${PROJECT_NAME}_bt
179211
ARCHIVE DESTINATION lib
180212
LIBRARY DESTINATION lib
181213
RUNTIME DESTINATION bin
182214
)
183215

184-
# install(TARGETS roadmap_exploration_server
185-
# RUNTIME DESTINATION lib/${PROJECT_NAME}
186-
# )
216+
install(TARGETS roadmap_exploration_server
217+
RUNTIME DESTINATION lib/${PROJECT_NAME}
218+
)
187219

188220
install(DIRECTORY include/
189221
DESTINATION include/
@@ -197,11 +229,12 @@ ament_export_include_directories(include ${EIGEN3_INCLUDE_DIR})
197229
ament_export_libraries(
198230
${PROJECT_NAME}_frontier
199231
${PROJECT_NAME}_utils
200-
roadmap_explorer_frontier_search_plugins
201-
roadmap_explorer_information_gain_plugins
202-
roadmap_explorer_planner_plugins
203-
# roadmap_exploration_bt
204-
# roadmap_explorer_bt_plugins
232+
${PROJECT_NAME}_frontier_search_plugins
233+
${PROJECT_NAME}_information_gain_plugins
234+
${PROJECT_NAME}_planner_plugins
235+
${PROJECT_NAME}_core
236+
${PROJECT_NAME}_bt_plugins
237+
${PROJECT_NAME}_bt
205238
)
206239
ament_export_dependencies(rosidl_default_runtime ${dependencies})
207240

‎roadmap_explorer/include/roadmap_explorer/CostAssigner.hpp‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@
2828
#include <tf2_ros/buffer.h>
2929
#include <tf2/LinearMath/Quaternion.h>
3030

31+
#include <pluginlib/class_loader.hpp>
32+
3133
#include "roadmap_explorer/util/Logger.hpp"
3234
#include "roadmap_explorer/util/RosVisualizer.hpp"
3335
#include "roadmap_explorer/util/EventLogger.hpp"
@@ -52,7 +54,7 @@ struct CalculateFrontierCostsRequest
5254
class CostAssigner
5355
{
5456
public:
55-
CostAssigner(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros);
57+
CostAssigner(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node);
5658

5759
~CostAssigner();
5860

@@ -69,7 +71,7 @@ class CostAssigner
6971

7072
void recomputeNormalizationFactors(FrontierPtr & frontier);
7173

72-
void initializePlugins(const std::string & informationGainPlugin, const std::string & plannerPlugin);
74+
void initializePlugins();
7375

7476
void processFrontier(FrontierPtr & frontier,
7577
const geometry_msgs::msg::Pose & start_pose_w);
@@ -82,6 +84,7 @@ class CostAssigner
8284
nav2_costmap_2d::LayeredCostmap * layered_costmap_;
8385
nav2_costmap_2d::Costmap2D * costmap_;
8486
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros_;
87+
std::shared_ptr<nav2_util::LifecycleNode> node_;
8588

8689
// map of blacklists with hash map and equality which only considers goal point
8790
std::unordered_map<FrontierPtr, bool, FrontierHash,
@@ -91,10 +94,16 @@ class CostAssigner
9194
double min_traversable_distance = std::numeric_limits<double>::max();
9295
double max_traversable_distance = -1.0 * std::numeric_limits<double>::max();
9396
std::shared_ptr<BasePlanner> planner_plugin_;
97+
std::shared_ptr<pluginlib::ClassLoader<BasePlanner>> planner_loader_;
98+
std::string planner_plugin_name_;
99+
std::string planner_plugin_type_;
94100

95101
double min_arrival_info_per_frontier = std::numeric_limits<double>::max();
96102
double max_arrival_info_per_frontier = -1.0 * std::numeric_limits<double>::max();
97103
std::shared_ptr<BaseInformationGain> information_gain_plugin_;
104+
std::shared_ptr<pluginlib::ClassLoader<BaseInformationGain>> information_gain_loader_;
105+
std::string information_gain_plugin_name_;
106+
std::string information_gain_plugin_type_;
98107

99108
};
100109
}

‎roadmap_explorer/include/roadmap_explorer/ExplorationBT.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,9 @@ class RoadmapExplorationBT
8181

8282
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros_;
8383
std::unique_ptr<nav2_util::NodeThread> explore_costmap_thread_;
84+
85+
std::shared_ptr<pluginlib::ClassLoader<FrontierSearchBase>> frontier_search_loader_;
86+
std::shared_ptr<pluginlib::ClassLoader<roadmap_explorer::BTPlugin>> bt_plugin_loader_;
8487
};
8588
}
8689

‎roadmap_explorer/include/roadmap_explorer/frontier_search/BaseFrontierSearch.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,8 @@ enum class FrontierSearchResult
2020
{
2121
ROBOT_OUT_OF_BOUNDS = 0, ///< Robot position is outside the costmap bounds
2222
CANNOT_FIND_CELL_TO_SEARCH = 1, ///< No valid cells found to start the search
23-
SUCCESSFUL_SEARCH = 2 ///< Search completed successfully
23+
SUCCESSFUL_SEARCH = 2, ///< Search completed successfully
24+
NO_FRONTIERS_FOUND = 3
2425
};
2526

2627
/**

‎roadmap_explorer/params/exploration.yaml‎

Lines changed: 0 additions & 51 deletions
This file was deleted.

‎roadmap_explorer/params/exploration_params.yaml‎

Lines changed: 21 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -4,25 +4,31 @@ roadmap_explorer_node:
44
localisation_only_mode: True # false if in slam mode (mapping is active and you want the exploration to run on that)
55

66
frontierSearch:
7-
min_frontier_cluster_size: 1.0
8-
max_frontier_cluster_size: 20.0
97
frontier_search_distance: 25.0 # This should be equal to the parameter frontier_search_distance in frontier_roadmap_node
10-
lethal_threshold: 160 # >= is lethal, < is free space.
118
max_permissable_frontier_search_distance: 140.0
12-
13-
costCalculator:
14-
max_camera_depth: 2.0
15-
delta_theta: 0.10
16-
camera_fov: 1.04
17-
factor_of_max_is_min: 0.70
18-
closeness_rejection_threshold: 0.5
19-
planner_allow_unknown: true
20-
max_planning_distance_roadmap: 12.5
9+
plugin_name: "GridBasedBFS"
10+
GridBasedBFS:
11+
plugin_type: "roadmap_explorer::FrontierBFSearch"
12+
min_frontier_cluster_size: 1.0
13+
max_frontier_cluster_size: 20.0
14+
lethal_threshold: 160 # >= is lethal, < is free space.
2115

2216
costAssigner:
23-
information_gain_plugin: "roadmap_explorer::CountBasedGain"
24-
planner_plugin: "roadmap_explorer::PluginFrontierRoadmap"
25-
17+
information_gain_plugin_name: "CountBasedGain"
18+
CountBasedGain:
19+
plugin_type: "roadmap_explorer::CountBasedGain"
20+
max_camera_depth: 2.0
21+
delta_theta: 0.10
22+
camera_fov: 1.04
23+
factor_of_max_is_min: 0.70
24+
25+
planner_plugin_name: "FrontierRoadmap"
26+
FrontierRoadmap:
27+
plugin_type: "roadmap_explorer::PluginFrontierRoadmap"
28+
closeness_rejection_threshold: 0.5
29+
planner_allow_unknown: true
30+
max_planning_distance_roadmap: 12.5
31+
2632
frontierRoadmap:
2733
max_graph_reconstruction_distance: 10.0
2834
grid_cell_size: 1.0 # Assuming each cell is 1x1 in size

0 commit comments

Comments
 (0)