Publish some (hardcoded) frames.

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

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>

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)