Merge pull request ros-navigation#4 from jmtc7/scanToPC-feature

jmtc7 · web-flow · commit 828047a26016 · 2021-01-21T23:17:44.000+01:00
Scan to pc feature
diff --git a/nav2_localization/CMakeLists.txt b/nav2_localization/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(message_filters REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(laser_geometry REQUIRED)
 
 set(dependencies
   rclcpp
@@ -31,6 +32,7 @@ set(dependencies
   tf2_ros
   tf2
   nav2_msgs
+  laser_geometry
 )
 
 include_directories(include)
diff --git a/nav2_localization/include/nav2_localization/interfaces/matcher2d_base.hpp b/nav2_localization/include/nav2_localization/interfaces/matcher2d_base.hpp
@@ -2,7 +2,7 @@
 #define NAV2_LOCALIZATION__MATCHER2D_BASE_HPP_
 
 #include "geometry_msgs/msg/transform_stamped.hpp"
-#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "nav2_util/lifecycle_node.hpp"
@@ -28,7 +28,7 @@ class Matcher2d
 	 * @param curr_pose A pose to match the scan from.
 	 * @return The probability of the robot being at curr_pose, given scan
 	 */
-	virtual double getScanProbability(const sensor_msgs::msg::LaserScan::ConstSharedPtr &scan,
+	virtual double getScanProbability(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &scan,
 									  const geometry_msgs::msg::TransformStamped &curr_pose) = 0;
 
 	/**
diff --git a/nav2_localization/include/nav2_localization/interfaces/solver_base.hpp b/nav2_localization/include/nav2_localization/interfaces/solver_base.hpp
@@ -9,7 +9,7 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
 // Others
@@ -37,7 +37,7 @@ class Solver
      */
 	virtual geometry_msgs::msg::TransformStamped solve(
 		const geometry_msgs::msg::TransformStamped& curr_odom,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr& laser_scan) = 0;
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) = 0;
 
 	/**
 	 * @brief Initializes the filter being used with a given pose
diff --git a/nav2_localization/include/nav2_localization/nav2_localization.hpp b/nav2_localization/include/nav2_localization/nav2_localization.hpp
@@ -12,11 +12,14 @@
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "message_filters/subscriber.h"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
 #include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "laser_geometry/laser_geometry.hpp"
+
 
 namespace nav2_localization
 {
@@ -85,7 +88,7 @@ class LocalizationServer : public nav2_util::LifecycleNode
     void initTransforms();
 
     /**
-     * @brief Initializes the laser scan message filter
+     * @brief Initializes the scan message filter
      */
     void initMessageFilters();
     
@@ -101,11 +104,17 @@ class LocalizationServer : public nav2_util::LifecycleNode
     void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
 
     /**
-     * @brief Callback when the laser is received
-     * @param laser_scan pointer to the received laser message
+     * @brief Callback when a LaserScan is received. It will convert it to a PC and use the callback for generic scans
+     * @param scan pointer to the received LaserScan message
      */
     void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
 
+    /**
+     * @brief Callback when the scan is received
+     * @param scan pointer to the received PointCloud2 message
+     */
+    void scanReceived(sensor_msgs::msg::PointCloud2::ConstSharedPtr scan);
+
     /**
      * @brief Callback when the initial pose of the robot is received
      * @param msg pointer to the received pose
@@ -116,7 +125,7 @@ class LocalizationServer : public nav2_util::LifecycleNode
     rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
     bool first_map_received_{false};
 
-    // Laser scan
+    // Scan
     std::string scan_topic_;
 
     // Transforms
@@ -130,8 +139,12 @@ class LocalizationServer : public nav2_util::LifecycleNode
 
     // Message filters
     std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
+    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> scan_sub_;
     std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
+    std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> scan_filter_;
     message_filters::Connection laser_scan_connection_;
+    message_filters::Connection scan_connection_;
+	laser_geometry::LaserProjection laser_to_pc_projector_;
 
     // Sample Motion Model Plugin
     pluginlib::ClassLoader<nav2_localization::SampleMotionModel> sample_motion_model_loader_;
diff --git a/nav2_localization/include/nav2_localization/particle_filter.hpp b/nav2_localization/include/nav2_localization/particle_filter.hpp
@@ -1,3 +1,4 @@
+
 #ifndef NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
 #define NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
 
@@ -26,4 +27,4 @@ class ParticleFilter
 };
 } // nav2_localization
 
-#endif // NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
+#endif // NAV2_LOCALIZATION__PARTICLE_FILTER_HPP_
diff --git a/nav2_localization/include/nav2_localization/plugins/matchers/likelihood_field_matcher2d.hpp b/nav2_localization/include/nav2_localization/plugins/matchers/likelihood_field_matcher2d.hpp
@@ -12,7 +12,7 @@ class LikelihoodFieldMatcher2d : public Matcher2d
 public:
 	LikelihoodFieldMatcher2d() : Matcher2d(){}
 
-	double getScanProbability(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan,
+	double getScanProbability(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan,
 							  const geometry_msgs::msg::TransformStamped &curr_pose) override;
 	void setMap(const nav_msgs::msg::OccupancyGrid::SharedPtr &map) override;
 	void setSensorPose(const geometry_msgs::msg::TransformStamped &sensor_pose) override;
@@ -46,4 +46,4 @@ class LikelihoodFieldMatcher2d : public Matcher2d
 };  
 } // nav2_localization
 
-#endif // NAV2_LOCALIZATION__LIKELIHOOD_FIELD_MATCHER2D_HPP_
+#endif // NAV2_LOCALIZATION__LIKELIHOOD_FIELD_MATCHER2D_HPP_
diff --git a/nav2_localization/include/nav2_localization/plugins/sample_motion_models/diff_drive_odom_motion_model.hpp b/nav2_localization/include/nav2_localization/plugins/sample_motion_models/diff_drive_odom_motion_model.hpp
@@ -30,4 +30,4 @@ class DiffDriveOdomMotionModel : public SampleMotionModel
 };
 } // nav2_localization
 
-#endif // NAV2_LOCALIZATION__DIFF_DRIVE_ODOM_MOTION_MODEL_HPP_
+#endif // NAV2_LOCALIZATION__DIFF_DRIVE_ODOM_MOTION_MODEL_HPP_
diff --git a/nav2_localization/include/nav2_localization/plugins/solvers/mcl_solver2d.hpp b/nav2_localization/include/nav2_localization/plugins/solvers/mcl_solver2d.hpp
@@ -9,7 +9,6 @@
 // Types
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "sensor_msgs/msg/laser_scan.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
 // Others
@@ -25,7 +24,7 @@ class MCLSolver2d : public nav2_localization::Solver
 
 	geometry_msgs::msg::TransformStamped solve(
 		const geometry_msgs::msg::TransformStamped& curr_odom,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr& laser_scan) override;
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) override;
 
 	void initFilter(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr &pose) override;
 
@@ -45,4 +44,4 @@ class MCLSolver2d : public nav2_localization::Solver
 };
 } // nav2_localization
 
-#endif // NAV2_LOCALIZATION__MCL_SOLVER2D_HPP_
+#endif // NAV2_LOCALIZATION__MCL_SOLVER2D_HPP_
diff --git a/nav2_localization/package.xml b/nav2_localization/package.xml
@@ -21,6 +21,7 @@
   <depend>sensor_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>laser_geometry</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/nav2_localization/src/.nav2_localization.cpp.swp b/nav2_localization/src/.nav2_localization.cpp.swp
diff --git a/nav2_localization/src/nav2_localization.cpp b/nav2_localization/src/nav2_localization.cpp
@@ -23,7 +23,7 @@ LocalizationServer::LocalizationServer()
     declare_parameter("sample_motion_model_id", default_sample_motion_model_id_);
     declare_parameter("matcher2d_id", default_matcher2d_id_);
     declare_parameter("solver_id", default_solver_id_);
-    declare_parameter("laser_scan_topic", "scan");
+    declare_parameter("scan_topic", "scan");
     declare_parameter("odom_frame_id", "odom");
     declare_parameter("base_frame_id", "base_link");
     declare_parameter("map_frame_id", "map");
@@ -46,7 +46,7 @@ LocalizationServer::on_configure(const rclcpp_lifecycle::State & state)
     get_parameter("sample_motion_model_id", sample_motion_model_id_);
     get_parameter("matcher2d_id", matcher2d_id_);
     get_parameter("solver_id", solver_id_);
-    get_parameter("laser_scan_topic", scan_topic_);
+    get_parameter("scan_topic", scan_topic_);
     get_parameter("odom_frame_id", odom_frame_id_);
     get_parameter("base_frame_id", base_frame_id_);
     get_parameter("map_frame_id", map_frame_id_);
@@ -83,10 +83,10 @@ LocalizationServer::on_cleanup(const rclcpp_lifecycle::State & state)
 {
     initial_pose_sub_.reset();
 
-    // Laser Scan
-    laser_scan_connection_.disconnect();
-    laser_scan_filter_.reset();
-    laser_scan_sub_.reset();
+    // Scan
+    scan_connection_.disconnect();
+    scan_filter_.reset();
+    scan_sub_.reset();
 
     // Transforms
     tf_broadcaster_.reset();
@@ -132,9 +132,11 @@ LocalizationServer::initTransforms()
     tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(rclcpp_node_);
 }
 
+
 void
 LocalizationServer::initMessageFilters()
 {
+	// LaserScan msg
     laser_scan_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
         rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
 
@@ -145,6 +147,18 @@ LocalizationServer::initMessageFilters()
         std::bind(
             &LocalizationServer::laserReceived,
             this, std::placeholders::_1));
+
+	// PointCloud msg
+    scan_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(
+        rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
+
+    scan_filter_ = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(
+        *scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_);
+
+    scan_connection_ = scan_filter_->registerCallback(
+        std::bind(
+            &LocalizationServer::scanReceived,
+            this, std::placeholders::_1));
 }
 
 void
@@ -210,6 +224,16 @@ LocalizationServer::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceSt
 
 void
 LocalizationServer::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
+{
+	sensor_msgs::msg::PointCloud2 scan;
+	laser_to_pc_projector_.projectLaser(*laser_scan, scan);
+	std::shared_ptr<sensor_msgs::msg::PointCloud2> scan_ptr;
+	scan_ptr = std::make_shared<sensor_msgs::msg::PointCloud2>(scan);
+	scanReceived(scan_ptr);
+}
+
+void
+LocalizationServer::scanReceived(sensor_msgs::msg::PointCloud2::ConstSharedPtr scan)
 {
     // Since the sensor data is continually being published by the simulator or robot,
     // we don't want our callbacks to fire until we're in the active state
@@ -220,7 +244,7 @@ LocalizationServer::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr la
     {
         odom_to_base_transform = tf_buffer_->lookupTransform(base_frame_id_,
                                                             odom_frame_id_,
-                                                            laser_scan->header.stamp,
+                                                            scan->header.stamp,
                                                             transform_tolerance_);
     }
     catch(const tf2::TransformException& e)
@@ -230,25 +254,25 @@ LocalizationServer::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr la
     }
 
     // TODO: should this run only once?
-    geometry_msgs::msg::TransformStamped laser_pose;
+    geometry_msgs::msg::TransformStamped sensor_pose;
     try
     {
-        laser_pose = tf_buffer_->lookupTransform(laser_scan->header.frame_id,
+        sensor_pose = tf_buffer_->lookupTransform(scan->header.frame_id,
                                                 base_frame_id_,                 
-                                                laser_scan->header.stamp,
+                                                scan->header.stamp,
                                                 transform_tolerance_);
     }
     catch(const tf2::TransformException& e)
     {
         RCLCPP_ERROR(get_logger(), "%s", e.what());
         return;
     }
-    matcher2d_->setSensorPose(laser_pose);
+    matcher2d_->setSensorPose(sensor_pose);
 
     // The estimated robot's pose in the global frame
-    geometry_msgs::msg::TransformStamped current_pose = solver_->solve(odom_to_base_transform, laser_scan);
+    geometry_msgs::msg::TransformStamped current_pose = solver_->solve(odom_to_base_transform, scan);
 
-    current_pose.header.stamp = laser_scan->header.stamp;
+    current_pose.header.stamp = scan->header.stamp;
     current_pose.header.frame_id = map_frame_id_;
     current_pose.child_frame_id = base_frame_id_;
 
diff --git a/nav2_localization/src/plugins/matchers/likelihood_field_matcher2d.cpp b/nav2_localization/src/plugins/matchers/likelihood_field_matcher2d.cpp
diff --git a/nav2_localization/src/plugins/solvers/mcl_solver2d.cpp b/nav2_localization/src/plugins/solvers/mcl_solver2d.cpp
