Added rtabmap_ros plugin interface

CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
              cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
              image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure message_filters class_loader rosgraph_msgs
-             genmsg stereo_msgs move_base_msgs image_geometry 
+             genmsg stereo_msgs move_base_msgs image_geometry pluginlib
 )
 
 # Optional components
@@ -185,6 +185,7 @@ SET(rtabmap_ros_lib_src
    src/MsgConversion.cpp
    src/MapsManager.cpp
    src/OdometryROS.cpp
+   src/PluginInterface.cpp
 )
   
 SET(rtabmap_plugins_lib_src
@@ -539,4 +540,3 @@ ENDIF(costmap_2d_FOUND)
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-

include/rtabmap_ros/PluginInterface.h

@@ -0,0 +1,45 @@
+#ifndef PLUGIN_INTERFACE_H_
+#define PLUGIN_INTERFACE_H_
+
+#include <ros/ros.h>
+#include <string>
+#include <sensor_msgs/PointCloud2.h>
+
+namespace rtabmap_ros
+{
+
+class PluginInterface
+{
+public:
+  PluginInterface();
+  virtual ~PluginInterface() {}
+
+  const std::string getName() const
+  {
+    return name_;
+  }
+
+  bool isEnabled() {
+    return enabled_;
+  }
+
+  void initialize(const std::string name, ros::NodeHandle & nh);
+
+  virtual sensor_msgs::PointCloud2 filterPointCloud(const sensor_msgs::PointCloud2 msg) = 0;
+
+protected:
+  /** @brief This is called at the end of initialize().  Override to
+   * implement subclass-specific initialization.
+   **/
+  virtual void onInitialize() {}
+
+  bool enabled_;  ///< Currently this var is managed by subclasses. TODO: make this managed by this class and/or container class.
+  std::string name_;
+  ros::NodeHandle nh_;
+
+};
+
+}  // namespace rtabmap_ros
+
+#endif  // PLUGIN_INTERFACE_H_
+

package.xml

@@ -43,6 +43,7 @@
   <build_depend>image_geometry</build_depend>
   <build_depend>find_object_2d</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>pluginlib</build_depend>
 
   <run_depend>cv_bridge</run_depend>
   <run_depend>roscpp</run_depend>
@@ -77,6 +78,7 @@
   <run_depend>image_geometry</run_depend>
   <run_depend>find_object_2d</run_depend>
   <run_depend>message_runtime</run_depend>
+  <run_depend>pluginlib</run_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
 

src/PluginInterface.cpp

@@ -0,0 +1,21 @@
+#include "rtabmap_ros/PluginInterface.h"
+
+namespace rtabmap_ros
+{
+
+PluginInterface::PluginInterface()
+  :  enabled_(false)
+  , name_()
+{
+}
+
+void PluginInterface::initialize(const std::string name, ros::NodeHandle & nh)
+{
+    name_ = name;
+    nh_ = ros::NodeHandle(nh, name);
+    onInitialize();
+}
+
+
+}  // end namespace rtabmap_ros
+

src/nodelets/icp_odometry.cpp

@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/OdometryROS.h>
 
 #include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_loader.hpp>
+
 #include <nodelet/nodelet.h>
 
 #include <laser_geometry/laser_geometry.h>
@@ -37,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 
 #include "rtabmap_ros/MsgConversion.h"
+#include "rtabmap_ros/PluginInterface.h"
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_surface.h>
@@ -63,7 +66,8 @@ public:
 		scanRangeMax_(0),
 		scanVoxelSize_(0.0),
 		scanNormalK_(0),
-		scanNormalRadius_(0.0)
+		scanNormalRadius_(0.0),
+		plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface")
 	{
 	}
 
@@ -84,6 +88,29 @@ private:
 		pnh.param("scan_range_max",		 scanRangeMax_, scanRangeMax_);
 		pnh.param("scan_voxel_size",		scanVoxelSize_, scanVoxelSize_);
 		pnh.param("scan_normal_k",		  scanNormalK_, scanNormalK_);
+
+		if (pnh.hasParam("plugins"))
+		{
+			XmlRpc::XmlRpcValue pluginsList;
+			pnh.getParam("plugins", pluginsList);
+
+			for (int32_t i = 0; i < pluginsList.size(); ++i)
+			{
+				std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
+				std::string type = static_cast<std::string>(pluginsList[i]["type"]);
+				NODELET_INFO("IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
+				try {
+					boost::shared_ptr<rtabmap_ros::PluginInterface> plugin = plugin_loader_.createInstance(type);
+					plugins_.push_back(plugin);
+					plugin->initialize(pluginName, pnh);
+				}
+				catch(pluginlib::PluginlibException & ex) {
+					ROS_ERROR("Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
+				}
+
+			}
+		}
+
 		if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
 		{
 			ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
@@ -307,19 +334,50 @@ private:
 		this->processData(data, scanMsg->header.stamp);
 	}
 
-	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
 	{
 		if(this->isPaused())
 		{
 			return;
 		}
+
+		sensor_msgs::PointCloud2 cloudMsg;
+		if (!plugins_.empty())
+		{
+			if (plugins_[0]->isEnabled())
+			{
+				cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
+			} else
+				{
+					NODELET_WARN("Plugin: %s is not enabled, filtering will not occur."
+								 " Make sure to set enabled_ to true once initialization is done.",
+								 plugins_[0]->getName().c_str());
+					cloudMsg = *pointCloudMsg;
+				}
+
+			if (plugins_.size() > 1)
+			{
+				for (int i = 1; i < plugins_.size(); i++) {
+					if (plugins_[i]->isEnabled())
+						cloudMsg = plugins_[i]->filterPointCloud(cloudMsg);
+					else
+						NODELET_WARN("Plugin: %s is not enabled, filtering will not occur."
+									 " Make sure to set enabled_ to true once initialization is done.",
+									 plugins_[i]->getName().c_str());
+				}
+			}
+		} else
+			{
+			cloudMsg = *pointCloudMsg;
+			}
+
 		cv::Mat scan;
 		bool containNormals = false;
 		if(scanVoxelSize_ == 0.0f)
 		{
-			for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+			for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
 			{
-				if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+				if(cloudMsg.fields[i].name.compare("normal_x") == 0)
 				{
 					containNormals = true;
 					break;
@@ -327,24 +385,24 @@ private:
 			}
 		}
 
-		Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+		Transform localScanTransform = getTransform(this->frameId(), cloudMsg.header.frame_id, cloudMsg.header.stamp);
 		if(localScanTransform.isNull())
 		{
-			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
+			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg.header.stamp.toSec());
 			return;
 		}
-		if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
+		if(scanCloudMaxPoints_ == 0 && cloudMsg.height > 1)
 		{
-			scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
+			scanCloudMaxPoints_ = cloudMsg.height * cloudMsg.width;
 			NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
 					"cloud is not dense, for convenience it will be set to %d (%dx%d)",
-					scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
+					scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
 		}
 		int maxLaserScans = scanCloudMaxPoints_;
 		if(containNormals)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			pcl::fromROSMsg(cloudMsg, *pclScan);
 			if(pclScan->size() && scanDownsamplingStep_ > 1)
 			{
 				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -355,14 +413,14 @@ private:
 			{
 				sensor_msgs::PointCloud2 msg;
 				pcl::toROSMsg(*pclScan, msg);
-				msg.header = cloudMsg->header;
+				msg.header = cloudMsg.header;
 				filtered_scan_pub_.publish(msg);
 			}
 		}
 		else
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			pcl::fromROSMsg(cloudMsg, *pclScan);
 			if(pclScan->size() && scanDownsamplingStep_ > 1)
 			{
 				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -394,7 +452,7 @@ private:
 					{
 						sensor_msgs::PointCloud2 msg;
 						pcl::toROSMsg(*pclScanNormal, msg);
-						msg.header = cloudMsg->header;
+						msg.header = cloudMsg.header;
 						filtered_scan_pub_.publish(msg);
 					}
 				}
@@ -406,7 +464,7 @@ private:
 					{
 						sensor_msgs::PointCloud2 msg;
 						pcl::toROSMsg(*pclScan, msg);
-						msg.header = cloudMsg->header;
+						msg.header = cloudMsg.header;
 						filtered_scan_pub_.publish(msg);
 					}
 				}
@@ -425,9 +483,9 @@ private:
 				cv::Mat(),
 				CameraModel(),
 				0,
-				rtabmap_ros::timestampFromROS(cloudMsg->header.stamp));
+				rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
 
-		this->processData(data, cloudMsg->header.stamp);
+		this->processData(data, cloudMsg.header.stamp);
 	}
 
 protected:
@@ -447,6 +505,9 @@ private:
 	double scanVoxelSize_;
 	int scanNormalK_;
 	double scanNormalRadius_;
+	std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
+	pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
+
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);