parse pointcloud sensors too

mrpt-ros-pkg · Jan 10, 2024 · 6db38d2 · 6db38d2
1 parent 4376739
commit 6db38d2
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 8 deletions.
diff --git a/mrpt_pf_localization/include/mrpt_pf_localization_node.h b/mrpt_pf_localization/include/mrpt_pf_localization_node.h
@@ -69,7 +69,7 @@ class PFLocalizationNode : public rclcpp::Node
 
 		int map_update_skip;
 
-		std::string base_frame_id = "base_link";
+		std::string base_footprint_frame_id = "base_footprint";
 		std::string odom_frame_id = "odom";
 		std::string global_frame_id = "map";
 
@@ -78,7 +78,11 @@ class PFLocalizationNode : public rclcpp::Node
 
 		std::string topic_odometry = "/odom";
 
-		std::string topic_sensors_2d_scan = "/laser1";	// Remove!
+		/// Comma "," separated list of topics to subscribe for LaserScan msgs
+		std::string topic_sensors_2d_scan;
+
+		/// Comma "," separated list of topics to subscribe for PointCloud2 msgs
+		std::string topic_sensors_point_clouds;
 
 		bool update_while_stopped;
 		bool update_sensor_pose;
@@ -101,6 +105,9 @@ class PFLocalizationNode : public rclcpp::Node
 	void loop();
 	void callbackLaser(
 		const sensor_msgs::msg::LaserScan& msg, const std::string& topicName);
+	void callbackPointCloud(
+		const sensor_msgs::msg::PointCloud2& msg, const std::string& topicName);
+
 	void callbackBeacon(const mrpt_msgs::msg::ObservationRangeBeacon&);
 	void callbackRobotPose(
 		const geometry_msgs::msg::PoseWithCovarianceStamped&);
@@ -127,7 +134,7 @@ class PFLocalizationNode : public rclcpp::Node
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr>
 		subs_2dlaser_;
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
-		subs_pointclouds_;
+		subs_point_clouds_;
 
 	// rclcpp::Subscriber sub_map_;
 	// rclcpp::ServiceClient client_map_;
@@ -160,8 +167,7 @@ class PFLocalizationNode : public rclcpp::Node
 
 	bool waitForTransform(
 		mrpt::poses::CPose3D& des, const std::string& target_frame,
-		const std::string& source_frame, const rclcpp::Time& time,
-		const int timeoutMilliseconds);
+		const std::string& source_frame, const int timeoutMilliseconds = 50);
 	bool mapCallback(
 		nav_msgs::srv::GetMap::Request& req,
 		nav_msgs::srv::GetMap::Response& res);

diff --git a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp b/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp
@@ -16,6 +16,7 @@
 #include <mrpt/obs/CObservationRobotPose.h>
 #include <mrpt/ros2bridge/laser_scan.h>
 #include <mrpt/ros2bridge/map.h>
+#include <mrpt/ros2bridge/point_cloud2.h>
 #include <mrpt/ros2bridge/pose.h>
 #include <mrpt/ros2bridge/time.h>
 #include <mrpt/system/COutputLogger.h>
@@ -99,6 +100,21 @@ PFLocalizationNode::PFLocalizationNode(const rclcpp::NodeOptions& options)
 					}));
 		}
 	}
+	{
+		std::vector<std::string> sources;
+		mrpt::system::tokenize(
+			nodeParams_.topic_sensors_point_clouds, " ,\t\n", sources);
+		for (const auto& topic : sources)
+		{
+			numSensors++;
+			subs_point_clouds_.push_back(
+				this->create_subscription<sensor_msgs::msg::PointCloud2>(
+					topic, 1,
+					[topic, this](const sensor_msgs::msg::PointCloud2& msg) {
+						callbackPointCloud(msg, topic);
+					}));
+		}
+	}
 
 	ASSERTMSG_(
 		numSensors > 0,
@@ -262,8 +278,7 @@ void PFLocalizationNode::loop()
 
 bool PFLocalizationNode::waitForTransform(
 	mrpt::poses::CPose3D& des, const std::string& target_frame,
-	const std::string& source_frame, const rclcpp::Time& time,
-	const int timeoutMilliseconds)
+	const std::string& source_frame, const int timeoutMilliseconds)
 {
 	const rclcpp::Duration timeout(0, 1000 * timeoutMilliseconds);
 	try
@@ -295,8 +310,10 @@ void PFLocalizationNode::callbackLaser(
 {
 	RCLCPP_DEBUG(get_logger(), "Received 2D scan (%s)", topicName.c_str());
 
-	MRPT_TODO("ask tf for the actual pose");
+	// get sensor pose on the robot:
 	mrpt::poses::CPose3D sensorPose;
+	waitForTransform(
+		sensorPose, msg.header.frame_id, nodeParams_.base_footprint_frame_id);
 
 	auto obs = mrpt::obs::CObservation2DRangeScan::Create();
 	mrpt::ros2bridge::fromROS(msg, sensorPose, *obs);
@@ -306,6 +323,27 @@ void PFLocalizationNode::callbackLaser(
 	core_.on_observation(obs);
 }
 
+void PFLocalizationNode::callbackPointCloud(
+	const sensor_msgs::msg::PointCloud2& msg, const std::string& topicName)
+{
+	RCLCPP_DEBUG(get_logger(), "Received point cloud (%s)", topicName.c_str());
+
+	// get sensor pose on the robot:
+	mrpt::poses::CPose3D sensorPose;
+	waitForTransform(
+		sensorPose, msg.header.frame_id, nodeParams_.base_footprint_frame_id);
+
+	auto obs = mrpt::obs::CObservationPointCloud::Create();
+	obs->sensorLabel = topicName;
+	auto pts = mrpt::maps::CSimplePointsMap::Create();
+	obs->pointcloud = pts;
+	mrpt::ros2bridge::fromROS(msg, *pts);
+
+	obs->sensorLabel = topicName;
+
+	core_.on_observation(obs);
+}
+
 void PFLocalizationNode::callbackBeacon(
 	const mrpt_msgs::msg::ObservationRangeBeacon& _msg)
 {