Add hysteresis, fix failing tests for kilted and jazzy (#22)

* Hysteresis function

* Fix build error for Kilted and Jazzy

* Build error and test failure fix

Author: suchetanrs

roadmap_explorer/include/roadmap_explorer/FullPathOptimizer.hpp

@@ -187,6 +187,14 @@ class FullPathOptimizer
   bool add_yaw_to_tsp = false;
   bool add_distance_to_robot_to_tsp = false;
 
+  // Goal hysteresis variables to prevent frequent goal switching
+  FrontierPtr current_committed_goal_;  // The goal we're currently committed to
+  double current_goal_path_cost_;       // Path cost to the current goal
+  bool has_committed_goal_ = false;     // Whether we have a committed goal
+  
+  // Hysteresis parameters
+  double goal_hysteresis_threshold_ = 0.15;  // New goal must be 15% better to switch
+  
   // Constants for path optimization
   static constexpr double PATH_NOT_FOUND_PENALTY_MULTIPLIER = 100000.0;
   static constexpr double YAW_COST_WEIGHT = 2.3;

roadmap_explorer/params/exploration_params.yaml

@@ -40,7 +40,8 @@ roadmap_explorer_node:
       num_frontiers_in_local_area: 5.0
       local_frontier_search_radius: 12.0
       add_yaw_to_tsp: false
-      add_distance_to_robot_to_tsp: false 
+      add_distance_to_robot_to_tsp: false
+      goal_hysteresis_threshold: 0.15  # New goal must be 15% better to switch (0.0 = always switch, 1.0 = never switch) 
 
     explorationBT:
       bt_sleep_ms: 70

roadmap_explorer/params/tb3_exploration_params.yaml

@@ -40,7 +40,8 @@ roadmap_explorer_node:
       num_frontiers_in_local_area: 5.0
       local_frontier_search_radius: 12.0
       add_yaw_to_tsp: false
-      add_distance_to_robot_to_tsp: false 
+      add_distance_to_robot_to_tsp: false
+      goal_hysteresis_threshold: 0.15  # New goal must be 15% better to switch (0.0 = always switch, 1.0 = never switch) 
 
     explorationBT:
       bt_sleep_ms: 70

roadmap_explorer/src/ExplorationServer.cpp

@@ -72,7 +72,13 @@ nav2_util::CallbackReturn ExplorationServer::on_activate(const rclcpp_lifecycle:
     "save_map_and_roadmap",
     std::bind(&ExplorationServer::handle_save_map_and_roadmap, this, 
               std::placeholders::_1, std::placeholders::_2),
+#ifdef ROS_DISTRO_HUMBLE
     rmw_qos_profile_services_default,
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+    rclcpp::ServicesQoS(),
+#else
+    #error "Unsupported ROS distro"
+#endif
     service_cb_group_);
 
   LOG_INFO("Created save_map_and_roadmap service");

roadmap_explorer/src/FullPathOptimizer.cpp

@@ -39,6 +39,8 @@ FullPathOptimizer::FullPathOptimizer(
   add_yaw_to_tsp = parameterInstance.getValue<bool>("fullPathOptimizer.add_yaw_to_tsp");
   add_distance_to_robot_to_tsp = parameterInstance.getValue<bool>(
     "fullPathOptimizer.add_distance_to_robot_to_tsp");
+  goal_hysteresis_threshold_ = parameterInstance.getValue<double>(
+    "fullPathOptimizer.goal_hysteresis_threshold");
 
   explore_costmap_ros_ = explore_costmap_ros;
   local_search_area_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(
@@ -375,6 +377,26 @@ bool FullPathOptimizer::getNextGoal(
   add_yaw_to_tsp = parameterInstance.getValue<bool>("fullPathOptimizer.add_yaw_to_tsp");
   add_distance_to_robot_to_tsp = parameterInstance.getValue<bool>(
     "fullPathOptimizer.add_distance_to_robot_to_tsp");
+  goal_hysteresis_threshold_ = parameterInstance.getValue<double>(
+    "fullPathOptimizer.goal_hysteresis_threshold");
+
+  // Check if current committed goal is still valid
+  bool current_goal_valid = false;
+  if (has_committed_goal_ && current_committed_goal_) {
+    // Check if the committed goal still exists in the frontier list and is valid
+    auto it = std::find_if(frontier_list.begin(), frontier_list.end(),
+      [this](const FrontierPtr& f) {
+        return f->getUID() == current_committed_goal_->getUID();
+      });
+    
+    if (it != frontier_list.end() && (*it)->isAchievable() && !(*it)->isBlacklisted()) {
+      current_goal_valid = true;
+      LOG_INFO("Current committed goal is still valid (UID: " << current_committed_goal_->getUID() << ")");
+    } else {
+      LOG_INFO("Current committed goal is no longer valid - will compute new goal");
+      has_committed_goal_ = false;
+    }
+  }
 
   SortedFrontiers sortedFrontiers;
   // sort based on path length
@@ -401,17 +423,25 @@ bool FullPathOptimizer::getNextGoal(
 
       rosVisualizerInstance.visualizePath("global_repositioning_path", globalReposPath);
 
+      // Update committed goal since we're switching to global repositioning
+      current_committed_goal_ = nextFrontier;
+      has_committed_goal_ = true;
+      current_goal_path_cost_ = nextFrontier->getPathLengthInM();
+      LOG_INFO("Committing to global repositioning goal (UID: " << nextFrontier->getUID() << ")");
+
       return true;
     } else {
       LOG_ERROR(
         "Could not find local or global frontiers. Returning a zero frontier. The program may crash if goal point is checked...");
       nextFrontier = zeroFrontier;
+      has_committed_goal_ = false;
       return false;
     }
   }
 
   if (!getBestFullPath(sortedFrontiers, bestFrontierWaypoint, robotP)) {
     nextFrontier = zeroFrontier;
+    has_committed_goal_ = false;
     return false;
   }
 
@@ -440,8 +470,54 @@ bool FullPathOptimizer::getNextGoal(
     rosVisualizerInstance.visualizePath("full_path", bestPathROS);
   }
   EventLoggerInstance.endEvent("publishPlan", 2);
+  
   // 0 is robot pose. Return the first frontier in the path.
-  nextFrontier = bestFrontierWaypoint[1];
+  FrontierPtr candidateGoal = bestFrontierWaypoint[1];
+  
+  // Apply hysteresis logic to prevent frequent goal switching
+  if (current_goal_valid) {
+    // Calculate cost to reach the current committed goal
+    FrontierPtr robotPoseFrontier = std::make_shared<Frontier>();
+    robotPoseFrontier->setGoalPoint(robotP.pose.position.x, robotP.pose.position.y);
+    robotPoseFrontier->setUID(generateUID(robotPoseFrontier));
+    
+    double cost_to_current_goal = calculateLengthRobotToGoal(robotPoseFrontier, current_committed_goal_);
+    double cost_to_new_goal = calculateLengthRobotToGoal(robotPoseFrontier, candidateGoal);
+    
+    // Calculate improvement ratio
+    double improvement = (cost_to_current_goal - cost_to_new_goal) / cost_to_current_goal;
+    
+    LOG_INFO("Hysteresis check - Current goal cost: " << cost_to_current_goal << 
+             ", New goal cost: " << cost_to_new_goal << 
+             ", Improvement: " << (improvement * 100.0) << "%");
+    
+    if (improvement >= goal_hysteresis_threshold_) {
+      // New goal is significantly better - switch to it
+      LOG_INFO("New goal is " << (improvement * 100.0) << "% better (threshold: " << 
+               (goal_hysteresis_threshold_ * 100.0) << "%) - switching goals");
+      LOG_INFO("Switching from goal UID " << current_committed_goal_->getUID() << 
+               " to " << candidateGoal->getUID());
+      current_committed_goal_ = candidateGoal;
+      current_goal_path_cost_ = cost_to_new_goal;
+      nextFrontier = candidateGoal;
+    } else {
+      // Stick with current goal
+      LOG_INFO("Sticking with current goal (improvement " << (improvement * 100.0) << 
+               "% below threshold " << (goal_hysteresis_threshold_ * 100.0) << "%)");
+      nextFrontier = current_committed_goal_;
+    }
+  } else {
+    // No current goal or it became invalid - commit to the new candidate
+    LOG_INFO("No valid committed goal - committing to new goal (UID: " << candidateGoal->getUID() << ")");
+    current_committed_goal_ = candidateGoal;
+    has_committed_goal_ = true;
+    FrontierPtr robotPoseFrontier = std::make_shared<Frontier>();
+    robotPoseFrontier->setGoalPoint(robotP.pose.position.x, robotP.pose.position.y);
+    robotPoseFrontier->setUID(generateUID(robotPoseFrontier));
+    current_goal_path_cost_ = calculateLengthRobotToGoal(robotPoseFrontier, candidateGoal);
+    nextFrontier = candidateGoal;
+  }
+  
   return true;
 }
 

roadmap_explorer/src/Parameters.cpp

@@ -103,6 +103,10 @@ void ParameterHandler::makeParameters(std::shared_ptr<nav2_util::LifecycleNode>
     node,
     "fullPathOptimizer.add_distance_to_robot_to_tsp", rclcpp::ParameterValue(
       false));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "fullPathOptimizer.goal_hysteresis_threshold", rclcpp::ParameterValue(
+      0.15));
 
   parameter_map_["fullPathOptimizer.num_frontiers_in_local_area"] = node->get_parameter(
     "fullPathOptimizer.num_frontiers_in_local_area").as_double();
@@ -112,6 +116,8 @@ void ParameterHandler::makeParameters(std::shared_ptr<nav2_util::LifecycleNode>
     "fullPathOptimizer.add_yaw_to_tsp").as_bool();
   parameter_map_["fullPathOptimizer.add_distance_to_robot_to_tsp"] = node->get_parameter(
     "fullPathOptimizer.add_distance_to_robot_to_tsp").as_bool();
+  parameter_map_["fullPathOptimizer.goal_hysteresis_threshold"] = node->get_parameter(
+    "fullPathOptimizer.goal_hysteresis_threshold").as_double();
 
   // --- explorationBT ---
   nav2_util::declare_parameter_if_not_declared(

roadmap_explorer/tests/bt_plugins/test_bt_plugin_integration.cpp

@@ -155,7 +155,13 @@ TEST_F(BTPluginIntegrationTest, DirectInstantiationIntegration)
 
   // Test execution
   size_t initial_count = EventLoggerInstance.getPlanningCount();
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   EXPECT_EQ(EventLoggerInstance.getPlanningCount(), initial_count + 1);
@@ -222,7 +228,13 @@ TEST_F(BTPluginIntegrationTest, BehaviorTreeCreation)
 
   // Test tree execution
   size_t initial_count = EventLoggerInstance.getPlanningCount();
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   EXPECT_EQ(EventLoggerInstance.getPlanningCount(), initial_count + 1);
@@ -258,7 +270,13 @@ TEST_F(BTPluginIntegrationTest, MultipleBTNodes)
 
   // Test tree execution - should increment counter 3 times
   size_t initial_count = EventLoggerInstance.getPlanningCount();
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   EXPECT_EQ(EventLoggerInstance.getPlanningCount(), initial_count + 3);
@@ -316,7 +334,13 @@ TEST_F(BTPluginIntegrationTest, DifferentNodeConfigurations)
 
   // Test execution
   size_t initial_count = EventLoggerInstance.getPlanningCount();
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   EXPECT_EQ(EventLoggerInstance.getPlanningCount(), initial_count + 2);
@@ -387,7 +411,13 @@ TEST_F(BTPluginIntegrationTest, ConcurrentFactoryAccess)
   const int num_executions = 5;
 
   for (int i = 0; i < num_executions; ++i) {
+#ifdef ROS_DISTRO_HUMBLE
     BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+    BT::NodeStatus status = tree.tickOnce();
+#else
+    #error "Unsupported ROS distro"
+#endif
     EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   }
 

roadmap_explorer/tests/bt_plugins/test_log_iteration_plugin.cpp

@@ -233,7 +233,13 @@ TEST_F(LogIterationPluginTest, DifferentNodeNames)
 
   // Test tree execution
   size_t initial_count = EventLoggerInstance.getPlanningCount();
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
   // Should have executed both nodes, so count should increase by 2
@@ -316,7 +322,8 @@ TEST_F(LogIterationPluginTest, BasicThreadSafety)
   context_ = createContext();
   plugin_->registerNodes(*factory_, context_);
 
-  // Create a simple behavior tree XML with multiple LogIterationBT nodes
+#ifdef ROS_DISTRO_HUMBLE
+  // For Humble (BT.CPP 3.x): Use Parallel node
   std::string bt_xml = R"(
     <root BTCPP_format="4">
       <BehaviorTree ID="ThreadSafetyTest">
@@ -327,6 +334,22 @@ TEST_F(LogIterationPluginTest, BasicThreadSafety)
       </BehaviorTree>
     </root>
   )";
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  // For Jazzy/Kilted (BT.CPP 4.x): Use Sequence instead
+  // Parallel nodes behave differently with tickOnce() in BT.CPP 4.x
+  std::string bt_xml = R"(
+    <root BTCPP_format="4">
+      <BehaviorTree ID="ThreadSafetyTest">
+        <Sequence>
+          <LogIterationBT name="thread_test_1" />
+          <LogIterationBT name="thread_test_2" />
+        </Sequence>
+      </BehaviorTree>
+    </root>
+  )";
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   // Create blackboard
   auto blackboard = BT::Blackboard::create();
@@ -339,7 +362,13 @@ TEST_F(LogIterationPluginTest, BasicThreadSafety)
   size_t initial_count = EventLoggerInstance.getPlanningCount();
 
   // Execute the tree (which runs both nodes in parallel)
+#ifdef ROS_DISTRO_HUMBLE
   BT::NodeStatus status = tree.tickRoot();
+#elif ROS_DISTRO_JAZZY || ROS_DISTRO_KILTED
+  BT::NodeStatus status = tree.tickOnce();
+#else
+  #error "Unsupported ROS distro"
+#endif
 
   EXPECT_EQ(status, BT::NodeStatus::SUCCESS);