From 68f3d6d61fb37b31bad4a6b2d1f0e424edb2d70d Mon Sep 17 00:00:00 2001
From: openhands <openhands@all-hands.dev>
Date: Mon, 17 Aug 2026 21:24:42 +0000
Subject: [PATCH] Fix sinusoidal_force_sensor build and runtime, set wave to 25
 Hz

- Fix compilation error: construct DQ from Vector3d instead of an
  initializer list (Eigen 3-scalar ctor requires fixed-size vector)
- Fix std::bad_weak_ptr crash at startup: build the node in main first,
  then attach the ForceSensorServer via attach_server() so that
  shared_from_this() is valid
- Set the sinusoidal wave frequency to 25 Hz while keeping the
  100 Hz publish tick rate (separate frequency_hz from tick_rate_hz)

Co-authored-by: openhands <openhands@all-hands.dev>
---
 src/examples/sinusoidal_force_sensor.cpp | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/examples/sinusoidal_force_sensor.cpp b/src/examples/sinusoidal_force_sensor.cpp
index cfd6808..f7a2996 100644
--- a/src/examples/sinusoidal_force_sensor.cpp
+++ b/src/examples/sinusoidal_force_sensor.cpp
@@ -28,7 +28,8 @@
 namespace
 {
 const double amplitude = 2.0;
-const double frequency_hz = 100.0;
+const double frequency_hz = 25.0;
+const double tick_rate_hz = 100.0;
 const double angular_frequency = 2.0 * M_PI * frequency_hz;
 }
 
@@ -37,11 +38,15 @@ class SinusoidalForceSensorNode : public rclcpp::Node
 public:
     SinusoidalForceSensorNode()
         : Node("sinusoidal_force_sensor")
+    {
+    }
+
+    void attach_server()
     {
         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::chrono::milliseconds(static_cast<int>(1000.0 / tick_rate_hz)),
             std::bind(&SinusoidalForceSensorNode::on_timer, this));
 
         RCLCPP_INFO(this->get_logger(), "Sinusoidal force sensor node started.");
@@ -50,14 +55,14 @@ class SinusoidalForceSensorNode : public rclcpp::Node
 private:
     void on_timer()
     {
-        const double time_s = static_cast<double>(tick_++) / frequency_hz;
+        const double time_s = static_cast<double>(tick_++) / tick_rate_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});
+        DQ force(Vector3d(value, 0.0, 0.0));
+        DQ torque(Vector3d(0.0, value, 0.0));
 
         server_->send_force_torque(force, torque);
     }
@@ -73,6 +78,7 @@ int main(int argc, char* argv[])
 {
     rclcpp::init(argc, argv);
     auto node = std::make_shared<SinusoidalForceSensorNode>();
+    node->attach_server();
     rclcpp::spin(node);
     return 0;
 }