Progress porting to mp2p_icp maps

mrpt_pf_localization/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(pose_cov_ops REQUIRED)
 find_package(mrpt_msgs_bridge REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(mp2p_icp_map REQUIRED)
 
 find_package(mrpt-ros2bridge REQUIRED)
 find_package(mrpt-gui REQUIRED)
@@ -54,6 +55,7 @@ target_link_libraries(${PROJECT_NAME}_core
   mrpt::gui
   mrpt::slam
   mrpt::ros2bridge
+  mola::mp2p_icp_map
 )
 
 add_executable(${PROJECT_NAME}_node

mrpt_pf_localization/include/mrpt_pf_localization_node.h

@@ -30,6 +30,8 @@
 #include <std_msgs/msg/header.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#include "mrpt_msgs/msg/generic_object.hpp"
+
 #define MRPT_LOCALIZATION_NODE_DEFAULT_PARAMETER_UPDATE_SKIP 1
 #define MRPT_LOCALIZATION_NODE_DEFAULT_PARTICLECLOUD_UPDATE_SKIP 5
 #define MRPT_LOCALIZATION_NODE_DEFAULT_MAP_UPDATE_SKIP 2
@@ -49,7 +51,9 @@ class PFLocalizationNode : public rclcpp::Node
 		NodeParameters() = default;
 		~NodeParameters() = default;
 
-		double rate_hz = 10.0;	//!< Execution rate in Hz
+		void loadFrom(const mrpt::containers::yaml& cfg);
+
+		double rate_hz = 2.0;  //!< Execution rate in Hz
 
 		/// projection into the future added to the published tf to extend its
 		/// validity
@@ -60,24 +64,21 @@ class PFLocalizationNode : public rclcpp::Node
 		double no_update_tolerance = 1.0;
 
 		/// maximum time without any observation before start complaining
-		double no_inputs_tolerance;
-
-		///
-		int parameter_update_skip;
-
-		int particlecloud_update_skip;
+		double no_inputs_tolerance = 2.0;
 
-		int map_update_skip;
+		int publish_particles_decimation = 1;
 
 		std::string base_footprint_frame_id = "base_footprint";
 		std::string odom_frame_id = "odom";
 		std::string global_frame_id = "map";
 
+		std::string topic_map = "/mrpt_map/metric_map";
 		std::string topic_initialpose = "/initialpose";
-		std::string topic_gridmap = "/map";
-
 		std::string topic_odometry = "/odom";
 
+		std::string pub_topic_particles = "/particlecloud";
+		std::string pub_topic_pose = "/pf_pose";
+
 		/// Comma "," separated list of topics to subscribe for LaserScan msgs
 		std::string topic_sensors_2d_scan;
 
@@ -86,17 +87,20 @@ class PFLocalizationNode : public rclcpp::Node
 
 		bool update_while_stopped;
 		bool update_sensor_pose;
-		bool pose_broadcast;
-		bool tf_broadcast;
-		bool use_map_topic;
-		bool first_map_only;
 	};
 
 	NodeParameters nodeParams_;
 
    private:
 	PFLocalizationCore core_;
 
+	int loopCount_ = 0;	 //!< For decimation purposes only
+
+	bool isTimeFor(int decimation) const
+	{
+		return decimation <= 1 || (loopCount_ % decimation == 0);
+	}
+
 	rclcpp::TimerBase::SharedPtr timer_;
 
 	///
@@ -116,7 +120,9 @@ class PFLocalizationNode : public rclcpp::Node
 	void callbackInitialpose(
 		const geometry_msgs::msg::PoseWithCovarianceStamped& msg);
 	void callbackOdometry(const nav_msgs::msg::Odometry&);
-	void callbackMap(const nav_msgs::msg::OccupancyGrid&);
+
+	void callbackMap(const mrpt_msgs::msg::GenericObject& obj);
+
 	void updateMap(const nav_msgs::msg::OccupancyGrid&);
 	void publishTF();
 	void publishPose();
@@ -126,27 +132,20 @@ class PFLocalizationNode : public rclcpp::Node
 	/** Sub for /initialpose */
 	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::
 		SharedPtr sub_init_pose_;
-	rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_gridmap_;
 
-	rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
+	rclcpp::Subscription<mrpt_msgs::msg::GenericObject>::SharedPtr subMap_;
+	rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subOdometry_;
 
 	// Sensors:
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr>
 		subs_2dlaser_;
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
 		subs_point_clouds_;
 
-	// rclcpp::Subscriber sub_map_;
-	// rclcpp::ServiceClient client_map_;
-
-	rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_particles_;
-
-	// Pubs for ROS standard occupancy grid map:
-	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
-	rclcpp::Publisher<nav_msgs::msg::MapMetaData>::SharedPtr pub_metadata_;
+	rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pubParticles_;
 
 	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-		pub_pose_;
+		pubPose_;
 
 	// rclcpp::ServiceServer service_map_;
 
@@ -168,8 +167,4 @@ class PFLocalizationNode : public rclcpp::Node
 	bool waitForTransform(
 		mrpt::poses::CPose3D& des, const std::string& target_frame,
 		const std::string& source_frame, const int timeoutMilliseconds = 50);
-	bool mapCallback(
-		nav_msgs::srv::GetMap::Request& req,
-		nav_msgs::srv::GetMap::Response& res);
-	void publishMap();
 };

mrpt_pf_localization/launch/localization.launch.py

@@ -35,6 +35,23 @@ def generate_launch_description():
         description="Logging level"
     )
 
+    topic_sensors_2d_scan_arg = DeclareLaunchArgument(
+        "topic_sensors_2d_scan",
+        default_value='',
+        description="Comma-separated list of topics to subscribe for LaserScan msgs as sensor inputs"
+    )
+    topic_sensors_point_clouds_arg = DeclareLaunchArgument(
+        "topic_sensors_point_clouds",
+        default_value='',
+        description="Comma-separated list of topics to subscribe for PointCloud2 msgs as sensor inputs"
+    )
+
+    gui_enable_arg = DeclareLaunchArgument(
+        "gui_enable",
+        default_value='False',
+        description="Whether to show a custom UI with details on the PF status"
+    )
+
     pf_localization_node = Node(
         package='mrpt_pf_localization',
         executable='mrpt_pf_localization_node',
@@ -44,6 +61,9 @@ def generate_launch_description():
             LaunchConfiguration('pf_params_file'),
             {
                 "mrpt_map_config_file": LaunchConfiguration('mrpt_map_config_file'),
+                "topic_sensors_2d_scan": LaunchConfiguration('topic_sensors_2d_scan'),
+                "topic_sensors_point_clouds": LaunchConfiguration('topic_sensors_point_clouds'),
+                "gui_enable": LaunchConfiguration('gui_enable'),
             }],
         arguments=['--ros-args', '--log-level',
                    LaunchConfiguration('log_level')]
@@ -53,5 +73,8 @@ def generate_launch_description():
         mrpt_map_config_file_launch_arg,
         pf_log_level_launch_arg,
         pf_params_file_launch_arg,
+        topic_sensors_2d_scan_arg,
+        topic_sensors_point_clouds_arg,
+        gui_enable_arg,
         pf_localization_node,
     ])

mrpt_pf_localization/package.xml

@@ -28,6 +28,7 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>mp2p_icp</depend>
   <depend>mrpt_msgs</depend>
   <depend>mrpt_msgs_bridge</depend>
   <depend>pose_cov_ops</depend>