ground truth tf from sim option

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml

@@ -1,4 +1,18 @@
 <launch>
+
+    <!-- the below is a hack to get ground truth TF from Isaac. this is to remove map noise for the
+    sponsor demo. set use_sim_ground_truth_tf to true for demo -->
+    <!-- basically we publish the ground truth base link tf from isaac. we then substitute that to
+    the base_link tf -->
+    <!-- I think the error is that MAVROS is running FASTER than our sim?? what about use_sim_time?? -->
+    <!-- it seems MAVROS is sending odometry faster than isaac publishes the TF. so when it does a
+    lookup, it doesn't yet exist?? -->
+    <arg name="use_sim_ground_truth_tf" default="false" />
+
+    <let name="convert_estimated_odom_to_tf" value="true" unless="$(var use_sim_ground_truth_tf)" />
+    <let name="convert_estimated_odom_to_tf" value="false" if="$(var use_sim_ground_truth_tf)" />
+
+
     <node pkg="robot_interface" exec="robot_interface_node"
         namespace="interface"
         output="screen">
@@ -10,21 +24,36 @@
         namespace="interface"
         output="screen" />
 
-    <!-- converts odometry topic into a TF -->
-    <node pkg="robot_interface" exec="odometry_conversion"
-        namespace="odometry_conversion"
+    <node pkg="robot_interface" namespace="odometry_conversion" exec="odometry_conversion"
         output="screen">
         <remap from="odometry_in" to="/$(env ROBOT_NAME)/interface/mavros/local_position/odom" />
         <remap from="odometry_out" to="odometry" />
 
         <param name="odom_input_qos_is_best_effort" value="true" />
         <param name="new_frame_id" value="map" />
         <param name="new_child_frame_id" value="base_link" />
-        <param name="odometry_output_type" value="2" /> <!-- 0: NONE, 1: TRANSFORM, 2: OVERWRITE -->
-        <param name="convert_odometry_to_transform" value="true" />
-        <param name="convert_odometry_to_stabilized_transform" value="true" />
-	<param name="restamp_now_pre" value="false" />
-	<param name="restamp_now_post" value="true" />
+        <param name="odometry_output_type" value="2" />
+
+        <!-- no longer publish TF from estimated odom, this is the hack -->
+        <param name="convert_odometry_to_transform" value="$(var convert_estimated_odom_to_tf)" />
+        <param name="convert_odometry_to_stabilized_transform"
+            value="$(var convert_estimated_odom_to_tf)" />
+        <param name="restamp_now_post" value="true" />
     </node>
+    <node
+        pkg="tf2_ros"
+        exec="static_transform_publisher"
+        name="tf_base_link_is_ground_truth"
+        args="0 0 0 0 0 0 base_link_ground_truth base_link" />
+
+    <!-- Create the stabilized TF from the ground truth TF, since odometry_conversion now isn't
+    doing this from odometry estimate  -->
+    <group if="$(var use_sim_ground_truth_tf)">
+        <node pkg="robot_interface" exec="stabilized_tf_node" output="screen">
+            <param name="parent_frame" value="map_FLU" />
+            <param name="child_frame" value="base_link" />
+        </node>
+    </group>
+
 
-</launch>
+</launch>

robot/ros_ws/src/autonomy/0_interface/robot_interface/CMakeLists.txt

