Making nav2_msgs optional (not available yet on jazzy). Fixed compila…

…tion errors on jazzy with vision_opencv related h->hpp headers.

rtabmap_conversions/CMakeLists.txt

@@ -46,6 +46,10 @@ if("$ENV{ROS_DISTRO}" STRLESS "humble")
   add_definitions(-DPRE_ROS_HUMBLE)
 endif()
 
+IF("$ENV{ROS_DISTRO}" STRLESS "iron")
+  add_definitions(-DPRE_ROS_IRON)
+ENDIF()
+
 ###########
 ## Build ##
 ###########
@@ -58,6 +62,10 @@ target_include_directories(rtabmap_conversions
 )
 ament_target_dependencies(rtabmap_conversions ${Libraries})
 
+IF("$ENV{ROS_DISTRO}" STRLESS "iron")
+  target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON)
+ENDIF()
+
 #############
 ## Install ##
 #############

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -40,7 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>

rtabmap_conversions/src/MsgConversion.cpp

@@ -38,16 +38,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <pcl_conversions/pcl_conversions.h>
+#ifdef PRE_ROS_IRON
 #include <image_geometry/pinhole_camera_model.h>
 #include <image_geometry/stereo_camera_model.h>
+#else
+#include <image_geometry/pinhole_camera_model.hpp>
+#include <image_geometry/stereo_camera_model.hpp>
+#endif
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/msg/point_field.hpp>
 #include <geometry_msgs/msg/transform.hpp>
 #include <laser_geometry/laser_geometry.hpp>
 #include <rtabmap/core/util3d_surface.h>
 #ifdef PRE_ROS_HUMBLE
 #include <tf2_eigen/tf2_eigen.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #else
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp

@@ -41,7 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 
 namespace rtabmap_odom
 {

rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp

@@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/image_transport.hpp>
 #include <image_transport/subscriber_filter.hpp>
 
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 #include <sensor_msgs/msg/image.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>

rtabmap_odom/src/OdometryROS.cpp

@@ -34,7 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl_conversions/pcl_conversions.h>
 
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 
 #include <rtabmap/core/odometry/OdometryF2M.h>
 #include <rtabmap/core/odometry/OdometryF2F.h>

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -27,7 +27,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_odom/rgbd_odometry.hpp>
 
+#ifdef PRE_ROS_IRON
 #include <image_geometry/stereo_camera_model.h>
+#else
+#include <image_geometry/stereo_camera_model.hpp>
+#endif
 
 #include <sensor_msgs/image_encodings.hpp>
 

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -29,7 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/image_encodings.hpp>
 
+#ifdef PRE_ROS_IRON
 #include <image_geometry/stereo_camera_model.h>
+#else
+#include <image_geometry/stereo_camera_model.hpp>
+#endif
 
 #include "rtabmap_conversions/MsgConversion.h"
 #include <rtabmap_msgs/msg/rgbd_images.hpp>

rtabmap_rviz_plugins/CMakeLists.txt

@@ -46,21 +46,6 @@ MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}")
 ## We also use Ogre for rviz plugins
 include_directories( ${OGRE_INCLUDE_DIRS} )
 
