Fixed timestamp conversion. Srv: changed "global" member to "global_map" (python reserved word). CoreWrapper: fixed imu subscription.

include/rtabmap_ros/MsgConversion.h

@@ -185,8 +185,8 @@ rtabmap::Landmarks landmarksFromROS(
 		double defaultLinVariance,
 		double defaultAngVariance);
 
-inline double timestampFromROS(const rclcpp::Time & stamp) {return double(stamp.seconds()) + double(stamp.nanoseconds())/1000000000.0;}
-inline rclcpp::Time timestampToROS(const double & t) {uint32_t sec= (uint32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));}
+inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
+inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));}
 
 // common stuff
 rtabmap::Transform getTransform(

launch/ros2/rtabmap.launch.py

@@ -41,6 +41,7 @@ def launch_setup(context, *args, **kwargs):
     return [
         DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
         DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
+        DeclareLaunchArgument('args',  default_value=LaunchConfiguration('rtabmap_args'), description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),
         
         #These arguments should not be modified directly, see referred topics without "_relay" suffix above
         DeclareLaunchArgument('rgb_topic_relay',      default_value=ConditionalText(''.join([LaunchConfiguration('rgb_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('rgb_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
@@ -328,7 +329,7 @@ def generate_launch_description():
         DeclareLaunchArgument('database_path',  default_value='~/.ros/rtabmap.db',  description='Where is the map saved/loaded.'),
         DeclareLaunchArgument('queue_size',     default_value='10',                 description=''),
         DeclareLaunchArgument('wait_for_transform', default_value='0.2',            description=''),
-        DeclareLaunchArgument('args',           default_value='',                   description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),
+        DeclareLaunchArgument('rtabmap_args',   default_value='',                   description='Backward compatibility, use "args" instead.'),
         DeclareLaunchArgument('launch_prefix',  default_value='',                   description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
         DeclareLaunchArgument('output',         default_value='screen',             description='Control node output (screen or log).'),
         
@@ -374,8 +375,8 @@ def generate_launch_description():
         DeclareLaunchArgument('odom_topic',                 default_value='odom',  description='Odometry topic name.'),
         DeclareLaunchArgument('vo_frame_id',                default_value=LaunchConfiguration('odom_topic'), description='Visual/Icp odometry frame ID for TF.'),
         DeclareLaunchArgument('publish_tf_odom',            default_value='true',  description=''),
-        DeclareLaunchArgument('odom_tf_angular_variance',   default_value='1.0',   description='If TF is used to get odometry, this is the default angular variance'),
-        DeclareLaunchArgument('odom_tf_linear_variance',    default_value='1.0',   description='If TF is used to get odometry, this is the default linear variance'),
+        DeclareLaunchArgument('odom_tf_angular_variance',   default_value='0.01',    description='If TF is used to get odometry, this is the default angular variance'),
+        DeclareLaunchArgument('odom_tf_linear_variance',    default_value='0.001',   description='If TF is used to get odometry, this is the default linear variance'),
         DeclareLaunchArgument('odom_args',                  default_value='',      description='More arguments for odometry (overwrite same parameters in rtabmap_args).'),
         DeclareLaunchArgument('odom_sensor_sync',           default_value='false', description=''),
         DeclareLaunchArgument('odom_guess_frame_id',        default_value='',      description=''),

src/CoreWrapper.cpp

@@ -784,7 +784,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 #ifdef WITH_APRILTAG_MSGS
 	tagDetectionsSub_ = this->create_subscription<apriltag_ros::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
 #endif
-	imuSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("imu", 100, std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
+	imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", 100, std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
 	republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
 
 	parametersClient_ = std::make_shared<rclcpp::SyncParametersClient>(this);
@@ -875,7 +875,7 @@ void CoreWrapper::saveParameters(const std::string & configFile)
 	}
 	else
 	{
-		RCLCPP_INFO(this->get_logger(), "Parameters are not saved! (No configuration file provided...)");
+		RCLCPP_INFO(this->get_logger(), "Parameters are not saved (No configuration file provided...)");
 	}
 }
 
@@ -3306,7 +3306,7 @@ void CoreWrapper::getMapDataCallback(
 		std::shared_ptr<rtabmap_ros::srv::GetMap::Response> res)
 {
 	RCLCPP_INFO(this->get_logger(), "rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
-			req->global?"true":"false",
+			req->global_map?"true":"false",
 			req->optimized?"true":"false",
 			req->graph_only?"true":"false");
 	std::map<int, Signature> signatures;
@@ -3317,7 +3317,7 @@ void CoreWrapper::getMapDataCallback(
 			poses,
 			constraints,
 			req->optimized,
-			req->global,
+			req->global_map,
 			&signatures,
 			!req->graph_only,
 			!req->graph_only,
@@ -3341,7 +3341,7 @@ void CoreWrapper::getMapData2Callback(
 		std::shared_ptr<rtabmap_ros::srv::GetMap2::Response> res)
 {
 	RCLCPP_INFO(get_logger(), "rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s)...",
-			req->global?"true":"false",
+			req->global_map?"true":"false",
 			req->optimized?"true":"false",
 			req->with_images?"true":"false",
 			req->with_scans?"true":"false",
@@ -3355,7 +3355,7 @@ void CoreWrapper::getMapData2Callback(
 			poses,
 			constraints,
 			req->optimized,
-			req->global,
+			req->global_map,
 			&signatures,
 			req->with_images,
 			req->with_scans,
@@ -3480,7 +3480,7 @@ void CoreWrapper::publishMapCallback(
 				poses,
 				constraints,
 				req->optimized,
-				req->global,
+				req->global_map,
 				&signatures,
 				!req->graph_only,
 				!req->graph_only,

src/GuiWrapper.cpp

@@ -257,7 +257,7 @@ bool GuiWrapper::callMapDataService(const std::string & name, bool global, bool
 	if(client->wait_for_service(std::chrono::seconds(2)))
 	{
 		auto request = std::make_shared<rtabmap_ros::srv::GetMap::Request>();
-		request->global = global;
+		request->global_map = global;
 		request->optimized = optimized;
 		request->graph_only = graphOnly;
 

src/OdometryROS.cpp

@@ -398,6 +398,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 				localTransform);
 
 		imus_.insert(std::make_pair(stamp, imu));
+		//RCLCPP_WARN(get_logger(), "Received imu: %f", stamp);
 
 		if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp())
 		{
@@ -423,7 +424,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 
 	if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < timestampFromROS(header.stamp)))
 	{
-		//NODELET_WARN("No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", stamp.toSec());
+		//RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp));
 
 		// keep in cache to process later when we will receive imu msgs
 		if(bufferedData_.first.isValid())
@@ -452,7 +453,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		imuProcessed_ = true;
 	}
 
-	//NODELET_WARN("img callback: process image %f", stamp.toSec());
+	//RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp));
 
 	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())

src/PreferencesDialogROS.cpp

@@ -42,8 +42,8 @@ using namespace rtabmap;
 
 PreferencesDialogROS::PreferencesDialogROS(rclcpp::Node * node, const QString & configFile, const std::string & rtabmapNodeName) :
 		configFile_(configFile),
-		rtabmapNodeName_(rtabmapNodeName),
-		node_(node)
+		node_(node),
+		rtabmapNodeName_(rtabmapNodeName)
 {
 	UASSERT(node_);
 }

src/impl/CommonDataSubscriberRGBD5.cpp

@@ -253,7 +253,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback(
 void CommonDataSubscriber::setupRGBD5Callbacks(
 		rclcpp::Node& node,
 		bool subscribeOdom,
-		bool subscribeUserData,
+		bool /*subscribeUserData*/,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
 		bool subscribeScanDesc,

src/impl/CommonDataSubscriberRGBD6.cpp

@@ -270,7 +270,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback(
 void CommonDataSubscriber::setupRGBD6Callbacks(
 		rclcpp::Node& node,
 		bool subscribeOdom,
-		bool subscribeUserData,
+		bool /*subscribeUserData*/,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
 		bool subscribeScanDesc,

srv/GetMap.srv

@@ -1,5 +1,5 @@
 #request
-bool global
+bool global_map
 bool optimized
 bool graph_only
 ---

srv/GetMap2.srv

@@ -1,5 +1,5 @@
 #request
-bool global
+bool global_map
 bool optimized
 bool with_images
 bool with_scans

srv/PublishMap.srv

@@ -1,5 +1,5 @@
 #request
-bool global
+bool global_map
 bool optimized
 bool graph_only
 ---