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

Humble devel

CMakeLists.txt

@@ -106,6 +106,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   srvs/AddSubmap.srv
   srvs/DeserializePoseGraph.srv
   srvs/SerializePoseGraph.srv
+  srvs/SetParametersService.srv
+  msgs/SavedTargetInfo.msg
+  msgs/SavedTargetInfoArray.msg
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs
 )
 

include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp

@@ -31,6 +31,7 @@ class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox
   virtual ~MapAndLocalizationSlamToolbox() {}
   void loadPoseGraphByParams() override;
   void configure() override;
+  void changeMapTopic(const std::string & map_topic);
 
 protected:
   void laserCallback(

include/slam_toolbox/merge_maps_kinematic.hpp

@@ -31,7 +31,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "interactive_markers/interactive_marker_server.hpp"
 #include "interactive_markers/menu_handler.hpp"
-#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/static_transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/message_filter.h"
 #include "tf2/LinearMath/Matrix3x3.h"
@@ -92,7 +92,7 @@ class MergeMapsKinematic : public rclcpp::Node
   std::vector<std::unique_ptr<karto::Dataset>> dataset_vec_;
 
   // TF
-  std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
+  std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tfB_;
 
   // visualization
   std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;

include/slam_toolbox/slam_toolbox_common.hpp

@@ -49,6 +49,8 @@
 #include "slam_toolbox/map_saver.hpp"
 #include "slam_toolbox/loop_closure_assistant.hpp"
 
+#include <std_msgs/msg/float32.hpp> // TODO: change here idk
+
 namespace slam_toolbox
 {
 
@@ -135,6 +137,8 @@ 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::SavedTargetInfoArray>> ssSaved_target_data_;
   std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::SerializePoseGraph>> ssSerialize_;
@@ -143,6 +147,7 @@ class SlamToolbox : public rclcpp::Node
   // Storage for ROS parameters
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   bool use_map_saver_;
+  bool publish_map_once_, update_map_once_ = true;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
   std_msgs::msg::Header scan_header;
   int throttle_scans_, scan_queue_size_;
@@ -152,6 +157,16 @@ class SlamToolbox : public rclcpp::Node
   double yaw_covariance_scale_;
   bool first_measurement_, enable_interactive_mode_;
 
+  // Position search service params
+  double position_search_distance_;
+  double position_search_resolution_;
+  double position_search_smear_deviation_;
+  double position_search_fine_angle_offset_;
+  double position_search_coarse_angle_offset_;
+  double position_search_coarse_angle_resolution_;
+  double position_search_minimum_best_response_;
+  bool position_search_do_relocalization_;
+
   // Book keeping
   std::unique_ptr<mapper_utils::SMapper> smapper_;
   std::unique_ptr<karto::Dataset> dataset_;
@@ -172,6 +187,7 @@ class SlamToolbox : public rclcpp::Node
   nav_msgs::srv::GetMap::Response map_;
   ProcessType processor_type_;
   std::unique_ptr<karto::Pose2> process_near_pose_;
+  std::unique_ptr<karto::Pose2> process_desired_pose_;
   tf2::Transform reprocessing_transform_;
 
   // pluginlib

include/slam_toolbox/slam_toolbox_localization.hpp

@@ -42,6 +42,9 @@ class LocalizationSlamToolbox : public SlamToolbox
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::Empty::Request> req,
     std::shared_ptr<std_srvs::srv::Empty::Response> resp);
+  void set_parameters_callback(
+    const std::shared_ptr<slam_toolbox::srv::SetParametersService::Request> request,
+    std::shared_ptr<slam_toolbox::srv::SetParametersService::Response> response);
 
   virtual bool serializePoseGraphCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
@@ -57,9 +60,25 @@ class LocalizationSlamToolbox : public SlamToolbox
     const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
     Pose2 & pose) override;
 
