Tutorial fixes and API change (#15)

* fix yaml file and lint simulator

* fixes to work with other frames

* pre-commit fixes

* undo tutorial changes

* Update fuse_models/src/transform_sensor.cpp

Co-authored-by: Richard Pratt <43280770+rlpratt12@users.noreply.github.com>

---------

Co-authored-by: Richard Pratt <43280770+rlpratt12@users.noreply.github.com>

fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp

@@ -83,6 +83,9 @@ struct TransformSensorParams : public ParameterBase
 
     target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame);
 
+    estimation_frame =
+        fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "estimation_frame"), estimation_frame);
+
     pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss"));
     pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"),
                                           std::vector<double>{ 1, 1, 1, 1, 1, 1 });
@@ -98,6 +101,7 @@ struct TransformSensorParams : public ParameterBase
   int queue_size{ 10 };
   std::vector<std::string> transforms;
   std::string target_frame;
+  std::string estimation_frame;
   std::vector<size_t> position_indices;
   std::vector<size_t> orientation_indices;
   fuse_core::Loss::SharedPtr pose_loss;

fuse_models/src/transform_sensor.cpp

@@ -31,6 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/impl/utils.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/message_filter.h>
 #include <memory>
@@ -101,6 +103,11 @@ void TransformSensor::onInit()
     transforms_of_interest_.insert(name);
   }
 
+  if (params_.estimation_frame.empty())
+  {
+    throw std::runtime_error("No estimation frame specified.");
+  }
+
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(clock_);
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_, &interfaces_);
 }
@@ -125,26 +132,75 @@ void TransformSensor::process(MessageType const& msg)
 {
   for (auto const& transform : msg.transforms)
   {
-    std::string const& tf_name = transform.child_frame_id;
-    if (transforms_of_interest_.find(tf_name) == transforms_of_interest_.end())
+    std::string const& parent_tf_name = transform.header.frame_id;
+    bool const parent_of_interest = transforms_of_interest_.find(parent_tf_name) != transforms_of_interest_.end();
+
+    std::string const& child_tf_name = transform.child_frame_id;
+    bool const child_of_interest = transforms_of_interest_.find(child_tf_name) != transforms_of_interest_.end();
+
+    // we should skip if child_of_interest xnor parent_of_interest (we want one or the other)
+    if ((child_of_interest && parent_of_interest) || (!child_of_interest && !parent_of_interest))
     {
       // we don't care about this transform, skip it
-      RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str());
+      RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(),
+                   child_tf_name.c_str());
       continue;
     }
-    RCLCPP_DEBUG(logger_, "Got transform of interest from %s to %s", transform.header.frame_id.c_str(), tf_name.c_str());
+    // we must either have april tag -> estimation frame or estimation frame -> april tag tf
+    if (child_of_interest)
+    {
+      if (parent_tf_name != params_.estimation_frame)
+      {
+        // we don't care about this transform , skip it
+        RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(),
+                     child_tf_name.c_str());
+        continue;
+      }
+    }
+    else
+    {
+      if (child_tf_name != params_.estimation_frame)
+      {
+        // we don't care about this transform , skip it
+        RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(),
+                     child_tf_name.c_str());
+        continue;
+      }
+    }
+    RCLCPP_DEBUG(logger_, "Got transform of interest from %s to %s", transform.header.frame_id.c_str(),
+                 child_tf_name.c_str());
     // Create a transaction object
     auto transaction = fuse_core::Transaction::make_shared();
     transaction->stamp(transform.header.stamp);
 
     // Create the pose from the transform
+    // we want a measurement from the april tag (transform of interest) to some reference frame
+    // if we have the opposite, invert it and use that
     auto pose = std::make_unique<geometry_msgs::msg::PoseWithCovarianceStamped>();
-    pose->header = transform.header;
-    pose->header.frame_id = pose->header.frame_id;
-    pose->pose.pose.orientation = transform.transform.rotation;
-    pose->pose.pose.position.x = transform.transform.translation.x;
-    pose->pose.pose.position.y = transform.transform.translation.y;
-    pose->pose.pose.position.z = transform.transform.translation.z;
+    if (parent_of_interest)
+    {
+      pose->header = transform.header;
+      pose->header.frame_id = parent_tf_name;
+      pose->pose.pose.orientation = transform.transform.rotation;
+      pose->pose.pose.position.x = transform.transform.translation.x;
+      pose->pose.pose.position.y = transform.transform.translation.y;
+      pose->pose.pose.position.z = transform.transform.translation.z;
+    }
+    else
+    {
+      // invert the transform
+      tf2::Transform tf_transform;
+      tf2::fromMsg(transform.transform, tf_transform);
+      tf_transform = tf_transform.inverse();
+
+      // use the inverted transform with the header frame id as the frame id of interest
+      pose->header = transform.header;
+      pose->header.frame_id = child_tf_name;
+      pose->pose.pose.orientation = tf2::toMsg(tf_transform.getRotation());
+      pose->pose.pose.position.x = tf_transform.getOrigin().x();
+      pose->pose.pose.position.y = tf_transform.getOrigin().y();
+      pose->pose.pose.position.z = tf_transform.getOrigin().z();
+    }
 
     // TODO(henrygerardmoore): figure out better method to set the covariance
     for (std::size_t i = 0; i < params_.pose_covariance.size(); ++i)

