Made costmap_2d and octomap_ros optional

CMakeLists.txt

@@ -8,13 +8,16 @@ 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 laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
-             genmsg stereo_msgs octomap_ros costmap_2d
+             genmsg stereo_msgs
 )
 
+# Optional components
+find_package(costmap_2d)
+find_package(octomap_ros)
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 find_package(RTABMap 0.8.11 REQUIRED)
-find_package(octomap REQUIRED)
 
 #Qt stuff
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
@@ -86,7 +89,7 @@ catkin_package(
   CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs
                  image_transport tf tf_conversions laser_geometry pcl_conversions 
                  pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
-                 stereo_msgs octomap_ros costmap_2d
+                 stereo_msgs
 )
 
 ###########
@@ -100,14 +103,12 @@ include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
   ${RTABMap_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
-  ${OCTOMAP_INCLUDE_DIRS}
 )
 
 # libraries
 SET(Libraries
    ${catkin_LIBRARIES}
    ${RTABMap_LIBRARIES}
-   ${OCTOMAP_LIBRARIES}
 )
 
 ## RVIZ plugin
@@ -124,8 +125,7 @@ set_property(
    PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
    )
    
-## Declare a cpp library
-add_library(rtabmap_ros
+SET(rtabmap_ros_lib_src
    src/nodelets/data_throttle.cpp
    src/nodelets/stereo_throttle.cpp
    src/nodelets/data_odom_sync.cpp
@@ -139,8 +139,26 @@ add_library(rtabmap_ros
    src/rviz/MapGraphDisplay.cpp
    src/rviz/InfoDisplay.cpp
    src/rviz/OrbitOrientedViewController.cpp
-   src/costmap_2d/static_layer.cpp
    ${MOC_FILES}
+)   
+
+# If costmap_2d is found, add the plugin
+IF(costmap_2d_FOUND)
+MESSAGE(STATUS "WITH costmap_2d")
+include_directories(${costmap_2d_INCLUDE_DIRS})
+SET(Libraries
+  ${costmap_2d_LIBRARIES}
+  ${Libraries}
+  )
+SET(rtabmap_ros_lib_src
+  src/costmap_2d/static_layer.cpp
+  ${rtabmap_ros_lib_src}
+  )
+ENDIF(costmap_2d_FOUND)
+
+## Declare a cpp library
+add_library(rtabmap_ros
+   ${rtabmap_ros_lib_src}
 )
 target_link_libraries(rtabmap_ros
   ${Libraries}
@@ -149,6 +167,19 @@ target_link_libraries(rtabmap_ros
 )
 add_dependencies(rtabmap_ros rtabmap_generate_messages_cpp)
 
+# If octomap is found, add definition
+IF(octomap_ros_FOUND)
+MESSAGE(STATUS "WITH octomap")
+include_directories(
+  ${octomap_ros_INCLUDE_DIRS}
+)
+SET(Libraries
+  ${octomap_ros_LIBRARIES}
+  ${Libraries}
+)
+add_definitions(-DWITH_OCTOMAP)
+ENDIF(octomap_ros_FOUND)
+
 add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
 add_dependencies(rtabmap rtabmap_generate_messages_cpp)
 target_link_libraries(rtabmap rtabmap_ros ${Libraries})

src/CoreWrapper.cpp

@@ -57,10 +57,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <laser_geometry/laser_geometry.h>
 #include <image_geometry/stereo_camera_model.h>
 
+#ifdef WITH_OCTOMAP
 #include <octomap/octomap.h>
 #include <octomap_ros/conversions.h>
 #include <octomap_msgs/Octomap.h>
 #include <octomap_msgs/conversions.h>
+#endif
+
 
 //msgs
 #include "rtabmap_ros/Info.h"
@@ -376,8 +379,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
 	setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
 	listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
+#ifdef WITH_OCTOMAP
 	octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
 	octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
+#endif
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
 
@@ -2509,6 +2514,7 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
 	}
 }
 
+#ifdef WITH_OCTOMAP
 // returned OcTree must be deleted
 // RTAB-Map optimizes the graph at almost each iteration, an octomap cannot
 // be updated online. Only available on service. To have an "online" octomap published as a topic,
@@ -2595,6 +2601,7 @@ bool CoreWrapper::octomapFullCallback(
 	}
 	return success;
 }
+#endif
 
 /**
  * exclusive callbacks:

src/CoreWrapper.h

@@ -47,7 +47,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/GetMap.h>
-#include <octomap_msgs/GetOctomap.h>
 
 #include <rtabmap/core/Statistics.h>
 #include <rtabmap/core/Parameters.h>
@@ -70,6 +69,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
+#ifdef WITH_OCTOMAP
+#include <octomap_msgs/GetOctomap.h>
+#endif
+
 #include <actionlib/client/simple_action_client.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <move_base_msgs/MoveBaseActionGoal.h>
@@ -195,8 +198,10 @@ private:
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
 	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
 	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
+#ifdef WITH_OCTOMAP
 	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
+#endif
 
 	rtabmap::ParametersMap loadParameters(const std::string & configFile);
 	void saveParameters(const std::string & configFile);
@@ -217,7 +222,9 @@ private:
 	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 	void publishLocalPath(const ros::Time & stamp);
 
+#ifdef WITH_OCTOMAP
 	octomap::OcTree * createOctomap();
+#endif
 
 private:
 	rtabmap::Rtabmap rtabmap_;
@@ -386,8 +393,10 @@ private:
 	ros::ServiceServer setGoalSrv_;
 	ros::ServiceServer setLabelSrv_;
 	ros::ServiceServer listLabelsSrv_;
+#ifdef WITH_OCTOMAP
 	ros::ServiceServer octomapBinarySrv_;
 	ros::ServiceServer octomapFullSrv_;
+#endif
 
 	MoveBaseClient mbClient_;