+  void setInitialParameters(
+    double position_search_distance, double position_search_maximum_distance,
+    double position_search_fine_angle_offset, double position_search_coarse_angle_offset,
+    double position_search_coarse_angle_resolution, double position_search_resolution, 
+    double position_search_smear_deviation, bool do_loop_closing_flag,
+    int scan_buffer_size);
+
+  void getSavedTableData();
+
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  bool tableSaveComplete_ = false;
+
+
   std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>>
   localization_pose_sub_;
+
   std::shared_ptr<rclcpp::Service<std_srvs::srv::Empty> > clear_localization_;
+  std::shared_ptr<rclcpp::Service<slam_toolbox::srv::SetParametersService> > set_parameters_srv_;
+
 };
 
 }  // namespace slam_toolbox

include/slam_toolbox/toolbox_msgs.hpp

@@ -29,6 +29,9 @@
 #include "visualization_msgs/msg/interactive_marker_control.hpp"
 #include "visualization_msgs/msg/interactive_marker_feedback.hpp"
 
+#include "slam_toolbox/msg/saved_target_info.hpp"
+#include "slam_toolbox/msg/saved_target_info_array.hpp"
+
 #include "slam_toolbox/srv/pause.hpp"
 #include "slam_toolbox/srv/clear_queue.hpp"
 #include "slam_toolbox/srv/toggle_interactive.hpp"
@@ -39,5 +42,6 @@
 #include "slam_toolbox/srv/deserialize_pose_graph.hpp"
 #include "slam_toolbox/srv/merge_maps.hpp"
 #include "slam_toolbox/srv/add_submap.hpp"
+#include "slam_toolbox/srv/set_parameters_service.hpp"
 
 #endif  // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_

include/slam_toolbox/toolbox_types.hpp

@@ -86,7 +86,8 @@ enum ProcessType
   PROCESS = 0,
   PROCESS_FIRST_NODE = 1,
   PROCESS_NEAR_REGION = 2,
-  PROCESS_LOCALIZATION = 3
+  PROCESS_LOCALIZATION = 3,
+  PROCESS_DESIRED_POSE = 4
 };
 
 // Pause state

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -1961,6 +1961,44 @@ class KARTO_EXPORT Mapper : public Module
   virtual ~Mapper();
 
 public:
+
+  /** 
+   * Purpose of this function is set the bestResponse, response and pose values to localization health information
+   */
+  struct LocalizationInfos
+  {
+      double bestResponse;
+      double bestPoseX;
+      double bestPoseY;
+  };
+
+  std::shared_ptr<double> m_pbestResponse;
+  std::shared_ptr<double> m_pbestPoseX;
+  std::shared_ptr<double> m_pbestPoseY;
+
+  std::shared_ptr<LocalizationInfos> GetBestResponse() const;
+  void SetBestResponse(const std::shared_ptr<LocalizationInfos>& response);
+
+  /** 
+   * its for saving the desired pose of the robot
+   */
+  struct TablePose {
+      double x;
+      double y;
+      double yaw;
+      kt_int32u scanId;
+      std::string targetName;
+  };
+
+  std::vector<TablePose> poseVector;
+  kt_bool saveTableData_{false};
+  std::string saveTargetName_ = "masa_0";
+  kt_bool tableVectorUpdated_{false};
+  void StartTableStorage(kt_bool saveTableData, const std::string & saveTargetName);
+  void StorePose(const LocalizedRangeScan* pScan);
+  void UpdateStoredPoses();
+  bool tableSaveComplete_{false};
+
   /**
    * Allocate memory needed for mapping
    * @param rangeThreshold
@@ -2156,9 +2194,9 @@ class KARTO_EXPORT Mapper : public Module
 
 public:
   void SetUseScanMatching(kt_bool val) {m_pUseScanMatching->SetValue(val);}
-
-protected:
   kt_bool m_Initialized;
+protected:  
+
   kt_bool m_Deserialized;
 
   ScanMatcher * m_pSequentialScanMatcher;