@@ -83,12 +83,37 @@ ament_target_dependencies(odometry_conversion
 install(TARGETS odometry_conversion
   DESTINATION lib/${PROJECT_NAME})
 
+# stabilized tf node conversion
+add_executable(stabilized_tf_node src/stabilized_tf_node.cpp)
+
+target_include_directories(stabilized_tf_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(stabilized_tf_node
+  rclcpp
+  geometry_msgs
+  visualization_msgs
+  nav_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  airstack_msgs
+  airstack_common
+  )
+install(TARGETS stabilized_tf_node
+  DESTINATION lib/${PROJECT_NAME})
+
+
+
 ament_export_include_directories(
   include
   ${mav_msgs_INCLUDE_DIRS}
   ${geometry_msgs_INCLUDE_DIRS}
 )
 
+
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
 

robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp

@@ -34,7 +34,7 @@ class OdometryConversion : public rclcpp::Node {
 
    public:
     OdometryConversion() : Node("odometry_conversion") {
-        ;
+        RCLCPP_INFO_STREAM(get_logger(), "OdometryConversion node started");
         odom_input_qos_is_best_effort =
             airstack::get_param(this, "odom_input_qos_is_best_effort", false);
         new_frame_id = airstack::get_param(this, "new_frame_id", std::string(""));
@@ -77,6 +77,8 @@ class OdometryConversion : public rclcpp::Node {
         tf_buffer = new tf2_ros::Buffer(this->get_clock());
         tf_listener = new tf2_ros::TransformListener(*tf_buffer);
         tf_broadcaster = new tf2_ros::TransformBroadcaster(*this);
+
+        RCLCPP_WARN_STREAM(get_logger(), "Use odometry estimate as TF: " << std::boolalpha << convert_odometry_to_transform);
     }
 
     void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {

robot/ros_ws/src/autonomy/0_interface/robot_interface/src/stabilized_tf_node.cpp

@@ -0,0 +1,79 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/buffer_interface.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+class StabilizedTFNode : public rclcpp::Node
+{
+public:
+    StabilizedTFNode() : Node("stabilized_tf_node")
+    {
+        // Initialize the TF listener and broadcaster
+        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+
+        // Declare parameters for the parent and child frames
+        parent_frame_ = declare_parameter<std::string>("parent_frame", "map");
+        child_frame_ = declare_parameter<std::string>("child_frame", "base_link");
+
+        // Timer to update the stabilized frame at a fixed rate
+        timer_ = create_wall_timer(
+            std::chrono::milliseconds(100),  // 10 Hz
+            std::bind(&StabilizedTFNode::publishStabilizedFrame, this));
+    }
+
+private:
+    void publishStabilizedFrame()
+    {
+        // Lookup the original transform
+        geometry_msgs::msg::TransformStamped transform_stamped;
+        try
+        {
+            transform_stamped = tf_buffer_->lookupTransform(parent_frame_, child_frame_, tf2::TimePointZero);
+        }
+        catch (tf2::TransformException &ex)
+        {
+            RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", parent_frame_.c_str(), child_frame_.c_str(), ex.what());
+            return;
+        }
+
+        // Extract and zero out roll and pitch from the orientation
+        tf2::Quaternion q_orig, q_stabilized;
+        tf2::fromMsg(transform_stamped.transform.rotation, q_orig);
+
+        // Get roll, pitch, and yaw from the original orientation
+        double roll, pitch, yaw;
+        tf2::Matrix3x3(q_orig).getRPY(roll, pitch, yaw);
+
+        // Set roll and pitch to zero, keeping only yaw
+        q_stabilized.setRPY(0.0, 0.0, yaw);
+
+        // Create a stabilized transform with the modified orientation
+        geometry_msgs::msg::TransformStamped stabilized_transform = transform_stamped;
+        stabilized_transform.child_frame_id = child_frame_ + "_stabilized"; // Naming convention
+        stabilized_transform.transform.rotation = tf2::toMsg(q_stabilized);
+
+        // Broadcast the stabilized transform
+        tf_broadcaster_->sendTransform(stabilized_transform);
+    }
+
+    std::string parent_frame_;
+    std::string child_frame_;
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<StabilizedTFNode>());
+    rclcpp::shutdown();
+    return 0;
+}

robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp

@@ -93,9 +93,10 @@ class Trajectory {
     void clear();
     /**
      * @brief Get the closest Waypoint of this trajectory to a given target point
-     * 
+     *
      * @param point the target point
-     * @return std::tuple<bool, Waypoint, size_t, double> whether any are valid, the closest waypoint, the index of the closest waypoint, the distance along the path
+     * @return std::tuple<bool, Waypoint, size_t, double> whether any are valid, the closest
+     * waypoint, the index of the closest waypoint, the distance along the path
      */
     std::tuple<bool, Waypoint, size_t, double> get_closest_point(tf2::Vector3 point);
     bool get_trajectory_distance_at_closest_point(tf2::Vector3 point, double* trajectory_distance);
@@ -285,10 +286,12 @@ class TrajectoryLibrary {
                             parameter_name.c_str());
             }
             RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), "parameter_value: " << parameter_value);
-            std::cout << str << " " << start << " " << end << std::endl;
+            RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+                                str << " " << start << " " << end << std::endl);
             str = str.substr(0, start) + get_string(parameter_value) +
                   str.substr(end + 1, str.size() - (end + 1));
-            std::cout << str << " " << str.find(param_label) << std::endl;
+            RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+                                str << " " << str.find(param_label) << std::endl);
             RCLCPP_DEBUG(node_ptr->get_logger(), "str: %s", str.c_str());
         }
 

robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp

@@ -1263,16 +1263,16 @@ TrajectoryLibrary::TrajectoryLibrary(std::string config_filename,
             dynamic_trajectories.push_back(traj);
         }
     }
-    RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
-                        "dt: " << node_ptr->get_parameter("dt").as_double());
-    RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
-                        "ht: " << node_ptr->get_parameter("ht").as_double());
-    RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
-                        "ht_long: " << node_ptr->get_parameter("ht_long").as_double());
-    RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
-                        "max_velocity: " << node_ptr->get_parameter("max_velocity").as_double());
-    RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
-                        "magnitude: " << node_ptr->get_parameter("magnitude").as_double());
+    // RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+    //                     "dt: " << node_ptr->get_parameter("dt").as_double());
+    // RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+    //                     "ht: " << node_ptr->get_parameter("ht").as_double());
+    // RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+    //                     "ht_long: " << node_ptr->get_parameter("ht_long").as_double());
+    // RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+    //                     "max_velocity: " << node_ptr->get_parameter("max_velocity").as_double());
+    // RCLCPP_DEBUG_STREAM(node_ptr->get_logger(),
+    //                     "magnitude: " << node_ptr->get_parameter("magnitude").as_double());
 }
 
 std::vector<Trajectory> TrajectoryLibrary::get_static_trajectories() {