Rviz panel clean exit, Fix logging and json dep

Author: suchetanrs

roadmap_explorer/CMakeLists.txt

@@ -55,6 +55,7 @@ set(dependencies
   eigen3_cmake_module
   Eigen3
   nav2_behavior_tree
+  nlohmann_json
 )
 
 ####################### GET ROS DISTRO ##########################
@@ -259,14 +260,3 @@ if(ament_cmake_gtest_FOUND)
 endif()
 
 ament_package()
-
-# ${PROJECT_NAME}_frontier 
-# ${PROJECT_NAME}_utils 
-# ${PROJECT_NAME}_frontier_search_plugins 
-# ${PROJECT_NAME}_information_gain_plugins
-# ${PROJECT_NAME}_planner_plugins 
-# ${PROJECT_NAME}_core
-
-# TODO
-# ${PROJECT_NAME}_bt_plugins 
-# ${PROJECT_NAME}_bt 

roadmap_explorer/include/roadmap_explorer/util/Logger.hpp

@@ -81,20 +81,22 @@ Attributes	        Foreground color	   Background color
 //   2 = WARN    (warnings only) [DEFAULT]
 //   1 = ERROR   (errors and critical messages only)
 //   0 = FATAL   (only fatal errors)
+//  -1 = HIDE   (hide all logs)
 //
 // TIME_LEVEL: Controls which timing logs are shown
 //   2 = Show all timing logs (EVENT, SUBMODULE, MODULE, ITERATION)
 //   1 = Show SUBMODULE, MODULE, and ITERATION timing logs
 //   0 = Show MODULE and ITERATION timing logs only [DEFAULT]
+//  -1 = Hide all timing logs 
 //
 // USE_MODULE_FLOW: Enable/disable module flow tracking logs
 //   true  = Show module flow logs [DEFAULT]
 //   false = Hide module flow logs
 // ======================================================================
 
-#define LOG_LEVEL 3
-#define TIME_LEVEL 0
-#define USE_MODULE_FLOW true
+#define LOG_LEVEL 2
+#define TIME_LEVEL -1
+#define USE_MODULE_FLOW false
 
 ////////////////////////////////////////////////////////////////////////////////////////
 

roadmap_explorer/package.xml

@@ -31,7 +31,7 @@
   <depend>nav2_theta_star_planner</depend>
   <depend>nav2_map_server</depend>
   <depend>boost</depend>
-  <depend>nlohmann_json</depend>
+  <depend>nlohmann-json-dev</depend>
 
   <build_depend>ros_environment</build_depend>
 

roadmap_explorer/src/ExplorationServer.cpp

