Merge pull request #4 from saha-robotics/humble-devel

Humble devel
saha-robotics · Dec 9, 2024 · 7bf57a8 · 7bf57a8
2 parents c65e46c + 5db9c1c
commit 7bf57a8
Show file tree

Hide file tree

Showing 8 changed files with 56 additions and 16 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -106,6 +106,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   srvs/AddSubmap.srv
   srvs/DeserializePoseGraph.srv
   srvs/SerializePoseGraph.srv
+  msgs/SlamMetrics.msg
   srvs/SetParametersService.srv
   msgs/SavedTargetInfo.msg
   msgs/SavedTargetInfoArray.msg

diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp
@@ -48,6 +48,7 @@
 #include "slam_toolbox/get_pose_helper.hpp"
 #include "slam_toolbox/map_saver.hpp"
 #include "slam_toolbox/loop_closure_assistant.hpp"
+#include "slam_toolbox/msg/slam_metrics.hpp"
 
 #include <std_msgs/msg/float32.hpp> // TODO: change here idk
 
@@ -137,7 +138,7 @@ class SlamToolbox : public rclcpp::Node
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
   std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
-  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float32>> localization_health_pub_; // TODO: Change here
+  std::shared_ptr<rclcpp::Publisher<slam_toolbox::msg::SlamMetrics>> localization_health_pub_; // TODO: Change here
   std::shared_ptr<rclcpp::Publisher<slam_toolbox::msg::SavedTargetInfoArray>> ssSaved_target_data_;
   std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;

diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -1970,11 +1970,13 @@ class KARTO_EXPORT Mapper : public Module
       double bestResponse;
       double bestPoseX;
       double bestPoseY;
+      int nearChainsCount;
   };
 
   std::shared_ptr<double> m_pbestResponse;
   std::shared_ptr<double> m_pbestPoseX;
   std::shared_ptr<double> m_pbestPoseY;
+  std::shared_ptr<int> m_pnearChainsCount;
 
   std::shared_ptr<LocalizationInfos> GetBestResponse() const;
   void SetBestResponse(const std::shared_ptr<LocalizationInfos>& response);

diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
@@ -48,6 +48,7 @@ namespace karto
 
 // enable this for verbose debug information
 // #define KARTO_DEBUG
+// #define KARTO_DEBUG2
 
   #define MAX_VARIANCE            500.0
   #define DISTANCE_PENALTY_GAIN   0.2
@@ -1298,6 +1299,9 @@ class BreadthFirstTraversal : public GraphTraversal<T>
     {
       objects.push_back((*iter)->GetObject());
     }
+#ifdef KARTO_DEBUG2
+    std::cout << "objects size: " << objects.size() << std::endl; 
+#endif
 
     return objects;
   }
@@ -1512,6 +1516,9 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian
       if ((rCandidateSensorName == rSensorName) ||
         (pSensorManager->GetScans(rCandidateSensorName).empty()))
       {
+#ifdef KARTO_DEBUG2
+        std::cout << "Skipping candidate sensor name: " << rCandidateSensorName << std::endl;
+#endif
         continue;
       }
 
@@ -1538,6 +1545,9 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian
   }
 
   // link to other near chains (chains that include new scan are invalid)
+#ifdef KARTO_DEBUG2
+  std::cout << "Linking to other near chains" << std::endl;
+#endif
   LinkNearChains(pScan, means, covariances);
 
   if (!means.empty()) {
@@ -1553,10 +1563,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens
 
   LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
 
-#ifdef KARTO_DEBUG
+#ifdef KARTO_DEBUG2
   std::cout << "TryCloseLoop candidateChain size: " << candidateChain.size() << std::endl;
 #endif
-
   while (!candidateChain.empty()) {
 #ifdef KARTO_DEBUG
     std::cout << "Checking of the candidatecChainsize inside of Loop CLosure: " << candidateChain.size() << std::endl;
@@ -1856,6 +1865,9 @@ std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRange
 
   const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
       m_pMapper->m_pLinkScanMaximumDistance->GetValue());
+#ifdef KARTO_DEBUG2
+  std::cout << "FindNearChains0 : nearLinkedScans.size() : " << nearLinkedScans.size() << std::endl;
+#endif
   const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
   {
     LocalizedRangeScan * pNearScan = *iter;
@@ -1909,7 +1921,9 @@ std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRange
     }
 
     chain.push_back(pNearScan);
-
+#ifdef KARTO_DEBUG2
+        std::cout << "FindNearChains : chain.size() : " << chain.size() << std::endl;
+#endif
     // add scans after current scan being processed
     kt_int32u end =
       static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(
@@ -1952,6 +1966,10 @@ std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRange
       // add chain to collection
       nearChains.push_back(tempChain);
     }
+#ifdef KARTO_DEBUG2
+        std::cout << "FindNearChains2 : chain.size() : " << nearChains.size() << std::endl;
+#endif
+  m_pMapper->m_pnearChainsCount = std::make_shared<int>(nearChains.size());  
   }
 
   return nearChains;
@@ -1965,6 +1983,9 @@ LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(
       m_pMapper->m_pUseScanBarycenter->GetValue());
   LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(GetVertex(
         pScan), pVisitor);
+#ifdef KARTO_DEBUG2
+  std::cout << "FindNearLinkedScans : nearLinkedScans.size() : " << nearLinkedScans.size() << std::endl;
+#endif
   delete pVisitor;
 
   return nearLinkedScans;
