From 2904a12b3867be3004020a696c92479dd45e90a9 Mon Sep 17 00:00:00 2001
From: openhands <openhands@all-hands.dev>
Date: Fri, 14 Aug 2026 10:38:30 +0000
Subject: [PATCH 1/2] Add sinusoidal force sensor example node

- Create src/examples/sinusoidal_force_sensor.cpp: a ROS 2 node that
  publishes sinusoidal force/torque readings via sas::ForceSensorServer
- Force readings oscillate with amplitude 2.0 at 100 Hz
- Store latest force and torque readings locally in the node
- Update CMakeLists.txt to build and install the example executable
---
 CMakeLists.txt                           | 18 ++++++
 src/examples/sinusoidal_force_sensor.cpp | 79 ++++++++++++++++++++++++
 2 files changed, 97 insertions(+)
 create mode 100644 src/examples/sinusoidal_force_sensor.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a1a49f9..194330a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,6 +60,24 @@ install(
 
 ##END## CPP LIBRARY #####
 
+##### EXAMPLES #####
+
+add_executable(sinusoidal_force_sensor src/examples/sinusoidal_force_sensor.cpp)
+
+ament_target_dependencies(sinusoidal_force_sensor rclcpp geometry_msgs sas_common sas_core sas_conversions)
+
+target_link_libraries(sinusoidal_force_sensor
+    ${PROJECT_NAME}
+    -ldqrobotics
+    Eigen3::Eigen
+    )
+
+install(TARGETS sinusoidal_force_sensor
+    DESTINATION lib/${PROJECT_NAME}
+    )
+
+##END## EXAMPLES #####
+
 ##### PYBIND11 LIBRARY #####
 
 ament_python_install_package(${PROJECT_NAME})
diff --git a/src/examples/sinusoidal_force_sensor.cpp b/src/examples/sinusoidal_force_sensor.cpp
new file mode 100644
index 0000000..2dd4062
--- /dev/null
+++ b/src/examples/sinusoidal_force_sensor.cpp
@@ -0,0 +1,79 @@
+/*
+# Copyright (c) 2020-2026 Murilo Marques Marinho
+#
+#    This file is part of sas_force_sensor.
+#
+#    sas_force_sensor is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_force_sensor is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_force_sensor.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Murilo M. Marinho, email: murilomarinho@ieee.org
+#
+# ################################################################*/
+#include <cmath>
+
+#include <sas_force_sensor/sas_force_sensor_server.hpp>
+
+namespace
+{
+const double amplitude = 2.0;
+const double frequency_hz = 100.0;
+const double angular_frequency = 2.0 * M_PI * frequency_hz;
+}
+
+class SinusoidalForceSensorNode : public rclcpp::Node
+{
+public:
+    SinusoidalForceSensorNode()
+        : Node("sinusoidal_force_sensor")
+    {
+        server_ = std::make_shared<sas::ForceSensorServer>(
+            std::shared_ptr<Node>(shared_from_this()), "sinusoidal_force_sensor");
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(static_cast<int>(1000.0 / frequency_hz)),
+            std::bind(&SinusoidalForceSensorNode::on_timer, this));
+
+        RCLCPP_INFO(this->get_logger(), "Sinusoidal force sensor node started.");
+    }
+
+private:
+    void on_timer()
+    {
+        const double time_s = static_cast<double>(tick_++) / frequency_hz;
+        const double value = amplitude * std::sin(angular_frequency * time_s);
+
+        force_reading_ = value;
+        torque_reading_ = value;
+
+        DQ force({value, 0.0, 0.0});
+        DQ torque({0.0, value, 0.0});
+
+        server_->send_force_torque(force, torque);
+    }
+
+    std::shared_ptr<sas::ForceSensorServer> server_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    double force_reading_ = 0.0;
+    double torque_reading_ = 0.0;
+    size_t tick_ = 0;
+};
+
+int main(int argc, char* argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<SinusoidalForceSensorNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file

From 6671a77e7ff97fdda08bf14cd4cf2910087e9e17 Mon Sep 17 00:00:00 2001
From: Murilo M Marinho <46012516+mmmarinho@users.noreply.github.com>
Date: Sat, 15 Aug 2026 21:21:54 +0100
Subject: [PATCH 2/2] Update sinusoidal_force_sensor.cpp

---
 src/examples/sinusoidal_force_sensor.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/examples/sinusoidal_force_sensor.cpp b/src/examples/sinusoidal_force_sensor.cpp
index 2dd4062..cfd6808 100644
--- a/src/examples/sinusoidal_force_sensor.cpp
+++ b/src/examples/sinusoidal_force_sensor.cpp
@@ -74,6 +74,5 @@ int main(int argc, char* argv[])
     rclcpp::init(argc, argv);
     auto node = std::make_shared<SinusoidalForceSensorNode>();
     rclcpp::spin(node);
-    rclcpp::shutdown();
     return 0;
-}
\ No newline at end of file
+}