From a80d602ac393abf46995d5103b5b46221254286a Mon Sep 17 00:00:00 2001
From: James Pace <jpace121@gmail.com>
Date: Thu, 3 Sep 2026 15:50:21 -0400
Subject: [PATCH] Publish some (hardcoded) frames.

---
 CMakeLists.txt         | 21 ++++++++++++-
 package.xml            |  2 ++
 src/FramePublisher.cpp | 67 ++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 89 insertions(+), 1 deletion(-)
 create mode 100644 src/FramePublisher.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c1a44e5..68858c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,8 @@ find_package(diagnostic_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(fmt REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
 
 add_library(image_publisher_component SHARED src/ImagePublisher.cpp)
 target_include_directories(image_publisher_component PUBLIC
@@ -71,8 +73,25 @@ rclcpp_components_register_node(diagnostic_publisher_component
     EXECUTABLE diagnostic_publisher
 )
 
+add_library(frame_publisher_component SHARED src/FramePublisher.cpp)
+target_include_directories(frame_publisher_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_compile_features(frame_publisher_component PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_link_libraries(frame_publisher_component
+    rclcpp::rclcpp
+    rclcpp_components::component
+    tf2_ros::tf2_ros
+    tf2::tf2
+)
+
+rclcpp_components_register_node(frame_publisher_component
+    PLUGIN "j7s::FramePublisher"
+    EXECUTABLE frame_publisher
+)
+
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-install(TARGETS image_publisher_component position_publisher_component diagnostic_publisher_component
+install(TARGETS image_publisher_component position_publisher_component diagnostic_publisher_component frame_publisher_component
         EXPORT export_${PROJECT_NAME}
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
diff --git a/package.xml b/package.xml
index c900fae..509c2ad 100644
--- a/package.xml
+++ b/package.xml
@@ -14,6 +14,8 @@
   <depend>image_transport</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
   <depend>rclcpp_components</depend>
 
   <export>
diff --git a/src/FramePublisher.cpp b/src/FramePublisher.cpp
new file mode 100644
index 0000000..6c77a70
--- /dev/null
+++ b/src/FramePublisher.cpp
@@ -0,0 +1,67 @@
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/header.hpp>
+#include "tf2_ros/transform_broadcaster.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+
+namespace j7s {
+    class FramePublisher : public rclcpp::Node {
+    public:
+        FramePublisher(const rclcpp::NodeOptions& options);
+    private:
+        std::shared_ptr<tf2_ros::TransformBroadcaster> m_broadcaster;
+        rclcpp::TimerBase::SharedPtr m_timer;
+    };
+};
+
+namespace j7s {
+    FramePublisher::FramePublisher(const rclcpp::NodeOptions& options):
+        Node("frame_publisher", options),
+        m_broadcaster(std::make_shared<tf2_ros::TransformBroadcaster>(this)),
+        m_timer{}
+    {
+        m_timer = this->create_timer(10ms, [this]() -> void {
+            rclcpp::Time now = this->get_clock()->now();
+            const double secs = now.seconds();
+            const double freq = 1.0/20.0;
+            const double magnitude = 4.0;
+
+            geometry_msgs::msg::TransformStamped transform;
+            transform.header.stamp = now;
+            transform.header.frame_id = "odom";
+            transform.child_frame_id = "base_link";
+            transform.transform.translation.x = magnitude * cos(2*M_PI*freq*secs);
+            transform.transform.translation.y = magnitude * sin(2*M_PI*freq*secs);
+            transform.transform.translation.z = 0.0;
+            tf2::Quaternion quat;
+            quat.setRPY(0.0, 0.0, 2*M_PI*freq*secs + M_PI/2.0);
+            transform.transform.rotation.x = quat.x();
+            transform.transform.rotation.y = quat.y();
+            transform.transform.rotation.z = quat.z();
+            transform.transform.rotation.w = quat.w();
+
+            m_broadcaster->sendTransform(transform);
+
+            geometry_msgs::msg::TransformStamped noseTransform;
+            noseTransform.header.stamp = now;
+            noseTransform.header.frame_id = "base_link";
+            noseTransform.child_frame_id = "nose";
+            noseTransform.transform.translation.x = 1.0;
+            noseTransform.transform.translation.y = 0.0;
+            noseTransform.transform.translation.z = 0.0;
+            noseTransform.transform.rotation.x = 0.0;
+            noseTransform.transform.rotation.y = 0.0;
+            noseTransform.transform.rotation.z = 0.0;
+            noseTransform.transform.rotation.w = 1.0;
+            m_broadcaster->sendTransform(noseTransform);
+        });
+    }
+};
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(j7s::FramePublisher)