From 500fe3b8aef9a0cb036800e6c2bf88d4f10cae2b Mon Sep 17 00:00:00 2001 From: Di Zeng Date: Wed, 27 Jul 2016 16:44:48 -0700 Subject: [PATCH 1/2] moved all the header file into include --- CMakeLists.txt | 2 +- {src => include/rtabmap_ros}/CoreWrapper.h | 0 {src => include/rtabmap_ros}/GuiWrapper.h | 0 {src => include/rtabmap_ros}/MapsManager.h | 0 {src/nodelets => include/rtabmap_ros}/OdometryROS.h | 0 {src => include/rtabmap_ros}/PreferencesDialogROS.h | 0 src/CoreNode.cpp | 2 +- src/CoreWrapper.cpp | 2 +- src/GuiNode.cpp | 2 +- src/GuiWrapper.cpp | 5 ++--- src/MapAssemblerNode.cpp | 2 +- src/MapsManager.cpp | 2 +- src/{nodelets => }/OdometryROS.cpp | 2 +- src/PreferencesDialogROS.cpp | 2 +- src/nodelets/rgbd_odometry.cpp | 4 ++-- src/nodelets/stereo_odometry.cpp | 2 +- 16 files changed, 13 insertions(+), 14 deletions(-) rename {src => include/rtabmap_ros}/CoreWrapper.h (100%) rename {src => include/rtabmap_ros}/GuiWrapper.h (100%) rename {src => include/rtabmap_ros}/MapsManager.h (100%) rename {src/nodelets => include/rtabmap_ros}/OdometryROS.h (100%) rename {src => include/rtabmap_ros}/PreferencesDialogROS.h (100%) rename src/{nodelets => }/OdometryROS.cpp (99%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 24eb11f6..1171f470 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/src/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h similarity index 100% rename from src/CoreWrapper.h rename to include/rtabmap_ros/CoreWrapper.h diff --git a/src/GuiWrapper.h b/include/rtabmap_ros/GuiWrapper.h similarity index 100% rename from src/GuiWrapper.h rename to include/rtabmap_ros/GuiWrapper.h diff --git a/src/MapsManager.h b/include/rtabmap_ros/MapsManager.h similarity index 100% rename from src/MapsManager.h rename to include/rtabmap_ros/MapsManager.h diff --git a/src/nodelets/OdometryROS.h b/include/rtabmap_ros/OdometryROS.h similarity index 100% rename from src/nodelets/OdometryROS.h rename to include/rtabmap_ros/OdometryROS.h diff --git a/src/PreferencesDialogROS.h b/include/rtabmap_ros/PreferencesDialogROS.h similarity index 100% rename from src/PreferencesDialogROS.h rename to include/rtabmap_ros/PreferencesDialogROS.h diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 4429c125..df608f12 100644 --- a/src/CoreNode.cpp +++ b/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 #include #include diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 9c82fb72..54df64f3 100644 --- a/src/CoreWrapper.cpp +++ b/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 #include diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index 4190ca3a..6fd74d2f 100644 --- a/src/GuiNode.cpp +++ b/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 diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 5682ae71..0b8c1db2 100644 --- a/src/GuiWrapper.cpp +++ b/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 #include @@ -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 "PreferencesDialogROS.h" +#include "rtabmap_ros/PreferencesDialogROS.h" #include #include diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index a8055d53..8e7c295d 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MsgConversion.h" -#include "MapsManager.h" +#include "rtabmap_ros/MapsManager.h" #include #include #include diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index c6f2012b..7b1e2b9c 100644 --- a/src/MapsManager.cpp +++ b/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 #include diff --git a/src/nodelets/OdometryROS.cpp b/src/OdometryROS.cpp similarity index 99% rename from src/nodelets/OdometryROS.cpp rename to src/OdometryROS.cpp index 317265db..5c41c4e2 100644 --- a/src/nodelets/OdometryROS.cpp +++ b/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 #include diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index dbd7957f..d93e7152 100644 --- a/src/PreferencesDialogROS.cpp +++ b/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 #include #include diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp index b0f480cf..8492b018 100644 --- a/src/nodelets/rgbd_odometry.cpp +++ b/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 #include #include @@ -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()) diff --git a/src/nodelets/stereo_odometry.cpp b/src/nodelets/stereo_odometry.cpp index de1a1657..3c8bca95 100644 --- a/src/nodelets/stereo_odometry.cpp +++ b/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" From 0f134d7ba45306586ce8d343afb7ab10f062b0c0 Mon Sep 17 00:00:00 2001 From: Di Zeng Date: Thu, 28 Jul 2016 12:09:18 -0700 Subject: [PATCH 2/2] changed dynamic cast into type function --- src/OdometryROS.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 5c41c4e2..cd5e0eba 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -426,7 +426,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } // local map / reference frame - if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) + if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M()) { pcl::PointCloud cloud; const std::multimap & 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(odometry_)) + // check which type of Odometry is using + if(odometry_->isF2M()) // If it's Frame to Map Odometry { const std::multimap & 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 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(odometry_) != 0; + return odometry_->isF2M(); } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)