Add service to toggle in and out of localization mode. (ros2 branch)

CMakeLists.txt

@@ -68,6 +68,7 @@ set(libraries
     sync_slam_toolbox
     localization_slam_toolbox
     lifelong_slam_toolbox
+    map_and_localization_slam_toolbox
 )
 
 find_package(PkgConfig REQUIRED)
@@ -167,6 +168,11 @@ target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${Bo
 add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp)
 target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox)
 
+add_library(map_and_localization_slam_toolbox src/experimental/slam_toolbox_map_and_localization.cpp)
+target_link_libraries(map_and_localization_slam_toolbox localization_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
+add_executable(map_and_localization_slam_toolbox_node src/experimental/slam_toolbox_map_and_localization_node.cpp)
+target_link_libraries(map_and_localization_slam_toolbox_node map_and_localization_slam_toolbox)
+
 #### Merging maps tool
 add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp)
 target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
@@ -185,6 +191,7 @@ target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
 install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node
                 localization_slam_toolbox_node
+                map_and_localization_slam_toolbox_node
                 merge_maps_kinematic
                 ${libraries}
   ARCHIVE DESTINATION lib

include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp

@@ -0,0 +1,61 @@
+/*
+ * slam_toolbox
+ * Copyright (c) 2022, Steve Macenski
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_
+#define SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_
+
+#include <memory>
+#include "slam_toolbox/slam_toolbox_localization.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+namespace slam_toolbox
+{
+
+class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox
+{
+public:
+  explicit MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options);
+  virtual ~MapAndLocalizationSlamToolbox() {}
+  void loadPoseGraphByParams() override;
+  void configure() override;
+
+protected:
+  void laserCallback(
+    sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override;
+  bool serializePoseGraphCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Request> req,
+    std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Response> resp) override;
+  bool deserializePoseGraphCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req,
+    std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Response> resp) override;
+  bool setLocalizationModeCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::SetBool::Request> req,
+    std::shared_ptr<std_srvs::srv::SetBool::Response> resp);
+  LocalizedRangeScan * addScan(
+    LaserRangeFinder * laser,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
+    Pose2 & pose) override;
+  void toggleMode(bool enable_localization);
+
+  std::shared_ptr<rclcpp::Service<std_srvs::srv::SetBool>> ssSetLocalizationMode_;
+};
+
+}  // namespace slam_toolbox
+
+#endif  // SLAM_TOOLBOX__SLAM_TOOLBOX_MAP_AND_LOCALIZATION_HPP_

include/slam_toolbox/slam_toolbox_common.hpp

@@ -61,8 +61,8 @@ class SlamToolbox : public rclcpp::Node
 public:
   explicit SlamToolbox(rclcpp::NodeOptions);
   SlamToolbox();
-  ~SlamToolbox();
-  void configure();
+  virtual ~SlamToolbox();
+  virtual void configure();
   virtual void loadPoseGraphByParams();
 
 protected:

include/slam_toolbox/slam_toolbox_localization.hpp

@@ -30,11 +30,11 @@ class LocalizationSlamToolbox : public SlamToolbox
 {
 public:
   explicit LocalizationSlamToolbox(rclcpp::NodeOptions options);
-  ~LocalizationSlamToolbox() {}
+  virtual ~LocalizationSlamToolbox() {}
   virtual void loadPoseGraphByParams();
 
 protected:
-  void laserCallback(
+  virtual void laserCallback(
     sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override;
   void localizePoseCallback(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
@@ -43,16 +43,16 @@ class LocalizationSlamToolbox : public SlamToolbox
     const std::shared_ptr<std_srvs::srv::Empty::Request> req,
     std::shared_ptr<std_srvs::srv::Empty::Response> resp);
 
-  bool serializePoseGraphCallback(
+  virtual bool serializePoseGraphCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Request> req,
     std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Response> resp) override;
-  bool deserializePoseGraphCallback(
+  virtual bool deserializePoseGraphCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req,
     std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Response> resp) override;
 
-  LocalizedRangeScan * addScan(
+  virtual LocalizedRangeScan * addScan(
     LaserRangeFinder * laser,
     const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
     Pose2 & pose) override;

src/experimental/slam_toolbox_map_and_localization.cpp

@@ -0,0 +1,177 @@
+/*
+ * slam_toolbox
+ * Copyright (c) 2022, Steve Macenski
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+#include <memory>
+#include <string>
+#include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp"
+
+namespace slam_toolbox
+{
+
+/*****************************************************************************/
+MapAndLocalizationSlamToolbox::MapAndLocalizationSlamToolbox(rclcpp::NodeOptions options)
+: LocalizationSlamToolbox(options)
+/*****************************************************************************/
+{
+  // disable interactive mode
+  enable_interactive_mode_ = false;
+
+  ssSetLocalizationMode_ = create_service<std_srvs::srv::SetBool>(
+    "slam_toolbox/set_localization_mode",
+    std::bind(&MapAndLocalizationSlamToolbox::setLocalizationModeCallback, this,
+    std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+}
+
+/*****************************************************************************/
+void MapAndLocalizationSlamToolbox::configure()
+/*****************************************************************************/
+{
+  SlamToolbox::configure();
+  toggleMode(false);
+}
+
+/*****************************************************************************/
+bool MapAndLocalizationSlamToolbox::setLocalizationModeCallback(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<std_srvs::srv::SetBool::Request> req,
+  std::shared_ptr<std_srvs::srv::SetBool::Response> resp)
+/*****************************************************************************/
+{
+  toggleMode(req->data);
+
+  resp->success = true;
+  return true;
+}
+
+void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) {
+  bool in_localization_mode = processor_type_ == PROCESS_LOCALIZATION;
+  if (in_localization_mode == enable_localization) {
+    return;
+  }
+
+  if (enable_localization) {
+    RCLCPP_INFO(get_logger(), "Enabling localization ...");
+    processor_type_ = PROCESS_LOCALIZATION;
+
+    localization_pose_sub_ =
+      create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "initialpose", 1, std::bind(&MapAndLocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1));
+    clear_localization_ = create_service<std_srvs::srv::Empty>(
+      "slam_toolbox/clear_localization_buffer",
+      std::bind(&MapAndLocalizationSlamToolbox::clearLocalizationBuffer, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+    // in localization mode, disable map saver
+    map_saver_.reset();
+  }
+  else {
+    RCLCPP_INFO(get_logger(), "Enabling mapping ...");
+    processor_type_ = PROCESS;
+    localization_pose_sub_.reset();
+    clear_localization_.reset();
+    map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(), map_name_);
+
+    boost::mutex::scoped_lock lock(smapper_mutex_);
+    if (smapper_ && !smapper_->getMapper()->GetLocalizationVertices().empty()) {
+      smapper_->clearLocalizationBuffer();
+    }
+  }
+}
+
+/*****************************************************************************/
+void MapAndLocalizationSlamToolbox::loadPoseGraphByParams()
+/*****************************************************************************/
+{
+  if (processor_type_ == PROCESS_LOCALIZATION) {
+    LocalizationSlamToolbox::loadPoseGraphByParams();
+  }
+  else {
+    SlamToolbox::loadPoseGraphByParams();
+  }
+}
+
+/*****************************************************************************/
+bool MapAndLocalizationSlamToolbox::serializePoseGraphCallback(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Request> req,
+  std::shared_ptr<slam_toolbox::srv::SerializePoseGraph::Response> resp)
+/*****************************************************************************/
+{
+  if (processor_type_ == PROCESS_LOCALIZATION) {
+    return LocalizationSlamToolbox::serializePoseGraphCallback(request_header, req, resp);
+  }
+  else {
+    return SlamToolbox::serializePoseGraphCallback(request_header, req, resp);
+  }
+}
+
+/*****************************************************************************/
+bool MapAndLocalizationSlamToolbox::deserializePoseGraphCallback(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req,
+  std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Response> resp)
+/*****************************************************************************/
+{
+  if (processor_type_ == PROCESS_LOCALIZATION) {
+    return LocalizationSlamToolbox::deserializePoseGraphCallback(request_header, req, resp);
+  }
+  else {
+    return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp);
+  }
+}
+
+/*****************************************************************************/
+void MapAndLocalizationSlamToolbox::laserCallback(
+  sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
+/*****************************************************************************/
+{
+  // store scan header
+  scan_header = scan->header;
+  // no odom info
+  Pose2 pose;
+  if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {
+    RCLCPP_WARN(get_logger(), "Failed to compute odom pose");
+    return;
+  }
+
+  // ensure the laser can be used
+  LaserRangeFinder * laser = getLaser(scan);
+
+  if (!laser) {
+    RCLCPP_WARN(get_logger(), "Failed to create laser device for"
+      " %s; discarding scan", scan->header.frame_id.c_str());
+    return;
+  }
+
+  addScan(laser, scan, pose);
+}
+
+/*****************************************************************************/
+LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan(
+  LaserRangeFinder * laser,
+  const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
+  Pose2 & odom_pose)
+/*****************************************************************************/
+{
+  if (processor_type_ == PROCESS_LOCALIZATION) {
+    return LocalizationSlamToolbox::addScan(laser, scan, odom_pose);
+  }
+  else {
+    return SlamToolbox::addScan(laser, scan, odom_pose);
+  }
+}
+
+}  // namespace slam_toolbox

src/experimental/slam_toolbox_map_and_localization_node.cpp

@@ -0,0 +1,47 @@
+/*
+ * slam_toolbox
+ * Copyright (c) 2022, Steve Macenski
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+#include <memory>
+#include "slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  int stack_size = 40000000;
+  {
+    auto temp_node = std::make_shared<rclcpp::Node>("slam_toolbox");
+    temp_node->declare_parameter("stack_size_to_use");
+    if (temp_node->get_parameter("stack_size_to_use", stack_size)) {
+      RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size);
+      const rlim_t max_stack_size = stack_size;
+      struct rlimit stack_limit;
+      getrlimit(RLIMIT_STACK, &stack_limit);
+      if (stack_limit.rlim_cur < stack_size) {
+        stack_limit.rlim_cur = stack_size;
+      }
+      setrlimit(RLIMIT_STACK, &stack_limit);
+    }
+  }
+
+  rclcpp::NodeOptions options;
+  auto node = std::make_shared<slam_toolbox::MapAndLocalizationSlamToolbox>(options);
+  node->configure();
+  node->loadPoseGraphByParams();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+  return 0;
+}