diff --git a/roadmap_explorer/params/exploration_params.yaml b/roadmap_explorer/params/exploration_params.yaml index e9bb3e8..96fdb90 100755 --- a/roadmap_explorer/params/exploration_params.yaml +++ b/roadmap_explorer/params/exploration_params.yaml @@ -42,10 +42,6 @@ roadmap_explorer_node: add_yaw_to_tsp: false add_distance_to_robot_to_tsp: false - goalHysteresis: - use_euclidean_distance: false - use_roadmap_planner_distance: true - explorationBT: bt_sleep_ms: 70 exploration_boundary: [310.0, 260.0, 310.0, -120.0, -70.0, -120.0, -70.0, 260.0] diff --git a/roadmap_explorer/params/tb3_exploration_params.yaml b/roadmap_explorer/params/tb3_exploration_params.yaml index acb508d..4136e84 100644 --- a/roadmap_explorer/params/tb3_exploration_params.yaml +++ b/roadmap_explorer/params/tb3_exploration_params.yaml @@ -42,10 +42,6 @@ roadmap_explorer_node: add_yaw_to_tsp: false add_distance_to_robot_to_tsp: false - goalHysteresis: - use_euclidean_distance: false - use_roadmap_planner_distance: true - explorationBT: bt_sleep_ms: 70 exploration_boundary: [310.0, 260.0, 310.0, -120.0, -70.0, -120.0, -70.0, 260.0] diff --git a/roadmap_explorer/src/ExplorationBT.cpp b/roadmap_explorer/src/ExplorationBT.cpp index b3aaac9..3cb2fba 100644 --- a/roadmap_explorer/src/ExplorationBT.cpp +++ b/roadmap_explorer/src/ExplorationBT.cpp @@ -85,7 +85,7 @@ bool RoadmapExplorationBT::incrementFrontierSearchDistance() double increment_value = parameterInstance.getValue("frontierSearch.increment_search_distance_by"); LOG_WARN("Incrementing frontier search distance by " << increment_value); auto distance_to_set = current_frontier_search_distance + increment_value; - if (distance_to_set > parameterInstance.getValue("frontierSearch.max_permissible_frontier_search_distance")) { + if (distance_to_set > parameterInstance.getValue("frontierSearch.max_permissable_frontier_search_distance")) { return false; } blackboard->set("current_frontier_search_distance", distance_to_set); diff --git a/roadmap_explorer/src/Parameters.cpp b/roadmap_explorer/src/Parameters.cpp index 40c0dd3..0b2b927 100644 --- a/roadmap_explorer/src/Parameters.cpp +++ b/roadmap_explorer/src/Parameters.cpp @@ -92,19 +92,6 @@ void ParameterHandler::makeParameters(std::shared_ptr parameter_map_["fullPathOptimizer.add_distance_to_robot_to_tsp"] = node->get_parameter( "fullPathOptimizer.add_distance_to_robot_to_tsp").as_bool(); - // --- goalHysteresis --- - nav2_util::declare_parameter_if_not_declared( - node, "goalHysteresis.use_euclidean_distance", rclcpp::ParameterValue( - false)); - nav2_util::declare_parameter_if_not_declared( - node, "goalHysteresis.use_roadmap_planner_distance", rclcpp::ParameterValue( - true)); - - parameter_map_["goalHysteresis.use_euclidean_distance"] = node->get_parameter( - "goalHysteresis.use_euclidean_distance").as_bool(); - parameter_map_["goalHysteresis.use_roadmap_planner_distance"] = node->get_parameter( - "goalHysteresis.use_roadmap_planner_distance").as_bool(); - // --- explorationBT --- nav2_util::declare_parameter_if_not_declared( node, "explorationBT.bt_sleep_ms", rclcpp::ParameterValue( @@ -189,17 +176,7 @@ void ParameterHandler::makeParameters(std::shared_ptr void ParameterHandler::sanityCheckParameters() { - if (getValue("goalHysteresis.use_euclidean_distance") == true && - getValue("goalHysteresis.use_roadmap_planner_distance") == true) - { - throw RoadmapExplorerException( - "Both use_euclidean_distance and use_roadmap_planner_distance are set to true. Please set only one of them to true."); - } else if (getValue("goalHysteresis.use_euclidean_distance") == false && - getValue("goalHysteresis.use_roadmap_planner_distance") == false) - { - throw RoadmapExplorerException( - "Both use_euclidean_distance and use_roadmap_planner_distance are set to false. Please set only one of them to true."); - } + // currently there are no sanity checks } rcl_interfaces::msg::SetParametersResult ParameterHandler::dynamicReconfigureCallback( diff --git a/roadmap_explorer/src/bt_plugins/SearchForFrontiersPlugin.cpp b/roadmap_explorer/src/bt_plugins/SearchForFrontiersPlugin.cpp index 5adbb6e..82fa237 100644 --- a/roadmap_explorer/src/bt_plugins/SearchForFrontiersPlugin.cpp +++ b/roadmap_explorer/src/bt_plugins/SearchForFrontiersPlugin.cpp @@ -36,6 +36,7 @@ namespace roadmap_explorer LOG_INFO("Using robot pose: " << robotP.pose.position.x << ", " << robotP.pose.position.y); std::vector frontier_list; auto current_frontier_search_distance = config().blackboard->get("current_frontier_search_distance"); + LOG_HIGHLIGHT("Current frontier search distance: " << current_frontier_search_distance); auto searchResult = frontierSearchPtr_->searchFrom(robotP.pose.position, frontier_list, current_frontier_search_distance); if (searchResult != FrontierSearchResult::SUCCESSFUL_SEARCH) {