Humble Sync 1: August 24 (#528)

* Publish pose with covariance in localization mode. (#511) * Visualize localization nodes and edges with different colors in the pose graph marker message. (#513) * Fix edge marker id to not conflict with the localization edge marker. (#515) * Stop ceres problem from freeing loss function (#519) * Add map_and_localization node that supports toggling between mapping and localization modes with a service. (#512) * resolve API change from galactic (#524) * Fix segfault after loading posegraph (#527) * Assign mapper to closure_assistant after loading posgraph * Fix linting * Midigate spamming for 50Hz lidar (#526) * Increase scan filter queue_size * Add scan_queue_size as parameter Co-authored-by: Marc Alban <[email protected]> Co-authored-by: john-maidbot <[email protected]> Co-authored-by: Samuel Lindgren <[email protected]>
SteveMacenski · Aug 24, 2022 · c2541aa · c2541aa
1 parent ae68b85
commit c2541aa
Show file tree

Hide file tree

Showing 14 changed files with 416 additions and 48 deletions.
diff --git a/CMakeLists.txt b/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

diff --git a/README.md b/README.md
@@ -222,6 +222,8 @@ The following settings and options are exposed to you. My default configuration
 
 `scan_topic` - scan topic, *absolute* path, ei `/scan` not `scan`
 
+`scan_queue_size` - The number of scan messages to queue up before throwing away old ones. Should always be set to 1 in async mode
+
 `map_file_name` - Name of the pose-graph file to load on startup if available
 
 `map_start_pose` - Pose to start pose-graph mapping/localization in, if available

diff --git a/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp b/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_
diff --git a/include/slam_toolbox/loop_closure_assistant.hpp b/include/slam_toolbox/loop_closure_assistant.hpp
@@ -53,6 +53,7 @@ class LoopClosureAssistant
   void processInteractiveFeedback(
     const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback);
   void publishGraph();
+  void setMapper(karto::Mapper * mapper);
 
 private:
   bool manualLoopClosureCallback(

diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/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:
@@ -144,7 +144,7 @@ class SlamToolbox : public rclcpp::Node
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
   std_msgs::msg::Header scan_header;
-  int throttle_scans_;
+  int throttle_scans_, scan_queue_size_;
 
   double resolution_;
   double position_covariance_scale_;

diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/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;

diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -2007,7 +2007,7 @@ class KARTO_EXPORT Mapper : public Module
   kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
   kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr);
   kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr);
-  kt_bool ProcessLocalization(LocalizedRangeScan * pScan);
+  kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
   kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan> *);
   void AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex<LocalizedRangeScan> * scan_vertex);
   void ClearLocalizationBuffer();
@@ -2086,6 +2086,11 @@ class KARTO_EXPORT Mapper : public Module
     m_pGraph->CorrectPoses();
   }
 
+  const LocalizationScanVertices& GetLocalizationVertices()
+  {
+    return m_LocalizationScanVertices;
+  }
+
 protected:
   void InitializeParameters();
 

diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
@@ -2828,7 +2828,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad
   return false;
 }
 
-kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
+kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance)
 {
   if (pScan == NULL) {
     return false;
@@ -2866,17 +2866,20 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
     return false;
   }
 
-  Matrix3 covariance;
-  covariance.SetToIdentity();
+  Matrix3 cov;
+  cov.SetToIdentity();
 
   // correct scan (if not first scan)
   if (m_pUseScanMatching->GetValue() && pLastScan != NULL) {
     Pose2 bestPose;
     m_pSequentialScanMatcher->MatchScan(pScan,
       m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
       bestPose,
-      covariance);
+      cov);
     pScan->SetSensorPose(bestPose);
+    if (covariance) {
+      *covariance = cov;
+    }
   }
 
   // add scan to buffer and assign id
@@ -2886,7 +2889,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
   if (m_pUseScanMatching->GetValue()) {
     // add to graph
     scan_vertex = m_pGraph->AddVertex(pScan);
-    m_pGraph->AddEdges(pScan, covariance);
+    m_pGraph->AddEdges(pScan, cov);
 
     m_pMapperSensorManager->AddRunningScan(pScan);
 

diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp
@@ -145,6 +145,10 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node)
     options_problem_.enable_fast_removal = true;
   }
 
+  // we do not want the problem definition to own these objects, otherwise they get
+  // deleted along with the problem 
+  options_problem_.loss_function_ownership = ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
+
   problem_ = new ceres::Problem(options_problem_);
 }
 
@@ -158,6 +162,9 @@ CeresSolver::~CeresSolver()
   if (nodes_ != NULL) {
     delete nodes_;
   }
+  if (blocks_ != NULL) {
+    delete blocks_;
+  }
   if (problem_ != NULL) {
     delete problem_;
   }
@@ -242,6 +249,8 @@ void CeresSolver::Reset()
   was_constant_set_ = false;
 
   if (problem_) {
+    // Note that this also frees anything the problem owns (i.e. local parameterization, cost
+    // function)
     delete problem_;
   }