Merge pull request #104 from willdzeng/master

Moved all header file in src into include folder, changed the dynamic cast into type function

CMakeLists.txt

@@ -159,7 +159,7 @@ SET(rtabmap_ros_lib_src
    src/nodelets/disparity_to_depth.cpp 
    src/nodelets/obstacles_detection.cpp
    src/nodelets/point_cloud_aggregator.cpp
-   src/nodelets/OdometryROS.cpp
+   src/OdometryROS.cpp
    src/MsgConversion.cpp
    src/MapsManager.cpp
 )   

src/CoreNode.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CoreWrapper.h"
+#include "rtabmap_ros/CoreWrapper.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>

src/CoreWrapper.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CoreWrapper.h"
+#include "rtabmap_ros/CoreWrapper.h"
 
 #include <stdio.h>
 #include <ros/ros.h>

src/GuiNode.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "GuiWrapper.h"
+#include "rtabmap_ros/GuiWrapper.h"
 #include "rtabmap/utilite/ULogger.h"
 
 #include <QApplication>

src/GuiWrapper.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "GuiWrapper.h"
+#include "rtabmap_ros/GuiWrapper.h"
 #include <QApplication>
 #include <QDir>
 
@@ -54,8 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/GetMap.h"
 #include "rtabmap_ros/SetGoal.h"
 #include "rtabmap_ros/SetLabel.h"
-
+#include "rtabmap_ros/PreferencesDialogROS.h"
-#include "PreferencesDialogROS.h"
 
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>

src/MapAssemblerNode.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/ros.h>
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
-#include "MapsManager.h"
+#include "rtabmap_ros/MapsManager.h"
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>

src/MapsManager.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "MapsManager.h"
+#include "rtabmap_ros/MapsManager.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>

src/OdometryROS.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
@@ -426,7 +426,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		}
 
 		// local map / reference frame
-		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
+		if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M())
 		{
 			pcl::PointCloud<pcl::PointXYZ> cloud;
 			const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
@@ -443,7 +443,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 		if(odomLastFrame_.getNumSubscribers())
 		{
-			if(dynamic_cast<OdometryF2M*>(odometry_))
+			// check which type of Odometry is using
+			if(odometry_->isF2M()) // If it's Frame to Map Odometry
 			{
 				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
 				if(words3.size())
@@ -463,10 +464,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					odomLastFrame_.publish(cloudMsg);
 				}
 			}
-			else
+			else if(odometry_->isF2F()) // if Using Frame to Frame Odometry
 			{
-				//Frame to Frame
 				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
+
 				if(refFrame.getWords3().size())
 				{
 					pcl::PointCloud<pcl::PointXYZ> cloud;
@@ -482,6 +483,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
+			}else{
+				NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
 			}
 		}
 	}
@@ -549,7 +552,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 bool OdometryROS::isOdometryF2M() const
 {
-	return dynamic_cast<OdometryF2M*>(odometry_) != 0;
+	return odometry_->isF2M();
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

src/PreferencesDialogROS.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "PreferencesDialogROS.h"
+#include "rtabmap_ros/PreferencesDialogROS.h"
 #include <rtabmap/core/Parameters.h>
 #include <QDir>
 #include <QFileInfo>

src/nodelets/rgbd_odometry.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include <rtabmap_ros/OdometryROS.h>
 
 #include <pluginlib/class_list_macros.h>
 #include <nodelet/nodelet.h>
@@ -208,7 +208,7 @@ private:
 				return;
 			}
 
-			ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+			ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
 
 			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
 			if(localTransform.isNull())

src/nodelets/stereo_odometry.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 #include "pluginlib/class_list_macros.h"
 #include "nodelet/nodelet.h"