diff --git a/CMakeLists.txt b/CMakeLists.txt index c5dec016..3fbb0eec 100644 --- a/CMakeLists.txt +++ b/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.12 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}) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 38bd7942..1e5e0901 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -57,10 +57,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef WITH_OCTOMAP #include #include #include #include +#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); @@ -2587,6 +2592,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, @@ -2673,6 +2679,7 @@ bool CoreWrapper::octomapFullCallback( } return success; } +#endif /** * exclusive callbacks: diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 18b8d775..8d79543b 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -47,7 +47,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include @@ -70,6 +69,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef WITH_OCTOMAP +#include +#endif + #include #include #include @@ -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_; @@ -387,8 +394,10 @@ private: ros::ServiceServer setGoalSrv_; ros::ServiceServer setLabelSrv_; ros::ServiceServer listLabelsSrv_; +#ifdef WITH_OCTOMAP ros::ServiceServer octomapBinarySrv_; ros::ServiceServer octomapFullSrv_; +#endif MoveBaseClient mbClient_;