Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel

.github/workflows/docker.yml

@@ -22,18 +22,17 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-#            linux/arm/v7
         - docker_tag: noetic
           docker_path: 'noetic'
           docker_platforms: |
             linux/amd64
             linux/arm64
-#            linux/arm/v7
         - docker_tag: noetic-latest
           docker_path: 'noetic/latest'
           docker_platforms: |
             linux/amd64
             linux/arm64
+            linux/arm/v7
     
     steps:
       -

.github/workflows/scheduled-stats.yml

@@ -0,0 +1,16 @@
+name: RTAB-Map ROS Scheduled Stats Extraction From GitHub
+
+on:
+    workflow_dispatch:
+    schedule:
+        - cron: '0 5 * * *'
+jobs:
+    get_stats:
+        runs-on: ubuntu-latest
+        steps:
+            - name: Update Stats
+              uses: introlab/github-stats-action@v1
+              with:
+                  github-stats-token: ${{ secrets.STATS_TOKEN }}
+                  google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }}
+                  spreadsheet-id: ${{ secrets.SPREADSHEET_ID }}

docker/noetic/latest/Dockerfile

@@ -7,7 +7,15 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . catkin_ws/src/rtabmap_ros
 
-RUN source /ros_entrypoint.sh && \
+RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \
+    source /ros_entrypoint.sh && \
+    cd catkin_ws && \
+    catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
+    cd && \
+    rm -rf catkin_ws ;fi
+
+RUN if [ "$TARGETPLATFORM" != "linux/arm/v7" ]; then \
+    source /ros_entrypoint.sh && \
     cd catkin_ws && \
     apt update && \
     rosdep install --from-paths src --ignore-src -y && \
@@ -15,4 +23,4 @@ RUN source /ros_entrypoint.sh && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
     catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
     cd && \
-    rm -rf catkin_ws
+    rm -rf catkin_ws ;fi

docker/noetic/superpoint/Dockerfile

@@ -50,7 +50,7 @@ RUN apt update && \
 # Install ros dependencies
 RUN apt-get update && \
     apt upgrade -y && \