@@ -2137,6 +2158,10 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(
   const LocalizedRangeScanVector nearLinkedScans =
     FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue());
 
+#ifdef KARTO_DEBUG2
+  std::cout << "FindPossibleLoopClosure0 : nearLinkedScans.size() : " << nearLinkedScans.size() << std::endl;
+#endif
+
   kt_int32u nScans =
     static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
 
@@ -2246,7 +2271,8 @@ Mapper::Mapper()
   m_pScanOptimizer(NULL),
   m_pbestResponse(std::make_shared<double>(0.0)),
   m_pbestPoseX(std::make_shared<double>(0.0)),     
-  m_pbestPoseY(std::make_shared<double>(0.0)) 
+  m_pbestPoseY(std::make_shared<double>(0.0)),
+  m_pnearChainsCount(std::make_shared<int>(0))
 {
   InitializeParameters();
 }
@@ -2264,7 +2290,8 @@ Mapper::Mapper(const std::string & rName)
   m_pScanOptimizer(NULL),
   m_pbestResponse(std::make_shared<double>(0.0)),
   m_pbestPoseX(std::make_shared<double>(0.0)),     
-  m_pbestPoseY(std::make_shared<double>(0.0)) 
+  m_pbestPoseY(std::make_shared<double>(0.0)),
+  m_pnearChainsCount(std::make_shared<int>(0))
 {
   InitializeParameters();
 }
@@ -3080,7 +3107,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad
 
 kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance)
 {
-  
+
   if (pScan == NULL) {
     return false;
   }
@@ -3167,7 +3194,6 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari
 
   m_pMapperSensorManager->SetLastScan(pScan);
   AddScanToLocalizationBuffer(pScan, scan_vertex);
-  // std::cout <<"\n\nAddedScan is "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl;
   return true;
 }
 
@@ -3522,6 +3548,7 @@ std::shared_ptr<Mapper::LocalizationInfos> Mapper::GetBestResponse() const
     response->bestResponse = *m_pbestResponse;
     response->bestPoseX = *m_pbestPoseX;
     response->bestPoseY = *m_pbestPoseY;
+    response->nearChainsCount = *m_pnearChainsCount;
     return response;
 }
 
@@ -3547,11 +3574,13 @@ void Mapper::StorePose(const LocalizedRangeScan * pScan)
     pose.scanId = pScan->GetStateId();
     pose.targetName = saveTargetName_;
     poseVector.push_back(pose);
+#ifdef KARTO_DEBUG
     std::cout << "Stored Pose: x=" << pose.x
               << ", y=" << pose.y
               << ", yaw=" << pose.yaw
               << ", nScans=" << pose.scanId 
               << ", targetName=" << pose.targetName << std::endl;
+#endif
 }
 
 void Mapper::UpdateStoredPoses()
@@ -3567,11 +3596,12 @@ void Mapper::UpdateStoredPoses()
         pose.x = scan->GetCorrectedPose().GetX();
         pose.y = scan->GetCorrectedPose().GetY();
         pose.yaw = scan->GetCorrectedPose().GetHeading();
-
+#ifdef KARTO_DEBUG
         std::cout << "Updated Stored Pose for scanId: " << pose.scanId
                   << " -> X=" << pose.x
                   << ", Y=" << pose.y
                   << ", Yaw=" << pose.yaw << std::endl;
+#endif
     }
     tableVectorUpdated_ = true;
 }

diff --git a/msgs/SlamMetrics.msg b/msgs/SlamMetrics.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header
+float64 best_response
+uint16 near_node_count
diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp
@@ -247,8 +247,8 @@ void SlamToolbox::setROSInterfaces()
 
   pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "pose", 10);
-  localization_health_pub_ = this->create_publisher<std_msgs::msg::Float32>(
-    "slam_toolbox/best_response", qos);
+  localization_health_pub_ = this->create_publisher<slam_toolbox::msg::SlamMetrics>(
+    "slam_toolbox/slam_metrics", qos);
 
   sst_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
     map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
@@ -307,15 +307,18 @@ void SlamToolbox::publishTransformLoop(
       double best_response = response->bestResponse;
       double best_pose_x = response->bestPoseX;
       double best_pose_y = response->bestPoseY;
+      int near_chain_count = response->nearChainsCount;
       static double previous_best_response = 0.0;  
 
       if (best_response > 0.02) {
           try {
             if (best_response != previous_best_response) {
-                std_msgs::msg::Float32 msg;
-                msg.data = static_cast<float>(best_response);  
+                slam_toolbox::msg::SlamMetrics msg;
+                msg.header.stamp = this->now();
+                msg.best_response = best_response;
+                msg.near_node_count = near_chain_count;
                 localization_health_pub_->publish(msg);
-                previous_best_response = best_response;  
+                previous_best_response = best_response; 
             }
           } 
           catch (std::exception & e) {

diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp
@@ -253,7 +253,7 @@ void LocalizationSlamToolbox::set_parameters_callback(
 ) 
 /*****************************************************************************/
 {
-    if(request->table_save_mode){
+    if(request->target_save_mode){
       if (processor_type_ == PROCESS){
         if (request->target_uid.empty()){
           RCLCPP_ERROR(get_logger(), "Triggering target save without target name or type");

diff --git a/srvs/SetParametersService.srv b/srvs/SetParametersService.srv
@@ -1,7 +1,7 @@
 string[] param_names
 float64[] param_values
 bool default_mode
-bool table_save_mode
+bool target_save_mode
 bool target_list_off
 string target_uid