Add sinusoidal force sensor example node

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})

src/examples/sinusoidal_force_sensor.cpp

@@ -0,0 +1,78 @@
+/*
+# 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);
+    return 0;
+}