fuse_tutorials/config/fuse_apriltag_tutorial.yaml

@@ -32,8 +32,9 @@ state_estimator:
       initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
 
     transform_sensor:
-      topic: 'april_tf'
+      transforms: ['april_1', 'april_2', 'april_3', 'april_4', 'april_5', 'april_6', 'april_7', 'april_8']
       target_frame: 'base_link'
+      estimation_frame: 'odom'
       position_dimensions: ['x', 'y', 'z']
       orientation_dimensions: ['roll', 'pitch', 'yaw']
       # these are the true covariance values used by the simulator; what happens if we change these?

fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py

@@ -30,7 +30,7 @@ def generate_launch_description():
             Node(
                 package="tf2_ros",
                 executable="static_transform_publisher",
-                arguments=["0", "0", "0", "0", "0", "0", "map", "odom"],
+                arguments=["0", "0", "0", "0", "0", "0", "odom", "map"],
             ),
             # run our simulator
             Node(
@@ -49,6 +49,7 @@ def generate_launch_description():
                         [pkg_dir, "config", "fuse_apriltag_tutorial.yaml"]
                     )
                 ],
+                # prefix=['gdbserver localhost:3000'],
             ),
             # run visualization
             Node(

fuse_tutorials/src/apriltag_simulator.cpp

@@ -32,11 +32,9 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <tf2/LinearMath/Quaternion.h>
-#include <chrono>
 #include <cmath>
 #include <fuse_core/eigen.hpp>
 #include <Eigen/Core>
-#include <memory>
 #include <random>
 
 #include <fuse_core/node_interfaces/node_interfaces.hpp>
@@ -48,8 +46,8 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <string>
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "tf2_msgs/msg/tf_message.hpp"
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
 
 namespace
 {
@@ -90,7 +88,8 @@ struct Robot
   double ay = 0;
   double az = 0;
 };
-
+namespace
+{
 /**
  * @brief Convert the robot state into a ground truth odometry message
  */
@@ -253,65 +252,15 @@ tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot)
   return msg;
 }
 
-void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
-                               const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger)
-{
-  // Send the initial localization signal to the state estimator
-  auto srv = std::make_shared<fuse_msgs::srv::SetPose::Request>();
-  srv->pose.header.frame_id = mapFrame;
-  srv->pose.pose.pose.position.x = state.x;
-  srv->pose.pose.pose.position.y = state.y;
-  srv->pose.pose.pose.position.z = state.z;
-  tf2::Quaternion q;
-  q.setEuler(state.yaw, state.pitch, state.roll);
-  srv->pose.pose.pose.orientation.w = q.w();
-  srv->pose.pose.pose.orientation.x = q.x();
-  srv->pose.pose.pose.orientation.y = q.y();
-  srv->pose.pose.pose.orientation.z = q.z();
-  srv->pose.pose.covariance[0] = 1.0;
-  srv->pose.pose.covariance[7] = 1.0;
-  srv->pose.pose.covariance[14] = 1.0;
-  srv->pose.pose.covariance[21] = 1.0;
-  srv->pose.pose.covariance[28] = 1.0;
-  srv->pose.pose.covariance[35] = 1.0;
-
-  auto const client = rclcpp::create_client<fuse_msgs::srv::SetPose>(
-      interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(),
-      interfaces.get_node_services_interface(), "/state_estimation/set_pose_service",
-      rclcpp::ServicesQoS().get_rmw_qos_profile(), interfaces.get_node_base_interface()->get_default_callback_group());
-
-  while (!client->wait_for_service(std::chrono::seconds(30)) &&
-         interfaces.get_node_base_interface()->get_context()->is_valid())
-  {
-    RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available.");
-  }
-
-  auto success = false;
-  while (!success)
-  {
-    clock->sleep_for(std::chrono::milliseconds(100));
-    srv->pose.header.stamp = clock->now();
-    auto result_future = client->async_send_request(srv);
-
-    if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future,
-                                           std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS)
-    {
-      RCLCPP_ERROR(logger, "set pose service call failed");
-      client->remove_pending_request(result_future);
-      return;
-    }
-    success = result_future.get()->success;
-  }
-}
+}  // namespace
 
 int main(int argc, char** argv)
 {
   // set up our ROS node
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("three_dimensional_simulator");
 
-  // create our sensor publishers
-  auto april_tf_publisher = node->create_publisher<tf2_msgs::msg::TFMessage>("april_tf", 1);
+  // create our sensor publisher
   auto tf_publisher = node->create_publisher<tf2_msgs::msg::TFMessage>("tf", 1);
 
   // create the ground truth publisher
@@ -394,7 +343,7 @@ int main(int argc, char** argv)
     rate.sleep();
 
     // publish simulated position after the static april tag poses since we need them to be in the tf buffer to run
-    april_tf_publisher->publish(simulateAprilTag(new_state));
+    tf_publisher->publish(simulateAprilTag(new_state));
   }
 
   rclcpp::shutdown();