-    apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros ros-noetic-pybind11-catkin && \
+    apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros ros-noetic-pybind11-catkin ros-noetic-image-transport-plugins ros-noetic-stereo-image-proc && \
     apt-get remove -y ros-noetic-rtabmap && \
     rosdep init && \
     apt-get clean && rm -rf /var/lib/apt/lists/

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -209,11 +209,11 @@ rtabmap::Transform getTransform(
 
 // get moving transform accordingly to a fixed frame. For example get
 // transform of /base_link between two stamps accordingly to /odom frame.
-rtabmap::Transform getTransform(
+rtabmap::Transform getMovingTransform(
-		const std::string & sourceTargetFrame,
+		const std::string & movingFrame,
 		const std::string & fixedFrame,
-		const rclcpp::Time & stampSource,
+		const ros::Time & stampFrom,
-		const rclcpp::Time & stampTarget,
+		const ros::Time & stampTo,
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 

rtabmap_conversions/src/MsgConversion.cpp

@@ -1944,11 +1944,11 @@ rtabmap::Landmarks landmarksFromROS(
 		if(!baseToTag.isNull())
 		{
 			// Correction of the global pose accounting the odometry movement since we received it
-			rtabmap::Transform correction = rtabmap_conversions::getTransform(
+			rtabmap::Transform correction = rtabmap_conversions::getMovingTransform(
 					frameId,
 					odomFrameId,
-					iter->second.first.header.stamp,
 					odomStamp,
+					iter->second.first.header.stamp,
 					listener,
 					waitForTransform);
 			if(!correction.isNull())
@@ -2005,11 +2005,11 @@ rtabmap::Transform getTransform(
 
 // get moving transform accordingly to a fixed frame. For example get
 // transform between moving /base_link between two stamps accordingly to /odom frame.
-rtabmap::Transform getTransform(
+rtabmap::Transform getMovingTransform(
-		const std::string & sourceTargetFrame,
+		const std::string & movingFrame,
 		const std::string & fixedFrame,
-		const rclcpp::Time & stampSource,
+		const ros::Time & stampFrom,
-		const rclcpp::Time & stampTarget,
+		const ros::Time & stampTo,
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform)
 {
@@ -2018,12 +2018,12 @@ rtabmap::Transform getTransform(
 	try
 	{
 		geometry_msgs::msg::TransformStamped tmp;
-		tmp = tfBuffer.lookupTransform(sourceTargetFrame, tf2_ros::fromMsg(stampTarget), sourceTargetFrame, tf2_ros::fromMsg(stampSource), fixedFrame, tf2::durationFromSec(waitForTransform));
+		tmp = tfBuffer.lookupTransform(movingFrame, tf2_ros::fromMsg(stampFrom), movingFrame, tf2_ros::fromMsg(stampTo), fixedFrame, tf2::durationFromSec(waitForTransform));
 		transform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform);
 	}
 	catch(tf2::TransformException & ex)
 	{
-		UWARN("(getting transform movement of %s according to fixed %s) %s", sourceTargetFrame.c_str(), fixedFrame.c_str(), ex.what());
+		UWARN("(getting transform movement of %s according to fixed %s) %s", movingFrame.c_str(), fixedFrame.c_str(), ex.what());
 	}
 	return transform;
 }
@@ -2175,7 +2175,7 @@ bool convertRGBDMsgs(
 		// sync with odometry stamp
 		if(!odomFrameId.empty() && odomStamp != stamp)
 		{
-			rtabmap::Transform sensorT = getTransform(
+			rtabmap::Transform sensorT = getMovingTransform(
 					frameId,
 					odomFrameId,
 					odomStamp,
@@ -2491,7 +2491,7 @@ bool convertStereoMsg(
 	// sync with odometry stamp
 	if(!odomFrameId.empty() && odomStamp != leftImageMsg->header.stamp)
 	{
-		rtabmap::Transform sensorT = getTransform(
+		rtabmap::Transform sensorT = getMovingTransform(
 				frameId,
 				odomFrameId,
 				odomStamp,
@@ -2589,7 +2589,7 @@ bool convertScanMsg(
 		bool outputInFrameId)
 {
 	// make sure the frame of the laser is updated during the whole scan time
-	rtabmap::Transform tmpT = getTransform(
+	rtabmap::Transform tmpT = getMovingTransform(
 			scan2dMsg.header.frame_id,
 			odomFrameId.empty()?frameId:odomFrameId,
 			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
@@ -2632,7 +2632,7 @@ bool convertScanMsg(
 	// sync with odometry stamp
 	if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp)
 	{
-		rtabmap::Transform sensorT = getTransform(
+		rtabmap::Transform sensorT = getMovingTransform(
 				frameId,
 				odomFrameId,
 				odomStamp,
@@ -2744,7 +2744,7 @@ bool convertScan3dMsg(
 	// sync with odometry stamp
 	if(!odomFrameId.empty() && odomStamp != scan3dMsg.header.stamp)
 	{
-		rtabmap::Transform sensorT = getTransform(
+		rtabmap::Transform sensorT = getMovingTransform(
 				frameId,
 				odomFrameId,
 				odomStamp,
@@ -3086,18 +3086,18 @@ bool deskew_impl(
 	{
 		if(tfBuffer != 0)
 		{
-			firstPose = rtabmap_conversions::getTransform(
+			firstPose = rtabmap_conversions::getMovingTransform(
 					input.header.frame_id,
 					fixedFrameId,
-					firstStamp,
 					input.header.stamp,
+					firstStamp,
 					*tfBuffer,
 					0);
-			lastPose = rtabmap_conversions::getTransform(
+			lastPose = rtabmap_conversions::getMovingTransform(
 					input.header.frame_id,
 					fixedFrameId,
-					lastStamp,
 					input.header.stamp,
+					lastStamp,
 					*tfBuffer,
 					0);
 		}
@@ -3183,11 +3183,11 @@ bool deskew_impl(
 			}
 			else
 			{
-				transform = rtabmap_conversions::getTransform(
+				transform = rtabmap_conversions::getMovingTransform(
 						output.header.frame_id,
 						fixedFrameId,
-						stamp,
 						output.header.stamp,
+						stamp,
 						*tfBuffer,
 						0);
 				if(transform.isNull())
@@ -3262,11 +3262,11 @@ bool deskew_impl(
 			}
 			else
 			{
-				transform = rtabmap_conversions::getTransform(
+				transform = rtabmap_conversions::getMovingTransform(
 						output.header.frame_id,
 						fixedFrameId,
-						stamp,
 						output.header.stamp,
+						stamp,
 						*tfBuffer,
 						0);
 				if(transform.isNull())

rtabmap_odom/src/RGBDOdometryNode.cpp

@@ -27,6 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap_odom/rgbd_odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
 
 
 int main(int argc, char **argv)
@@ -67,6 +70,9 @@ int main(int argc, char **argv)
 		arguments.push_back(argv[i]);
 	}
 
+#ifdef RTABMAP_PYTHON
+	rtabmap::PythonInterface pythonInterface;
+#endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);

rtabmap_odom/src/StereoOdometryNode.cpp

@@ -27,6 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap_odom/stereo_odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
 
 int main(int argc, char **argv)
 {
@@ -67,6 +70,9 @@ int main(int argc, char **argv)
 
 	}
 
+#ifdef RTABMAP_PYTHON
+	rtabmap::PythonInterface pythonInterface;
+#endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -300,7 +300,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
 	{
 		// make sure the frame of the laser is updated during the whole scan time
-		rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+		rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
 				scanMsg->header.frame_id,
 				guessFrameId().empty()?frameId():guessFrameId(),
 				scanMsg->header.stamp,

rtabmap_slam/src/CoreWrapper.cpp

@@ -1944,11 +1944,11 @@ void CoreWrapper::process(
 				globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
 
 				// Correction of the global pose accounting the odometry movement since we received it
-				Transform correction = rtabmap_conversions::getTransform(
+				Transform correction = rtabmap_conversions::getMovingTransform(
 						frameId_,
 						odomFrameId,
-						rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec),
 						lastPoseStamp_,
+						rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec),
 						*tfBuffer_,
 						waitForTransform_);
 				if(!correction.isNull())

rtabmap_util/scripts/yaml_to_camera_info.py

@@ -8,7 +8,7 @@ from sensor_msgs.msg import Image
 
 def yaml_to_CameraInfo(yaml_fname):
     with open(yaml_fname, "r") as file_handle:
-        calib_data = yaml.load(file_handle)
+        calib_data = yaml.load(file_handle, Loader=yaml.FullLoader)
  
     msg = CameraInfo()
     msg.width = calib_data["image_width"]
@@ -26,7 +26,9 @@ class YamlToCameraInfo(Node):
         super().__init__('yaml_to_camera_info')
         
         self.declare_parameter('yaml_path', '')
+        self.declare_parameter('scale', 1.0)
         yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
+        scale = self.get_parameter('scale').get_parameter_value().double_value
 
         if not yaml_path:
             print('yaml_path parameter should be set to path of the calibration file!')
@@ -37,6 +39,19 @@ class YamlToCameraInfo(Node):
         
         self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
         
+        if scale!=1.0:
+            self.camera_info_msg.k[0] = self.camera_info_msg.k[0]*scale
+            self.camera_info_msg.k[2] = self.camera_info_msg.k[2]*scale
+            self.camera_info_msg.k[4] = self.camera_info_msg.k[4]*scale
+            self.camera_info_msg.k[5] = self.camera_info_msg.k[5]*scale
+            self.camera_info_msg.p[0] = self.camera_info_msg.p[0]*scale
+            self.camera_info_msg.p[2] = self.camera_info_msg.p[2]*scale
+            self.camera_info_msg.p[3] = self.camera_info_msg.p[3]*scale
+            self.camera_info_msg.p[5] = self.camera_info_msg.p[5]*scale
+            self.camera_info_msg.p[6] = self.camera_info_msg.p[6]*scale
+            self.camera_info_msg.width = int(self.camera_info_msg.width*scale)
+            self.camera_info_msg.height = int(self.camera_info_msg.height*scale)
+
         self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
         self.subscription = self.create_subscription(
             Image,

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -51,7 +51,7 @@ LidarDeskewing::~LidarDeskewing()
 void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
 {
 	// make sure the frame of the laser is updated during the whole scan time
-	rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+	rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
 			msg->header.frame_id,
 			fixedFrameId_,
 			msg->header.stamp,

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -289,11 +289,11 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 			   cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
 			{
 				// approx sync
-				cloudDisplacement = rtabmap_conversions::getTransform(
+				cloudDisplacement = rtabmap_conversions::getMovingTransform(
 						frameId, //sourceTargetFrame
 						fixedFrameId_, //fixedFrame
-						cloudMsgs[i]->header.stamp, //stampSource
 						cloudMsgs[0]->header.stamp, //stampTarget
+						cloudMsgs[i]->header.stamp, //stampSource
 						*tfBuffer_,
 						waitForTransform_);
 			}

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -138,11 +138,11 @@ void PointCloudToDepthImage::callback(
 		if(!fixedFrameId_.empty())
 		{
 			// approx sync
-			cloudDisplacement = rtabmap_conversions::getTransform(
+			cloudDisplacement = rtabmap_conversions::getMovingTransform(
 					pointCloud2Msg->header.frame_id,
 					fixedFrameId_,
-					cameraInfoMsg->header.stamp,
 					pointCloud2Msg->header.stamp,
+					cameraInfoMsg->header.stamp,
 					*tfBuffer_,
 					waitForTransform_);
 		}