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,12 +78,13 @@
   <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>
 
   <export>
 	<nodelet plugin="${prefix}/nodelet_plugins.xml" />
 	<rviz plugin="${prefix}/rviz_plugins.xml"/>
-    <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
+        <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
   </export>
 </package>

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

@@ -4,14 +4,14 @@ All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
+	* Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
+	  notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
+	* Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
+	  notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
+	  documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
+	* Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
+	  names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
+	  derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
@@ -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")
 	{
 	}
 
@@ -80,10 +84,33 @@ private:
 
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 		pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
-		pnh.param("scan_range_min",         scanRangeMin_, scanRangeMin_);
+		pnh.param("scan_range_min",		 scanRangeMin_, scanRangeMin_);
-		pnh.param("scan_range_max",         scanRangeMax_, scanRangeMax_);
+		pnh.param("scan_range_max",		 scanRangeMax_, scanRangeMax_);
-		pnh.param("scan_voxel_size",        scanVoxelSize_, scanVoxelSize_);
+		pnh.param("scan_voxel_size",		scanVoxelSize_, scanVoxelSize_);
-		pnh.param("scan_normal_k",          scanNormalK_, scanNormalK_);
+		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);