-## RVIZ plugin
-IF(QT4_FOUND)
-    qt4_wrap_cpp(MOC_FILES
-      include/${PROJECT_NAME}/MapCloudDisplay.h
-      include/${PROJECT_NAME}/MapGraphDisplay.h
-      include/${PROJECT_NAME}/InfoDisplay.h
-    )
-ELSE()
-    qt5_wrap_cpp(MOC_FILES
-      include/${PROJECT_NAME}/MapCloudDisplay.h
-      include/${PROJECT_NAME}/MapGraphDisplay.h
-      include/${PROJECT_NAME}/InfoDisplay.h
-    )
-ENDIF()
-
 # tf:message_filters, mixing boost and Qt signals
 set_property(
     SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp src/OrbitOrientedViewController.cpp
@@ -71,8 +56,11 @@ add_library(rtabmap_rviz_plugins SHARED
        src/MapCloudDisplay.cpp
        src/MapGraphDisplay.cpp
        src/InfoDisplay.cpp
-       ${MOC_FILES}
+       include/${PROJECT_NAME}/MapCloudDisplay.h
+       include/${PROJECT_NAME}/MapGraphDisplay.h
+       include/${PROJECT_NAME}/InfoDisplay.h
 )
+set_property(TARGET rtabmap_rviz_plugins PROPERTY AUTOMOC ON)
 target_include_directories(rtabmap_rviz_plugins
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -81,10 +69,6 @@ target_include_directories(rtabmap_rviz_plugins
 
 ament_target_dependencies(rtabmap_rviz_plugins ${Libraries})
 
-IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
-ENDIF(Qt5_FOUND)
-
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
@@ -111,6 +95,4 @@ install(TARGETS
    INCLUDES DESTINATION include
 )
 
-pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml)
-
 ament_package()

rtabmap_slam/CMakeLists.txt

@@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
-find_package(nav2_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -28,12 +27,9 @@ find_package(rtabmap_msgs REQUIRED)
 find_package(rtabmap_util REQUIRED)
 find_package(rtabmap_sync REQUIRED)
 
-IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
-   ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
-ENDIF()
-
 #optional
 find_package(apriltag_msgs)
+find_package(nav2_msgs)
 
 IF(WIN32)
 add_compile_options(-bigobj)
@@ -48,7 +44,6 @@ SET(Libraries
    cv_bridge
    geometry_msgs
    nav_msgs
-   nav2_msgs
    rclcpp
    rclcpp_components
    sensor_msgs
@@ -80,6 +75,19 @@ SET(Libraries
 )
 ENDIF(apriltag_msgs_FOUND)
 
+# If nav2_msgs is found, add definition
+IF(nav2_msgs_FOUND)
+MESSAGE(STATUS "WITH nav2_msgs")
+ADD_DEFINITIONS("-DWITH_NAV2_MSGS")
+SET(Libraries
+   ${Libraries}
+   nav2_msgs
+)
+IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
+   ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
+ENDIF()
+ENDIF(nav2_msgs_FOUND)
+
 ############################
 ## Declare a cpp library
 ############################
@@ -128,4 +136,4 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
-ament_package()
+ament_package()

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -88,8 +88,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <apriltag_msgs/msg/april_tag_detection_array.hpp>
 #endif
 
+#ifdef WITH_NAV2_MSGS
 #include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
+#endif
 
 //#define WITH_FIDUCIAL_MSGS
 #ifdef WITH_FIDUCIAL_MSGS
@@ -109,8 +111,10 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib
 	explicit CoreWrapper(const rclcpp::NodeOptions & options);
 	virtual ~CoreWrapper();
 
+#ifdef WITH_NAV2_MSGS
 	using NavigateToPose = nav2_msgs::action::NavigateToPose;
 	using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
+#endif
 
 private:
 	bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
@@ -246,12 +250,14 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib
 
 	void publishStats(const rclcpp::Time & stamp);
 	void publishCurrentGoal(const rclcpp::Time & stamp);
+#ifdef WITH_NAV2_MSGS
 #ifdef NAV_MSGS_FOXY
 	void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
 #else
         void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
 #endif
 	void resultCallback(const GoalHandleNav2::WrappedResult & result);
+#endif
 
 	void publishLocalPath(const rclcpp::Time & stamp);
 	void publishGlobalPath(const rclcpp::Time & stamp);
@@ -373,7 +379,9 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib
 	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
 	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
 #endif
+#ifdef WITH_NAV2_MSGS
 	rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
+#endif
 
 	std::thread* transformThread_;
 	bool tfThreadRunning_;

rtabmap_slam/src/CoreWrapper.cpp

@@ -36,9 +36,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/msg/bool.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <sensor_msgs/image_encodings.hpp>
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 #include <pcl_conversions/pcl_conversions.h>
+#if PCL_VERSION_COMPARE(>, 1, 12, 0)
+#include <pcl/common/io.h>
+#else
 #include <pcl/io/io.h>
+#endif
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -196,6 +204,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
 	initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
 	useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
+#ifndef WITH_NAV2_MSGS
+	if(useActionForGoal_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support.");
+	}
+#endif
 	useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
 	genScan_ = this->declare_parameter("gen_scan", genScan_);
 	genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_);
@@ -2199,7 +2213,11 @@ void CoreWrapper::process(
 					{
 						// Don't send status yet if nav2 actionlib is used unless it failed,
 						// let nav2 finish reaching the goal
+#ifdef WITH_NAV2_MSGS
 						if(nav2Client_ == 0 || rtabmap_.getPathStatus() <= 0)
+#else
+						if(rtabmap_.getPathStatus() <= 0)
+#endif
 						{
 							if(rtabmap_.getPathStatus() > 0)
 							{
@@ -2209,10 +2227,12 @@ void CoreWrapper::process(
 							else if(rtabmap_.getPathStatus() <= 0)
 							{
 								RCLCPP_WARN(this->get_logger(), "Planning: Plan failed!");
+#ifdef WITH_NAV2_MSGS
 								if(nav2Client_.get()!=NULL && nav2Client_->action_server_is_ready())
 								{
 									nav2Client_->async_cancel_all_goals();
 								}
+#endif
 							}
 
 							if(goalReachedPub_->get_subscription_count())
@@ -3964,11 +3984,12 @@ void CoreWrapper::cancelGoalCallback(
 			goalReachedPub_->publish(result);
 		}
 	}
-
+#ifdef WITH_NAV2_MSGS
 	if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready())
 	{
 		nav2Client_->async_cancel_all_goals();
 	}
+#endif
 }
 
 void CoreWrapper::setLabelCallback(
@@ -4364,6 +4385,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 		poseMsg.header.frame_id = mapFrameId_;
 		poseMsg.header.stamp = stamp;
 		rtabmap_conversions::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
+#ifdef WITH_NAV2_MSGS
 		if(useActionForGoal_)
 		{
 			if(nav2Client_.get() == NULL || !nav2Client_->action_server_is_ready())
@@ -4395,6 +4417,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 				RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
 			}
 		}
+#endif
 		if(nextMetricGoalPub_->get_subscription_count())
 		{
 			nextMetricGoalPub_->publish(poseMsg);
@@ -4405,7 +4428,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 		}
 	}
 }
-
+#ifdef WITH_NAV2_MSGS
 void CoreWrapper::goalResponseCallback(
 #ifdef NAV_MSGS_FOXY
 		std::shared_future<GoalHandleNav2::SharedPtr> future)
@@ -4473,6 +4496,7 @@ void CoreWrapper::resultCallback(
 		latestNodeWasReached_ = false;
 	}
 }
+#endif
 
 void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp)
 {

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h

@@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/image_transport.hpp>
 #include <image_transport/subscriber_filter.hpp>
 
+#ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/msg/image.hpp>

rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap_conversions/MsgConversion.h>
-#include <cv_bridge/cv_bridge.h>
 
 namespace rtabmap_sync {