ROS node: new param "publish_tf_odom2baselink"

MRPT · Dec 20, 2024 · 1f6069a · 1f6069a
1 parent 67d31fa
commit 1f6069a
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 1 deletion.
diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h
@@ -166,6 +166,8 @@ class MVSimNode
 	/// vehicle without the need to launch additional nodes.
 	bool do_fake_localization_ = true;
 
+	bool publish_tf_odom2baselink_ = true;
+
 	int publisher_history_len_ = 50;
 
    protected:

diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp
@@ -141,6 +141,7 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n)
 	localn_.param("headless", headless_, headless_);
 	localn_.param("period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_);
 	localn_.param("do_fake_localization", do_fake_localization_, do_fake_localization_);
+	localn_.param("publish_tf_odom2baselink", publish_tf_odom2baselink_, publish_tf_odom2baselink_);
 	localn_.param(
 		"force_publish_vehicle_namespace", force_publish_vehicle_namespace_,
 		force_publish_vehicle_namespace_);
@@ -179,6 +180,9 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n)
 	do_fake_localization_ =
 		n_->declare_parameter<bool>("do_fake_localization", do_fake_localization_);
 
+	publish_tf_odom2baselink_ =
+		n_->declare_parameter<bool>("publish_tf_odom2baselink", publish_tf_odom2baselink_);
+
 	publisher_history_len_ =
 		n_->declare_parameter<int>("publisher_history_len", publisher_history_len_);
 
@@ -834,6 +838,7 @@ void MVSimNode::spinNotifyROS()
 				gtOdoMsg.child_frame_id = "base_link";
 
 				pubs.pub_ground_truth->publish(gtOdoMsg);
+
 				if (do_fake_localization_)
 				{
 					Msg_PoseWithCovarianceStamped currentPos;
@@ -901,6 +906,7 @@ void MVSimNode::spinNotifyROS()
 				const mrpt::math::TPose3D odo_pose = gh_veh_pose;
 
 				// TF(namespace <Ri>): /odom -> /base_link
+				if (publish_tf_odom2baselink_)
 				{
 					Msg_TransformStamped tx;
 					tx.header.frame_id = "odom";

diff --git a/mvsim_tutorial/demo_warehouse.launch.py b/mvsim_tutorial/demo_warehouse.launch.py
@@ -23,7 +23,10 @@ def generate_launch_description():
         "headless", default_value='False')
 
     do_fake_localization_arg = DeclareLaunchArgument(
-        "do_fake_localization", default_value='True', description='publish tf odom -> base_link')
+        "do_fake_localization", default_value='True', description='publish tf map -> odom')
+
+    publish_tf_odom2baselink_arg = DeclareLaunchArgument(
+        "publish_tf_odom2baselink", default_value='True', description='publish tf odom -> base_link')
 
     force_publish_vehicle_namespace_arg = DeclareLaunchArgument(
         "force_publish_vehicle_namespace", default_value='False',
@@ -46,6 +49,7 @@ def generate_launch_description():
                 "world_file": LaunchConfiguration('world_file'),
                 "headless": LaunchConfiguration('headless'),
                 "do_fake_localization": LaunchConfiguration('do_fake_localization'),
+                "publish_tf_odom2baselink": LaunchConfiguration('publish_tf_odom2baselink'),
                 "force_publish_vehicle_namespace": LaunchConfiguration('force_publish_vehicle_namespace'),
             }]
     )
@@ -63,6 +67,7 @@ def generate_launch_description():
         world_file_launch_arg,
         headless_launch_arg,
         do_fake_localization_arg,
+        publish_tf_odom2baselink_arg,
         force_publish_vehicle_namespace_arg,
         use_rviz_arg,
         mvsim_node,