Broadcast map-to-odom transform (#81)

Related to #40.

Signed-off-by: Nahuel Espinosa <nespinosa@ekumenlabs.com>
Co-authored-by: Nahuel Espinosa <nespinosa@ekumenlabs.com>

beluga_amcl/src/amcl_node.cpp

@@ -172,6 +172,14 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
     declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description =
+      "Set this to false to prevent amcl from publishing the transform "
+      "between the global frame and the odometry frame.";
+    declare_parameter("tf_broadcast", rclcpp::ParameterValue(true), descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description =
@@ -751,15 +759,14 @@ void AmclNode::laser_callback(
     pose_pub_->publish(message);
   }
 
-  {
-    // TODO(nahuel): Publish estimated map to odom transform.
+  if (get_parameter("tf_broadcast").as_bool()) {
     auto message = geometry_msgs::msg::TransformStamped{};
     // Sending a transform that is valid into the future so that odom can be used.
     message.header.stamp = now() +
       tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
     message.header.frame_id = get_parameter("global_frame_id").as_string();
     message.child_frame_id = get_parameter("odom_frame_id").as_string();
-    message.transform = tf2::toMsg(Sophus::SE2d{});
+    message.transform = tf2::toMsg(odom_to_base_transform * pose.inverse());
     tf_broadcaster_->sendTransform(message);
   }
 }

beluga_example/config/params.yaml

@@ -26,6 +26,7 @@ amcl:
     z_hit: 0.5
     z_rand: 0.5
     sigma_hit: 0.2
+    tf_broadcast: true
     transform_tolerance: 1.0
     initial_pose: [0, 2, 0]
     execution_policy: seq

beluga_example/models/turtlebot.model.yaml

@@ -17,6 +17,7 @@ plugins:
     odom_frame_id: odom
     pub_rate: 10
     enable_odom_pub: true
+    odom_pose_noise: [0.005, 0.005, 0.01]
 
   - type: ModelTfPublisher
     name: tf_publisher