@@ -42,7 +42,8 @@ nav2_util::CallbackReturn ExplorationServer::on_configure(const rclcpp_lifecycle
 {
   LOG_INFO("Configured " << get_name());
   node_ = shared_from_this();
-  node_->declare_parameter("localisation_only_mode", false);
+
+  nav2_util::declare_parameter_if_not_declared(node_, "localisation_only_mode", rclcpp::ParameterValue(false));
   node_->get_parameter("localisation_only_mode", localisation_only_mode_);
 
   return nav2_util::CallbackReturn::SUCCESS;

roadmap_explorer/src/FullPathOptimizer.cpp

@@ -386,7 +386,7 @@ bool FullPathOptimizer::getNextGoal(
     // LOG_ERROR("Could not find local frontiers. Returning a zero frontiers. The program may crash if goal point is checked...");
     if (sortedFrontiers.global_frontiers.size() >= 1) {
       LOG_WARN(
-        "Could not find local frontiers. Returning the best global frontier.");
+        "Global repositioning!! -> Could not find local frontiers. Returning the best global frontier.");
       nextFrontier = sortedFrontiers.closest_global_frontier;
 
       nav_msgs::msg::Path globalReposPath;

roadmap_explorer/src/Nav2Interface.cpp

@@ -92,7 +92,7 @@ bool Nav2Interface<ActionT>::sendCancelWaitForResponse(
   // Wait for cancel response with timeout (ROS2 recommended approach)
   const int timeout_seconds = 5;
   int elapsed_seconds = 0;
-  while (cancel_future.wait_for(std::chrono::seconds(1)) != std::future_status::ready) {
+  while (cancel_future.wait_for(std::chrono::seconds(1)) != std::future_status::ready && rclcpp::ok()) {
     elapsed_seconds++;
     LOG_WARN("Waiting for cancel response (" << elapsed_seconds << "/" << timeout_seconds << " seconds)");
 
@@ -106,7 +106,7 @@ bool Nav2Interface<ActionT>::sendCancelWaitForResponse(
 
   // Wait until the cancel request has been accepted or an error occurs.
   auto result = cancel_future.get();
-  LOG_WARN("Got cancel response");
+  LOG_INFO("Got cancel response");
   if (!result) {
     LOG_ERROR("The future response is null");
     return false;
@@ -123,7 +123,7 @@ void Nav2Interface<ActionT>::cancelAllGoals()
   action_msgs::srv::CancelGoal::Response response;
   if (sendCancelWaitForResponse(response)) {
     if (response.return_code == action_msgs::srv::CancelGoal::Response::ERROR_NONE) {
-      LOG_ERROR("Cancellation request accepted by nav2 server. Will cancel the goal.");
+      LOG_INFO("Cancellation request accepted by nav2 server. Will cancel the goal.");
       nav2_goal_state_ = NavGoalStatus::CANCELLING;
     } else if (response.return_code == action_msgs::srv::CancelGoal::Response::ERROR_REJECTED) {
       LOG_INFO("Cancellation request rejected by Nav2 action server, not cancelling goal");

roadmap_explorer/src/bt_plugins/SearchForFrontiersPlugin.cpp

@@ -57,7 +57,7 @@ namespace roadmap_explorer
         LOG_INFO("Using robot pose: " << robotP.pose.position.x << ", " << robotP.pose.position.y);
         std::vector<FrontierPtr> frontier_list;
         auto current_frontier_search_distance = config().blackboard->get<double>("current_frontier_search_distance");
-        LOG_HIGHLIGHT("Current frontier search distance: " << current_frontier_search_distance);
+        LOG_INFO("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)
         {

roadmap_explorer/src/bt_plugins/SendNav2GoalPlugin.cpp

@@ -107,7 +107,7 @@ namespace roadmap_explorer
 
     void onHalted()
     {
-        LOG_WARN("SendNav2Goal onHalted");
+        LOG_INFO("SendNav2Goal onHalted");
         return;
     }
 

roadmap_explorer/src/information_gain/CountBasedGain.cpp

@@ -252,9 +252,9 @@ double CountBasedGain::setArrivalInformationLimits()
   }
   arrival_info_limits_set_ = true;
   max_arrival_info_gt_ = maxValue * 1.2;
-  LOG_WARN("Max arrival cost GT: " << max_arrival_info_gt_);
+  LOG_INFO("Max arrival cost GT: " << max_arrival_info_gt_);
   min_arrival_info_gt_ = FACTOR_OF_MAX_IS_MIN_ * max_arrival_info_gt_;
-  LOG_WARN("Min arrival cost GT: " << min_arrival_info_gt_);
+  LOG_INFO("Min arrival cost GT: " << min_arrival_info_gt_);
   return maxValue;
 }
 } // namespace roadmap_explorer

roadmap_explorer_rviz_plugins/src/panel.cpp

@@ -127,8 +127,46 @@ ExplorationPanel::ExplorationPanel(QWidget * parent)
   ros_timer_->start();
 }
 
+ExplorationPanel::~ExplorationPanel()
+{
+  // Stop the timer first to prevent any further ROS calls
+  if (ros_timer_) {
+    ros_timer_->stop();
+  }
+
+  // Cancel any active goals before shutting down
+  if (goal_handle_) {
+    try {
+      // Don't wait for response during shutdown
+      action_client_->async_cancel_goal(goal_handle_);
+    } catch (const std::exception & e) {
+      // Ignore exceptions during shutdown
+    }
+    goal_handle_.reset();
+  }
+
+  // Clean up ROS clients before the node
+  if (action_client_) {
+    action_client_.reset();
+  }
+
+  if (save_map_client_) {
+    save_map_client_.reset();
+  }
+
+  // Finally, clean up the node
+  if (node_) {
+    node_.reset();
+  }
+}
+
 void ExplorationPanel::spinnerOnGUI()
 {
+  // Safety check: if node is being destroyed, don't proceed
+  if (!node_ || !rclcpp::ok()) {
+    return;
+  }
+
   rclcpp::spin_some(node_);
   if (!action_client_->wait_for_action_server(0ms)) {
     updateButtons(ButtonSetting::IN_PROCESS);