Fixed humble build

CMakeLists.txt

@@ -37,6 +37,7 @@ find_package(image_transport REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 #find_package(eigen_conversions REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(pcl_conversions REQUIRED)
@@ -81,6 +82,9 @@ ELSEIF(COMPILER_SUPPORTS_CXX11)
 ENDIF()
 endif()
 
+IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0 AND (${nav2_msgs_VERSION_MINOR} LESS 4 OR (${nav2_msgs_VERSION_MINOR} EQUAL 4 AND ${nav2_msgs_VERSION_PATCH} LESS 7)))
+   ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
+ENDIF()
 
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
 option(RTABMAP_SYNC_USER_DATA "Build with input user data support"  OFF)
@@ -223,6 +227,7 @@ SET(Libraries
    tf2
    tf2_eigen
    tf2_ros
+   tf2_geometry_msgs
    laser_geometry
    message_filters
    class_loader
@@ -335,13 +340,26 @@ ament_target_dependencies(rtabmap_common ${Libraries})
 ament_target_dependencies(rtabmap_sync ${Libraries})
 ament_target_dependencies(rtabmap_plugins ${Libraries})
 
-target_link_libraries(rtabmap_common ${RTABMap_LIBRARIES})
+if("$ENV{ROS_DISTRO}" STRGREATER_EQUAL "humble")
-target_link_libraries(rtabmap_sync rtabmap_common ${RTABMap_LIBRARIES})
+  rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
-target_link_libraries(rtabmap_plugins rtabmap_common ${RTABMap_LIBRARIES})
 
-rosidl_target_interfaces(rtabmap_common ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(rtabmap_common ${cpp_typesupport_target} ${RTABMap_LIBRARIES})
-rosidl_target_interfaces(rtabmap_sync ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(rtabmap_sync rtabmap_common ${cpp_typesupport_targeT} ${RTABMap_LIBRARIES})
-rosidl_target_interfaces(rtabmap_plugins ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(rtabmap_plugins rtabmap_common ${cpp_typesupport_target} ${RTABMap_LIBRARIES})
+else()
+  # foxy, galatic
+  target_link_libraries(rtabmap_common ${RTABMap_LIBRARIES})
+  target_link_libraries(rtabmap_sync rtabmap_common ${RTABMap_LIBRARIES})
+  target_link_libraries(rtabmap_plugins rtabmap_common ${RTABMap_LIBRARIES})
+
+  rosidl_target_interfaces(rtabmap_common ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  rosidl_target_interfaces(rtabmap_sync ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  rosidl_target_interfaces(rtabmap_plugins ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+  function(rosidl_get_typesupport_target var generate_interfaces_target typesupport_name)
+    rosidl_target_interfaces(var generate_interfaces_target typesupport_name)
+  endfunction()
+endif()
 
 rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::RGBDOdometry")
 rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::StereoOdometry")
@@ -504,41 +522,41 @@ find_package("${rmw_implementation}" REQUIRED)
 get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp")
 
 foreach(typesupport_impl ${typesupport_impls})
-  rosidl_target_interfaces(rtabmap_rgbd_odometry
+  rosidl_get_typesupport_target(rtabmap_rgbd_odometry
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_stereo_odometry
+  rosidl_get_typesupport_target(rtabmap_stereo_odometry
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_icp_odometry
+  rosidl_get_typesupport_target(rtabmap_icp_odometry
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_rgbd_relay
+  rosidl_get_typesupport_target(rtabmap_rgbd_relay
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_rgbd_sync
+  rosidl_get_typesupport_target(rtabmap_rgbd_sync
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_rgbdx_sync
+  rosidl_get_typesupport_target(rtabmap_rgbdx_sync
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_rgb_sync
+  rosidl_get_typesupport_target(rtabmap_rgb_sync
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_stereo_sync
+  rosidl_get_typesupport_target(rtabmap_stereo_sync
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_pointcloud_to_depthimage
+  rosidl_get_typesupport_target(rtabmap_pointcloud_to_depthimage
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap_point_cloud_xyzrgb
+  rosidl_get_typesupport_target(rtabmap_point_cloud_xyzrgb
     ${PROJECT_NAME} ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmap
+  rosidl_get_typesupport_target(rtabmap
     ${PROJECT_NAME} ${typesupport_impl}
   )
   IF(RTABMAP_GUI)
-    rosidl_target_interfaces(rtabmapviz
+    rosidl_get_typesupport_target(rtabmapviz
       ${PROJECT_NAME} ${typesupport_impl}
     )
   ENDIF()
@@ -598,7 +616,7 @@ IF(rviz_default_plugins_FOUND)
     ENDIF(Qt5_FOUND)
 
     foreach(typesupport_impl ${typesupport_impls})
-	  rosidl_target_interfaces(rtabmap_rviz_plugins
+	  rosidl_get_typesupport_target(rtabmap_rviz_plugins
 	    ${PROJECT_NAME} ${typesupport_impl}
 	  )
     endforeach()

include/rtabmap_ros/CoreWrapper.h

@@ -245,8 +245,11 @@ private:
 
 	void publishStats(const rclcpp::Time & stamp);
 	void publishCurrentGoal(const rclcpp::Time & stamp);
-
+#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);
 
 	void publishLocalPath(const rclcpp::Time & stamp);

package.xml

@@ -37,6 +37,7 @@
   <build_depend>tf2</build_depend>
   <build_depend>tf2_eigen</build_depend>
   <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>laser_geometry</build_depend>
   <build_depend>pcl_conversions</build_depend>
   <build_depend>message_filters</build_depend>
@@ -70,6 +71,7 @@
   <exec_depend>tf2</exec_depend>
   <exec_depend>tf2_eigen</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
   <exec_depend>laser_geometry</exec_depend>
   <exec_depend>pcl_conversions</exec_depend>
   <exec_depend>message_filters</exec_depend>

src/CoreWrapper.cpp

@@ -680,7 +680,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 				if(!odomFrameId_.empty())
 				{
 					mapToOdomMutex_.lock();
-					rclcpp::Time tfExpiration = now() + rclcpp::Duration(tfTolerance*10e9);
+					rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance*10e9);
 					geometry_msgs::msg::TransformStamped msg;
 					msg.child_frame_id = odomFrameId_;
 					msg.header.frame_id = mapFrameId_;
@@ -906,7 +906,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::msg::Image::ConstSharedPtr
 
 		if(rate_>0.0f)
 		{
-			if(previousStamp_.seconds() > 0.0 && stamp.seconds() > previousStamp_.seconds() && stamp - previousStamp_ < rclcpp::Duration(1.0f/rate_))
+			if(previousStamp_.seconds() > 0.0 && stamp.seconds() > previousStamp_.seconds() && stamp - previousStamp_ < rclcpp::Duration::from_seconds(1.0f/rate_))
 			{
 				return;
 			}
@@ -3592,7 +3592,7 @@ void CoreWrapper::publishMapCallback(
 					marker.color.r = 1.0;
 					marker.color.g = 1.0;
 					marker.color.b = 0.0;
-					marker.lifetime = rclcpp::Duration(2.0f/rate_);
+					marker.lifetime = rclcpp::Duration::from_seconds(2.0f/rate_);
 
 					marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 					marker.text = uNumber2Str(iter->first);
@@ -3705,7 +3705,7 @@ void CoreWrapper::publishMapCallback(
 							marker.color.r = 1.0;
 							marker.color.g = 1.0;
 							marker.color.b = 1.0;
-							marker.lifetime = rclcpp::Duration(2.0f/rate_);
+							marker.lifetime = rclcpp::Duration::from_seconds(2.0f/rate_);
 
 							marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 							marker.text = uNumber2Str(iter->first);
@@ -4279,7 +4279,7 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 				marker.color.r = 0.0;
 				marker.color.g = 1.0;
 				marker.color.b = 0.0;
-				marker.lifetime = rclcpp::Duration(2.0f/rate_);
+				marker.lifetime = rclcpp::Duration::from_seconds(2.0f/rate_);
 
 				marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 				marker.text = uNumber2Str(iter->first);
@@ -4360,7 +4360,7 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 					marker.color.r = 1.0;
 					marker.color.g = 1.0;
 					marker.color.b = 1.0;
-					marker.lifetime = rclcpp::Duration(2.0f/rate_);
+					marker.lifetime = rclcpp::Duration::from_seconds(2.0f/rate_);
 
 					marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 					marker.text = uNumber2Str(poseIter->first);
@@ -4445,9 +4445,14 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 }
 
 void CoreWrapper::goalResponseCallback(
+#ifdef NAV_MSGS_FOXY
 		std::shared_future<GoalHandleNav2::SharedPtr> future)
 {
-	auto goal_handle = future.get();
+        auto goal_handle = future.get();
+#else
+               const GoalHandleNav2::SharedPtr & goal_handle)
+{
+#endif
 	if (!goal_handle) {
 		RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
 		rtabmap_.clearPath(1);

src/MsgConversion.cpp

@@ -2139,7 +2139,7 @@ bool convertScanMsg(
 	rtabmap::Transform tmpT = getTransform(
 			odomFrameId.empty()?frameId:odomFrameId,
 			scan2dMsg.header.frame_id,
-			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration(scan2dMsg.ranges.size()*scan2dMsg.time_increment*10e9),
+			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds(scan2dMsg.ranges.size()*scan2dMsg.time_increment*10e9),
 			tfBuffer,
 			waitForTransform);
 	if(tmpT.isNull())

src/nodelets/icp_odometry.cpp

@@ -306,7 +306,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	// make sure the frame of the laser is updated too
 	Transform localScanTransform = getTransform(this->frameId(),
 			scanMsg->header.frame_id,
-			rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration(scanMsg->ranges.size()*scanMsg->time_increment*10e9),
+			rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment*10e9),
 			tfBuffer(), waitForTransform());
 	if(localScanTransform.isNull())
 	{

src/nodelets/rgb_sync.cpp

@@ -145,7 +145,7 @@ void RGBSync::callback(
 			bool publishCompressed = true;
 			if (compressedRate_ > 0.0)
 			{
-				if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
+				if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
 				{
 					RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
 					publishCompressed = false;

src/nodelets/rgbd_sync.cpp

@@ -190,7 +190,7 @@ void RGBDSync::callback(
 			bool publishCompressed = true;
 			if (compressedRate_ > 0.0)
 			{
-				if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
+				if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
 				{
 					RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
 					publishCompressed = false;

src/nodelets/stereo_sync.cpp

@@ -146,7 +146,7 @@ void StereoSync::callback(
 			bool publishCompressed = true;
 			if (compressedRate_ > 0.0)
 			{
-				if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
+				if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
 				{
 					RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
 					publishCompressed = false;