publish_tf param added (#75)

README.md

@@ -96,6 +96,7 @@ ros2 service call /map_save std_srvs/Empty
 |initial_pose_qy|double|0.0|Quaternion y of the initial pose value|
 |initial_pose_qz|double|0.0|Quaternion z of the initial pose value|
 |initial_pose_qw|double|1.0|Quaternion w of the initial pose value|
+|publish_tf|bool|true|Whether or not to publish tf from global frame to robot frame|
 |use_odom|bool|false|whether odom is used or not for initial attitude in point cloud registration|
 |use_imu|bool|false|whether 9-axis imu(Angular velocity, acceleration and orientation must be included.) is used or not for point cloud distortion correction.(Note that you must also set the `scan_period`.)|
 |debug_flag|bool|false|Whether or not to display the registration information|

scanmatcher/include/scanmatcher/scanmatcher_component.h

@@ -146,6 +146,7 @@ namespace graphslam
     int num_targeted_cloud_;
 
     bool set_initial_pose_ {false};
+    bool publish_tf_ {true};
     bool use_odom_ {false};
     bool use_imu_ {false};
     bool debug_flag_ {false};

scanmatcher/src/scanmatcher_component.cpp

@@ -45,7 +45,7 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options)
   declare_parameter("scan_period", 0.1);
   get_parameter("scan_period", scan_period_);
   declare_parameter("map_publish_period", 15.0);
-  get_parameter("map_publish_period", map_publish_period_);
+  get_parameter("map_publish_period", map_publish_period_);  
   declare_parameter("num_targeted_cloud", 10);
   get_parameter("num_targeted_cloud", num_targeted_cloud_);
   if (num_targeted_cloud_ < 1) {
@@ -70,6 +70,8 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options)
 
   declare_parameter("set_initial_pose", false);
   get_parameter("set_initial_pose", set_initial_pose_);
+  declare_parameter("publish_tf", true);
+  get_parameter("publish_tf", publish_tf_);
   declare_parameter("use_odom", false);
   get_parameter("use_odom", use_odom_);
   declare_parameter("use_imu", false);
@@ -88,6 +90,7 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options)
   std::cout << "scan_min_range[m]:" << scan_min_range_ << std::endl;
   std::cout << "scan_max_range[m]:" << scan_max_range_ << std::endl;
   std::cout << "set_initial_pose:" << std::boolalpha << set_initial_pose_ << std::endl;
+  std::cout << "publish_tf:" << std::boolalpha << publish_tf_ << std::endl;
   std::cout << "use_odom:" << std::boolalpha << use_odom_ << std::endl;
   std::cout << "use_imu:" << std::boolalpha << use_imu_ << std::endl;
   std::cout << "scan_period[sec]:" << scan_period_ << std::endl;
@@ -383,15 +386,17 @@ void ScanMatcherComponent::publishMapAndPose(
   Eigen::Quaterniond quat_eig(rot_mat);
   geometry_msgs::msg::Quaternion quat_msg = tf2::toMsg(quat_eig);
 
-  geometry_msgs::msg::TransformStamped transform_stamped;
-  transform_stamped.header.stamp = stamp;
-  transform_stamped.header.frame_id = global_frame_id_;
-  transform_stamped.child_frame_id = robot_frame_id_;
-  transform_stamped.transform.translation.x = position.x();
-  transform_stamped.transform.translation.y = position.y();
-  transform_stamped.transform.translation.z = position.z();
-  transform_stamped.transform.rotation = quat_msg;
-  broadcaster_.sendTransform(transform_stamped);
+  if(publish_tf_){
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header.stamp = stamp;
+    transform_stamped.header.frame_id = global_frame_id_;
+    transform_stamped.child_frame_id = robot_frame_id_;
+    transform_stamped.transform.translation.x = position.x();
+    transform_stamped.transform.translation.y = position.y();
+    transform_stamped.transform.translation.z = position.z();
+    transform_stamped.transform.rotation = quat_msg;
+    broadcaster_.sendTransform(transform_stamped);
+  }
 
   corrent_pose_stamped_.header.stamp = stamp;
   corrent_pose_stamped_.pose.position.x = position.x();